手眼标定学习记录（更新）

一、Eigen中quaternion的构造函数

Quaternion (const Scalar &w, const Scalar &x, const Scalar &y, const Scalar &z)，注意w在前。然而在内部存储时eigen将四元数的w放在最后
例如通过Eigen::Vector4d q = q_AB.coeffs();访问时，q中的最后一个元素才是w。

Eigen中用四元数表示向量的旋转
Quaternion：

Eigen::Quaterniond q(2, 0, 1, -3); std::cout << “This quaternion consists of a scalar ” << q.w() << ” and a vector ” << std::endl << q.vec() << std::endl; q.normalize(); std::cout << “To represent rotation, we need to normalize it such that its length is ” << q.norm() << std::endl; Eigen::Vector3d v(1, 2, -1); Eigen::Quaterniond p; p.w() = 0; p.vec() = v; Eigen::Quaterniond rotatedP = q * p * q.inverse(); Eigen::Vector3d rotatedV = rotatedP.vec(); std::cout << “We can now use it to rotate a vector ” << std::endl << v << ” to ” << std::endl << rotatedV << std::endl;

 

 

Eigen::Matrix3d R = q.toRotationMatrix(); // convert a quaternion to a 3x3 rotation matrix std::cout << “Compare with the result using an rotation matrix ” << std::endl << R * v << std::endl;

 

 

Eigen::Quaterniond a = Eigen::Quterniond::Identity(); Eigen::Quaterniond b = Eigen::Quterniond::Identity(); Eigen::Quaterniond c; // Adding two quaternion as two 4x1 vectors is not supported by the EIgen API. That is, c = a + b is not allowed. We have to do this in a hard way c.w() = a.w() + b.w(); c.x() = a.x() + b.x(); c.y() = a.y() + b.y(); c.z() = a.z() + b.z();

 

要输出Eigen形式的四元数，并且由Eigen::Affine转化，或者是由Eigen::matrix转化方法
 

Eigen::Transform resultAffine(result);//其中result的类型是Eigen::matrix.实现的是从Eigen::matrix到Eigen::Affine类型的转换

Eigen::Quaternion Target_Quaternion(Target_Pose.rotation());
    std::stringstream ss;
    ss<
 
  
tf::StamptedTransform(),对于参数，相当于
 
  
frame_id=father
 
  
child_frame_id=child
 
  
frame_id->child_frame_id
 
  
date form child_frame to  frame_id
 
  
the origin coordination of child_frame_id in frame_id
 
  
 
  
 tf::lookupTransform(),相当于
 
  
target_frame=father
 
  
source_frame=child
 
  
target_frame->source_frame
 
  
date from source_frame to target_frame
 
  
the origin coordination of source_frame in target_frame
 
  
 
  
 
  
solvepnp相当于
 
  
camera frame=father
 
  
world frame=child
 
  
camera frame->world frame
 
  
data from world frame to camera frame
 
  
the origin coorfination of world frame in camera frame
 
  
与相机的外参正好相反，外参需要的是将相机坐标系下的点转到世界坐标系下，即data from camera frame to world frame
 
  
 
 
  
