赞
踩
上一节介绍了IMU数据相关转换。
本节就开始介绍cartographer中的LandmarkData数据和Ros中的cartographer_ros_msgs::LandmarkList数据相互转换,方便引入Rosbag进行储存和读取。
目录
【landmark_data.h】中查看数据结构:
- struct LandmarkObservation {
- std::string id;
- transform::Rigid3d landmark_to_tracking_transform;
- double translation_weight;
- double rotation_weight;
- };
-
- struct LandmarkData {
- common::Time time;
- std::vector<LandmarkObservation> landmark_observations;
- };
- cartographer_ros_msgs::LandmarkList ToRosMsg(cartographer::sensor::LandmarkData landmark_data)
- {
- cartographer_ros_msgs::LandmarkList landmarkList;
- cartographer_ros_msgs::LandmarkEntry landmarkEntry;
-
- landmarkList.header.frame_id = "base_link";
- landmarkList.header.stamp = common::RosFromCarto(landmark_data.time);
- landmarkList.landmarks.clear();
-
- for (auto var : landmark_data.landmark_observations)
- {
- landmarkEntry.id = var.id;
- landmarkEntry.tracking_from_landmark_transform.position.x = var.landmark_to_tracking_transform.translation().x();
- landmarkEntry.tracking_from_landmark_transform.position.y = var.landmark_to_tracking_transform.translation().y();
- landmarkEntry.tracking_from_landmark_transform.position.z = 0;
- landmarkEntry.tracking_from_landmark_transform.orientation.x = var.landmark_to_tracking_transform.rotation().x();
- landmarkEntry.tracking_from_landmark_transform.orientation.y = var.landmark_to_tracking_transform.rotation().y();
- landmarkEntry.tracking_from_landmark_transform.orientation.z = var.landmark_to_tracking_transform.rotation().z();
- landmarkEntry.tracking_from_landmark_transform.orientation.w = var.landmark_to_tracking_transform.rotation().w();
- landmarkEntry.translation_weight = var.translation_weight;
- landmarkEntry.rotation_weight = var.rotation_weight;
- landmarkList.landmarks.push_back(landmarkEntry);
- }
- return landmarkList;
- }
- cartographer::sensor::LandmarkData ToCartoSensor(cartographer_ros_msgs::LandmarkList landmark_list)
- {
- cartographer::sensor::LandmarkData landmark_data;
- landmark_data.time = FromRos(landmark_list.header.stamp);
- for (const LandmarkEntry& entry : landmark_list.landmarks) {
- landmark_data.landmark_observations.push_back(
- {entry.id, ToRigid3d(entry.tracking_from_landmark_transform),
- entry.translation_weight, entry.rotation_weight});
- }
- return landmark_data;
- }
【完】
下一节会介绍Rosbag的引入和使用,即使脱离了ros环境依然能够进行存包读包等操作,方便支持离线建图定位等功能,以及对原始数据进行分析
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。