From e91c5d53fd81b3d4d4c1e8d0af926c5a1680506f Mon Sep 17 00:00:00 2001 From: John Zhao Date: Sun, 6 Jan 2019 15:03:29 +0800 Subject: [PATCH] feat(ros): publish camera info with kannala_brandt model --- samples/tutorials/data/get_img_params.cc | 5 +- .../src/wrapper_nodelet.cc | 110 ++++++++++++------ 2 files changed, 77 insertions(+), 38 deletions(-) diff --git a/samples/tutorials/data/get_img_params.cc b/samples/tutorials/data/get_img_params.cc index 4e3d6d7..6f92660 100644 --- a/samples/tutorials/data/get_img_params.cc +++ b/samples/tutorials/data/get_img_params.cc @@ -26,8 +26,9 @@ int main(int argc, char *argv[]) { if (!ok) return 1; api->ConfigStreamRequest(request); - LOG(INFO) << "Intrinsics left: {" << api->GetIntrinsics(Stream::LEFT) << "}"; - LOG(INFO) << "Intrinsics right: {" << api->GetIntrinsics(Stream::RIGHT) + LOG(INFO) << "Intrinsics left: {" << *api->GetIntrinsicsBase(Stream::LEFT) + << "}"; + LOG(INFO) << "Intrinsics right: {" << *api->GetIntrinsicsBase(Stream::RIGHT) << "}"; LOG(INFO) << "Extrinsics right to left: {" << api->GetExtrinsics(Stream::RIGHT, Stream::LEFT) << "}"; 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 08acb81..842ad11 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 @@ -674,14 +674,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet { // if (points_publisher_.getNumSubscribers() == 0) // return; - auto &&in = api_->GetIntrinsics(Stream::LEFT); + auto &&in = api_->GetIntrinsicsBase(Stream::LEFT); sensor_msgs::PointCloud2 msg; msg.header.seq = seq; msg.header.stamp = stamp; msg.header.frame_id = frame_ids_[Stream::POINTS]; - msg.width = in.width; - msg.height = in.height; + msg.width = in->width; + msg.height = in->height; msg.is_dense = true; sensor_msgs::PointCloud2Modifier modifier(msg); @@ -702,8 +702,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { sensor_msgs::PointCloud2Iterator iter_g(msg, "g"); sensor_msgs::PointCloud2Iterator iter_b(msg, "b"); - for (std::size_t y = 0; y < in.height; ++y) { - for (std::size_t x = 0; x < in.width; ++x) { + for (std::size_t y = 0; y < in->height; ++y) { + for (std::size_t x = 0; x < in->width; ++x) { auto &&point = data.frame.at(y, x); *iter_x = point[0] * 0.001; @@ -923,9 +923,16 @@ class ROSWrapperNodelet : public nodelet::Nodelet { void computeRectTransforms() { ROS_ASSERT(api_); - auto &&in_left = api_->GetIntrinsics(Stream::LEFT); - auto &&in_right = api_->GetIntrinsics(Stream::RIGHT); - auto &&ex_right_to_left = api_->GetExtrinsics(Stream::RIGHT, Stream::LEFT); + auto in_left_base = api_->GetIntrinsicsBase(Stream::LEFT); + auto in_right_base = api_->GetIntrinsicsBase(Stream::RIGHT); + if (in_left_base->calib_model() != CalibrationModel::PINHOLE || + in_right_base->calib_model() != CalibrationModel::PINHOLE) { + return; + } + auto in_left = *std::dynamic_pointer_cast(in_left_base); + auto in_right = *std::dynamic_pointer_cast( + in_right_base); + auto ex_right_to_left = api_->GetExtrinsics(Stream::RIGHT, Stream::LEFT); cv::Size size{in_left.width, in_left.height}; cv::Mat M1 = @@ -964,44 +971,75 @@ class ROSWrapperNodelet : public nodelet::Nodelet { sensor_msgs::CameraInfo *camera_info = new sensor_msgs::CameraInfo(); camera_info_ptrs_[stream] = sensor_msgs::CameraInfoPtr(camera_info); - Intrinsics in; + std::shared_ptr in_base; if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) { - in = api_->GetIntrinsics(Stream::RIGHT); + in_base = api_->GetIntrinsicsBase(Stream::RIGHT); } else { - in = api_->GetIntrinsics(Stream::LEFT); + in_base = api_->GetIntrinsicsBase(Stream::LEFT); } camera_info->header.frame_id = frame_ids_[stream]; - camera_info->width = in.width; - camera_info->height = in.height; + camera_info->width = in_base->width; + camera_info->height = in_base->height; - // [fx 0 cx] - // K = [ 0 fy cy] - // [ 0 0 1] - camera_info->K.at(0) = in.fx; - camera_info->K.at(2) = in.cx; - camera_info->K.at(4) = in.fy; - camera_info->K.at(5) = in.cy; - camera_info->K.at(8) = 1; + if (in_base->calib_model() == CalibrationModel::PINHOLE) { + auto in = std::dynamic_pointer_cast(in_base); + // [fx 0 cx] + // K = [ 0 fy cy] + // [ 0 0 1] + camera_info->K.at(0) = in->fx; + camera_info->K.at(2) = in->cx; + camera_info->K.at(4) = in->fy; + camera_info->K.at(5) = in->cy; + camera_info->K.at(8) = 1; - // [fx' 0 cx' Tx] - // P = [ 0 fy' cy' Ty] - // [ 0 0 1 0] - cv::Mat p = left_p_; - if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) { - p = right_p_; - } - 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); + // [fx' 0 cx' Tx] + // P = [ 0 fy' cy' Ty] + // [ 0 0 1 0] + cv::Mat p = left_p_; + if (stream == Stream::RIGHT || stream == Stream::RIGHT_RECTIFIED) { + p = right_p_; + } + 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); + } } - } - camera_info->distortion_model = "plumb_bob"; + camera_info->distortion_model = "plumb_bob"; - // D of plumb_bob: (k1, k2, t1, t2, k3) - for (int i = 0; i < 5; i++) { - camera_info->D.push_back(in.coeffs[i]); + // D of plumb_bob: (k1, k2, t1, t2, k3) + for (int i = 0; i < 5; i++) { + camera_info->D.push_back(in->coeffs[i]); + } + } else if (in_base->calib_model() == CalibrationModel::KANNALA_BRANDT) { + auto in = std::dynamic_pointer_cast(in_base); + + camera_info->distortion_model = "kannala_brandt"; + + // coeffs: k2,k3,k4,k5,mu,mv,u0,v0 + camera_info->D.push_back(in->coeffs[0]); // k2 + camera_info->D.push_back(in->coeffs[1]); // k3 + camera_info->D.push_back(in->coeffs[2]); // k4 + camera_info->D.push_back(in->coeffs[3]); // k5 + + camera_info->K[0] = in->coeffs[4]; // mu + camera_info->K[4] = in->coeffs[5]; // mv + camera_info->K[2] = in->coeffs[6]; // u0 + camera_info->K[5] = in->coeffs[7]; // v0 + camera_info->K[8] = 1; + + // auto baseline = api_->GetInfo(Info::NOMINAL_BASELINE); + // Eigen::Matrix3d K = Eigen::Matrix3d::Identity(); + // K(0, 0) = camera_info->K[0]; + // K(0, 2) = camera_info->K[2]; + // K(1, 1) = camera_info->K[4]; + // K(1, 2) = camera_info->K[5]; + // Eigen::Map>( + // camera_info->P.data()) = (Eigen::Matrix() << + // K, Eigen::Vector3d(baseline * K(0, 0), 0, 0)).finished(); + } else { + NODELET_FATAL_STREAM("Unknown calib model, please use latest SDK."); } // R to identity matrix