void BaseReferenceFrame::deserialize(ObjectData& data, IdContext& context){ Identifiable::deserialize(data,context); Eigen::Quaterniond q; q.coeffs().fromBOSS(data,"rotation"); _transform.translation().fromBOSS(data,"translation"); _transform.linear()=q.toRotationMatrix(); }
TEST(TFEigenConversions, tf_eigen_quaternion) { tf::Quaternion t; t[0] = gen_rand(-1.0,1.0); t[1] = gen_rand(-1.0,1.0); t[2] = gen_rand(-1.0,1.0); t[3] = gen_rand(-1.0,1.0); t.normalize(); Eigen::Quaterniond k; quaternionTFToEigen(t,k); ASSERT_NEAR(t[0],k.coeffs()(0),1e-6); ASSERT_NEAR(t[1],k.coeffs()(1),1e-6); ASSERT_NEAR(t[2],k.coeffs()(2),1e-6); ASSERT_NEAR(t[3],k.coeffs()(3),1e-6); ASSERT_NEAR(k.norm(),1.0,1e-10); }
/// \brief Plus matrix of a quaternion, i.e. q_AB*q_BC = plus(q_AB)*q_BC.coeffs(). /// @param[in] q_AB A Quaternion. inline Eigen::Matrix4d plus(const Eigen::Quaterniond & q_AB) { Eigen::Vector4d q = q_AB.coeffs(); Eigen::Matrix4d Q; Q(0,0) = q[3]; Q(0,1) = -q[2]; Q(0,2) = q[1]; Q(0,3) = q[0]; Q(1,0) = q[2]; Q(1,1) = q[3]; Q(1,2) = -q[0]; Q(1,3) = q[1]; Q(2,0) = -q[1]; Q(2,1) = q[0]; Q(2,2) = q[3]; Q(2,3) = q[2]; Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) = q[3]; return Q; }
TEST(TFEigenConversions, eigen_tf_transform) { tf::Transform t; Eigen::Affine3d k; Eigen::Quaterniond kq; kq.coeffs()(0) = gen_rand(-1.0,1.0); kq.coeffs()(1) = gen_rand(-1.0,1.0); kq.coeffs()(2) = gen_rand(-1.0,1.0); kq.coeffs()(3) = gen_rand(-1.0,1.0); kq.normalize(); k.translate(Eigen::Vector3d(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10))); k.rotate(kq); transformEigenToTF(k,t); for(int i=0; i < 3; i++) { ASSERT_NEAR(t.getOrigin()[i],k.matrix()(i,3),1e-6); for(int j=0; j < 3; j++) { ASSERT_NEAR(t.getBasis()[i][j],k.matrix()(i,j),1e-6); } } }
IGL_INLINE void igl::trackball( const double w, const double h, const double speed_factor, const Eigen::Quaterniond & down_quat, const double down_mouse_x, const double down_mouse_y, const double mouse_x, const double mouse_y, Eigen::Quaterniond & quat) { using namespace std; return trackball( w, h, speed_factor, down_quat.coeffs().data(), down_mouse_x, down_mouse_y, mouse_x, mouse_y, quat.coeffs().data()); }
void mouse_drag(int mouse_x, int mouse_y) { using namespace igl; if(trackball_on) { glutSetCursor(GLUT_CURSOR_CYCLE); // Rotate according to trackball trackball<double>( width, height, 2.0, down_rot.coeffs().data(), down_x, down_y, mouse_x, mouse_y, s.rot.coeffs().data()); } glutPostRedisplay(); }
int main() { //PelvRobotState rs; //PelvIkCon ik; HatRobotState rs; HatIkCon ik; for (int i = 0; i < 3; i++) ik.ik_com_p[i] = 0; ik.ik_joint_p = 50; ik.printParams(); double joints[N_JOINTS] = {0}; double jointsd[N_JOINTS] = {0}; double pos[3] = {0}; double rootd[3] = {0}; double root_b_w[3] = {0}; Eigen::Quaterniond q = Eigen::Quaterniond::Identity(); ((RobotState &)rs).computeSDFvars(pos, q.coeffs().data(), rootd, root_b_w, joints, jointsd); Command cmd; for (int i = 0; i < 5000; i++) { fill_cmd(rs, cmd); assert(ik.IK(rs, cmd)); } for (int i = 0; i < 35; i++) { if (i < 6) printf("%10s %5g\n", "q", cmd.ik_q[i]); else printf("%10s %5g\n", RobotState::joint_names[i-6].c_str(), cmd.ik_q[i]); } return 0; }
int main ( int argc, char** argv ) { //注意一下类型名的最后一个字符为d表示双精度类型,换成f表示单精度类型,两种类型不能混用,必须显示转换 // Eigen/Geometry 模块提供了各种旋转和平移的表示 // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f /****旋转向量****/ // 旋转向量使用 AngleAxis, 它底层不直接是Matrix,但运算可以当作矩阵(因为重载了运算符) // 乘以该向量,表示进行一个坐标变换 //任意旋转可用一个旋转轴和一个旋转角度来表示。 //旋转向量,旋转向量的方向与旋转轴一致,长度为旋转角度。 /*********************************/ /*旋转向量 沿 Z 轴旋转 45 度 角度 轴 */ Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度 cout .precision(3);//输出精度 cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //用matrix()转换成矩阵 // 也可以直接赋值 /*********************************/ /*旋转矩阵*/ Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();//单位阵 3x3 rotation_matrix = rotation_vector.toRotationMatrix(); // 旋转向量转成旋转矩阵 由罗德里格公式进行转换 // 用 AngleAxis 可以进行坐标变换 Eigen::Vector3d v ( 1,0,0 ); /*************旋转向量进行坐标变换********************/ Eigen::Vector3d v_rotated = rotation_vector * v; cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;// transpose纯粹是为了输出美观一些 // 或者用旋转矩阵 /*****************旋转矩阵进行坐标变换****************/ v_rotated = rotation_matrix * v; cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl; /**欧拉角表示的旋转**/ // 欧拉角: 可以将 旋转矩阵直接转换成欧拉角 Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // (2,1,0)表示ZYX顺序,即roll pitch yaw顺序 cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl; /***欧式变换矩阵表示旋转+平移**/ // R t;0 0 0 1 // 欧氏变换矩阵使用 Eigen::Isometry Eigen::Isometry3d T=Eigen::Isometry3d::Identity();// 虽然称为3d,实质上是4*4的矩阵 T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转 T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4) cout << "转换矩阵 Transform matrix = \n" << T.matrix() <<endl; // 用变换矩阵进行坐标变换 Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t cout<<"(1,0,0) after Isometry3d tranformed = "<<v_transformed.transpose()<<endl; // 对于仿射和射影变换,使用 Eigen::Affine3d 和 Eigen::Projective3d 即可,略 /*******四元数表示的旋转***********/ // 可以直接把AngleAxis赋值给四元数,反之亦然 Quaterniond 表示双精度 四元素 Quaternionf 表示单精度四元素 Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );// 表示沿Z 轴旋转 45 度 的四元素变换 cout<<"quaternion from AngleAxis rotation_vector = \n"<<q.coeffs() <<endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部,前三者为虚部 // 也可以把 旋转矩阵 赋给它 q = Eigen::Quaterniond ( rotation_matrix ); cout<<"quaternion from rotation_matrix = \n"<<q.coeffs() <<endl; // 使用四元数旋转一个向量,使用重载的乘法即可 /*注意程序表达形式和实际运算的不一样*/ v_rotated = q*v; // 注意数学上是q*v*q^{-1} 而程序为了简化表示 直接使用 q*v代替 cout<<"(1,0,0) after Quaterniond rotation = "<<v_rotated.transpose()<<endl; /*编程题目 小萝卜1号位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]' 世界坐标系到相机变换 小萝卜2号位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]' 小萝卜1号看到位于自身坐标系下p=[0.5,0,0.2]' 求该向量在小萝卜2号下的坐标 */ Eigen::Quaterniond q1(0.35,0.2,0.3,0.1);//wxyz q1.coeffs() xyzw q1.vec() xyz //q1 << 0.35,0.2,0.3,0.1; Eigen::Matrix<double, 3, 1> t1;//double类型 t1 << 0.3,0.1,0.1; Eigen::Quaterniond q2(-0.5,0.4,-0.1,0.2); //q2 << -0.5,0.4,-0.1,0.2; Eigen::Matrix<double, 3, 1> t2;//double类型 t2 << -0.1,0.5,0.3; Eigen::Matrix<double, 3, 1> p1;//double类型 坐标 p1 << 0.5,0,0.2; cout<<"q1= \n"<< q1.coeffs() <<endl; cout<<"t1= \n"<< t1 <<endl; cout<<"q2= \n"<< q2.coeffs() <<endl; cout<<"t2= \n"<< t2 <<endl; /* q1.setIdentity(); //单位四元素 cout<<"q1 after setIdentity \n"<<q1.coeffs() <<endl; q2.setIdentity(); cout<<"q2 after setIdentity \n"<<q2.coeffs() <<endl; */ //规范化 归一化 除以模长 q1=q1.normalized(); cout<<"q1 after normalized\n"<<q1.coeffs() <<endl; q2=q2.normalized(); cout<<"q2 after normalized \n"<<q2.coeffs() <<endl; Eigen::Matrix3d q1rotation_matrix = Eigen::Matrix3d::Identity();//初始化为单位阵 q1rotation_matrix=q1.toRotationMatrix();//四元素到旋转矩阵 Eigen::Isometry3d Tc1w=Eigen::Isometry3d::Identity();// 虽然称为3d,实质上是4*4的矩阵 //以上也可 Eigen::Isometry3d Tc1w(q1) 一步 按四元素表示的旋转 旋转 转换矩阵 Tc1w.rotate (q1rotation_matrix ); // 按照q1rotation_matrix进行旋转 Tc1w.pretranslate ( t1); // 把平移向量设成t1 //Eigen::Isometry3d Twc1=Tc1w.inverse();//由world 到c1的逆变换 成 c1到world Eigen::Matrix<double, 3, 1> pw=Tc1w.inverse()*p1; //将c1坐标系下的点p1变换到world坐标系下 Eigen::Matrix3d q2rotation_matrix = Eigen::Matrix3d::Identity();//初始化为单位阵 q2rotation_matrix=q2.toRotationMatrix();//四元素到旋转矩阵 Eigen::Isometry3d Tc2w=Eigen::Isometry3d::Identity();// 虽然称为3d,实质上是4*4的矩阵 齐次坐标 Tc2w.rotate (q2rotation_matrix ); // 按照q1rotation_matrix进行旋转 // Eigen::Isometry3d Tc2w(q2) //以上也可 Eigen::Isometry3d Tc2w(q2) 一步 Tc2w.pretranslate ( t2); // 把平移向量设成t1 Eigen::Matrix<double, 3, 1> p2=Tc2w*pw; //将world坐标系下的点pw变换到c2坐标系下 cout<<"the loc of p1 in c1 = \n"<< p1<<endl; cout<<"the loc of p1 in world = \n"<< pw<<endl; cout<<"the loc of p1 in c2 = \n"<< p2<<endl; return 0; }