test_rectify.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <gtest/gtest.h>
00003 #include <camera_calibration_parsers/parse.h>
00004 #include <cv_bridge/cv_bridge.h>
00005 #include <opencv2/highgui/highgui.hpp>
00006 #include <image_transport/image_transport.h>
00007 #include <sensor_msgs/CameraInfo.h>
00008 #include <sensor_msgs/distortion_models.h>
00009 
00010 class ImageProcRectifyTest : public testing::Test
00011 {
00012 protected:
00013   virtual void SetUp()
00014   {
00015     // Determine topic names
00016     std::string camera_ns = nh_.resolveName("camera") + "/";
00017     if (camera_ns == "/camera")
00018       throw "Must remap 'camera' to the camera namespace.";
00019     topic_raw_        = camera_ns + "image_raw";
00020     topic_mono_       = camera_ns + "image_mono";
00021     topic_rect_       = camera_ns + "image_rect";
00022     topic_color_      = camera_ns + "image_color";
00023     topic_rect_color_ = camera_ns + "image_rect_color";
00024 
00025     // Taken from vision_opencv/image_geometry/test/utest.cpp
00026     double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0};
00027     double K[] = {430.15433020105519,                0.0, 311.71339830549732,
00028                                  0.0, 430.60920415473657, 221.06824942698509,
00029                                  0.0,                0.0,                1.0};
00030     double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
00031                   -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
00032                   -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516};
00033     double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0,
00034                   0.0, 295.53402059708782, 223.29617881774902, 0.0,
00035                   0.0, 0.0, 1.0, 0.0};
00036 
00037     cam_info_.header.frame_id = "tf_frame";
00038     cam_info_.height = 480;
00039     cam_info_.width  = 640;
00040     // No ROI
00041     cam_info_.D.resize(5);
00042     std::copy(D, D+5, cam_info_.D.begin());
00043     std::copy(K, K+9, cam_info_.K.begin());
00044     std::copy(R, R+9, cam_info_.R.begin());
00045     std::copy(P, P+12, cam_info_.P.begin());
00046     cam_info_.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
00047 
00048     distorted_image_ = cv::Mat(cv::Size(cam_info_.width, cam_info_.height), CV_8UC3);
00049     // draw a grid
00050     const cv::Scalar color = cv::Scalar(255, 255, 255);
00051     // draw the lines thick so the proportion of error due to
00052     // interpolation is reduced
00053     const int thickness = 7;
00054     const int type = 8;
00055     for (size_t y = 0; y <= cam_info_.height; y += cam_info_.height/10)
00056     {
00057       cv::line(distorted_image_,
00058                cv::Point(0, y), cv::Point(cam_info_.width, y),
00059                color, type, thickness);
00060     }
00061     for (size_t x = 0; x <= cam_info_.width; x += cam_info_.width/10)
00062     {
00063       // draw the lines thick so the prorportion of interpolation error is reduced
00064       cv::line(distorted_image_,
00065                cv::Point(x, 0), cv::Point(x, cam_info_.height),
00066                color, type, thickness);
00067     }
00068 
00069     raw_image_ = cv_bridge::CvImage(std_msgs::Header(), "bgr8",
00070                                     distorted_image_).toImageMsg();
00071 
00072     // Create raw camera subscriber and publisher
00073     image_transport::ImageTransport it(nh_);
00074     cam_pub_ = it.advertiseCamera(topic_raw_, 1);
00075   }
00076 
00077   ros::NodeHandle nh_;
00078   std::string topic_raw_;
00079   std::string topic_mono_;
00080   std::string topic_rect_;
00081   std::string topic_color_;
00082   std::string topic_rect_color_;
00083 
00084   cv::Mat distorted_image_;
00085   sensor_msgs::ImagePtr raw_image_;
00086   bool has_new_image_;
00087   cv::Mat received_image_;
00088   sensor_msgs::CameraInfo cam_info_;
00089   image_transport::CameraPublisher cam_pub_;
00090   image_transport::Subscriber cam_sub_;
00091 
00092 public:
00093   void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00094   {
00095     cv_bridge::CvImageConstPtr cv_ptr;
00096     try
00097     {
00098       cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8);
00099     }
00100     catch (cv_bridge::Exception& e)
00101     {
00102       ROS_FATAL("cv_bridge exception: %s", e.what());
00103       return;
00104     }
00105     received_image_ = cv_ptr->image.clone();
00106     has_new_image_ = true;
00107   }
00108 
00109   void publishRaw()
00110   {
00111     has_new_image_ = false;
00112     cam_pub_.publish(*raw_image_, cam_info_);
00113   }
00114 };
00115 
00116 TEST_F(ImageProcRectifyTest, rectifyTest)
00117 {
00118   ROS_INFO("In test. Subscribing.");
00119   image_transport::ImageTransport it(nh_);
00120   cam_sub_ = it.subscribe(topic_rect_, 1, &ImageProcRectifyTest::imageCallback,
00121                           dynamic_cast<ImageProcRectifyTest*>(this));
00122   // Wait for image_proc to be operational
00123   bool wait_for_topic = true;
00124   while (wait_for_topic)
00125   {
00126     // @todo this fails without the additional 0.5 second sleep after the
00127     // publisher comes online, which means on a slower or more heavily
00128     // loaded system it may take longer than 0.5 seconds, and the test
00129     // would hang until the timeout is reached and fail.
00130     if (cam_sub_.getNumPublishers() > 0)
00131        wait_for_topic = false;
00132     ros::Duration(0.5).sleep();
00133   }
00134 
00135   // All the tests are the same as from 
00136   // vision_opencv/image_geometry/test/utest.cpp
00137   // default cam info
00138 
00139   // Just making this number up, maybe ought to be larger
00140   // since a completely different image would be on the order of
00141   // width * height * 255 = 78e6
00142   const double diff_threshold = 10000.0;
00143   double error;
00144 
00145   // use original cam_info
00146   publishRaw();
00147   while (!has_new_image_)
00148   {
00149     ros::spinOnce();
00150     ros::Duration(0.5).sleep();
00151   }
00152   // Test that rectified image is sufficiently different
00153   // using default distortion
00154   error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
00155   // Just making this number up, maybe ought to be larger
00156   EXPECT_GT(error, diff_threshold);
00157 
00158   // Test that rectified image is sufficiently different
00159   // using default distortion but with first element zeroed
00160   // out.
00161   sensor_msgs::CameraInfo cam_info_orig = cam_info_;
00162   cam_info_.D[0] = 0.0;
00163   publishRaw();
00164   while (!has_new_image_)
00165   {
00166     ros::spinOnce();
00167     ros::Duration(0.5).sleep();
00168   }
00169   error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
00170   EXPECT_GT(error, diff_threshold);
00171 
00172   // Test that rectified image is the same using zero distortion
00173   cam_info_.D.assign(cam_info_.D.size(), 0);
00174   publishRaw();
00175   while (!has_new_image_)
00176   {
00177     ros::spinOnce();
00178     ros::Duration(0.5).sleep();
00179   }
00180   error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
00181   EXPECT_EQ(error, 0);
00182 
00183 
00184   // Test that rectified image is the same using empty distortion
00185   cam_info_.D.clear();
00186   publishRaw();
00187   while (!has_new_image_)
00188   {
00189     ros::spinOnce();
00190     ros::Duration(0.5).sleep();
00191   }
00192   error = cv::norm(distorted_image_, received_image_, cv::NORM_L1);
00193 
00194   EXPECT_EQ(error, 0);
00195 
00196   // restore the original cam_info for other tests added in the future
00197   cam_info_ = cam_info_orig;
00198 }
00199 
00200 int main(int argc, char** argv)
00201 {
00202   ros::init(argc, argv, "image_proc_test_rectify");
00203   testing::InitGoogleTest(&argc, argv);
00204   return RUN_ALL_TESTS();
00205 }


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed May 1 2019 02:51:47