Ensenso手眼标定cpp实现

原理Ensenso SDK有介绍，这里是代码实现的简易版本。

需要修改serial number ,我的是194224。

使用方法：

将halcon标定板固定在机械臂上，运行代码，移动机械臂，输入机械臂上标定板当前的姿态（重复次数大于5即可）

成功的话，相机参数的link就已经被修改了，此时的坐标系统一为机械臂基座标系。

目前机械臂的移动是我手动控制的，也可以固定某些点输入，自动运行。

有问题的话，可以加我QQ：1441405602

#include <stdio.h>

#include "nxLib.h"
#include <ros/ros.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> // Needed for conversion from geometry_msgs to tf2::Transform
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <string>
#include <tf2_ros/buffer.h>
#include <tf2/LinearMath/Transform.h>

#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>

#include <iostream>
using namespace ros;
using namespace std;
bool poseIsValid(const tf::Pose& pose)
{
  auto origin = pose.getOrigin();
  if (std::isnan(origin.x()) || std::isnan(origin.y()) || std::isnan(origin.z()))
  {
    return false;
  }

  auto rotation = pose.getRotation();
  if (std::isnan(rotation.getAngle()))
  {
    return false;
  }

  auto rotationAxis = rotation.getAxis();
  if (std::isnan(rotationAxis.x()) || std::isnan(rotationAxis.y()) || std::isnan(rotationAxis.z()))
  {
    return false;
  }

  return true;
}

void writePoseToNxLib(tf::Pose const& pose, NxLibItem const& node)
{
  // Initialize the node to be empty. This is necessary, because there is a bug in some versions of the NxLib that
  // overwrites the whole transformation node with an identity transformation as soon as a new node in /Links gets
  // created.
  node.setNull();

  if (poseIsValid(pose))
  {
    auto origin = pose.getOrigi