diff --git a/README.md b/README.md index a4aa5f2..660fcbb 100644 --- a/README.md +++ b/README.md @@ -14,10 +14,10 @@ This repo contains six related ros nodes- * **[point_to_bearing_node](#point_to_bearing_node):** Takes in a 2D image location and transforms it into a bearing vector. ## Dependencies -Image undistort depends on ROS, OpenCV and Eigen. The point to bearing node also depends on NLopt (installed with `apt install libnlopt-dev`) and will only be built if it is found. +Image undistort depends on ROS, OpenCV and Eigen. The point to bearing node also depends on NLopt (installed with `apt install libnlopt-dev`) and will only be built if it is found. ## Supported Camera and Distortion Models -The only supported output is the pinhole camera model with no distortion. +The only supported output is the pinhole camera model with no distortion. Supported input models: * Pinhole with no distortion @@ -28,7 +28,7 @@ Supported input models: * FOV * Unified * Extended Unified -* Double Sphere +* Double Sphere # image_undistort_node: @@ -75,10 +75,11 @@ Camera information can be loaded from ROS parameters. These parameters are typic 1. A 3x3 intrinscs matrix named **K** is searched for. If it is found it is loaded. If it is not found a 1x4 vector named **intrinsics** is loaded, this contains the parameters (fx, fy, cx, cy). If neither parameters are given the node displays an error and terminates. 2. A 1x2 vector named **resolution** is loaded which contains the parameters (width, height). Again, if not given the node displays an error and terminates. 3. A 4x4 transformation matrix **T** is searched for. If it is found it is loaded. Otherwise it is searched for under the name **T_cn_cnm1** and if found loaded. If neither are found the node continues. -4. A 4x3 projection matrix **P** is searched for. If it is found it is loaded. If **P** was found but **T** was not, **P** and **K** are used to construct **T**, otherwise **T** is set to identity. If **P** was not found it is constructed from **K** and **T**. -5. If an output is being loaded, the loading of parameters is completed. For input cameras the distortion properties are now loaded -6. A 1xn vector **D** is loaded. If it is not found or is less than 5 elements long it is padded with zeros. -7. A string **distortion_model** is loaded and converted to lower-case. If it is not found it is set to "radtan". +4. A 4x4 transformation matrix **T_cam_imu** is searched for (provided by IMU camera calibration using Kalibr). If it is found it is loaded and a tf from imu to cam is published. If it can't find the value the node continues. +5. A 4x3 projection matrix **P** is searched for. If it is found it is loaded. If **P** was found but **T** was not, **P** and **K** are used to construct **T**, otherwise **T** is set to identity. If **P** was not found it is constructed from **K** and **T**. +6. If an output is being loaded, the loading of parameters is completed. For input cameras the distortion properties are now loaded. +7. A 1xn vector **D** is loaded. If it is not found or is less than 5 elements long it is padded with zeros. +8. A string **distortion_model** is loaded and converted to lower-case. If it is not found it is set to "radtan". # stereo_info_node: A node that takes in the properties of two cameras and outputs the camera info required to rectify them so that stereo reconstruction can be performed. The rectification is performed such that only x translation is present between the cameras. The focal points are in the image centers, fx=fy and the image resolution is set to be the largest frame that contains no empty pixels. Note this node can also be run as a nodelet named image_undistort/StereoInfo @@ -93,8 +94,8 @@ A node that takes in the properties of two cameras and outputs the camera info r ## Input/Output Topics Many of these topics are dependent on the parameters set above and may not appear or may be renamed under some settings. -* **raw/first/image** first input image topic, only needed if loading camera parameters from ros params (used for timing information) -* **raw/second/image** second input image topic, only needed if loading camera parameters from ros params (used for timing information) +* **raw/first/image** first input image topic, only needed if loading camera parameters from ros params (used for timing information) +* **raw/second/image** second input image topic, only needed if loading camera parameters from ros params (used for timing information) * **raw/first/camera_info** first input camera info topic * **raw/second/camera_info** second input camera info topic * **rect/first/camera_info** first output camera info topic diff --git a/include/image_undistort/camera_parameters.h b/include/image_undistort/camera_parameters.h index 2a77b8e..f965e63 100644 --- a/include/image_undistort/camera_parameters.h +++ b/include/image_undistort/camera_parameters.h @@ -41,6 +41,7 @@ class BaseCameraParameters { const cv::Size& resolution() const; // get image size const Eigen::Matrix& T() const; // get transformation matrix + const Eigen::Matrix& T_cam_imu() const; const Eigen::Ref> R() const; // get rotation matrix const Eigen::Ref> p() @@ -77,6 +78,7 @@ class BaseCameraParameters { cv::Size resolution_; Eigen::Matrix T_; + Eigen::Matrix T_cam_imu_; Eigen::Matrix P_; Eigen::Matrix K_; }; diff --git a/include/image_undistort/image_undistort.h b/include/image_undistort/image_undistort.h index 417dd0d..186ee7b 100644 --- a/include/image_undistort/image_undistort.h +++ b/include/image_undistort/image_undistort.h @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -22,7 +23,7 @@ namespace image_undistort { // Default values // queue size -constexpr int kImageQueueSize = 10; +constexpr int kImageQueueSize = 1; // true to load input cam_info from ros parameters, false to get it from a // cam_info topic constexpr bool kDefaultInputCameraInfoFromROSParams = false; @@ -59,8 +60,14 @@ const std::string kDefaultOutputImageType = ""; constexpr double kDefaultScale = 1.0; // if a tf between the input and output frame should be created constexpr bool kDefaultPublishTF = true; +// if a tf between the imu and camera frame should be created +constexpr bool kDefaultPublishImuCamTF = true; // name of output image frame const std::string kDefaultOutputFrame = "output_camera"; +// name of input imu_cam tf frame +const std::string kDefaultImuCamInputFrame = "input_imu"; +// name of output imu_cam tf frame +const std::string kDefaultImuCamOutputFrame = "output_camera"; // rename input frames constexpr bool kDefaultRenameInputFrame = false; // new name of input frame (only used if rename_input_frame = true) @@ -110,7 +117,7 @@ class ImageUndistort { std::shared_ptr undistorter_ptr_; // tf broadcaster - tf::TransformBroadcaster br_; + tf2_ros::StaticTransformBroadcaster br_; // camera info std::shared_ptr camera_parameters_pair_ptr_; @@ -130,13 +137,16 @@ class ImageUndistort { std::string output_image_type_; double scale_; bool publish_tf_; + bool publish_imu_cam_tf_; std::string output_frame_; + std::string input_imu_cam_frame_; + std::string output_imu_cam_frame_; bool rename_input_frame_; std::string input_frame_; bool rename_radtan_plumb_bob_; int frame_counter_; }; -} +} // namespace image_undistort #endif diff --git a/src/camera_parameters.cpp b/src/camera_parameters.cpp index 2d57795..45e1eb6 100644 --- a/src/camera_parameters.cpp +++ b/src/camera_parameters.cpp @@ -10,6 +10,9 @@ BaseCameraParameters::BaseCameraParameters(const ros::NodeHandle& nh, XmlRpc::XmlRpcValue K_in; bool K_loaded = false; + std::string k_param = camera_namespace + "/K"; + std::cout << k_param << std::endl; + std::cout << nh.getParam("/K", K_in) << std::endl; if (nh.getParam(camera_namespace + "/K", K_in)) { xmlRpcToMatrix(K_in, &K_); K_loaded = true; @@ -32,6 +35,7 @@ BaseCameraParameters::BaseCameraParameters(const ros::NodeHandle& nh, K_(0, 2) = intrinsics_in[intrinsics_in.size() - 2]; K_(1, 2) = intrinsics_in[intrinsics_in.size() - 1]; } else if (!K_loaded) { + ROS_ERROR_STREAM("Tried to load from " << camera_namespace + "/intrinsics"); throw std::runtime_error("Could not find K or camera intrinsics vector"); } @@ -46,6 +50,15 @@ BaseCameraParameters::BaseCameraParameters(const ros::NodeHandle& nh, throw std::runtime_error("Could not find camera resolution"); } + XmlRpc::XmlRpcValue T_cam_imu; + bool T_cam_imu_loaded = false; + if (nh.getParam(camera_namespace + "/T_cam_imu", T_cam_imu)) { + xmlRpcToMatrix(T_cam_imu, &T_cam_imu_); + T_cam_imu_loaded = true; + } else { + T_cam_imu_ = Eigen::Matrix4d::Identity(); + } + XmlRpc::XmlRpcValue T_in; bool T_loaded = false; if (nh.getParam(camera_namespace + "/T_cn_cnm1", T_in) || @@ -125,6 +138,9 @@ const cv::Size& BaseCameraParameters::resolution() const { return resolution_; } const Eigen::Matrix& BaseCameraParameters::T() const { return T_; } +const Eigen::Matrix& BaseCameraParameters::T_cam_imu() const { + return T_cam_imu_; +} const Eigen::Ref> BaseCameraParameters::R() const { return T_.topLeftCorner<3, 3>(); @@ -701,4 +717,4 @@ const CameraParametersPair& StereoCameraParameters::getFirst() const { const CameraParametersPair& StereoCameraParameters::getSecond() const { return second_; } -} // namespace image_undistort \ No newline at end of file +} // namespace image_undistort diff --git a/src/image_undistort.cpp b/src/image_undistort.cpp index d19906d..a0afca5 100644 --- a/src/image_undistort.cpp +++ b/src/image_undistort.cpp @@ -83,12 +83,28 @@ ImageUndistort::ImageUndistort(const ros::NodeHandle& nh, } nh_private_.param("publish_tf", publish_tf_, kDefaultPublishTF); + nh_private_.param("publish_imu_cam_tf", publish_imu_cam_tf_, + kDefaultPublishImuCamTF); nh_private_.param("output_frame", output_frame_, kDefaultOutputFrame); if (output_frame_.empty()) { ROS_ERROR("Output frame cannot be blank, setting to default"); output_frame_ = kDefaultOutputFrame; } + nh_private_.param("input_imu_cam_frame", input_imu_cam_frame_, + kDefaultImuCamInputFrame); + if (input_imu_cam_frame_.empty()) { + ROS_ERROR("Input imu cam frame cannot be blank, setting to default"); + input_imu_cam_frame_ = kDefaultImuCamInputFrame; + } + + nh_private_.param("output_imu_cam_frame", output_imu_cam_frame_, + kDefaultImuCamOutputFrame); + if (output_imu_cam_frame_.empty()) { + ROS_ERROR("Output imu cam frame cannot be blank, setting to default"); + output_imu_cam_frame_ = kDefaultImuCamOutputFrame; + } + nh_private_.param("rename_input_frame", rename_input_frame_, kDefaultRenameInputFrame); nh_private_.param("input_frame", input_frame_, kDefaultInputFrame); @@ -228,7 +244,6 @@ void ImageUndistort::imageCallback( tf::Vector3 p_ros; tf::matrixEigenToTF(T.topLeftCorner<3, 3>(), R_ros); tf::vectorEigenToTF(T.topRightCorner<3, 1>(), p_ros); - tf::Transform(R_ros, p_ros); std::string frame = image_in_ptr->header.frame_id; if (rename_input_frame_) { @@ -237,11 +252,50 @@ void ImageUndistort::imageCallback( if (frame.empty()) { ROS_ERROR_ONCE("Image frame name is blank, cannot construct tf"); } else { - br_.sendTransform(tf::StampedTransform(tf::Transform(R_ros, p_ros), - image_out_ptr->header.stamp, frame, - output_frame_)); + geometry_msgs::TransformStamped static_transform_stamped; + static_transform_stamped.header.stamp = image_out_ptr->header.stamp; + static_transform_stamped.header.frame_id = frame; + static_transform_stamped.child_frame_id = output_frame_; + static_transform_stamped.transform.translation.x = p_ros.x(); + static_transform_stamped.transform.translation.y = p_ros.y(); + static_transform_stamped.transform.translation.z = p_ros.z(); + tf::Quaternion quat; + R_ros.getRotation(quat); + static_transform_stamped.transform.rotation.x = quat.x(); + static_transform_stamped.transform.rotation.y = quat.y(); + static_transform_stamped.transform.rotation.z = quat.z(); + static_transform_stamped.transform.rotation.w = quat.w(); + br_.sendTransform(static_transform_stamped); } } + if (publish_imu_cam_tf_) { + // TODO(ff): Publish also imu cam transform. + // We invert this by default, as we are typically interested in a tf tree + // from imu to cam. + const Eigen::Matrix4d T_cam_imu = + camera_parameters_pair_ptr_->getInputPtr()->T_cam_imu(); + const Eigen::Matrix T_imu_cam = T_cam_imu.inverse(); + tf::Matrix3x3 imu_cam_R_ros; + tf::Vector3 imu_cam_p_ros; + tf::matrixEigenToTF(T_imu_cam.block<3, 3>(0, 0), imu_cam_R_ros); + tf::vectorEigenToTF(T_imu_cam.block<3, 1>(0, 3), imu_cam_p_ros); + + + geometry_msgs::TransformStamped static_transform_stamped; + static_transform_stamped.header.stamp = image_out_ptr->header.stamp; + static_transform_stamped.header.frame_id = input_imu_cam_frame_; + static_transform_stamped.child_frame_id = output_imu_cam_frame_; + static_transform_stamped.transform.translation.x = imu_cam_p_ros.x(); + static_transform_stamped.transform.translation.y = imu_cam_p_ros.y(); + static_transform_stamped.transform.translation.z = imu_cam_p_ros.z(); + tf::Quaternion quat; + imu_cam_R_ros.getRotation(quat); + static_transform_stamped.transform.rotation.x = quat.x(); + static_transform_stamped.transform.rotation.y = quat.y(); + static_transform_stamped.transform.rotation.z = quat.z(); + static_transform_stamped.transform.rotation.w = quat.w(); + br_.sendTransform(static_transform_stamped); + } } void ImageUndistort::cameraCallback( @@ -265,4 +319,4 @@ void ImageUndistort::cameraInfoCallback( ROS_ERROR("Setting output camera from ros message failed"); } } -} +} // namespace image_undistort diff --git a/src/undistorter.cpp b/src/undistorter.cpp index 9d02809..b40b871 100644 --- a/src/undistorter.cpp +++ b/src/undistorter.cpp @@ -60,11 +60,11 @@ Undistorter::Undistorter( void Undistorter::undistortImage(const cv::Mat& image, cv::Mat* undistorted_image) { if (empty_pixels_) { - cv::remap(image, *undistorted_image, map_x_, map_y_, cv::INTER_LINEAR, + cv::remap(image, *undistorted_image, map_x_, map_y_, cv::INTER_NEAREST, cv::BORDER_CONSTANT); } else { // replicate is more efficient for gpus to calculate - cv::remap(image, *undistorted_image, map_x_, map_y_, cv::INTER_LINEAR, + cv::remap(image, *undistorted_image, map_x_, map_y_, cv::INTER_NEAREST, cv::BORDER_REPLICATE); } } @@ -89,7 +89,8 @@ void Undistorter::distortPixel(const Eigen::Matrix& K_in, const double& x = norm_pixel_location.x(); const double& y = norm_pixel_location.y(); - Eigen::Vector3d norm_distorted_pixel_location(0.0, 0.0, norm_pixel_location.z()); + Eigen::Vector3d norm_distorted_pixel_location(0.0, 0.0, + norm_pixel_location.z()); double& xd = norm_distorted_pixel_location.x(); double& yd = norm_distorted_pixel_location.y(); @@ -170,12 +171,12 @@ void Undistorter::distortPixel(const Eigen::Matrix& K_in, const double& p1 = D[3]; const double& p2 = D[4]; - //apply omni model - const double d = std::sqrt(x*x + y*y + 1.0); + // apply omni model + const double d = std::sqrt(x * x + y * y + 1.0); const double scaling = 1.0 / (1.0 + xi * d); - const double x_temp = x*scaling; - const double y_temp = y*scaling; + const double x_temp = x * scaling; + const double y_temp = y * scaling; // Undistort const double r2 = x_temp * x_temp + y_temp * y_temp; @@ -183,8 +184,10 @@ void Undistorter::distortPixel(const Eigen::Matrix& K_in, const double r6 = r4 * r2; const double kr = (1.0 + k1 * r2 + k2 * r4 + k3 * r6); - xd = (x_temp * kr + 2.0 * p1 * x_temp * y_temp + p2 * (r2 + 2.0 * x_temp * x_temp)); - yd = (y_temp * kr + 2.0 * p2 * x_temp * y_temp + p1 * (r2 + 2.0 * y_temp * y_temp)); + xd = (x_temp * kr + 2.0 * p1 * x_temp * y_temp + + p2 * (r2 + 2.0 * x_temp * x_temp)); + yd = (y_temp * kr + 2.0 * p2 * x_temp * y_temp + + p1 * (r2 + 2.0 * y_temp * y_temp)); } break; case DistortionModel::DOUBLESPHERE: { // Split out parameters for easier reading @@ -192,8 +195,10 @@ void Undistorter::distortPixel(const Eigen::Matrix& K_in, const double& alpha = D[1]; const double d1 = std::sqrt(x * x + y * y + 1.0); - const double d2 = std::sqrt(x * x + y * y + (epsilon*d1 + 1.0)*(epsilon*d1 + 1.0)); - const double scaling = 1.0f/(alpha*d2 + (1-alpha)*(epsilon*d1+1.0)); + const double d2 = std::sqrt(x * x + y * y + + (epsilon * d1 + 1.0) * (epsilon * d1 + 1.0)); + const double scaling = + 1.0f / (alpha * d2 + (1 - alpha) * (epsilon * d1 + 1.0)); xd = x * scaling; yd = y * scaling; } break; @@ -202,7 +207,7 @@ void Undistorter::distortPixel(const Eigen::Matrix& K_in, const double& alpha = D[0]; const double d = std::sqrt(x * x + y * y + 1.0); - const double scaling = 1.0/(alpha*d + (1-alpha)); + const double scaling = 1.0 / (alpha * d + (1 - alpha)); xd = x * scaling; yd = y * scaling; } break; @@ -211,8 +216,8 @@ void Undistorter::distortPixel(const Eigen::Matrix& K_in, const double& alpha = D[0]; const double& beta = D[1]; - const double d = std::sqrt(beta*(x * x + y * y) + 1.0); - const double scaling = 1.0/(alpha*d + (1-alpha)); + const double d = std::sqrt(beta * (x * x + y * y) + 1.0); + const double scaling = 1.0 / (alpha * d + (1 - alpha)); xd = x * scaling; yd = y * scaling; } break; @@ -229,4 +234,4 @@ void Undistorter::distortPixel(const Eigen::Matrix& K_in, distorted_pixel_location->y() = distorted_pixel_location_3.y() / distorted_pixel_location_3.z(); } -} \ No newline at end of file +} // namespace image_undistort