Parametrization

include/fast_checkerboard_detector/fast_checkerboard_detector.h

@@ -42,6 +42,8 @@ class FastCheckerboardDetector
   ros::Subscriber camera_info_subscriber_;
   ros::Publisher camera_pose_publisher_;
   tf::TransformBroadcaster broadcast_;
+  bool inverse_transform_;
+  std::string camera_frame_id_;
 
   int grid_size_x, grid_size_y;
   double rect_size_x, rect_size_y;
@@ -66,7 +68,7 @@ class FastCheckerboardDetector
   std::string TrajectoryFile;
   bool show_image_;
 
-public:  
+public:
 
   FastCheckerboardDetector(ros::NodeHandle& nh, ros::NodeHandle& nh_private);
 

launch/fast_checkerboard_detector.launch

@@ -1,6 +1,8 @@
 <launch>
   <arg name="camera_info" default="/usb_cam/camera_info"/>
   <arg name="camera_image" default="/usb_cam/image_raw"/>
+  <arg name="camera_frame_id" default="cam"/>
+  <arg name="inverse_transform" default="true"/>
 
   <node pkg="chessboard_tracker" type="fast_checkerboard_detector" name="checkerboard_detector" ns="checkerboard" output="screen">
     <param name="grid_size_x" value="5" />
@@ -10,6 +12,8 @@
    	<!--param name="trajectory_file" value="$(find fast_checkerboard_detector)/output/Checkerboard_marker_trajectory.txt" /-->
    	<param name="write_trajectory" value="false" />
     <param name="show_image" value="true" />
+    <param name="camera_frame_id" value="$(arg camera_frame_id)"/>
+    <param name="inverse_transform" value="$(arg inverse_transform)"/>
     <!--<remap from="camera_info" to="/usb_cam/camera_info" />
     <remap from="camera_image"  to="/usb_cam/image_raw" /> -->
     <remap from="camera_info" to="$(arg camera_info)" />

src/fast_checkerboard_detector.cpp

@@ -109,6 +109,14 @@ FastCheckerboardDetector::FastCheckerboardDetector(ros::NodeHandle& nh, ros::Nod
     {
       ROS_ERROR("Missing parameter 'show_image!");
     }
+    if(!nh_private.getParam("inverse_transform", inverse_transform_))
+    {
+      ROS_ERROR("Missing parameter 'inverse_transform'!");
+    }
+    if(!nh_private.getParam("camera_frame_id", camera_frame_id_))
+    {
+      ROS_ERROR("Missing parameter 'camera_frame_id'!");
+    }
     corners_.reserve(grid_size_x * grid_size_y);
 
     double x_offset = rect_size_x * (grid_size_x - 1) / 2.0;
@@ -121,7 +129,7 @@ FastCheckerboardDetector::FastCheckerboardDetector(ros::NodeHandle& nh, ros::Nod
         object_points_.push_back(cv::Point3f(x * rect_size_x - x_offset, y * rect_size_y - y_offset, 0));
       }
     }
-    
+
     if(writePoseToFile)
     {
       if(nh_private.getParam("trajectory_file", TrajectoryFile))
@@ -181,7 +189,7 @@ void FastCheckerboardDetector::handleImageMessage(const sensor_msgs::ImageConstP
   cv_bridge::CvImageConstPtr image_color_bridge;
   if (show_image_)
   {
-    image_color_bridge = cv_bridge::toCvShare(message, "bgr8");    
+    image_color_bridge = cv_bridge::toCvShare(message, "bgr8");
   }
 
   const cv::Mat image = image_bridge->image;
@@ -190,7 +198,7 @@ void FastCheckerboardDetector::handleImageMessage(const sensor_msgs::ImageConstP
 
   //ROS_INFO_STREAM("ROI x: " << roi_.x << " y: " << roi_.y << " width: " << roi_.width << " height: " << roi_.height);
   // todo the original code was using the grey image, and not the color one.
-  bool found_chessboard = cv::findChessboardCorners(image_color_bridge->image, cv::Size(grid_size_x, grid_size_y), corners_, 
+  bool found_chessboard = cv::findChessboardCorners(image_color_bridge->image, cv::Size(grid_size_x, grid_size_y), corners_,
                 CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK | CV_CALIB_CB_NORMALIZE_IMAGE);
 
   fixCorners();
@@ -201,9 +209,9 @@ void FastCheckerboardDetector::handleImageMessage(const sensor_msgs::ImageConstP
   {
     // todo: in our code, the last flag is 0.1
     cv::cornerSubPix(image, corners_, cv::Size(4, 4), cv::Size(-1, -1), cv::TermCriteria(CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 30, 0.01));
-    
+
     cv::solvePnP(object_points_, corners_, intrinsic_matrix_, distortion_vector, cv_rotation, cv_translation);
-    
+
     // if (show_image_)
     // {
     //   cv::Mat image_color = image_color_bridge->image;
@@ -251,13 +259,13 @@ void FastCheckerboardDetector::handleImageMessage(const sensor_msgs::ImageConstP
     }
 
     Eigen::Affine3d transform = translation_ * rotation_;
-    
+
     if (show_image_)
     {
       cv::Mat image_color = image_color_bridge->image;
       cv::drawChessboardCorners(image_color, cv::Size(grid_size_x,grid_size_y), cv::Mat(corners_), found_chessboard);
       drawAxis(image_color, intrinsic_matrix_, distortion_vector, cv_rotation, cv_translation, 0.1);
-      
+
       drawAxis(image_color, intrinsic_matrix_, distortion_vector, r2, t2, 0.1);
 
       cv::imshow("Corners", image_color);
@@ -319,14 +327,27 @@ void FastCheckerboardDetector::handleImageMessage(const sensor_msgs::ImageConstP
      }
     ROS_WARN_STREAM("Checkerboard tracking lost!");
   }
-  tf::Transform tf_transform;
-  Eigen::Affine3d inversed_transform = previous_transform_.inverse();
-  tf::transformEigenToTF(inversed_transform, tf_transform);
-
-  broadcast_.sendTransform(tf::StampedTransform(tf_transform,
-                                                ros::Time::now(),
-                                                "chessboard",
-                                                "cam"));
+
+  if( inverse_transform_ )
+  {
+    Eigen::Affine3d inverse_transform = previous_transform_.inverse();
+    tf::Transform tf_transform;
+    tf::transformEigenToTF(inverse_transform, tf_transform);
+    broadcast_.sendTransform(tf::StampedTransform(tf_transform,
+                                                  ros::Time::now(),
+                                                  "chessboard",
+                                                  camera_frame_id_));
+  }
+  else
+  {
+    tf::Transform tf_transform;
+    tf::transformEigenToTF(previous_transform_, tf_transform);
+    broadcast_.sendTransform(tf::StampedTransform(tf_transform,
+                                                  ros::Time::now(),
+                                                  camera_frame_id_,
+                                                  "chessboard"));
+  }
+
 }
 
 /**