distortion.h
Go to the documentation of this file.
1 #ifndef GAZEBO_PLUGINS_TEST_CAMERA_DISTORTION_H
2 #define GAZEBO_PLUGINS_TEST_CAMERA_DISTORTION_H
3 
4 // OpenCV includes
5 #include <opencv2/core/core.hpp>
6 #include <opencv2/imgproc/imgproc.hpp>
7 
8 // Test includes
9 #include <gtest/gtest.h>
10 
11 // ROS includes
12 #include <ros/ros.h>
14 #include <cv_bridge/cv_bridge.h>
15 
16 using namespace cv;
17 
18 void diffBetween(Mat& orig, Mat& diff, long& total_diff)
19 {
20  MatIterator_<Vec3b> it, end;
21  Vec3b orig_pixel, diff_pixel;
22  total_diff = 0;
23 
24  for(int i=0; i<orig.rows; i++)
25  {
26  for(int j=0; j<orig.cols; j++)
27  {
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]);
33  }
34  }
35 }
36 
37 class DistortionTest : public testing::Test
38 {
39  protected:
41 
42  // Used to listen for images
45 
46  // Stores found images
47  sensor_msgs::ImageConstPtr cam_image_distorted_;
48  sensor_msgs::ImageConstPtr cam_image_undistorted_;
49 
50  // Listens for camera metadata to be published
52  // Stores received camera metadata
53  sensor_msgs::CameraInfoConstPtr cam_info_distorted_;
54 
55  public:
56  void cameraDistortionTest();
57 
58  void imageCallback(const sensor_msgs::ImageConstPtr& msg, int cam_index)
59  {
60  // for now, only support 2 cameras
61  assert(cam_index == 0 || cam_index == 1);
62  if(cam_index == 0)
63  {
64  cam_image_undistorted_ = msg;
65  }
66  else
67  {
68  cam_image_distorted_ = msg;
69  }
70  }
71  void camInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg)
72  {
73  cam_info_distorted_ = msg;
74  }
75 };
76 
78 {
80  spinner.start();
81 
83  cam_sub_undistorted_ =
84  trans.subscribe("/camera_undistorted/image_raw",
85  1,
86  boost::bind(&DistortionTest::imageCallback,
87  dynamic_cast<DistortionTest*>(this), _1, 0)
88  );
89 
90  cam_info_distorted_ = nullptr;
91  cam_image_distorted_ = nullptr;
92  // acquire information from ROS topics
93  cam_sub_distorted_ =
94  trans.subscribe("/camera_distorted/image_raw",
95  1,
96  boost::bind(&DistortionTest::imageCallback,
97  dynamic_cast<DistortionTest*>(this), _1, 1)
98  );
99  cam_info_distorted_sub_ =
100  nh_.subscribe("/camera_distorted/camera_info",
101  1,
103  dynamic_cast<DistortionTest*>(this)
104  );
105 
106  // keep waiting until we have an image
107 
108  if(cam_info_distorted_ && cam_image_distorted_) {
109  std::cerr << "available immediately" << std::endl;
110  }
111  while (!cam_info_distorted_ ||
112  !cam_image_distorted_ ||
113  !cam_image_undistorted_)
114  {
115  ros::Duration(0.1).sleep();
116  }
117  cam_sub_undistorted_.shutdown();
118  cam_sub_distorted_.shutdown();
119  cam_info_distorted_sub_.shutdown();
120 
121  // load camera coefficients from published ROS information
122  Mat intrinsic_distorted_matrix = Mat(3, 3, CV_64F);
123  if(cam_info_distorted_->K.size() == 9)
124  {
125  memcpy(intrinsic_distorted_matrix.data, cam_info_distorted_->K.data(),
126  cam_info_distorted_->K.size()*sizeof(double));
127  }
128  Mat distortion_coeffs = Mat(5, 1, CV_64F);
129  if(cam_info_distorted_->D.size() == 5)
130  {
131  memcpy(distortion_coeffs.data, cam_info_distorted_->D.data(),
132  cam_info_distorted_->D.size()*sizeof(double));
133  }
134 
135  // Information acquired, now test the quality of the undistortion
136 
137  Mat distorted = Mat(cv_bridge::toCvCopy(cam_image_distorted_)->image);
138  Mat fixed = distorted.clone();
139  Mat undistorted = Mat(cv_bridge::toCvCopy(cam_image_undistorted_)->image);
140 
141  //crop the image to remove black borders leftover from (un)distortion
142  int cropBorder = 50;
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);
147 
148 
149  undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
150 
151  //Ensure that we didn't crop away everything
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);
158 
159  // The difference between the undistorted image and the no-distortion camera
160  // image should be the lowest when we use the correct distortion parameters.
161  long diff1 = 0, diff2 = 0;
162  diffBetween(fixed_crop, undistorted_crop, diff1);
163 
164  const double OFFSET = 0.01;
165 
166  // test each parameter, varying one at a time
167  for(size_t i = 0; i < 5; ++i)
168  {
169  distortion_coeffs.at<double>(i,0) += OFFSET;
170  undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
171  diffBetween(fixed_crop, undistorted_crop, diff2);
172  EXPECT_GE(diff2, diff1);
173  distortion_coeffs.at<double>(i,0) -= OFFSET;
174 
175  distortion_coeffs.at<double>(i,0) -= OFFSET;
176  undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs);
177  diffBetween(fixed_crop, undistorted_crop, diff2);
178  EXPECT_GE(diff2, diff1);
179  distortion_coeffs.at<double>(i,0) += OFFSET;
180  }
181 }
182 
183 #endif
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_
Definition: distortion.h:43
void cameraDistortionTest()
Definition: distortion.h:77
image_transport::Subscriber cam_sub_undistorted_
Definition: distortion.h:44
ros::Subscriber cam_info_distorted_sub_
Definition: distortion.h:51
bool sleep() const
sensor_msgs::ImageConstPtr cam_image_distorted_
Definition: distortion.h:47
void diffBetween(Mat &orig, Mat &diff, long &total_diff)
Definition: distortion.h:18
void spinner()
void imageCallback(const sensor_msgs::ImageConstPtr &msg, int cam_index)
Definition: distortion.h:58
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
sensor_msgs::ImageConstPtr cam_image_undistorted_
Definition: distortion.h:48
void camInfoCallback(const sensor_msgs::CameraInfoConstPtr &msg)
Definition: distortion.h:71
sensor_msgs::CameraInfoConstPtr cam_info_distorted_
Definition: distortion.h:53
ros::NodeHandle nh_
Definition: distortion.h:40


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39