C++ Quaterniond::coeffs方法代码示例

 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;
  /*编程题目
   小萝卜１号位姿q1=[0.35,0.2,0.3,0.1],t1=[0.3,0.1,0.1]'　　　 世界坐标系到相机变换
   小萝卜２号位姿q2=[-0.5,0.4,-0.1,0.2],t2=[-0.1,0.5,0.3]'
   小萝卜１号看到位于自身坐标系下p=[0.5,0,0.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();//四元素到旋转矩阵
//.........这里部分代码省略.........