Eigen矩阵，四元素，旋转向量以及tf之间的转化总结

1
2
3
4
5
6
 Eigen::Translation3f tl_btol(_tf_x, _tf_y, _tf_z);                 // tl: translation
  Eigen::AngleAxisf rot_x_btol(_tf_roll, Eigen::Vector3f::UnitX());  // rot: rotation
  Eigen::AngleAxisf rot_y_btol(_tf_pitch, Eigen::Vector3f::UnitY());
  Eigen::AngleAxisf rot_z_btol(_tf_yaw, Eigen::Vector3f::UnitZ());
  tf_btol = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();//得到4*4的旋转矩阵

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
     ESKF_ptr->get_mean_pose(predict_state);
 
     Eigen::Quaternionf rotation_quat;
     rotation_quat.w() = predict_state[3];
     rotation_quat.x() = predict_state[4];
     rotation_quat.y() = predict_state[5];
     rotation_quat.z() = predict_state[6];

     Eigen::Vector3f translate;
     translate[0] = predict_state[0];
     translate[1] = predict_state[1];
     translate[2] = predict_state[2];

     eskf_predict.block<3,3>(0,0) = rotation_quat.toRotationMatrix();
     eskf_predict.block<3,1>(0,3) = translate;

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
Eigen::Matrix4f t_localizer(Eigen::Matrix4f::Identity());
tf::Matrix3x3 mat_l, mat_b;
  mat_l.setValue(static_cast<double>(t_localizer(0, 0)), static_cast<double>(t_localizer(0, 1)),
                 static_cast<double>(t_localizer(0, 2)), static_cast<double>(t_localizer(1, 0)),
                 static_cast<double>(t_localizer(1, 1)), static_cast<double>(t_localizer(1, 2)),
                 static_cast<double>(t_localizer(2, 0)), static_cast<double>(t_localizer(2, 1)),
                 static_cast<double>(t_localizer(2, 2)));
  mat_b.setValue(static_cast<double>(t_base_link(0, 0)), static_cast<double>(t_base_link(0, 1)),
                 static_cast<double>(t_base_link(0, 2)), static_cast<double>(t_base_link(1, 0)),
                 static_cast<double>(t_base_link(1, 1)), static_cast<double>(t_base_link(1, 2)),
                 static_cast<double>(t_base_link(2, 0)), static_cast<double>(t_base_link(2, 1)),
                 static_cast<double>(t_base_link(2, 2)));

  // Update localizer_pose.
  localizer_pose.x = t_localizer(0, 3);
  localizer_pose.y = t_localizer(1, 3);
  localizer_pose.z = t_localizer(2, 3);
  mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);

1
2
3
4
5
6
7
8
9
10
 tf::Quaternion q;
 q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw);
 current_pose_msg.pose.position.x = current_pose.x;
  current_pose_msg.pose.position.y = current_pose.y;
  current_pose_msg.pose.position.z = current_pose.z;
  current_pose_msg.pose.orientation.x = q.x();
  current_pose_msg.pose.orientation.y = q.y();
  current_pose_msg.pose.orientation.z = q.z();
  current_pose_msg.pose.orientation.w = q.w();

1
2
3
4
5
6
7
8
 nav_msgs::Odometry laserOdometry = imuOdomQueue.back();
        laserOdometry.pose.pose.position.x = x;
        laserOdometry.pose.pose.position.y = y;
        laserOdometry.pose.pose.position.z = z;
        laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        pubImuOdometry.publish(laserOdometry);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
   //1.先转Eigen四元素
    Eigen::Matrix4f trans;
    Eigen::Quaternionf quat;
    quat.x() = odom_trans.transform.rotation.x;   
    quat.y() = odom_trans.transform.rotation.y;
    quat.z() = odom_trans.transform.rotation.z;
    quat.w() = odom_trans.transform.rotation.w;
    //2.转成Eigen 旋转矩阵
    trans.block<3,3>(0,0) = quat.toRotationMatrix();
    trans.block<3,1>(0,3) = pose.block<3,1>(0,3);
    tf::Matrix3x3 mat_l;
    //3.求得欧拉角
    double roll,pitch,yaw;
    mat_l.setValue(static_cast<double>(trans(0, 0)),  static_cast<double>(trans(0,1)), static_cast<double>(trans(0, 2)),
                   static_cast<double>(trans(1, 0)), static_cast<double>(trans(1, 1)), static_cast<double>(trans(1, 2)),
                   static_cast<double>(trans(2, 0)), static_cast<double>(trans(2, 1)), static_cast<double>(trans(2, 2))
                   );
    mat_l.getRPY(roll,pitch,yaw);

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
geometry_msgs::PoseStamped opiti_odom_msgs;
    opiti_odom_msgs.header.frame_id = "map";
    opiti_odom_msgs.header.stamp = keyframe->stamp;
    opiti_odom_msgs.pose.position.x = trans_odom2map(0, 3);
    opiti_odom_msgs.pose.position.y = trans_odom2map(1, 3);
    opiti_odom_msgs.pose.position.z = trans_odom2map(2, 3);

    tf::Matrix3x3 mat_l;
    double roll,pitch,yaw;
    mat_l.setValue(static_cast<double>(trans_odom2map(0, 0)),  static_cast<double>(trans_odom2map(0,1)), static_cast<double>(trans_odom2map(0, 2)),
                   static_cast<double>(trans_odom2map(1, 0)), static_cast<double>(trans_odom2map(1, 1)), static_cast<double>(trans_odom2map(1, 2)),
                   static_cast<double>(trans_odom2map(2, 0)), static_cast<double>(trans_odom2map(2, 1)), static_cast<double>(trans_odom2map(2, 2))
                   );
    mat_l.getRPY(roll,pitch,yaw);
    tf::Quaternion q;
    q.setRPY(roll,pitch,yaw);
    opiti_odom_msgs.pose.orientation.x = q.x();
    opiti_odom_msgs.pose.orientation.y = q.y();
    opiti_odom_msgs.pose.orientation.z = q.z();
    opiti_odom_msgs.pose.orientation.w = q.w();

    nav_msgs::Path opti_odom_path;
    opti_odom_path.header.stamp = keyframe->stamp;
    opti_odom_path.header.frame_id = "map";
    opti_odom_path.poses.push_back(opiti_odom_msgs);
    odom_pub_path.publish(opti_odom_path);