WIP: added ganty calibration

README.md

@@ -1,7 +1,7 @@
 industrial_calibration
 ======================
 
-:warning: EXPERIMENTAL KINETIC-DEVEL branch :warning:
+:warning: EXPERIMENTAL MELODIC-DEVEL branch :warning:
 ===
 
 Contains libraries/algorithms for calibration industrial systems.
@@ -10,6 +10,7 @@ extrinsic_cal calibration nodes:
 service_node   -- peforms quiet a number of calbirations using very complex yaml files
 range_excal    -- performs extrinsic calibration of a 3D camera by finding the target points in the image and the xyz values from the depth image.
 wrist_cal_srv  -- performs extrinsic calibration when either the target or the camera is mounted on the end of arm tooling
+gantry_cal_srv -- performs extrinsic calibration to refine the transform between the gantry_tool and the robot_base
 stereo_cal_srv -- performs stereo extrinsic cal using a robot (target or pair mounted on end of arm tooling)
 
 extrinsic_cal helper nodes:

calibration_guis/src/calibration_gui.cpp

@@ -30,7 +30,7 @@ namespace calibration_guis
 
     // this client does not change with the type of calibration
     ros::NodeHandle pnh("~");
-    ecp_client_   = pnh.serviceClient<std_srvs::Trigger>("store_mutable_joint_states");
+    ecp_client_   = pnh.serviceClient<std_srvs::Trigger>("/store_mutable_joint_states");
 
     // these are the currently implemented types of service defined calibration routines
     // Note, WristCal, ICal and StereoCal expect a robot moves to change the scene
@@ -40,6 +40,7 @@ namespace calibration_guis
     calibration_selection_->addItem("WristCalSrv"); // Either Camera or Target on EOAT, uses TF to get IC for Camera to Target Transform
     calibration_selection_->addItem("ICalSrv");     // Either Camera or Target on EOAT, uses TF to get IC for Camera to Target Transform
     calibration_selection_->addItem("StereoCalSrv");// Either Cameras or Target on EOAT, uses TF to get IC for Camera to Target Transform
+    calibration_selection_->addItem("GantryCalSrv");     // Calibrate the mount of a manipulator to a gantry's tool plate
     calibration_selection_->addItem("RailICalSrv"); // uses the 8D method where target to rail is changed after each sweep of rail, must happen at least once
     calibration_selection_->setCurrentIndex(0);
     reset_services();		// setup the services to use the default calibration type

industrial_extrinsic_cal/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(industrial_extrinsic_cal)
-add_compile_options(-std=c++11)
+add_compile_options(-std=c++14)
 
 find_package(catkin REQUIRED COMPONENTS
   actionlib
@@ -168,6 +168,7 @@ add_executable(range_excal                      src/nodes/range_camera_excal.cpp
 add_executable(rangeNmono                       src/nodes/rangeNmono_excal.cpp)
 add_executable(target_display                   src/nodes/target_display_node.cpp)
 add_executable(wrist_cal_srv                    src/nodes/wrist_cal_srv.cpp)
+add_executable(gantry_cal_srv                    src/nodes/gantry_cal_srv.cpp)
 add_executable(stereo_cal_srv                   src/nodes/stereo_cal_srv.cpp)
 #add_executable(check_visible                    src/check_if_points_in_pic.cpp)
 
@@ -180,6 +181,7 @@ add_dependencies(range_excal                      ${catkin_EXPORTED_TARGETS} ${i
 add_dependencies(rangeNmono                       ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS})
 add_dependencies(target_display                   ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS})
 add_dependencies(wrist_cal_srv                    ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS})
+add_dependencies(gantry_cal_srv                    ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS})
 add_dependencies(stereo_cal_srv                   ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS})
 #add_dependencies(check_visible                    ${catkin_EXPORTED_TARGETS} ${industrial_extrinsic_cal_EXPORTED_TARGETS})
 
@@ -193,6 +195,7 @@ target_link_libraries(trigger_service ${catkin_LIBRARIES})
 target_link_libraries(range_excal industrial_extrinsic_cal ${CERES_LIBRARIES})
 target_link_libraries(rangeNmono industrial_extrinsic_cal ${CERES_LIBRARIES})
 target_link_libraries(wrist_cal_srv industrial_extrinsic_cal ${CERES_LIBRARIES})
+target_link_libraries(gantry_cal_srv industrial_extrinsic_cal ${CERES_LIBRARIES})
 target_link_libraries(stereo_cal_srv industrial_extrinsic_cal ${CERES_LIBRARIES})
 #target_link_libraries(check_visible industrial_extrinsic_cal ${CERES_LIBRARIES})
 
