Skip to content

Commit

Permalink
Generalize solution to all stereo sensors (fisheye)
Browse files Browse the repository at this point in the history 
Remove updateExtrinsicsCalibData function.
  • Loading branch information
@doronhi
doronhi committed May 2, 2021
1 parent 681df9d commit 805d1b3
Show file tree
Hide file tree
Showing 2 changed files with 8 additions and 69 deletions.
1 change: 0 additions & 1 deletion realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,7 +203,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 Down
76 changes: 8 additions & 68 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1812,9 +1812,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 @@ -1824,18 +1821,10 @@ 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;
std::map<std::string, rs2::sensor> active_sensors;
Expand Down Expand Up @@ -1903,11 +1892,15 @@ void BaseRealSenseNode::updateStreamCalibData(const rs2::video_stream_profile& v
_camera_info[stream_index].P.at(11) = 0;

// Set Tx, Ty for right camera
if (stream_index == INFRA2 && _enable[INFRA1])
if (stream_index.second == 2)
{
const auto& ex = getAProfile(INFRA2).get_extrinsics_to(getAProfile(INFRA1));
_camera_info[stream_index].P.at(3) = -intrinsic.fx * ex.translation[0]; // Tx
_camera_info[stream_index].P.at(7) = -intrinsic.fy * ex.translation[1]; // Ty
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].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.

This comment has been minimized.

Sign in to view

Copy link
@meyerj

meyerj Aug 23, 2022

Should the rectification matrix _camera_info[stream_index].R also have been set from the rotation part of ex, and be applied to calculate the camera matrix _camera_info[stream_index].P for the right camera? That is what was done in the old updateExtrinsicsCalibData() method at least, which was the result of #1242. Apparently there was no extra PR for this commit.

See sensor_msgs/CameraInfo and https://wiki.ros.org/image_pipeline/CameraInfo for the "official" specs.
}
}

if (intrinsic.model == RS2_DISTORTION_KANNALA_BRANDT4)
Expand Down Expand Up @@ -1954,59 +1947,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);

}

tf::Quaternion BaseRealSenseNode::rotationMatrixToQuaternion(const float rotation[9]) const
{
Eigen::Matrix3f m;
Expand Down

0 comments on commit 805d1b3

Please sign in to comment.