1 #ifndef GAZEBO_PLUGINS_TEST_CAMERA_DISTORTION_H 2 #define GAZEBO_PLUGINS_TEST_CAMERA_DISTORTION_H 5 #include <opencv2/core/core.hpp> 6 #include <opencv2/imgproc/imgproc.hpp> 9 #include <gtest/gtest.h> 20 MatIterator_<Vec3b> it, end;
21 Vec3b orig_pixel, diff_pixel;
24 for(
int i=0; i<orig.rows; i++)
26 for(
int j=0; j<orig.cols; j++)
28 orig_pixel = orig.at<cv::Vec3b>(i,j);
29 diff_pixel = diff.at<cv::Vec3b>(i,j);
30 total_diff += abs(orig_pixel[0] - diff_pixel[0]) +
31 abs(orig_pixel[1] - diff_pixel[1]) +
32 abs(orig_pixel[2] - diff_pixel[2]);
56 void cameraDistortionTest();
58 void imageCallback(
const sensor_msgs::ImageConstPtr& msg,
int cam_index)
61 assert(cam_index == 0 || cam_index == 1);
64 cam_image_undistorted_ = msg;
68 cam_image_distorted_ = msg;
73 cam_info_distorted_ = msg;
83 cam_sub_undistorted_ =
84 trans.
subscribe(
"/camera_undistorted/image_raw",
87 dynamic_cast<DistortionTest*>(
this), _1, 0)
90 cam_info_distorted_ =
nullptr;
91 cam_image_distorted_ =
nullptr;
94 trans.
subscribe(
"/camera_distorted/image_raw",
97 dynamic_cast<DistortionTest*>(
this), _1, 1)
99 cam_info_distorted_sub_ =
100 nh_.subscribe(
"/camera_distorted/camera_info",
103 dynamic_cast<DistortionTest*>(
this)
108 if(cam_info_distorted_ && cam_image_distorted_) {
109 std::cerr <<
"available immediately" << std::endl;
111 while (!cam_info_distorted_ ||
112 !cam_image_distorted_ ||
113 !cam_image_undistorted_)
117 cam_sub_undistorted_.shutdown();
118 cam_sub_distorted_.shutdown();
119 cam_info_distorted_sub_.shutdown();
122 Mat intrinsic_distorted_matrix = Mat(3, 3, CV_64F);
123 if(cam_info_distorted_->K.size() == 9)
125 memcpy(intrinsic_distorted_matrix.data, cam_info_distorted_->K.data(),
126 cam_info_distorted_->K.size()*
sizeof(double));
128 Mat distortion_coeffs = Mat(5, 1, CV_64F);
129 if(cam_info_distorted_->D.size() == 5)
131 memcpy(distortion_coeffs.data, cam_info_distorted_->D.data(),
132 cam_info_distorted_->D.size()*
sizeof(double));
138 Mat fixed = distorted.clone();
143 cv::Rect myROI(cropBorder, cropBorder,
144 fixed.rows - 2 * cropBorder, fixed.cols - 2 * cropBorder);
145 cv::Mat fixed_crop = fixed(myROI);
146 cv::Mat undistorted_crop = undistorted(myROI);
149 undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
152 ASSERT_GT(distorted.rows, 0);
153 ASSERT_GT(distorted.cols, 0);
154 ASSERT_GT(undistorted.rows, 0);
155 ASSERT_GT(undistorted.cols, 0);
156 ASSERT_GT(fixed.rows, 0);
157 ASSERT_GT(fixed.cols, 0);
161 long diff1 = 0, diff2 = 0;
164 const double OFFSET = 0.01;
167 for(
size_t i = 0; i < 5; ++i)
169 distortion_coeffs.at<
double>(i,0) += OFFSET;
170 undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
172 EXPECT_GE(diff2, diff1);
173 distortion_coeffs.at<
double>(i,0) -= OFFSET;
175 distortion_coeffs.at<
double>(i,0) -= OFFSET;
176 undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
178 EXPECT_GE(diff2, diff1);
179 distortion_coeffs.at<
double>(i,0) += OFFSET;
Subscriber subscribe(const std::string &base_topic, uint32_t queue_size, const boost::function< void(const sensor_msgs::ImageConstPtr &)> &callback, const ros::VoidPtr &tracked_object=ros::VoidPtr(), const TransportHints &transport_hints=TransportHints())
image_transport::Subscriber cam_sub_distorted_
void cameraDistortionTest()
image_transport::Subscriber cam_sub_undistorted_
ros::Subscriber cam_info_distorted_sub_
sensor_msgs::ImageConstPtr cam_image_distorted_
void diffBetween(Mat &orig, Mat &diff, long &total_diff)
void imageCallback(const sensor_msgs::ImageConstPtr &msg, int cam_index)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
sensor_msgs::ImageConstPtr cam_image_undistorted_
void camInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg)
sensor_msgs::CameraInfoConstPtr cam_info_distorted_