From 64d905a44e4970e361e3a9e455218ed850acff69 Mon Sep 17 00:00:00 2001 From: Danil Tolkachev Date: Tue, 13 Aug 2019 10:03:50 +0300 Subject: [PATCH] Fix for case when left.Tx() != 0 Such a case is possible when using more than two cameras. For example, KITTI projection matrices for color cameras. --- image_geometry/include/image_geometry/stereo_camera_model.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/image_geometry/include/image_geometry/stereo_camera_model.h b/image_geometry/include/image_geometry/stereo_camera_model.h index a4f5880cd..20592f2ce 100644 --- a/image_geometry/include/image_geometry/stereo_camera_model.h +++ b/image_geometry/include/image_geometry/stereo_camera_model.h @@ -110,19 +110,19 @@ inline const cv::Matx44d& StereoCameraModel::reprojectionMatrix() const { return inline double StereoCameraModel::baseline() const { /// @todo Currently assuming horizontal baseline - return -right_.Tx() / right_.fx(); + return (left_.Tx() - right_.Tx()) / right_.fx(); } inline double StereoCameraModel::getZ(double disparity) const { assert( initialized() ); - return -right_.Tx() / (disparity - (left().cx() - right().cx())); + return (left_.Tx() - right_.Tx()) / (disparity - (left().cx() - right().cx())); } inline double StereoCameraModel::getDisparity(double Z) const { assert( initialized() ); - return -right_.Tx() / Z + (left().cx() - right().cx()); ; + return (left_.Tx() - right_.Tx()) / Z + (left().cx() - right().cx()); ; } } //namespace image_geometry