Fix/linear interpolation and interprete t cam imu

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

include/image_undistort/camera_parameters.h

@@ -41,6 +41,7 @@ class BaseCameraParameters {
   const cv::Size& resolution() const;  // get image size
 
   const Eigen::Matrix<double, 4, 4>& T() const;  // get transformation matrix
+  const Eigen::Matrix<double, 4, 4>& T_cam_imu() const;
   const Eigen::Ref<const Eigen::Matrix<double, 3, 3>> R()
       const;  // get rotation matrix
   const Eigen::Ref<const Eigen::Matrix<double, 3, 1>> p()
@@ -77,6 +78,7 @@ class BaseCameraParameters {
 
   cv::Size resolution_;
   Eigen::Matrix<double, 4, 4> T_;
+  Eigen::Matrix<double, 4, 4> T_cam_imu_;
   Eigen::Matrix<double, 3, 4> P_;
   Eigen::Matrix<double, 3, 3> K_;
 };

include/image_undistort/image_undistort.h

@@ -10,6 +10,7 @@
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 #include <tf_conversions/tf_eigen.h>
+#include <tf2_ros/static_transform_broadcaster.h>
 
 #include <cv_bridge/cv_bridge.h>
 #include <sensor_msgs/CameraInfo.h>
@@ -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> undistorter_ptr_;
 
   // tf broadcaster
-  tf::TransformBroadcaster br_;
+  tf2_ros::StaticTransformBroadcaster br_;
 
   // camera info
   std::shared_ptr<CameraParametersPair> 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

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<double, 4, 4>& BaseCameraParameters::T() const {
   return T_;
 }
+const Eigen::Matrix<double, 4, 4>& BaseCameraParameters::T_cam_imu() const {
+  return T_cam_imu_;
+}
 const Eigen::Ref<const Eigen::Matrix<double, 3, 3>> 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
+}  // namespace image_undistort

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<double, 4, 4> 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

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<double, 3, 3>& 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,30 +171,34 @@ void Undistorter::distortPixel(const Eigen::Matrix<double, 3, 3>& 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;
       const double r4 = r2 * r2;
       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
       const double& epsilon = D[0];
       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<double, 3, 3>& 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<double, 3, 3>& 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<double, 3, 3>& K_in,
   distorted_pixel_location->y() =
       distorted_pixel_location_3.y() / distorted_pixel_location_3.z();
 }
-}
+}  // namespace image_undistort