Merge pull request IntelRealSense#22 from otcshare/RAR-153-identity-m…

…atrix-rotation

Rar 153 identity matrix rotation

camera/src/realsense_camera_nodelet.cpp

@@ -397,6 +397,17 @@ namespace realsense_camera
 
     camera_info_[stream_index]->distortion_model = "plumb_bob";
 
+    // set R (rotation matrix) values to identity matrix
+    camera_info_[stream_index]->R.at(0) = (double) 1;
+    camera_info_[stream_index]->R.at(1) = (double) 0;
+    camera_info_[stream_index]->R.at(2) = (double) 0;
+    camera_info_[stream_index]->R.at(3) = (double) 0;
+    camera_info_[stream_index]->R.at(4) = (double) 1;
+    camera_info_[stream_index]->R.at(5) = (double) 0;
+    camera_info_[stream_index]->R.at(6) = (double) 0;
+    camera_info_[stream_index]->R.at(7) = (double) 0;
+    camera_info_[stream_index]->R.at(8) = (double) 1;
+
     for (int i = 0; i < 5; i++)
     camera_info_[stream_index]->D.push_back (0);
   }

camera/test/realsense_camera_test_node.cpp

@@ -59,6 +59,13 @@ void imageInfrared1Callback(const sensor_msgs::ImageConstPtr & msg, const sensor
   inf1_caminfo_height_recv = info_msg->height;
   inf1_caminfo_width_recv = info_msg->width;
   inf1_dmodel_recv = info_msg->distortion_model;
+
+  // copy rotation matrix
+  for (unsigned int i = 0; i < sizeof(info_msg->R)/sizeof(double); i++)
+  {
+    inf1_caminfo_rotation_recv[i] = info_msg->R[i];
+  }
+
   infrared1_recv = true;
 }
 
@@ -89,6 +96,13 @@ void imageInfrared2Callback(const sensor_msgs::ImageConstPtr & msg, const sensor
   inf2_caminfo_height_recv = info_msg->height;
   inf2_caminfo_width_recv = info_msg->width;
   inf2_dmodel_recv = info_msg->distortion_model;
+
+  // copy rotation matrix
+  for (unsigned int i = 0; i < sizeof(info_msg->R)/sizeof(double); i++)
+  {
+    inf2_caminfo_rotation_recv[i] = info_msg->R[i];
+  }
+
   infrared2_recv = true;
 }
 
@@ -122,6 +136,12 @@ void imageDepthCallback(const sensor_msgs::ImageConstPtr & msg, const sensor_msg
   depth_caminfo_width_recv = info_msg->width;
   depth_dmodel_recv = info_msg->distortion_model;
 
+  // copy rotation matrix
+  for (unsigned int i = 0; i < sizeof(info_msg->R)/sizeof(double); i++)
+  {
+    depth_caminfo_rotation_recv[i] = info_msg->R[i];
+  }
+
   depth_recv = true;
 }
 
@@ -180,6 +200,12 @@ void imageColorCallback(const sensor_msgs::ImageConstPtr & msg, const sensor_msg
   color_caminfo_width_recv = info_msg->width;
   color_dmodel_recv = info_msg->distortion_model;
 
+  // copy rotation matrix
+  for (unsigned int i = 0; i < sizeof(info_msg->R)/sizeof(double); i++)
+  {
+    color_caminfo_rotation_recv[i] = info_msg->R[i];
+  }
+
   color_recv = true;
 }
 
@@ -227,6 +253,12 @@ TEST (RealsenseTests, testColorCameraInfo)
     EXPECT_EQ (color_width_recv, color_caminfo_width_recv);
     EXPECT_EQ (color_height_recv, color_caminfo_height_recv);
     EXPECT_STREQ (color_dmodel_recv.c_str (), "plumb_bob");
+
+    // verify rotation is equal to identity matrix
+    for (unsigned int i = 0; i < sizeof(ROTATION_IDENTITY)/sizeof(double); i++)
+    {
+      EXPECT_EQ (ROTATION_IDENTITY[i], color_caminfo_rotation_recv[i]);
+    }
   }
 }
 
@@ -274,6 +306,12 @@ TEST (RealsenseTests, testDepthCameraInfo)
     EXPECT_EQ (depth_width_recv, depth_caminfo_width_recv);
     EXPECT_EQ (depth_height_recv, depth_caminfo_height_recv);
     EXPECT_STREQ (depth_dmodel_recv.c_str (), "plumb_bob");
+
+    // verify rotation is equal to identity matrix
+    for (unsigned int i = 0; i < sizeof(ROTATION_IDENTITY)/sizeof(double); i++)
+    {
+      EXPECT_EQ (ROTATION_IDENTITY[i], depth_caminfo_rotation_recv[i]);
+    }
   }
 }
 
@@ -320,6 +358,12 @@ TEST (RealsenseTests, testInfrared1CameraInfo)
     EXPECT_EQ (infrared1_width_recv, inf1_caminfo_width_recv);
     EXPECT_EQ (infrared1_height_recv, inf1_caminfo_height_recv);
     EXPECT_STREQ (inf1_dmodel_recv.c_str (), "plumb_bob");
+
+    // verify rotation is equal to identity matrix
+    for (unsigned int i = 0; i < sizeof(ROTATION_IDENTITY)/sizeof(double); i++)
+    {
+      EXPECT_EQ (ROTATION_IDENTITY[i], inf1_caminfo_rotation_recv[i]);
+    }
   }
 }
 
@@ -358,6 +402,12 @@ TEST (RealsenseTests, testInfrared2CameraInfo)
     EXPECT_EQ (infrared2_width_recv, inf2_caminfo_width_recv);
     EXPECT_EQ (infrared2_height_recv, inf2_caminfo_height_recv);
     EXPECT_STREQ (inf2_dmodel_recv.c_str (), "plumb_bob");
+
+    // verify rotation is equal to identity matrix
+    for (unsigned int i = 0; i < sizeof(ROTATION_IDENTITY)/sizeof(double); i++)
+    {
+      EXPECT_EQ (ROTATION_IDENTITY[i], inf2_caminfo_rotation_recv[i]);
+    }
   }
 }
 

camera/test/realsense_camera_test_node.h

@@ -66,6 +66,7 @@ const char *DEPTH_DEF_FRAME = "camera_depth_frame";
 const char *COLOR_DEF_FRAME = "camera_rgb_frame";
 const char *DEPTH_OPTICAL_DEF_FRAME = "camera_depth_optical_frame";
 const char *COLOR_OPTICAL_DEF_FRAME = "camera_rgb_optical_frame";
+const double ROTATION_IDENTITY[] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
 
 //utest commandline args
 int color_height_exp = 0;
@@ -124,12 +125,16 @@ 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};
 int color_caminfo_height_recv = 0;
 int color_caminfo_width_recv = 0;
+double color_caminfo_rotation_recv[9] = {0};
 int inf1_caminfo_height_recv = 0;
 int inf1_caminfo_width_recv = 0;
+double inf1_caminfo_rotation_recv[9] = {0};
 int inf2_caminfo_height_recv = 0;
 int inf2_caminfo_width_recv = 0;
+double inf2_caminfo_rotation_recv[9] = {0};
 
 std::string inf1_dmodel_recv;
 std::string inf2_dmodel_recv;