Hand2Eye Calibrator return first matrix that was sent to it

When i send data and ask Hand2Eye Calibrator to compute it return just the first matrices Also i saw that the same happen when you use Client example

is calibrator is just an example? can i use as is for computing my eye2hand transfrom?

im conducting an expriment when i only move in Z direction and i p

I place an object on the end of the arm and i recodring with camera from the base

my data as wrriten in record file (Arm data acuired from encoders, Obj Data Aqcuired from Camera):

XArm YArm ZArm RXArm RYArm RZArm XObj YObj Zobj RXObj RYObj RObj 0 1 0.050 0 0 0 0.0219116 -0.00346060 0.183634 0.02077090 3.09210 -0.00843524 0 1 0.100 0 0 0 0.0223035 -0.00149662 0.233661 0.02169330 3.09055 -0.01862820 0 1 0.150 0 0 0 0.0228440 4.04948E-05 0.283102 0.02165850 3.06555 -0.02642330 0 1 0.200 0 0 0 0.0233103 0.001705890 0.333236 0.02033780 3.07090 -0.03269140 0 1 0.250 0 0 0 0.0239772 0.003746710 0.383682 0.02088440 3.07675 -0.03525030 0 1 0.300 0 0 0 0.0245928 0.005414250 0.433303 0.01893250 3.06094 -0.04659980 0 1 0.350 0 0 0 0.0254822 0.007065170 0.483543 0.01902080 3.06146 -0.02645310 0 1 0.400 0 0 0 0.0262967 0.008849140 0.534785 0.01721250 3.07937 -0.05460800 0 1 0.450 0 0 0 0.0267376 0.010349200 0.583546 0.01583760 3.04599 -0.05025220 0 1 0.500 0 0 0 0.0277891 0.012845000 0.633612 0.01212500 3.02253 -0.04751030 0 1 0.550 0 0 0 0.0283095 0.014789700 0.685370 0.00780469 3.06154 -0.02021860

I use ArUco to detect postion of the using ArUco_Test exectuable

In order to send data i modified client example to load data from file as following:


#include "client.h"
#include <geometry_msgs/Transform.h>
#include "visp_hand2eye_calibration/TransformArray.h"
#include <visp_bridge/3dpose.h>
#include "names.h"

#include <visp/vpCalibration.h>
#include <visp/vpExponentialMap.h>

//read csv
#include <iostream>
#include <fstream>
#include <string>
using namespace std;

