Merge pull request IntelRealSense#25 from otcshare/RAR-154-depth-tran…

…slation-to-color

Rar 154 depth translation to color

camera/src/realsense_camera_nodelet.cpp

@@ -374,26 +374,36 @@ 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;
+
+    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";
 

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);
+
   }
 }
 

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;