1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
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();
}
|