@@ -209,6 +212,7 @@ install(
     rangeNmono
     target_display
     wrist_cal_srv
+    gantry_cal_srv
     stereo_cal_srv
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

industrial_extrinsic_cal/cfg/circle_grid_finder.cfg

@@ -7,17 +7,17 @@ gen = ParameterGenerator()
 
 
 gen.add("filter_by_area",        bool_t,   0, "filter blobs by their area", True)
-gen.add("filter_by_color",       bool_t,   0, "filter blobs by their color", True)
-gen.add("filter_by_circularity", bool_t,   0, "filter blobs by their circularity", True)
-gen.add("filter_by_convexity",   bool_t,   0, "filter blobs by their convexity", True)
-gen.add("filter_by_inertia",     bool_t,   0, "filter blobs by their aspect ration", True)
+gen.add("filter_by_color",       bool_t,   0, "filter blobs by their color", False)
+gen.add("filter_by_circularity", bool_t,   0, "filter blobs by their circularity", False)
+gen.add("filter_by_convexity",   bool_t,   0, "filter blobs by their convexity", False)
+gen.add("filter_by_inertia",     bool_t,   0, "filter blobs by their aspect ration", False)
 gen.add("min_repeatability",     int_t,    0, "Minimum repeatability", 2, 1, 8)
 gen.add("min_area",              int_t,    0, "Minimum area of a blob", 25, 1, 5000)
-gen.add("max_area",              int_t,    0, "Maximum area of a blob", 40000, 1, 80000)
+gen.add("max_area",              int_t,    0, "Maximum area of a blob", 600, 1, 80000)
 gen.add("min_circularity",       double_t, 0, "The min aspect ratio necessary to be a circlular blob", 0.0, .1, 1.0)
 gen.add("max_circularity",       double_t, 0, "The max aspect ratio necessary to be a circlular blob", 0.5, .75, 1.0)
-gen.add("min_threshold",         int_t,    0, "The contrast minimum threshold to be a blob", 50, 1, 1000)
-gen.add("max_threshold",         int_t,    0, "The contrast maximum threshold to be a blob", 120, 1, 1000)
+gen.add("min_threshold",         int_t,    0, "The contrast minimum threshold to be a blob", 10, 1, 1000)
+gen.add("max_threshold",         int_t,    0, "The contrast maximum threshold to be a blob", 65, 1, 1000)
 gen.add("min_distance",          double_t, 0, "The min distance between blobs", 5.0, 1.0, 500)
 gen.add("min_radius_diff",       double_t, 0, "The min radius difference between blobs", 10.0, 1.0, 500)
 gen.add("min_convexity",         double_t, 0, "The mininum convexity", 0.0, 0.1, 1.0)

industrial_extrinsic_cal/include/industrial_extrinsic_cal/camera_observer.hpp

@@ -136,7 +136,7 @@ class CameraObserver
   bool save_current_image(int scene, std::string& filename)
   {
     std::string full_file_path_name;
-    char scene_chars[8];
+    char scene_chars[30];
     sprintf(scene_chars,"_%03d.jpg",scene);
     if(filename == ""){ // build file name from image_directory_, 
       full_file_path_name  = image_directory_ + std::string("/") +  camera_name_ + std::string(scene_chars);
@@ -163,7 +163,7 @@ class CameraObserver
   bool load_current_image(int scene, std::string& filename)
   {
     std::string full_image_file_path_name;
-    char scene_chars[8];
+    char scene_chars[30];
     sprintf(scene_chars,"_%03d.jpg",scene);
     if(filename == ""){ // build file name from image_directory_,
       full_image_file_path_name  = image_directory_ + std::string("/") +  camera_name_ + std::string(scene_chars);

industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.h

@@ -35,6 +35,7 @@ namespace industrial_extrinsic_cal
       LinkTargetCameraReprjError,
       LinkTargetCameraReprjErrorPK,
       wristCal,
+      GantryCal,
       PosedTargetCameraReprjErrorPK,
       LinkCameraTargetReprjError,
       LinkCameraTargetReprjErrorPK,

industrial_extrinsic_cal/include/industrial_extrinsic_cal/ceres_costs_utils.hpp

@@ -1323,7 +1323,6 @@ class LinkTargetCameraReprjErrorPK
 };
 
 // used whenever the target or camera is on the wrist of a robot while the camera or target is in the workcell
-// The 
 class WristCal
 {
 public:
@@ -1367,8 +1366,7 @@ class WristCal
                                      const double cx, const double cy, Pose6d pose, Point3d point)
 
   {
-    return (new ceres::AutoDiffCostFunction<LinkTargetCameraReprjErrorPK, 2, 6, 6>(
-        new LinkTargetCameraReprjErrorPK(o_x, o_y, fx, fy, cx, cy, pose, point)));
+    return (new ceres::AutoDiffCostFunction<WristCal, 2, 6, 6>(new WristCal(o_x, o_y, fx, fy, cx, cy, pose, point)));
   }
   double ox_;                 /** observed x location of object in image */
   double oy_;                 /** observed y location of object in image */
@@ -1385,6 +1383,73 @@ class WristCal
 			      */
 };
 
+// used to calibrate a robot slung from a gantry, must first have performed wrist calibration (eye-hand calibration)
+class GantryCal
+{
+public:
+ GantryCal(double ob_x, double ob_y, double fx, double fy, double cx, double cy,
+	   Pose6d target_to_robotm,
+	   Pose6d robot_base_to_optical,
+	   Point3d point)
+    : ox_(ob_x), oy_(ob_y), fx_(fx), fy_(fy), cx_(cx), cy_(cy),
+      target_to_robotm_(target_to_robotm),
+      robot_base_to_optical_(robot_base_to_optical),
+      point_(point)
+  {
+  }
+
+  template <typename T>
+  bool operator()(const T* const p1, /** mount transform parameters */
+                  T* residual) const
+  {
+    const T* mount_T(&p1[0]);
+    T robot_mount_point[3];   /** point in robot mount frame */
+    T robot_base_point[3];  /** point in robot base frame */
+    T camera_point[3]; /** point in camera frame */
+    T point[3];
+    point[0] = T(point_.x);
+    point[1] = T(point_.y);
+    point[2] = T(point_.z);
+
+    /** transform point into camera coordinates */
+    poseTransformPoint(target_to_robotm_, point, robot_mount_point);
+    eTransformPoint(mount_T, robot_mount_point, robot_base_point);
+    poseTransformPoint(robot_base_to_optical_, robot_base_point, camera_point);
+
+    /** compute project point into image plane and compute residual */
+    T fx = T(fx_);
+    T fy = T(fy_);
+    T cx = T(cx_);
+    T cy = T(cy_);
+    T ox = T(ox_);
+    T oy = T(oy_);
+    cameraPntResidual(camera_point, fx, fy, cx, cy, ox, oy, residual);
+
+    return true;
+  } /** end of operator() */
+
+  /** Factory to hide the construction of the CostFunction object from */
+  /** the client code. */
+  static ceres::CostFunction* Create(const double o_x, const double o_y,
+				     const double fx, const double fy,
+                                     const double cx, const double cy,
+				     const Pose6d pose1, const Pose6d pose2,
+				     const Point3d point)
+
+  {
+    return (new ceres::AutoDiffCostFunction<GantryCal, 2, 6>(new GantryCal(o_x, o_y, fx, fy, cx, cy, pose1, pose2, point)));
+  }
+  double ox_;                    /** observed x location of object in image */
+  double oy_;                    /** observed y location of object in image */
+  double fx_;                    /*!< known focal length of camera in x */
+  double fy_;                    /*!< known focal length of camera in y */
+  double cx_;                    /*!< known optical center of camera in x */
+  double cy_;                    /*!< known optical center of camera in y */
+  Point3d point_;                /*! location of point in target coordinates */
+  Pose6d target_to_robotm_;      /*!< transform from target to robot's mounting frame */
+  Pose6d robot_base_to_optical_; /*!< transform from robot_base to optical frame */
+};
+
 // reprojection error of a single point attatched to a target observed by a camera with NO lens distortion
 // should subscribe to a rectified image when using the error function
 //

industrial_extrinsic_cal/src/basic_types.cpp

@@ -217,7 +217,7 @@ void Pose6d::show(std::string message)
   this->getEulerZYX(ez_yaw, ey_pitch, ex_roll);
   this->getQuaternion(qx, qy, qz, qw);
   printf("%s =[\n %6.4lf  %6.4lf  %6.4lf  %6.4lf\n  %6.4lf  %6.4lf  %6.4lf  %6.4lf\n  %6.4lf  %6.4lf %6.4lf  %6.4lf\n  "
-         "%6.4lf  %6.4lf %6.4lf  %6.4lf];\n rpy= %6.4lf %6.4lf %6.4lf\n quat= %6.4lf  %6.4lf  %6.4lf %6.4lf\n ",
+         "%6.4lf  %6.4lf %6.4lf  %6.4lf];\n rpy= %6.4lf %6.4lf %6.4lf\n xyzw= %6.4lf  %6.4lf  %6.4lf %6.4lf\n ",
          message.c_str(), basis[0][0], basis[0][1], basis[0][2], this->x, basis[1][0], basis[1][1], basis[1][2],
          this->y, basis[2][0], basis[2][1], basis[2][2], this->z, 0.0, 0.0, 0.0, 1.0, ez_yaw, ey_pitch, ex_roll, qx, qy,
          qz, qw);