FAST_LIO_SAM
FAST_LIO_SAM copied to clipboard
kahowang
获取groundtruth真实位姿
你好，关于真实位姿的问题想请教一下。 我用的FAST-LIO-LC的数据集做的测试。为了能够得到真实的位姿，我订阅了代码中的odometry话题，但是得到的真实位姿的xyz的值和optimize_pose的xyz值相差太多，想请教一下是否订阅的话题出现了错误还是我写的代码的问题？ `void publish_odometry(const ros::Publisher &pubOdomAftMapped) { odomAftMapped.header.frame_id = "camera_init"; odomAftMapped.child_frame_id = "body"; odomAftMapped.header.stamp = ros::Time().fromSec(lidar_end_time); // ros::Time().fromSec(lidar_end_time); set_posestamp(odomAftMapped.pose); pubOdomAftMapped.publish(odomAftMapped); auto P = kf.get_P(); for (int i = 0; i < 6; i++) { int k = i < 3 ? i + 3 : i - 3; odomAftMapped.pose.covariance[i * 6 + 0] = P(k, 3); odomAftMapped.pose.covariance[i * 6 + 1] = P(k, 4); odomAftMapped.pose.covariance[i * 6 + 2] = P(k, 5); odomAftMapped.pose.covariance[i * 6 + 3] = P(k, 0); odomAftMapped.pose.covariance[i * 6 + 4] = P(k, 1); odomAftMapped.pose.covariance[i * 6 + 5] = P(k, 2); }
static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; transform.setOrigin(tf::Vector3(odomAftMapped.pose.pose.position.x, odomAftMapped.pose.pose.position.y, odomAftMapped.pose.pose.position.z)); q.setW(odomAftMapped.pose.pose.orientation.w); q.setX(odomAftMapped.pose.pose.orientation.x); q.setY(odomAftMapped.pose.pose.orientation.y); q.setZ(odomAftMapped.pose.pose.orientation.z);
/添加***********/ PointTypePose thisPose6D; thisPose6D.x = odomAftMapped.pose.pose.position.x; thisPose6D.y = odomAftMapped.pose.pose.position.y; thisPose6D.z = odomAftMapped.pose.pose.position.z; thisPose6D.roll = 0; thisPose6D.pitch = 0; thisPose6D.yaw = 0; //thisPose6D.intensity = 0; //thisPose6D.time = lidar_end_time; odometry_cloudKeyPoses6D->push_back(thisPose6D); /**********************************************/ transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "body"));
}` /写入txt代码部分***/ for(int i = 0; i < odometry_cloudKeyPoses6D->size(); i++){ pose_groundtruth.t = Eigen::Vector3d(odometry_cloudKeyPoses6D->points[i].x, odometry_cloudKeyPoses6D->points[i].y, odometry_cloudKeyPoses6D->points[i].z ); pose_gnss.R = Exp(double(odometry_cloudKeyPoses6D->points[i].roll), double(odometry_cloudKeyPoses6D->points[i].pitch), double(odometry_cloudKeyPoses6D->points[i].yaw) ); WriteText(file_pose_groundtruth, pose_groundtruth); } cout << "Sucess groundtruth poses to pose files ..." << endl; /**********************************/
