3 #include <gtest/gtest.h> 18 double D[] = {-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0};
19 double K[] = {430.15433020105519, 0.0, 311.71339830549732,
20 0.0, 430.60920415473657, 221.06824942698509,
22 double R[] = {0.99806560714807102, 0.0068562422224214027, 0.061790256276695904,
23 -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664,
24 -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516};
25 double P[] = {295.53402059708782, 0.0, 285.55760765075684, 0.0,
26 0.0, 295.53402059708782, 223.29617881774902, 0.0,
60 cv::Point2d uv(100, 100);
62 EXPECT_NEAR(-0.62787224048135637, xyz.x, 1e-8);
63 EXPECT_NEAR(-0.41719792045817677, xyz.y, 1e-8);
64 EXPECT_DOUBLE_EQ(1.0, xyz.z);
71 EXPECT_DOUBLE_EQ(0.0, xyz.x);
72 EXPECT_DOUBLE_EQ(0.0, xyz.y);
73 EXPECT_DOUBLE_EQ(1.0, xyz.z);
77 const size_t step = 10;
78 for (
size_t row = 0; row <=
cam_info_.height; row += step) {
79 for (
size_t col = 0; col <=
cam_info_.width; col += step) {
80 cv::Point2d uv(row, col), uv_back;
84 EXPECT_NEAR(uv.x, uv_back.x, 1.14e-13) <<
"at (" << row <<
", " << col <<
")";
85 EXPECT_NEAR(uv.y, uv_back.y, 1.14e-13) <<
"at (" << row <<
", " << col <<
")";
94 cv::Point2d uv_raw(100, 100), uv_rect;
96 EXPECT_DOUBLE_EQ(142.30311584472656, uv_rect.x);
97 EXPECT_DOUBLE_EQ(132.061065673828, uv_rect.y);
105 cv::Point2d uv_raw(cxp, cyp), uv_rect;
107 EXPECT_NEAR(uv_rect.x,
model_.
cx(), 1e-4);
108 EXPECT_NEAR(uv_rect.y,
model_.
cy(), 1e-4);
115 EXPECT_NEAR(uv_raw.x, cxp, 1e-4);
116 EXPECT_NEAR(uv_raw.y, cyp, 1e-4);
121 const size_t step = 5;
122 const size_t border = 65;
123 for (
size_t row = border; row <=
cam_info_.height - border; row += step) {
124 for (
size_t col = border; col <=
cam_info_.width - border; col += step) {
125 cv::Point2d uv_raw(row, col), uv_rect, uv_unrect;
129 EXPECT_NEAR(uv_raw.x, uv_unrect.x, 1.0);
130 EXPECT_NEAR(uv_raw.y, uv_unrect.y, 1.0);
137 double u = 100.0, v = 200.0, du = 17.0, dv = 23.0, Z = 2.0;
138 cv::Point2d uv0(u, v), uv1(u + du, v + dv);
139 cv::Point3d xyz0, xyz1;
141 xyz0 *= (Z / xyz0.z);
143 xyz1 *= (Z / xyz1.z);
154 sensor_msgs::CameraInfo info;
191 cv::Mat distorted_image(cv::Size(
cam_info_.width,
cam_info_.height), CV_8UC3, cv::Scalar(0, 0, 0));
194 const cv::Scalar color = cv::Scalar(255, 255, 255);
197 const int thickness = 7;
201 cv::line(distorted_image,
202 cv::Point(0, y), cv::Point(
cam_info_.width, y),
203 color, type, thickness);
208 cv::line(distorted_image,
209 cv::Point(x, 0), cv::Point(x,
cam_info_.height),
210 color, type, thickness);
213 cv::Mat rectified_image;
217 const double diff_threshold = 10000.0;
223 error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
225 EXPECT_GT(error, diff_threshold);
230 sensor_msgs::CameraInfo cam_info_2 =
cam_info_;
231 cam_info_2.D[0] = 0.0;
234 error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
235 EXPECT_GT(error, diff_threshold);
238 cam_info_2.D.assign(cam_info_2.D.size(), 0);
241 error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
245 cam_info_2.D.clear();
248 error = cv::norm(distorted_image, rectified_image, cv::NORM_L1);
255 int main(
int argc,
char** argv)
257 testing::InitGoogleTest(&argc, argv);
258 return RUN_ALL_TESTS();
cv::Point2d unrectifyPoint(const cv::Point2d &uv_rect) const
Compute the raw image coordinates of a pixel in the rectified image.
double getDeltaY(double deltaV, double Z) const
Compute delta Y, given Z in Cartesian space and delta v in pixels.
double getDeltaU(double deltaX, double Z) const
Compute delta u, given Z and delta X in Cartesian space.
image_geometry::PinholeCameraModel model_
cv::Point2d project3dToPixel(const cv::Point3d &xyz) const
Project a 3d point to rectified pixel coordinates.
bool initialized() const
Returns true if the camera has been initialized.
double getDeltaX(double deltaU, double Z) const
Compute delta X, given Z in Cartesian space and delta u in pixels.
double cy() const
Returns the y coordinate of the optical center.
void rectifyImage(const cv::Mat &raw, cv::Mat &rectified, int interpolation=cv::INTER_LINEAR) const
Rectify a raw camera image.
TEST_F(PinholeTest, accessorsCorrect)
double getDeltaV(double deltaY, double Z) const
Compute delta v, given Z and delta Y in Cartesian space.
double fx() const
Returns the focal length (pixels) in x direction of the rectified image.
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
Set the camera parameters from the sensor_msgs/CameraInfo message.
int main(int argc, char **argv)
const std::string PLUMB_BOB
Simplifies interpreting images geometrically using the parameters from sensor_msgs/CameraInfo.
std::string tfFrame() const
Get the name of the camera coordinate frame in tf.
sensor_msgs::CameraInfo cam_info_
cv::Point2d rectifyPoint(const cv::Point2d &uv_raw) const
Compute the rectified image coordinates of a pixel in the raw image.
double cx() const
Returns the x coordinate of the optical center.
const cv::Matx33d & intrinsicMatrix() const
Returns the original camera matrix.
const cv::Matx34d & projectionMatrix() const
Returns the projection matrix.
double fy() const
Returns the focal length (pixels) in y direction of the rectified image.
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
Project a rectified pixel to a 3d ray.