上一节介绍了在脱离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的交叉编译为例。