Fix camera info

Signed-off-by: Alejandro Hernández Cordero <[email protected]>
RobotLocomotion · Jan 8, 2024 · c410beb · c410beb
1 parent 496adf0
commit c410beb
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 4 deletions.
diff --git a/drake_ros/core/camera_info_system.cc b/drake_ros/core/camera_info_system.cc
@@ -24,6 +24,22 @@ void CameraInfoSystem::CalcCameraInfo(
   output_value->width = this->camera_info.width();
   output_value->distortion_model = "plumb_bob";
 
+  output_value->d.push_back(0);
+  output_value->d.push_back(0);
+  output_value->d.push_back(0);
+  output_value->d.push_back(0);
+  output_value->d.push_back(0);
+
+  output_value->r[0] = 1;
+  output_value->r[1] = 0;
+  output_value->r[2] = 0;
+  output_value->r[3] = 0;
+  output_value->r[4] = 1;
+  output_value->r[5] = 0;
+  output_value->r[6] = 0;
+  output_value->r[7] = 0;
+  output_value->r[8] = 1;
+
   //      │ f_x  0    c_x │
   //  K = │ 0    f_y  c_y │
   //      │ 0    0    1   │
@@ -34,14 +50,14 @@ void CameraInfoSystem::CalcCameraInfo(
   output_value->k[0] = this->camera_info.focal_x();
   output_value->k[2] = this->camera_info.center_x();
   output_value->k[4] = this->camera_info.focal_y();
-  output_value->k[5] = this->camera_info.center_x();
+  output_value->k[5] = this->camera_info.center_y();
   output_value->k[8] = 1.0;
 
   output_value->p[0] = this->camera_info.focal_x();
   output_value->p[2] = this->camera_info.center_x();
   output_value->p[3] = 0;
   output_value->p[5] = this->camera_info.focal_y();
-  output_value->p[6] = this->camera_info.center_x();
+  output_value->p[6] = this->camera_info.center_y();
   output_value->p[10] = 1.0;
 }
 

diff --git a/drake_ros_examples/examples/rgbd/rgbd.cpp b/drake_ros_examples/examples/rgbd/rgbd.cpp
@@ -133,7 +133,7 @@ int do_main() {
       {"renderer", {640, 480, M_PI_4}, {0.01, 10.0}, {}}, false};
   const DepthRenderCamera depth_camera{color_camera.core(), {0.01, 10.0}};
   const RigidTransformd X_WB =
-      ParseCameraPose("0.0, 1.0, -0.12, 1.57, 3.14, 0.0");
+      ParseCameraPose("0.8, 0.0, 0.7, -2.2, 0.0, 1.57");
 
   std::get<0>(camera_info_system)
       ->SetCameraInfo(color_camera.core().intrinsics());

diff --git a/drake_ros_examples/examples/rgbd/rgbd.sdf b/drake_ros_examples/examples/rgbd/rgbd.sdf
@@ -38,7 +38,7 @@
     </link>
 
     <link name="camera_optical">
-      <pose>0.0 1.0 -0.18 1.57 3.14 0.0</pose>
+      <pose>0.8 0.0 0.7 -2.2 0.0 1.57</pose>
     </link>
 
     <link name="Pole">