一、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再輸出
二,各種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
在傳入位姿信息時,需要傳入的是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