Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Depth cameras #181

Merged
merged 6 commits into from
Jul 26, 2020
Merged

Depth cameras #181

Changes from 1 commit
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3a04f9f
Depth cameras ros bridge, documentatio
SijmenHuizenga Jul 24, 2020
6a0909b
Better description for depth cameras
SijmenHuizenga Jul 25, 2020
1aefb1f
Added noise
SijmenHuizenga Jul 26, 2020
c6e8b61
Write some rules about depth cameras
SijmenHuizenga Jul 26, 2020
588a437
Depth camera's not allowed during competition
SijmenHuizenga Jul 26, 2020
df4809a
Do not have depth camera in default settings.json
SijmenHuizenga Jul 26, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 26 additions & 2 deletions ros/src/fsds_ros_bridge/src/fsds_ros_bridge_camera.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
#include "statistics.h"
#include "rpc/rpc_error.h"
#include <cv_bridge/cv_bridge.h>

#include <math.h>



Expand Down Expand Up @@ -96,6 +96,30 @@ cv::Mat manual_decode_depth(const ImageResponse& img_response)
return mat;
}

float roundUp(float numToRound, float multiple)
{
assert(multiple);
return ((numToRound + multiple - 1) / multiple) * multiple;
}
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am slightly confused about the rounding - how exactly does it work? Can you provide an example?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

copied it from here ;) https://stackoverflow.com/a/9194117/2328729

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

it rounds up to the nearest multiple of another number


cv::Mat noisify_depthimage(cv::Mat in)
{
cv::Mat out = in.clone();

// Blur
cv::Mat kernel = cv::Mat::ones( 7, 7, CV_32F ) / (float)(7 * 7);
cv::filter2D(in, out, -1 , kernel, cv::Point( -1, -1 ), 0, cv::BORDER_DEFAULT );

// Decrease depth resolution
for (int row = 0; row < in.rows; row++) {
for (int col = 0; col < in.cols; col++) {
float roundtarget = ceil(std::min(std::max(out.at<float>(row, col), 1.0f), 10.0f));
out.at<float>(row, col) = roundUp(out.at<float>(row, col), roundtarget);
}
}

return out;
}

void doDepthImageUpdate(const ros::TimerEvent&) {
auto img_responses = getImage(ImageRequest(camera_name, ImageType::DepthPerspective, true, false));
Expand All @@ -106,7 +130,7 @@ void doDepthImageUpdate(const ros::TimerEvent&) {

ImageResponse img_response = img_responses[0];

cv::Mat depth_img = manual_decode_depth(img_response);
cv::Mat depth_img = noisify_depthimage(manual_decode_depth(img_response));
sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_img).toImageMsg();
img_msg->header.stamp = make_ts(img_response.time_stamp);
img_msg->header.frame_id = "/fsds/camera/"+camera_name;
Expand Down