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();//四元素到旋转矩阵
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;
}