Skip to content

Commit

Permalink
Merge pull request #1849 from doronhi/ros2-tx-ty
Browse files Browse the repository at this point in the history 
ROS2 - Add filling correct Tx, Ty values in projection matrix of right camera.
  • Loading branch information
@doronhi
doronhi authored May 5, 2021
2 parents f513883 + 978dbfc commit c843f9c
Show file tree
Hide file tree
Showing 2 changed files with 26 additions and 79 deletions.
2 changes: 0 additions & 2 deletions realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -221,7 +220,6 @@ namespace realsense2_camera
const std::map<stream_index_pair, image_transport::Publisher>& image_publishers,
std::map<stream_index_pair, int>& seq,
std::map<stream_index_pair, sensor_msgs::msg::CameraInfo>& camera_info,
const std::map<stream_index_pair, std::string>& optical_frame_id,
const std::map<rs2_stream, std::string>& encoding);
bool getEnabledProfile(const stream_index_pair& stream_index, rs2::stream_profile& profile);

Expand Down
103 changes: 26 additions & 77 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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)
Expand All @@ -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);
}
}
Expand All @@ -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);
}
}
Expand Down Expand Up @@ -1903,9 +1903,6 @@ void BaseRealSenseNode::setupStreams()
{
ROS_INFO("setupStreams...");
try{
std::shared_ptr<rs2::video_stream_profile> left_profile;
std::shared_ptr<rs2::video_stream_profile> right_profile;

// Publish image stream info
for (auto& profiles : _enabled_profiles)
{
Expand All @@ -1915,16 +1912,9 @@ void BaseRealSenseNode::setupStreams()
{
auto video_profile = profile.as<rs2::video_stream_profile>();
updateStreamCalibData(video_profile);

// stream index: 1=left, 2=right
if (video_profile.stream_index() == 1) { left_profile = std::make_shared<rs2::video_stream_profile>(video_profile); }
if (video_profile.stream_index() == 2) { right_profile = std::make_shared<rs2::video_stream_profile>(video_profile); }
}
}
}
if (left_profile && right_profile) {
updateExtrinsicsCalibData(*left_profile, *right_profile);
}

// Streaming IMAGES
std::map<std::string, std::vector<rs2::stream_profile> > profiles;
Expand Down Expand Up @@ -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";
Expand Down Expand Up @@ -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<Eigen::Matrix<float,3,3,Eigen::RowMajor>>(LEFT_T_RIGHT.rotation);
auto T = Eigen::Map<Eigen::Matrix<float,3,1>>(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<float,3,4,Eigen::RowMajor> RT;
RT << R, T;

auto K_right = Eigen::Map<Eigen::Matrix<double,3,3,Eigen::RowMajor>>(_camera_info[right].k.data());

// Compute Projection matrix for the right camera
auto P_right = K_right.cast<float>() * 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;
Expand Down Expand Up @@ -2493,7 +2443,6 @@ void BaseRealSenseNode::publishFrame(rs2::frame f, const rclcpp::Time& t,
const std::map<stream_index_pair, image_transport::Publisher>& image_publishers,
std::map<stream_index_pair, int>& seq,
std::map<stream_index_pair, sensor_msgs::msg::CameraInfo>& camera_info,
const std::map<stream_index_pair, std::string>& optical_frame_id,
const std::map<rs2_stream, std::string>& encoding)
{
ROS_DEBUG("publishFrame(...)");
Expand Down Expand Up @@ -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)
{
Expand All @@ -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()));
}
Expand Down

0 comments on commit c843f9c

Please sign in to comment.