test_rectify.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
2 #include <gtest/gtest.h>
3 #include <camera_calibration_parsers/parse.h>
4 #include <cv_bridge/cv_bridge.h>
5 #include <opencv2/highgui/highgui.hpp>
7 #include <sensor_msgs/CameraInfo.h>
9 
10 class ImageProcRectifyTest : public testing::Test
11 {
12 protected:
13  virtual void SetUp()
14  {
15  // Determine topic names
16  std::string camera_ns = nh_.resolveName("camera") + "/";
17  if (camera_ns == "/camera")
18  throw "Must remap 'camera' to the camera namespace.";
19  topic_raw_ = camera_ns + "image_raw";
20  topic_mono_ = camera_ns + "image_mono";
21  topic_rect_ = camera_ns + "image_rect";
22  topic_color_ = camera_ns + "image_color";
23  topic_rect_color_ = camera_ns + "image_rect_color";
24 
25  // Taken from vision_opencv/image_geometry/test/utest.cpp
26  double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0};
27  double K[] = {430.15433020105519, 0.0, 311.71339830549732,
28  0.0, 430.60920415473657, 221.06824942698509,
29  0.0, 0.0, 1.0};
30  double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
31  -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
32  -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516};
33  double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0,
34  0.0, 295.53402059708782, 223.29617881774902, 0.0,
35  0.0, 0.0, 1.0, 0.0};
36 
37  cam_info_.header.frame_id = "tf_frame";
38  cam_info_.height = 480;
39  cam_info_.width = 640;
40  // No ROI
41  cam_info_.D.resize(5);
42  std::copy(D, D+5, cam_info_.D.begin());
43  std::copy(K, K+9, cam_info_.K.begin());
44  std::copy(R, R+9, cam_info_.R.begin());
45  std::copy(P, P+12, cam_info_.P.begin());
47 
48  distorted_image_ = cv::Mat(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3);
49  // draw a grid
50  const cv::Scalar color = cv::Scalar(255, 255, 255);
51  // draw the lines thick so the proportion of error due to
52  // interpolation is reduced
53  const int thickness = 7;
54  const int type = 8;
55  for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
56  {
57  cv::line(distorted_image_,
58  cv::Point(0, y), cv::Point(cam_info_.width, y),
59  color, type, thickness);
60  }
61  for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
62  {
63  // draw the lines thick so the prorportion of interpolation error is reduced
64  cv::line(distorted_image_,
65  cv::Point(x, 0), cv::Point(x, cam_info_.height),
66  color, type, thickness);
67  }
68 
69  raw_image_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8",
71 
72  // Create raw camera subscriber and publisher
75  }
76 
78  std::string topic_raw_;
79  std::string topic_mono_;
80  std::string topic_rect_;
81  std::string topic_color_;
82  std::string topic_rect_color_;
83 
85  sensor_msgs::ImagePtr raw_image_;
87  cv::Mat received_image_;
88  sensor_msgs::CameraInfo cam_info_;
91 
92 public:
93  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
94  {
96  try
97  {
99  }
100  catch (cv_bridge::Exception& e)
101  {
102  ROS_FATAL("cv_bridge exception: %s", e.what());
103  return;
104  }
105  received_image_ = cv_ptr->image.clone();
106  has_new_image_ = true;
107  }
108 
109  void publishRaw()
110  {
111  has_new_image_ = false;
112  cam_pub_.publish(*raw_image_, cam_info_);
113  }
114 };
115 
117 {
118  ROS_INFO("In test. Subscribing.");
121  dynamic_cast<ImageProcRectifyTest*>(this));
122  // Wait for image_proc to be operational
123  bool wait_for_topic = true;
124  while (wait_for_topic)
125  {
126  // @todo this fails without the additional 0.5 second sleep after the
127  // publisher comes online, which means on a slower or more heavily
128  // loaded system it may take longer than 0.5 seconds, and the test
129  // would hang until the timeout is reached and fail.
130  if (cam_sub_.getNumPublishers() > 0)
131  wait_for_topic = false;
132  ros::Duration(0.5).sleep();
133  }
134 
135  // All the tests are the same as from
136  // vision_opencv/image_geometry/test/utest.cpp
137  // default cam info
138 
139  // Just making this number up, maybe ought to be larger
140  // since a completely different image would be on the order of
141  // width * height * 255 = 78e6
142  const double diff_threshold = 10000.0;
143  double error;
144 
145  // use original cam_info
146  publishRaw();
147  while (!has_new_image_)
148  {
149  ros::spinOnce();
150  ros::Duration(0.5).sleep();
151  }
152  // Test that rectified image is sufficiently different
153  // using default distortion
154  error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
155  // Just making this number up, maybe ought to be larger
156  EXPECT_GT(error, diff_threshold);
157 
158  // Test that rectified image is sufficiently different
159  // using default distortion but with first element zeroed
160  // out.
161  sensor_msgs::CameraInfo cam_info_orig = cam_info_;
162  cam_info_.D[0] = 0.0;
163  publishRaw();
164  while (!has_new_image_)
165  {
166  ros::spinOnce();
167  ros::Duration(0.5).sleep();
168  }
169  error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
170  EXPECT_GT(error, diff_threshold);
171 
172  // Test that rectified image is the same using zero distortion
173  cam_info_.D.assign(cam_info_.D.size(), 0);
174  publishRaw();
175  while (!has_new_image_)
176  {
177  ros::spinOnce();
178  ros::Duration(0.5).sleep();
179  }
180  error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
181  EXPECT_EQ(error, 0);
182 
183 
184  // Test that rectified image is the same using empty distortion
185  cam_info_.D.clear();
186  publishRaw();
187  while (!has_new_image_)
188  {
189  ros::spinOnce();
190  ros::Duration(0.5).sleep();
191  }
192  error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
193 
194  EXPECT_EQ(error, 0);
195 
196  // restore the original cam_info for other tests added in the future
197  cam_info_ = cam_info_orig;
198 }
199 
200 int main(int argc, char** argv)
201 {
202  ros::init(argc, argv, "image_proc_test_rectify");
203  testing::InitGoogleTest(&argc, argv);
204  return RUN_ALL_TESTS();
205 }
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
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())
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
#define ROS_FATAL(...)
sensor_msgs::ImagePtr raw_image_
virtual void SetUp()
std::string resolveName(const std::string &name, bool remap=true) const
bool sleep() const
sensor_msgs::CameraInfo cam_info_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
std::string topic_raw_
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
#define ROS_INFO(...)
image_transport::Subscriber cam_sub_
ros::NodeHandle nh_
uint32_t getNumPublishers() const
std::string topic_color_
TEST_F(ImageProcRectifyTest, rectifyTest)
std::string topic_mono_
ROSCPP_DECL void spinOnce()
std::string topic_rect_
std::string topic_rect_color_
sensor_msgs::ImagePtr toImageMsg() const
image_transport::CameraPublisher cam_pub_


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Thu Nov 7 2019 03:44:58