From 978dbfc2ec4e265f51fefd396ac5d0f0c0b3986b Mon Sep 17 00:00:00 2001 From: doronhi Date: Wed, 5 May 2021 15:19:24 +0300 Subject: [PATCH] Migrating PR#901 by @pavloblindnology Added filling correct Tx, Ty values in projection matrix of right camera. Generalize solution to all stereo sensors (fisheye) Fixed frame_id of right sensor to match left sensor in a stereo pair. Based on suggestion by @pavloblindnology (https://github.com/IntelRealSense/realsense-ros/pull/1242#issuecomment-785778234) --- .../include/base_realsense_node.h | 2 - realsense2_camera/src/base_realsense_node.cpp | 103 +++++------------- 2 files changed, 26 insertions(+), 79 deletions(-) diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 3c46d3aead..6a00fd7d9e 100644 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -204,7 +204,6 @@ namespace realsense2_camera cv::Mat& fix_depth_scale(const cv::Mat& from_image, cv::Mat& to_image); void clip_depth(rs2::depth_frame depth_frame, float clipping_dist); void updateStreamCalibData(const rs2::video_stream_profile& video_profile); - void updateExtrinsicsCalibData(const rs2::video_stream_profile& left_video_profile, const rs2::video_stream_profile& right_video_profile); void SetBaseStream(); void publishStaticTransforms(); void publishDynamicTransforms(); @@ -221,7 +220,6 @@ namespace realsense2_camera const std::map& image_publishers, std::map& seq, std::map& camera_info, - const std::map& optical_frame_id, const std::map& encoding); bool getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile); diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 48912df0a8..cd0ca3f67e 100644 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -1793,7 +1793,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) _depth_aligned_image, _depth_aligned_info_publisher, _depth_aligned_image_publishers, _depth_aligned_seq, - _depth_aligned_camera_info, _optical_frame_id, + _depth_aligned_camera_info, _depth_aligned_encoding); continue; } @@ -1803,7 +1803,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) _image, _info_publisher, _image_publishers, _seq, - _camera_info, _optical_frame_id, + _camera_info, _encoding); } if (original_depth_frame && _align_depth) @@ -1819,7 +1819,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) _image, _info_publisher, _image_publishers, _seq, - _camera_info, _optical_frame_id, + _camera_info, _encoding); } } @@ -1844,7 +1844,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) _image, _info_publisher, _image_publishers, _seq, - _camera_info, _optical_frame_id, + _camera_info, _encoding); } } @@ -1903,9 +1903,6 @@ void BaseRealSenseNode::setupStreams() { ROS_INFO("setupStreams..."); try{ - std::shared_ptr left_profile; - std::shared_ptr right_profile; - // Publish image stream info for (auto& profiles : _enabled_profiles) { @@ -1915,16 +1912,9 @@ void BaseRealSenseNode::setupStreams() { auto video_profile = profile.as(); updateStreamCalibData(video_profile); - - // stream index: 1=left, 2=right - if (video_profile.stream_index() == 1) { left_profile = std::make_shared(video_profile); } - if (video_profile.stream_index() == 2) { right_profile = std::make_shared(video_profile); } } } } - if (left_profile && right_profile) { - updateExtrinsicsCalibData(*left_profile, *right_profile); - } // Streaming IMAGES std::map > profiles; @@ -1992,6 +1982,19 @@ void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& v _camera_info[stream_index].p.at(10) = 1; _camera_info[stream_index].p.at(11) = 0; + // Set Tx, Ty for right camera + if (stream_index.second == 2) + { + stream_index_pair sip1{stream_index.first, 1}; + if (_enable[sip1]) + { + const auto& ex = getAProfile(stream_index).get_extrinsics_to(getAProfile(sip1)); + _camera_info[stream_index].header.frame_id = _optical_frame_id[sip1]; + _camera_info[stream_index].p.at(3) = -intrinsic.fx * ex.translation[0] + 0.0; // Tx - avoid -0.0 values. + _camera_info[stream_index].p.at(7) = -intrinsic.fy * ex.translation[1] + 0.0; // Ty - avoid -0.0 values. + } + } + if (intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4) { _camera_info[stream_index].distortion_model = "equidistant"; @@ -2036,59 +2039,6 @@ void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& v } } -void BaseRealSenseNode::updateExtrinsicsCalibData(const rs2::video_stream_profile& left_video_profile, const rs2::video_stream_profile& right_video_profile) -{ - stream_index_pair left{left_video_profile.stream_type(), left_video_profile.stream_index()}; - stream_index_pair right{right_video_profile.stream_type(), right_video_profile.stream_index()}; - - // Get the relative extrinsics between the left and right camera - auto LEFT_T_RIGHT = right_video_profile.get_extrinsics_to(left_video_profile); - - auto R = Eigen::Map>(LEFT_T_RIGHT.rotation); - auto T = Eigen::Map>(LEFT_T_RIGHT.translation); - - // force y- and z-axis components to be 0 (but do we also need to force P(0,3) and P(1,3) to be 0?) - T[1] = 0; - T[2] = 0; - - Eigen::Matrix RT; - RT << R, T; - - auto K_right = Eigen::Map>(_camera_info[right].k.data()); - - // Compute Projection matrix for the right camera - auto P_right = K_right.cast() * RT; - - // Note that all matrices are stored in row-major format - // 1. Leave the left rotation matrix as identity - // 2. Set the right rotation matrix - _camera_info[right].r.at(0) = LEFT_T_RIGHT.rotation[0]; - _camera_info[right].r.at(1) = LEFT_T_RIGHT.rotation[1]; - _camera_info[right].r.at(2) = LEFT_T_RIGHT.rotation[2]; - _camera_info[right].r.at(3) = LEFT_T_RIGHT.rotation[3]; - _camera_info[right].r.at(4) = LEFT_T_RIGHT.rotation[4]; - _camera_info[right].r.at(5) = LEFT_T_RIGHT.rotation[5]; - _camera_info[right].r.at(6) = LEFT_T_RIGHT.rotation[6]; - _camera_info[right].r.at(7) = LEFT_T_RIGHT.rotation[7]; - _camera_info[right].r.at(8) = LEFT_T_RIGHT.rotation[8]; - - // 3. Leave the left projection matrix - // 4. Set the right projection matrix - _camera_info[right].p.at(0) = P_right(0,0); - _camera_info[right].p.at(1) = P_right(0,1); - _camera_info[right].p.at(2) = P_right(0,2); - _camera_info[right].p.at(3) = P_right(0,3); - _camera_info[right].p.at(4) = P_right(1,0); - _camera_info[right].p.at(5) = P_right(1,1); - _camera_info[right].p.at(6) = P_right(1,2); - _camera_info[right].p.at(7) = P_right(1,3); - _camera_info[right].p.at(8) = P_right(2,0); - _camera_info[right].p.at(9) = P_right(2,1); - _camera_info[right].p.at(10) = P_right(2,2); - _camera_info[right].p.at(11) = P_right(2,3); - -} - tf2::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotation[9]) const { Eigen::Matrix3f m; @@ -2493,7 +2443,6 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, const std::map& image_publishers, std::map& seq, std::map& camera_info, - const std::map& optical_frame_id, const std::map& encoding) { ROS_DEBUG("publishFrame(...)"); @@ -2526,15 +2475,6 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, if(0 != info_publisher->get_subscription_count() || 0 != image_publisher.getNumSubscribers()) { - sensor_msgs::msg::Image::SharedPtr img; - img = cv_bridge::CvImage(std_msgs::msg::Header(), encoding.at(stream.first), image).toImageMsg(); - img->width = width; - img->height = height; - img->is_bigendian = false; - img->step = width * bpp; - img->header.frame_id = optical_frame_id.at(stream); - img->header.stamp = t; - auto& cam_info = camera_info.at(stream); if (cam_info.width != width) { @@ -2543,6 +2483,15 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t, cam_info.header.stamp = t; info_publisher->publish(cam_info); + sensor_msgs::msg::Image::SharedPtr img; + img = cv_bridge::CvImage(std_msgs::msg::Header(), encoding.at(stream.first), image).toImageMsg(); + img->width = width; + img->height = height; + img->is_bigendian = false; + img->step = width * bpp; + img->header.frame_id = cam_info.header.frame_id; + img->header.stamp = t; + image_publisher.publish(img); ROS_DEBUG("%s stream published", rs2_stream_to_string(f.get_profile().stream_type())); }