namespace visp_hand2eye_calibration
{
Client::Client()
{
  camera_object_publisher_
      = n_.advertise<geometry_msgs::Transform> (visp_hand2eye_calibration::camera_object_topic, 1000);
  world_effector_publisher_
      = n_.advertise<geometry_msgs::Transform> (visp_hand2eye_calibration::world_effector_topic, 1000);

  reset_service_
      = n_.serviceClient<visp_hand2eye_calibration::reset> (visp_hand2eye_calibration::reset_service);
  compute_effector_camera_service_
      = n_.serviceClient<visp_hand2eye_calibration::compute_effector_camera> (
                                                                                      visp_hand2eye_calibration::compute_effector_camera_service);
  compute_effector_camera_quick_service_
      = n_.serviceClient<visp_hand2eye_calibration::compute_effector_camera_quick> (
                                                                                            visp_hand2eye_calibration::compute_effector_camera_quick_service);
}

void Client::initAndSimulate()
{
  ROS_INFO("Waiting for topics...");
  ros::Duration(1.).sleep();
  while(!reset_service_.call(reset_comm)){
    if(!ros::ok()) return;
    ros::Duration(1).sleep();
  }

  // We want to calibrate the hand to eye extrinsic camera parameters from 6 couple of poses: cMo and wMe

  // Input: six couple of poses used as input in the calibration proces
  vpHomogeneousMatrix cMo; // eye (camera) to object transformation. The object frame is attached to the calibrartion grid
  vpHomogeneousMatrix wMe; // world to hand (end-effector) transformation
//  vpHomogeneousMatrix eMc; // hand (end-effector) to eye (camera) transformation

  // Initialize an eMc transformation used to produce the simulated input transformations cMo and wMe
//  vpTranslationVector etc(0.3, 0.06, 0.01);
//  vpThetaUVector erc;
//  erc[0] = vpMath::rad(30);
//  erc[1] = vpMath::rad(60);
//  erc[2] = vpMath::rad(90);

//  eMc.buildFrom(etc, erc);
//  ROS_INFO("1) GROUND TRUTH:");

//  ROS_INFO_STREAM("hand to eye transformation: " <<std::endl<<visp_bridge::toGeometryMsgsTransform(eMc)<<std::endl);

  ifstream CalibFile("/home/calib_info.csv");
  const int NumOfRow = 11;
  const int NumOfCol = 12;
  double CalibTextRow[NumOfCol];
  string getcell;
  string::size_type sz; // needed for String to double conversion
  getline(CalibFile,getcell); // get rid of titles line
  //each line sent to processing
  for (int RowNum = 0; RowNum < NumOfRow; RowNum++)
  {
    for (int ColNum=0; ColNum < NumOfCol; ColNum++) {
      if (ColNum<NumOfCol-1){
        getline(CalibFile, getcell, ' ');
      }
      else
      {
        getline(CalibFile, getcell, '\n'); //end of line
      }
      CalibTextRow[ColNum]= stod (getcell,&sz); // convert to double
    }

    //build poses matrix
    wMe.buildFrom(CalibTextRow[0], CalibTextRow[1], CalibTextRow[2], CalibTextRow[3], CalibTextRow[4], CalibTextRow[5]);
    cMo.buildFrom(CalibTextRow[6], CalibTextRow[7], CalibTextRow[8], CalibTextRow[9], CalibTextRow[10], CalibTextRow[11]);

    geometry_msgs::Transform pose_w_e;
    pose_w_e = visp_bridge::toGeometryMsgsTransform(wMe);
    ROS_INFO_STREAM(" world to hand transformation: " <<std::endl<<pose_w_e<<std::endl);

    geometry_msgs::Transform pose_c_o;
    pose_c_o = visp_bridge::toGeometryMsgsTransform(cMo);
    ROS_INFO_STREAM(" eye to object transformation: " <<std::endl<<pose_c_o<<std::endl);

    camera_object_publisher_.publish(pose_c_o);
    world_effector_publisher_.publish(pose_w_e);
    emc_quick_comm.request.camera_object.transforms.push_back(pose_c_o);
    emc_quick_comm.request.world_effector.transforms.push_back(pose_w_e);

  }
  ros::Duration(1.).sleep();

}

void Client::computeUsingQuickService()
{
  vpHomogeneousMatrix eMc;
  vpThetaUVector erc;
  ROS_INFO("2) QUICK SERVICE:");
  if (compute_effector_camera_quick_service_.call(emc_quick_comm))
  {
    ROS_INFO_STREAM("hand_camera: "<< std::endl << emc_quick_comm.response.effector_camera);
  }
  else
  {
    ROS_ERROR("Failed to call service");
  }
}

void Client::computeFromTopicStream()
{
  vpHomogeneousMatrix eMc;
  vpThetaUVector erc;
  ROS_INFO("3) TOPIC STREAM:");
  if (compute_effector_camera_service_.call(emc_comm))
  {
    ROS_INFO_STREAM("hand_camera: " << std::endl << emc_comm.response.effector_camera);
  }
  else
  {
    ROS_ERROR("Failed to call service");
  }

}
}

/*
 * Local variables:
 * c-basic-offset: 2
 * End:
 */
`

Or Hirshfeld
Control SW Engineer

lagadic / vision_visp

Hand2Eye Calibrator return first matrix that was sent to it #82