From c410beb8a412502dd5e0e30926a40af1b3b7098e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 8 Jan 2024 19:29:41 +0100 Subject: [PATCH] Fix camera info MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- drake_ros/core/camera_info_system.cc | 20 ++++++++++++++++++-- drake_ros_examples/examples/rgbd/rgbd.cpp | 2 +- drake_ros_examples/examples/rgbd/rgbd.sdf | 2 +- 3 files changed, 20 insertions(+), 4 deletions(-) diff --git a/drake_ros/core/camera_info_system.cc b/drake_ros/core/camera_info_system.cc index 6e82373d..67779c36 100644 --- 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 index ce53e393..94754a51 100644 --- 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 index 5b7bdbc4..6c47023d 100644 --- a/drake_ros_examples/examples/rgbd/rgbd.sdf +++ b/drake_ros_examples/examples/rgbd/rgbd.sdf @@ -38,7 +38,7 @@ - 0.0 1.0 -0.18 1.57 3.14 0.0 + 0.8 0.0 0.7 -2.2 0.0 1.57