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 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(i, j); + int scale = (i == 2 && j == 2)?1:1000; + camera_info->P.at(i * p.cols + j) = p.at(i, j) / scale; } }