手眼标定学习记录(更新)

一、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<手眼标定学习记录(更新)_第1张图片

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

手眼标定学习记录(更新)_第2张图片

 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


手眼标定学习记录(更新)_第3张图片

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";

 

你可能感兴趣的:(ros程序记录)