From 7bc74f4d1ba45c2d92706f440adeacd41000bcfe Mon Sep 17 00:00:00 2001
From: kalman <kalmanxiong@slightech.com>
Date: Fri, 22 Feb 2019 15:09:06 +0800
Subject: [PATCH] fix(ros): fix camera info bug

---
 .../ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc  | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index 72b5541..c1da91b 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -623,11 +623,11 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     std::vector<Stream> all_streams{
         Stream::RIGHT,
         Stream::LEFT,
-        Stream::LEFT_RECTIFIED, 
+        Stream::LEFT_RECTIFIED,
         Stream::RIGHT_RECTIFIED,
-        Stream::DISPARITY,      
+        Stream::DISPARITY,
         Stream::DISPARITY_NORMALIZED,
-        Stream::POINTS,         
+        Stream::POINTS,
         Stream::DEPTH
         };
 
@@ -1290,7 +1290,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       }
       for (int i = 0; i < p.rows; i++) {
         for (int j = 0; j < p.cols; j++) {
-          camera_info->P.at(i * p.cols + j) = p.at<double>(i, j);
+          int scale = (i == 2 && j == 2)?1:1000;
+          camera_info->P.at(i * p.cols + j) = p.at<double>(i, j) / scale;
         }
       }