2 #include <gtest/gtest.h> 3 #include <camera_calibration_parsers/parse.h> 5 #include <opencv2/highgui/highgui.hpp> 7 #include <sensor_msgs/CameraInfo.h> 17 if (camera_ns ==
"/camera")
18 throw "Must remap 'camera' to the camera namespace.";
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,
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,
50 const cv::Scalar color = cv::Scalar(255, 255, 255);
53 const int thickness = 7;
58 cv::Point(0, y), cv::Point(
cam_info_.width, y),
59 color, type, thickness);
65 cv::Point(x, 0), cv::Point(x,
cam_info_.height),
66 color, type, thickness);
102 ROS_FATAL(
"cv_bridge exception: %s", e.what());
105 received_image_ = cv_ptr->image.clone();
106 has_new_image_ =
true;
111 has_new_image_ =
false;
112 cam_pub_.
publish(*raw_image_, cam_info_);
121 dynamic_cast<ImageProcRectifyTest*>(
this));
123 bool wait_for_topic =
true;
124 while (wait_for_topic)
131 wait_for_topic =
false;
142 const double diff_threshold = 10000.0;
156 EXPECT_GT(error, diff_threshold);
161 sensor_msgs::CameraInfo cam_info_orig =
cam_info_;
170 EXPECT_GT(error, diff_threshold);
200 int main(
int argc,
char** argv)
202 ros::init(argc, argv,
"image_proc_test_rectify");
203 testing::InitGoogleTest(&argc, argv);
204 return RUN_ALL_TESTS();
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
sensor_msgs::ImagePtr raw_image_
std::string resolveName(const std::string &name, bool remap=true) 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)
CameraPublisher advertiseCamera(const std::string &base_topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
image_transport::Subscriber cam_sub_
const std::string PLUMB_BOB
uint32_t getNumPublishers() const
TEST_F(ImageProcRectifyTest, rectifyTest)
ROSCPP_DECL void spinOnce()
std::string topic_rect_color_
sensor_msgs::ImagePtr toImageMsg() const
image_transport::CameraPublisher cam_pub_