From 4ecbae16b18fd00086885077476ed87d314b06a2 Mon Sep 17 00:00:00 2001 From: Matthew Hansen Date: Mon, 7 Mar 2016 15:46:01 -0800 Subject: [PATCH 1/3] change arrays to use at(#) instead of [#] consistently --- camera/src/realsense_camera_nodelet.cpp | 40 ++++++++++++------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index 361163cdac..11d1b5a656 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -374,26 +374,26 @@ namespace realsense_camera camera_info_[stream_index]->width = intrinsic.width; camera_info_[stream_index]->height = intrinsic.height; - camera_info_[stream_index]->K.at (0) = intrinsic.fx; - camera_info_[stream_index]->K.at (2) = intrinsic.ppx; - camera_info_[stream_index]->K.at (4) = intrinsic.fy; - camera_info_[stream_index]->K.at (5) = intrinsic.ppy; - camera_info_[stream_index]->K.at (8) = 1; - - camera_info_[stream_index]->P[0] = camera_info_[stream_index]->K.at (0); - camera_info_[stream_index]->P[1] = 0; - camera_info_[stream_index]->P[2] = camera_info_[stream_index]->K.at (2); - camera_info_[stream_index]->P[3] = 0; - - camera_info_[stream_index]->P[4] = 0; - camera_info_[stream_index]->P[5] = camera_info_[stream_index]->K.at (4); - camera_info_[stream_index]->P[6] = camera_info_[stream_index]->K.at (5); - camera_info_[stream_index]->P[7] = 0; - - camera_info_[stream_index]->P[8] = 0; - camera_info_[stream_index]->P[9] = 0; - camera_info_[stream_index]->P[10] = 1; - camera_info_[stream_index]->P[11] = 0; + camera_info_[stream_index]->K.at(0) = intrinsic.fx; + camera_info_[stream_index]->K.at(2) = intrinsic.ppx; + camera_info_[stream_index]->K.at(4) = intrinsic.fy; + camera_info_[stream_index]->K.at(5) = intrinsic.ppy; + camera_info_[stream_index]->K.at(8) = 1; + + camera_info_[stream_index]->P.at(0) = camera_info_[stream_index]->K.at(0); + camera_info_[stream_index]->P.at(1) = 0; + camera_info_[stream_index]->P.at(2) = camera_info_[stream_index]->K.at(2); + camera_info_[stream_index]->P.at(3) = 0; + + camera_info_[stream_index]->P.at(4) = 0; + camera_info_[stream_index]->P.at(5) = camera_info_[stream_index]->K.at(4); + camera_info_[stream_index]->P.at(6) = camera_info_[stream_index]->K.at(5); + camera_info_[stream_index]->P.at(7) = 0; + + camera_info_[stream_index]->P.at(8) = 0; + camera_info_[stream_index]->P.at(9) = 0; + camera_info_[stream_index]->P.at(10) = 1; + camera_info_[stream_index]->P.at(11) = 0; camera_info_[stream_index]->distortion_model = "plumb_bob"; From 6f0f1d3c044bec7f67bd0e64e0d56bca91d0f11b Mon Sep 17 00:00:00 2001 From: Matthew Hansen Date: Mon, 7 Mar 2016 17:19:17 -0800 Subject: [PATCH 2/3] add depth to color translation to projection matrix --- camera/src/realsense_camera_nodelet.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/camera/src/realsense_camera_nodelet.cpp b/camera/src/realsense_camera_nodelet.cpp index 11d1b5a656..f5a64d4997 100644 --- a/camera/src/realsense_camera_nodelet.cpp +++ b/camera/src/realsense_camera_nodelet.cpp @@ -395,6 +395,16 @@ namespace realsense_camera camera_info_[stream_index]->P.at(10) = 1; camera_info_[stream_index]->P.at(11) = 0; + if (stream_index == RS_STREAM_DEPTH) + { + // set depth to color translation values in Projection matrix (P) + rs_extrinsics z_extrinsic; + rs_get_device_extrinsics (rs_device_, RS_STREAM_DEPTH, RS_STREAM_COLOR, &z_extrinsic, &rs_error_); check_error (); + camera_info_[stream_index]->P.at(3) = z_extrinsic.translation[0]/1000; // Tx + camera_info_[stream_index]->P.at(7) = z_extrinsic.translation[1]/1000; // Ty + camera_info_[stream_index]->P.at(11) = z_extrinsic.translation[2]/1000; // Tz + } + camera_info_[stream_index]->distortion_model = "plumb_bob"; // set R (rotation matrix) values to identity matrix From 2ad69beb105d9583c45c92478edbaa7dc03bee95 Mon Sep 17 00:00:00 2001 From: Matthew Hansen Date: Fri, 11 Mar 2016 15:54:33 -0800 Subject: [PATCH 3/3] Add tests for depth translation to color --- camera/test/realsense_camera_test_node.cpp | 83 ++++++++++++++++++++++ camera/test/realsense_camera_test_node.h | 5 ++ 2 files changed, 88 insertions(+) diff --git a/camera/test/realsense_camera_test_node.cpp b/camera/test/realsense_camera_test_node.cpp index 7cfccd9e01..2515ab1bee 100644 --- a/camera/test/realsense_camera_test_node.cpp +++ b/camera/test/realsense_camera_test_node.cpp @@ -66,6 +66,12 @@ void imageInfrared1Callback(const sensor_msgs::ImageConstPtr & msg, const sensor inf1_caminfo_rotation_recv[i] = info_msg->R[i]; } + // copy projection matrix + for (unsigned int i = 0; i < sizeof(info_msg->P)/sizeof(double); i++) + { + inf1_caminfo_projection_recv[i] = info_msg->P[i]; + } + infrared1_recv = true; } @@ -103,6 +109,12 @@ void imageInfrared2Callback(const sensor_msgs::ImageConstPtr & msg, const sensor inf2_caminfo_rotation_recv[i] = info_msg->R[i]; } + // copy projection matrix + for (unsigned int i = 0; i < sizeof(info_msg->P)/sizeof(double); i++) + { + inf2_caminfo_projection_recv[i] = info_msg->P[i]; + } + infrared2_recv = true; } @@ -141,6 +153,11 @@ void imageDepthCallback(const sensor_msgs::ImageConstPtr & msg, const sensor_msg { depth_caminfo_rotation_recv[i] = info_msg->R[i]; } + // copy projection matrix + for (unsigned int i = 0; i < sizeof(info_msg->P)/sizeof(double); i++) + { + depth_caminfo_projection_recv[i] = info_msg->P[i]; + } depth_recv = true; } @@ -206,6 +223,12 @@ void imageColorCallback(const sensor_msgs::ImageConstPtr & msg, const sensor_msg color_caminfo_rotation_recv[i] = info_msg->R[i]; } + // copy projection matrix + for (unsigned int i = 0; i < sizeof(info_msg->P)/sizeof(double); i++) + { + color_caminfo_projection_recv[i] = info_msg->P[i]; + } + color_recv = true; } @@ -259,6 +282,21 @@ TEST (RealsenseTests, testColorCameraInfo) { EXPECT_EQ (ROTATION_IDENTITY[i], color_caminfo_rotation_recv[i]); } + + // check projection matrix values are set + EXPECT_TRUE(color_caminfo_projection_recv[0] != (double) 0); + EXPECT_EQ(color_caminfo_projection_recv[1], (double) 0); + EXPECT_TRUE(color_caminfo_projection_recv[2] != (double) 0); + EXPECT_EQ(color_caminfo_projection_recv[3], (double) 0); + EXPECT_EQ(color_caminfo_projection_recv[4], (double) 0); + EXPECT_TRUE(color_caminfo_projection_recv[5] != (double) 0); + EXPECT_TRUE(color_caminfo_projection_recv[6] != (double) 0); + EXPECT_EQ(color_caminfo_projection_recv[7], (double) 0); + EXPECT_EQ(color_caminfo_projection_recv[8], (double) 0); + EXPECT_EQ(color_caminfo_projection_recv[9], (double) 0); + EXPECT_TRUE(color_caminfo_projection_recv[10] != (double) 0); + EXPECT_EQ(color_caminfo_projection_recv[11], (double) 0); + } } @@ -312,6 +350,21 @@ TEST (RealsenseTests, testDepthCameraInfo) { EXPECT_EQ (ROTATION_IDENTITY[i], depth_caminfo_rotation_recv[i]); } + + // check projection matrix values are set + EXPECT_TRUE(depth_caminfo_projection_recv[0] != (double) 0); + EXPECT_EQ(depth_caminfo_projection_recv[1], (double) 0); + EXPECT_TRUE(depth_caminfo_projection_recv[2] != (double) 0); + EXPECT_TRUE(depth_caminfo_projection_recv[3] != (double) 0); + EXPECT_EQ(depth_caminfo_projection_recv[4], (double) 0); + EXPECT_TRUE(depth_caminfo_projection_recv[5] != (double) 0); + EXPECT_TRUE(depth_caminfo_projection_recv[6] != (double) 0); + EXPECT_TRUE(depth_caminfo_projection_recv[7] != (double) 0); + EXPECT_EQ(depth_caminfo_projection_recv[8], (double) 0); + EXPECT_EQ(depth_caminfo_projection_recv[9], (double) 0); + EXPECT_TRUE(depth_caminfo_projection_recv[10] != (double) 0); + EXPECT_TRUE(depth_caminfo_projection_recv[11] != (double) 0); + } } @@ -364,6 +417,21 @@ TEST (RealsenseTests, testInfrared1CameraInfo) { EXPECT_EQ (ROTATION_IDENTITY[i], inf1_caminfo_rotation_recv[i]); } + + // check projection matrix values are set + EXPECT_TRUE(inf1_caminfo_projection_recv[0] != (double) 0); + EXPECT_EQ(inf1_caminfo_projection_recv[1], (double) 0); + EXPECT_TRUE(inf1_caminfo_projection_recv[2] != (double) 0); + EXPECT_EQ(inf1_caminfo_projection_recv[3], (double) 0); + EXPECT_EQ(inf1_caminfo_projection_recv[4], (double) 0); + EXPECT_TRUE(inf1_caminfo_projection_recv[5] != (double) 0); + EXPECT_TRUE(inf1_caminfo_projection_recv[6] != (double) 0); + EXPECT_EQ(inf1_caminfo_projection_recv[7], (double) 0); + EXPECT_EQ(inf1_caminfo_projection_recv[8], (double) 0); + EXPECT_EQ(inf1_caminfo_projection_recv[9], (double) 0); + EXPECT_TRUE(inf1_caminfo_projection_recv[10] != (double) 0); + EXPECT_EQ(inf1_caminfo_projection_recv[11], (double) 0); + } } @@ -408,6 +476,21 @@ TEST (RealsenseTests, testInfrared2CameraInfo) { EXPECT_EQ (ROTATION_IDENTITY[i], inf2_caminfo_rotation_recv[i]); } + + // check projection matrix values are set + EXPECT_TRUE(inf2_caminfo_projection_recv[0] != (double) 0); + EXPECT_EQ(inf2_caminfo_projection_recv[1], (double) 0); + EXPECT_TRUE(inf2_caminfo_projection_recv[2] != (double) 0); + EXPECT_EQ(inf2_caminfo_projection_recv[3], (double) 0); + EXPECT_EQ(inf2_caminfo_projection_recv[4], (double) 0); + EXPECT_TRUE(inf2_caminfo_projection_recv[5] != (double) 0); + EXPECT_TRUE(inf2_caminfo_projection_recv[6] != (double) 0); + EXPECT_EQ(inf2_caminfo_projection_recv[7], (double) 0); + EXPECT_EQ(inf2_caminfo_projection_recv[8], (double) 0); + EXPECT_EQ(inf2_caminfo_projection_recv[9], (double) 0); + EXPECT_TRUE(inf2_caminfo_projection_recv[10] != (double) 0); + EXPECT_EQ(inf2_caminfo_projection_recv[11], (double) 0); + } } diff --git a/camera/test/realsense_camera_test_node.h b/camera/test/realsense_camera_test_node.h index 5e9dc5c6aa..df3cc4049a 100644 --- a/camera/test/realsense_camera_test_node.h +++ b/camera/test/realsense_camera_test_node.h @@ -123,18 +123,23 @@ std::string infrared1_encoding_recv; // Expected depth encoding. std::string infrared2_encoding_recv; // Expected depth encoding. std::string color_encoding_recv; // Expected color encoding. + int depth_caminfo_height_recv = 0; int depth_caminfo_width_recv = 0; double depth_caminfo_rotation_recv[9] = {0}; +double depth_caminfo_projection_recv[12] = {0}; int color_caminfo_height_recv = 0; int color_caminfo_width_recv = 0; double color_caminfo_rotation_recv[9] = {0}; +double color_caminfo_projection_recv[12] = {0}; int inf1_caminfo_height_recv = 0; int inf1_caminfo_width_recv = 0; double inf1_caminfo_rotation_recv[9] = {0}; +double inf1_caminfo_projection_recv[12] = {0}; int inf2_caminfo_height_recv = 0; int inf2_caminfo_width_recv = 0; double inf2_caminfo_rotation_recv[9] = {0}; +double inf2_caminfo_projection_recv[12] = {0}; std::string inf1_dmodel_recv; std::string inf2_dmodel_recv;