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

一、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<double, 3, Eigen::Affine>
resultAffine(result);//其中result的类型是Eigen::matrix.实现的是从Eigen::matrix到Eigen::Affine类型的转换
Eigen::Quaternion<double> Target_Quaternion(Target_Pose.rotation());
std::stringstream ss; ss<<Target_Quaternion.w()<<" "<<Target_Quaternion.x()<<"
"<<Target_Quaternion.y()<< " "<<Target_Quaternion.z()<<std::endl; std::cerr <<
"the target quaternion eigen type w,x,y,z= "<<ss.str()<<std::endl;
或者利用tf::transformEigenToTf转化为TF形式的Transform再输出

 

注意,对于四元数的官方表达,绕x,y,z轴旋转对应的欧拉角为pitch,yaw,roll

但其实在定义过程中也不是一一对应,有时候rpy也对应这x,y,z,所以要具体问题具体分析.

 

二，各种TF关系



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=
"<<Target_Pose.translation().x()<<","<<Target_Pose.translation().y()<<","<<Target_Pose.translation().z()<<std::endl;
得到位姿矩阵的位置信息，返回其x,y,z值用于传入Target_Pose.Position.x,y,z

 

手眼标定ros包的安装

根据github的readme,手眼标定 <https://github.com/jhu-lcsr/handeye_calib_camodocal>
，需要安装的额外扩展包有三个，分别是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
<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_conversions/tf_eigen.h>下,位于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设定的参数路径找到路径
<param name="A_B_transform_pairs_record_filename" type="str" value="$(arg
data_folder)/$(arg filename)" />
   
如果找到了路径,下面的代码输出的位置应该是~/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";
 

• « 上一篇：前端布局是响应式好？还是手机和PC端分开来写好？
• » 下一篇：MySQL面试问题