【cartographer without ros】八、使用MapBuilder建图，存储，定位

最新推荐文章于 2024-06-13 10:45:26 发布

CloudFlame

最新推荐文章于 2024-06-13 10:45:26 发布

阅读量471

点赞数

分类专栏： cartographer without ros 文章标签：自动驾驶人工智能机器人

本文链接：https://blog.csdn.net/Cloud_H/article/details/132856506

版权

cartographer without ros 专栏收录该内容

10 篇文章 14 订阅

订阅专栏

上一节介绍了在脱离ros环境后，引入和使用Rosbag，并进行传感器数据的读写。
本节就将介绍在自己的工程中，使用cartographer的接口进行功能实现。
事实上，这部分是仿照cartographer_ros进行实现：通过使用MapBuilderInterface和TrajectoryBuilderInterface实现对应的建图，存储和定位。

1：定义和实现接口指针以及参数文件

#include "cartographer/mapping/map_builder.h"

std::unique_ptr<MapBuilderInterface> map_builder_;
std::unique_ptr<TrajectoryBuilderInterface> trajectory_builder_;
proto::MapBuilderOptions map_builder_options_;
proto::TrajectoryBuilderOptions trajectory_builder_options_;
int trajectory_id_;

void MyMapBuilder::MyMapBuilder()
{
    const std::string kMapBuilderLua = R"text(
            include "my_map_builder.lua"
            return MAP_BUILDER)text";
    auto map_builder_parameters = cartographer::mapping::slamhelper::ResolveLuaParameters(kMapBuilderLua);
    map_builder_options_ = CreateMapBuilderOptions(map_builder_parameters.get());

    const std::string kTrajectoryBuilderLua = R"text(
        include "my_trajectory_builder.lua"
        return TRAJECTORY_BUILDER)text"
    auto trajectory_builder_parameters = cartographer::mapping::slamhelper::ResolveLuaParameters(kTrajectoryBuilderLua);
    trajectory_builder_options_ = CreateTrajectoryBuilderOptions(trajectory_builder_parameters.get());

    map_builder_ = CreateMapBuilder(map_builder_options_);

    std::set<TrajectoryBuilderInterface::SensorId> expected_sensors;
    expected_sensors.insert(kRangeSensorId);
    trajectory_id_ = map_builder_->AddTrajectoryBuilder(
        expected_sensors, trajectory_builder_options_,
         GetLocalSlamResultCallback());
    trajectory_builder_ = map_builder_->GetTrajectoryBuilder(trajectory_id_);
}

2：添加传感器数据

void MyMapBuilder::AddData(cartographer::sensor::TimedPointCloudData measurement)
{
   trajectory_builder_->AddSensorData(kRangeSensorId.id, measurement); 
}

3：建图保存

void MyMapBuilder::SaveLoadState()
{
    const std::string filename = "mymap.pbstream";

    LOG(WARNING) << "  MyMapBuilder::SaveLoadState: Save '" << filename << "' Start";
    map_builder_->FinishTrajectory(trajectory_id_);
    LOG(WARNING) << "  MyMapBuilder::SaveLoadState: Running final trajectory optimization...";
    map_builder_->pose_graph()->RunFinalOptimization();
    int num_constraints = map_builder_->pose_graph()->constraints().size();
    int num_nodes =
        map_builder_->pose_graph()->GetTrajectoryNodes().SizeOfTrajectoryOrZero(
            trajectory_id_);
    EXPECT_GT(num_constraints, 0);
    EXPECT_GT(num_nodes, 0);

    cartographer::io::ProtoStreamWriter writer(filename);
    map_builder_->SerializeState(/*include_unfinished_submaps=*/true, &writer);
    writer.Close();
    LOG(WARNING) << "  MyMapBuilder::SaveLoadState: Save '" << filename << "' End";
}

4：加载地图

void MyMapBuilder::LoadMap()
{
    LOG(WARNING) << "  MyMapBuilder::LoadMap: Loading '" << map_load_path << "'";
    cartographer::io::ProtoStreamReader reader(map_load_path);
    map_builder_->LoadState(&reader, true /* load_frozen_state */);
    map_builder_->pose_graph()->RunFinalOptimization();
}

5：定位回调

    MapBuilderInterface::LocalSlamResultCallback MyMapBuilder::GetLocalSlamResultCallback()
    {
        return [=](const int trajectory_id_, const ::cartographer::common::Time time,
                   const ::cartographer::transform::Rigid3d local_pose,
                   ::cartographer::sensor::RangeData range_data_in_local,
                   const std::unique_ptr<
                       const cartographer::mapping::TrajectoryBuilderInterface::
                           InsertionResult>)
        {
               cartographer::transform::Rigid3d global_pose = map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id_) * local_pose;
        }
    }

【完】

至此在不带ros的环境中，引入cartographer的开发部分结束。
下一节将介绍cartographer的移植，以ARM的交叉编译为例。