issue ros-perception#180 Check if all distortion coefficients are zero.

Test with:

rostest --reuse-master --text image_proc test_rectify.xml

Can also test interactively with vimjay image_rect.launch, which brings up an rqt gui and camera info distortion coefficients can be dynamically reconfigured.

image_proc/src/nodelets/rectify.cpp

@@ -122,7 +122,17 @@ void RectifyNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg,
   }
 
   // If zero distortion, just pass the message along
-  if (info_msg->D.empty() || info_msg->D[0] == 0.0)
+  bool zero_distortion = true;
+  for (size_t i = 0; i < info_msg->D.size(); ++i)
+  {
+    if (info_msg->D[i] != 0.0)
+    {
+      zero_distortion = false;
+      break;
+    }
+  }
+  // This will be true if D is empty/zero sized
+  if (zero_distortion)
   {
     pub_rect_.publish(image_msg);
     return;

image_proc/test/CMakeLists.txt

@@ -1,2 +1,5 @@
+find_package(rostest REQUIRED)
 #catkin_add_gtest(image_proc_rostest rostest.cpp)
 #target_link_libraries(image_proc_rostest ${catkin_LIBRARIES}  ${Boost_LIBRARIES})
+add_rostest_gtest(image_proc_test_rectify test_rectify.xml test_rectify.cpp)
+target_link_libraries(image_proc_test_rectify ${catkin_LIBRARIES}  ${Boost_LIBRARIES})

image_proc/test/test_rectify.cpp

@@ -0,0 +1,205 @@
+#include <ros/ros.h>
+#include <gtest/gtest.h>
+#include <camera_calibration_parsers/parse.h>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/highgui/highgui.hpp>
+#include <image_transport/image_transport.h>
+#include <sensor_msgs/CameraInfo.h>
+#include <sensor_msgs/distortion_models.h>
+
+class ImageProcRectifyTest : public testing::Test
+{
+protected:
+  virtual void SetUp()
+  {
+    // Determine topic names
+    std::string camera_ns = nh_.resolveName("camera") + "/";
+    if (camera_ns == "/camera")
+      throw "Must remap 'camera' to the camera namespace.";
+    topic_raw_        = camera_ns + "image_raw";
+    topic_mono_       = camera_ns + "image_mono";
+    topic_rect_       = camera_ns + "image_rect";
+    topic_color_      = camera_ns + "image_color";
+    topic_rect_color_ = camera_ns + "image_rect_color";
+
+    // Taken from vision_opencv/image_geometry/test/utest.cpp
+    double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0};
+    double K[] = {430.15433020105519,                0.0, 311.71339830549732,
+                                 0.0, 430.60920415473657, 221.06824942698509,
+                                 0.0,                0.0,                1.0};
+    double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
+                  -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
+                  -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516};
+    double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0,
+                  0.0, 295.53402059708782, 223.29617881774902, 0.0,
+                  0.0, 0.0, 1.0, 0.0};
+
+    cam_info_.header.frame_id = "tf_frame";
+    cam_info_.height = 480;
+    cam_info_.width  = 640;
+    // No ROI
+    cam_info_.D.resize(5);
+    std::copy(D, D+5, cam_info_.D.begin());
+    std::copy(K, K+9, cam_info_.K.begin());
+    std::copy(R, R+9, cam_info_.R.begin());
+    std::copy(P, P+12, cam_info_.P.begin());
+    cam_info_.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
+
+    distorted_image_ = cv::Mat(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3);
+    // draw a grid
+    const cv::Scalar color = cv::Scalar(255, 255, 255);
+    // draw the lines thick so the proportion of error due to
+    // interpolation is reduced
+    const int thickness = 7;
+    const int type = 8;
+    for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
+    {
+      cv::line(distorted_image_,
+               cv::Point(0, y), cv::Point(cam_info_.width, y),
+               color, type, thickness);
+    }
+    for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
+    {
+      // draw the lines thick so the prorportion of interpolation error is reduced
+      cv::line(distorted_image_,
+               cv::Point(x, 0), cv::Point(x, cam_info_.height),
+               color, type, thickness);
+    }
+
+    raw_image_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8",
+                                    distorted_image_).toImageMsg();
+
+    // Create raw camera subscriber and publisher
+    image_transport::ImageTransport it(nh_);
+    cam_pub_ = it.advertiseCamera(topic_raw_, 1);
+  }
+
+  ros::NodeHandle nh_;
+  std::string topic_raw_;
+  std::string topic_mono_;
+  std::string topic_rect_;
+  std::string topic_color_;
+  std::string topic_rect_color_;
+
+  cv::Mat distorted_image_;
+  sensor_msgs::ImagePtr raw_image_;
+  bool has_new_image_;
+  cv::Mat received_image_;
+  sensor_msgs::CameraInfo cam_info_;
+  image_transport::CameraPublisher cam_pub_;
+  image_transport::Subscriber cam_sub_;
+
+public:
+  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
+  {
+    cv_bridge::CvImageConstPtr cv_ptr;
+    try
+    {
+      cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
+    }
+    catch (cv_bridge::Exception& e)
+    {
+      ROS_FATAL("cv_bridge exception: %s", e.what());
+      return;
+    }
+    received_image_ = cv_ptr->image.clone();
+    has_new_image_ = true;
+  }
+
+  void publishRaw()
+  {
+    has_new_image_ = false;
+    cam_pub_.publish(*raw_image_, cam_info_);
+  }
+};
+
+TEST_F(ImageProcRectifyTest, rectifyTest)
+{
+  ROS_INFO("In test. Subscribing.");
+  image_transport::ImageTransport it(nh_);
+  cam_sub_ = it.subscribe(topic_rect_, 1, &ImageProcRectifyTest::imageCallback,
+                          dynamic_cast<ImageProcRectifyTest*>(this));
+  // Wait for image_proc to be operational
+  bool wait_for_topic = true;
+  while (wait_for_topic)
+  {
+    // @todo this fails without the additional 0.5 second sleep after the
+    // publisher comes online, which means on a slower or more heavily
+    // loaded system it may take longer than 0.5 seconds, and the test
+    // would hang until the timeout is reached and fail.
+    if (cam_sub_.getNumPublishers() > 0)
+       wait_for_topic = false;
+    ros::Duration(0.5).sleep();
+  }
+
+  // All the tests are the same as from 
+  // vision_opencv/image_geometry/test/utest.cpp
+  // default cam info
+
+  // Just making this number up, maybe ought to be larger
+  // since a completely different image would be on the order of
+  // width * height * 255 = 78e6
+  const double diff_threshold = 10000.0;
+  double error;
+
+  // use original cam_info
+  publishRaw();
+  while (!has_new_image_)
+  {
+    ros::spinOnce();
+    ros::Duration(0.5).sleep();
+  }
+  // Test that rectified image is sufficiently different
+  // using default distortion
+  error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
+  // Just making this number up, maybe ought to be larger
+  EXPECT_GT(error, diff_threshold);
+
+  // Test that rectified image is sufficiently different
+  // using default distortion but with first element zeroed
+  // out.
+  sensor_msgs::CameraInfo cam_info_orig = cam_info_;
+  cam_info_.D[0] = 0.0;
+  publishRaw();
+  while (!has_new_image_)
+  {
+    ros::spinOnce();
+    ros::Duration(0.5).sleep();
+  }
+  error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
+  EXPECT_GT(error, diff_threshold);
+
+  // Test that rectified image is the same using zero distortion
+  cam_info_.D.assign(cam_info_.D.size(), 0);
+  publishRaw();
+  while (!has_new_image_)
+  {
+    ros::spinOnce();
+    ros::Duration(0.5).sleep();
+  }
+  error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
+  EXPECT_EQ(error, 0);
+
+
+  // Test that rectified image is the same using empty distortion
+  cam_info_.D.clear();
+  publishRaw();
+  while (!has_new_image_)
+  {
+    ros::spinOnce();
+    ros::Duration(0.5).sleep();
+  }
+  error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
+
+  EXPECT_EQ(error, 0);
+
+  // restore the original cam_info for other tests added in the future
+  cam_info_ = cam_info_orig;
+}
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "image_proc_test_rectify");
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

image_proc/test/test_rectify.xml

@@ -0,0 +1,9 @@
+<launch>
+  <group ns="camera">
+  <node name="rectify_image_proc" pkg="image_proc" type="image_proc"
+      output="screen">
+  </node>
+  </group>
+  <test test-name="image_proc_test_rectify" pkg="image_proc" type="image_proc_test_rectify">
+  </test>
+</launch>