【cartographer without ros】六、路标landmark数据转换

上一节介绍了IMU数据相关转换。

本节就开始介绍cartographer中的LandmarkData数据和Ros中的cartographer_ros_msgs::LandmarkList数据相互转换，方便引入Rosbag进行储存和读取。

目录

1：LandmarkData数据类型

2：cartographer数据转换Ros数据

3：Ros数据转换cartographer数据

---

1：LandmarkData数据类型

【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;
};

---

2：cartographer数据转换Ros数据

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;
    }

---

3：Ros数据转换cartographer数据

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环境依然能够进行存包读包等操作，方便支持离线建图定位等功能，以及对原始数据进行分析