注意!!
 
  
python接口中
 
  
br.sendTransform(child_frame, father_frame)
 
  
与c++的接口或者说与tf::StampedTransform()形参的位置有所不同,后者的参数是(father_frame,child_frame)
 
  
因此,对于python接口来说,假设
 
  
        br.sendTransform((0.0, 2.0, 0.0),
                         (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(),
                         "carrot1",
                         "turtle1")
 
  
相当于:
 
  
turtle1=father
 
  
carrot1=child
 
  
turtle1->carrot1
 
  
data from carrot1 frame to turtle1
 
  
the origin coorfination of carrot1 in turtle1
 
  
 
 
  
在传入位姿信息时，需要传入的是Eigen类型的Quaternion和position作为姿态和位置信息，最好不要转换为tf类型的数据，会出现莫名的错误。
 
  
对于Eigen::Affine3d来说，除了上述直接得到Eigen::Quaternion之外，也可以通过
 
  
std::cerr << "the target translation eigen type x,y,z= "<
 
  
得到位姿矩阵的位置信息，返回其x,y,z值用于传入Target_Pose.Position.x,y,z
 
  
 
 
  
手眼标定ros包的安装
 
  
根据github的readme,手眼标定，需要安装的额外扩展包有三个，分别是gflags,glog,ceres-solver,在github上有对应的源码可下载
 
  
在编译过程中，gflags，glog的安装方法按照源码中的方法安装，在安装过程中，可能会遇到错误
 
  
/usr/local/lib/libgflags.a: error adding symbols: Bad value collect2: error: ld returned 1 exit status
 
  
类似的问题很多，大多是在移植到64位系统时，某个库文件没有编译成为动态库，解决方案是：
 
  
如果错误的库是通过源码安装，那么进入源码后，在 cmake ..之前加上
 
  
export CXXFLAGS="-fPIC" && cmake .. && make VERBOSE=1
 
  
如果是通过./configure文件进行配置，则需要输入
 
  
 ./configure --disable-shared -with-pic
 
  
如果可以在makefile文件中，或者是CMakeList.txt文件中找到g++或者gcc的编译flags选项加上-fPIC,参考见https://www.cnblogs.com/laodageblog/p/5993742.html
 
  
在glog的编译过程中如果出现上述问题无法解决，可以不按照官方方法，而是按照标准操作，即
 
  
mkdir build && cd build
 
  
cmake ..
 
  
make
 
  
sudo make install
 
  
对于ceres-solver来说，方法同上。
 
  
 
 
  
三.Eigen与TF的关系
 
  
  3.1 Eigen各种旋转的初始化与定义
 
  
    //角向量表示旋转
    Eigen::AngleAxisd rotation_vector(1.57, Eigen::Vector3d(0.5, 0.25, 0.13));
    //通过角向量转化为四元数
    Eigen::Quaterniond q = Eigen::Quaterniond(rotation_vector);
    //输出四元数
    std::cout<< q.coeffs() << std::endl << std::endl;
    //通过角向量转化为旋转矩阵
    Eigen::Matrix3d rotation_matrix = rotation_vector.toRotationMatrix();
    std::cout << rotation_vector.matrix() << std::endl << std::endl;
    //建立3*1矩阵
    Eigen::Vector3d translation_vector (1.5, 2.5, 1.8);
    //使用vector矩阵来初始化Eigen::Translation3d对象,后者用于Eigen::Affine3d的初始化
    Eigen::Translation3d tl(translation_vector);
    //Eigen::Affine3d初始化,方法为Eigen::Translation3d对象(对应的Position)乘上旋转矩阵(对应的rotation)构成4*4的齐次矩阵
    Eigen::Affine3d transform;
    transform = tl * rotation_matrix;
    std::cout << transform.matrix() << std::endl << std::endl;
 
  
   3.2 TF各种变量的初始化
 
  
  除去包含与头文件#include 下,位于tf命名空间下的eigen,tf,msg之间的转换函数,对于tf各种变量的初始化如下:
 
  
   
 
  
//tf中的vector3的初始化,其中translation_vector是上述Eigen::trnaslation3d变量
tf::Vector3 tfv(translation_vector(0), translation_vector(1), translation_vector(2));
//对之前Eigen::Quaternion生成的四元数进行归一化处理
	double norm = sqrt(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w());
	q.x() = q.x() / norm;
	q.y() = q.y() / norm;
	q.z() = q.z() / norm;
	q.w() = q.w() / norm;
//归一化后通过()直接赋值,注意x,y,z,w的位置
	tf::Quaternion tfq(q.x(), q.y(), q.z(), q.w());
//下面一行是指之前提到的在tf下的转换函数
	tf::transformEigenToTF(transform, tf);

//通过之前的tf::vector3与tf::Quaternion对象来初始化tf::Transform对象,注意前后顺序
	tf::Transform tf(tfq, tfv);
//输出各种信息
	std::cout << tfq.getX() << " " << tfq.getY() << " " << tfq.getZ() << " " << tfq.getW() << std::endl << std::endl;
	std::cout << "tf type position: " << tf.getOrigin().getX() << " " << tf.getOrigin().getY() << " " << tf.getOrigin().getZ() << " " << std::endl << std::endl;
	std::cout << "tf type rotation: " << tf.getRotation().getX() << " " << tf.getRotation().getY() << " " << tf.getRotation().getZ() << " " << tf.getRotation().getW() << std::endl << std::endl;
 
  
注意:上述变量的转换有几个注意事项:
 
  
1.对于Eigen::Quaternion初始化,例如之前通过角向量来生成的四元数,并没有归一化
 
  
  但是传入tf向量的四元数,必须进行归一化,同样的道理,对于Moveit的group.setposetarget()的Pose中的orientation,必须传入归一化后的四元数,不然会错误运动!!!!
 
  
 
 
  
2.在输出tf的rotation值时,orientation的w值通过tf.getRotation().getW()即可,而对于x,y,z来说,tf.getRotation.getAxis.getX()的值有一定的错误,对于y,z同理,要想输出一样的结果需要的是tf.getRotation.getX()即可.
 
  
 
 
  
四.错误记录
 
  
    在将从摄像头到底座的坐标变换发送到tf树的过程中,因为要从指定路径中去读取之前手眼标定得到的结果x,所以需要在launch文件中声明读取的文件路径
 
  
    在程序中,参数服务器里面的参数只是记录了需要读取的文件的名字,因此在声明ros::NodeHandle时,必须给他赋初值,即
 
  
ros::NodeHandle nh("~");
 
  
    才能保证launch文件中,能够根据value设定的参数路径找到路径
 
  
 
  
    如果找到了路径,下面的代码输出的位置应该是~/kinova_ws/....TransformPairsInput.yml,前者是在上述launch文件中声明的value值,如果最初ros::NodeHandle没有初始化,则最终输出的结果只有TransformPairsInput.yml,没有到对应的路径去找这个文件,相当于launch文件没有发生作用!
 
  
    nh.param("A_B_transform_pairs_record_filename", transformPairsRecordFile,
             std::string("TransformPairsInput.yml"));
    std::cerr << "loading BasetoTip and CameraToAr frame: " << transformPairsRecordFile
                      << "\n";