HKUST-Aerial-Robotics / A-LOAM

Advanced implementation of LOAM
Other
2.05k stars 789 forks source link

Timestamps of kitti label and odomAftMapped cannot be aligned #67

Open a-github-learner opened 5 months ago

a-github-learner commented 5 months ago

Thanks for your impressive work! Here I've met some problem while using evo to visualization the results. I first use following codes to transform the trajectory and pose of the topic "/aft_mapped_to_init" to txt files.


ros::Subscriber save_path= nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 100, path_save); 
// the implementation of recall function
void path_save(nav_msgs::Odometry odomAftMapped)
{
    FILE* fp = fp_kitti;
    Eigen::Matrix4d T_lidar_to_cam;
    T_lidar_to_cam << 0.00042768, -0.999967, -0.0080845, -0.01198, -0.00721062,
    0.0080811998, -0.99994131, -0.0540398, 0.999973864, 0.00048594,
    -0.0072069, -0.292196, 0, 0, 0, 1.0;
    Eigen::Matrix<double, 4, 4> T;
    Eigen::Quaterniond rotation(odomAftMapped.pose.pose.orientation.w, \
                         odomAftMapped.pose.pose.orientation.x, \
                         odomAftMapped.pose.pose.orientation.y, \
                         odomAftMapped.pose.pose.orientation.z);
    Eigen::Vector3d p(odomAftMapped.pose.pose.position.x, \
                         odomAftMapped.pose.pose.position.y, \
                         odomAftMapped.pose.pose.position.z);
    T.block<3, 3>(0, 0) = rotation.toRotationMatrix();
    T.block<3, 1>(0, 3) = p;
    T(3, 0) = 0;
    T(3, 1) = 0;
    T(3, 2) = 0;
    T(3, 3) = 1;
    T = T_lidar_to_cam * T * T_lidar_to_cam.inverse();
    for (int i = 0; i < 3; i++) 
    {
        if (i == 2)
          fprintf(fp, "%lf %lf %lf %lf", T(i, 0), T(i, 1), T(i, 2), T(i, 3));
        else
          fprintf(fp, "%lf %lf %lf %lf ", T(i, 0), T(i, 1), T(i, 2), T(i, 3));
  }
    fprintf(fp, "\n");
    // Eigen::Quaterniond q(state.rot_end);
    // fprintf(fp, "%lf %lf %lf %lf %lf %lf %lf \r\n", state.pos_end[0],
    //         state.pos_end[1], state.pos_end[2], q.w(), q.x(), q.y(), q.z());
    fflush(fp);
}

Then I use the evo to evaluate the output of aloam. However, because of the frequency of laser mapping algorithm, I just get 4028 poses in aloam while there exists about 4541 poses in kitti label. Thus I get trouble in aligning them for evaluation. Can you give me some advice? image