added missing visualization

ndt_calibration/include/ndt_calibration/ndt_calib_rviz.h

@@ -106,13 +106,53 @@ visualization_msgs::MarkerArray getMarkerArrayFromNDTCalibScanPairs(const std::v
   }
   return m;
 }
+visualization_msgs::Marker getMarkerCylinder(const Eigen::Affine3d &T,
+                                             int id, int color,
+                                             double length, double radius,
+                                             const std::string &ns) {
+  visualization_msgs::Marker m;
+  assignDefault(m);
+  assignColor(m, color);
+  m.ns = ns;
+  m.type = visualization_msgs::Marker::CYLINDER;
+  m.action = visualization_msgs::Marker::ADD;
+  m.id = id;
 
+  m.scale.x = radius; m.scale.y = radius; m.scale.z = length;
+  tf::poseEigenToMsg(T, m.pose);
+  return m;
+}
 void appendMarkerArray(visualization_msgs::MarkerArray &array, const visualization_msgs::MarkerArray &add) {
   for (size_t i = 0; i < add.markers.size(); i++) {
     array.markers.push_back(add.markers[i]);
   }
 }
+/// Draw an x,y,z coordsystem given an affine3d.
+visualization_msgs::MarkerArray getMarkerFrameAffine3d(const Eigen::Affine3d &T, const std::string &ns, double length, double radius) {
 
+  visualization_msgs::MarkerArray m;
+  // X
+  {
+    Eigen::Affine3d T_x =
+        Eigen::Translation3d(length / 2.0, 0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitY());
+    T_x = T * T_x;
+    m.markers.push_back(getMarkerCylinder(T_x, 0, 0, length, radius, ns));
+  }
+  // Y
+  {
+    Eigen::Affine3d T_y =
+        Eigen::Translation3d(0, length / 2.0, 0) * Eigen::AngleAxisd(M_PI / 2.0, Eigen::Vector3d::UnitX());
+    T_y = T * T_y;
+    m.markers.push_back(getMarkerCylinder(T_y, 1, 1, length, radius, ns));
+  }
+  // Z
+  {
+    Eigen::Affine3d T_z = Eigen::Translation3d(0, 0, length / 2.0) * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
+    T_z = T * T_z;
+    m.markers.push_back(getMarkerCylinder(T_z, 2, 2, length, radius, ns));
+  }
+  return m;
+}
 visualization_msgs::MarkerArray getMarkerArrayRelFromNDTCalibScanPair(const NDTCalibScanPair &pair, const Eigen::Affine3d &Ts) {
   visualization_msgs::MarkerArray m;
 

ndt_calibration/src/ndt_calib_online_node.cpp

@@ -42,8 +42,9 @@ public:
 
   }
 
-  bool getTransformationForTime(ros::Time t0, const std::string &frame_id,Eigen::Affine3d &T){
+  bool getTransformationForTime(ros::Time t0, const std::string &frame_id, Eigen::Affine3d &T){
     std::string str;
+    std::cout<<"Get transformation from"<<fixedframe_<<" to "<<frame_id<<std::endl;
     if (!transformer_.canTransform(fixedframe_, frame_id, t0, &str)) {
       return false;
     }