cartographer自动更新地图，2条路径数据合并为1条数据

这一步操作感觉就像是把原来的骨头（子图）和经络（约束关系）打断再接上（有种涅磐重生的感觉）
重写保存函数WritePbStream

void UpdateAndWritePbStream(const mapping::PoseGraph& pose_graph,const std::map<int, mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&trajectory_builder_options,ProtoStreamWriterInterface* const writer, bool include_unfinished_submaps) {writer->WriteProto(CreateHeader());writer->WriteProto(UpdateAndSerializePoseGraph(pose_graph, include_unfinished_submaps));writer->WriteProto(UpdateAndSerializeTrajectoryBuilderOptions(trajectory_builder_options,GetValidTrajectoryIds(pose_graph.GetTrajectoryStates())));UpdateAndSerializeSubmaps(pose_graph.GetAllSubmapDataAfterUpdate(), include_unfinished_submaps,writer);UpdateAndSerializeTrajectoryNodes(pose_graph.GetTrajectoryNodesAfterUpdate(), writer);UpdateAndSerializeTrajectoryData(pose_graph.GetTrajectoryDataAfterUpdate(), writer);UpdateAndSerializeImuData(pose_graph.GetImuData(), writer);UpdateAndSerializeOdometryData(pose_graph.GetOdometryData(), writer);UpdateAndSerializeFixedFramePoseData(pose_graph.GetFixedFramePoseData(), writer);UpdateAndSerializeLandmarkNodes(pose_graph.GetLandmarkNodes(), writer);
}

UpdateAndSerializePoseGraph函数

SerializedData UpdateAndSerializePoseGraph(const mapping::PoseGraph& pose_graph,bool include_unfinished_submaps) {SerializedData proto;*proto.mutable_pose_graph() = pose_graph.ToProtoWithUpdate(include_unfinished_submaps);return proto;
}

UpdateAndSerializeTrajectoryBuilderOptions函数

SerializedData UpdateAndSerializeTrajectoryBuilderOptions(const std::map<int, mapping::proto::TrajectoryBuilderOptionsWithSensorIds>&trajectory_builder_options,const std::vector<int>& trajectory_ids_to_serialize) {std::map<int, mapping::proto::TrajectoryBuilderOptionsWithSensorIds>trajectory_builder_option_temp;std::vector<int> trajectory_ids_to_serialize_temp;trajectory_ids_to_serialize_temp.push_back(0);trajectory_builder_option_temp.emplace(0,trajectory_builder_options.at(0));SerializedData proto;*proto.mutable_all_trajectory_builder_options() =CreateAllTrajectoryBuilderOptionsProto(trajectory_builder_option_temp,trajectory_ids_to_serialize_temp);return proto;
}

UpdateAndSerializeSubmaps、UpdateAndSerializeTrajectoryNodes、UpdateAndSerializeTrajectoryData、UpdateAndSerializeImuData、UpdateAndSerializeOdometryData、UpdateAndSerializeFixedFramePoseData、UpdateAndSerializeLandmarkNodes与原接口相同，并没有改动，改动的是传入的参数

MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetAllSubmapDataAfterUpdate() const {absl::MutexLock locker(&mutex_);return GetSubmapDataAfterUpdateUnderLock();
}MapById<SubmapId, PoseGraphInterface::SubmapData>
PoseGraph2D::GetSubmapDataAfterUpdateUnderLock() const {MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;int old_map_last_submap_index = 0;for (const auto& submap_id_data : data_.submap_data) {if (submap_id_data.id.trajectory_id == 0) {submaps.Insert(submap_id_data.id,GetSubmapDataUnderLock(submap_id_data.id));old_map_last_submap_index = submap_id_data.id.submap_index;} else if (submap_id_data.id.trajectory_id == 1001) {SubmapId id(0, submap_id_data.id.submap_index + old_map_last_submap_index + 1);submaps.Insert(id, GetSubmapDataUnderLock(submap_id_data.id));}}return submaps;
}

MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodesAfterUpdate() const {absl::MutexLock locker(&mutex_);MapById<NodeId, TrajectoryNode> nodes;int old_map_last_node_index = 0;for (const auto& node_id_data : data_.trajectory_nodes) {if (node_id_data.id.trajectory_id == 0) {nodes.Insert(node_id_data.id, node_id_data.data);old_map_last_node_index = node_id_data.id.node_index ;} else if (node_id_data.id.trajectory_id == 1001) {NodeId id(0,node_id_data.id.node_index + old_map_last_node_index + 2);nodes.Insert(id, node_id_data.data);}}return nodes;}

std::map<int, PoseGraphInterface::TrajectoryData>
PoseGraph2D::GetTrajectoryDataAfterUpdate() const {absl::MutexLock locker(&mutex_);std::map<int, PoseGraphInterface::TrajectoryData> first_trajectory_data_;first_trajectory_data_[0] = optimization_problem_->trajectory_data().at(0);return first_trajectory_data_;
}

这里还重写了posegraph数据的保存

  proto::PoseGraph PoseGraph::ToProtoWithUpdate(bool include_unfinished_submaps) const {proto::PoseGraph proto;std::map<int, proto::Trajectory* const> trajectory_protos;const auto trajectory = [&proto, &trajectory_protos](const int trajectory_id) -> proto::Trajectory* {if (trajectory_protos.count(trajectory_id) == 0) {auto* const trajectory_proto = proto.add_trajectory();trajectory_proto->set_trajectory_id(trajectory_id);CHECK(trajectory_protos.emplace(trajectory_id, trajectory_proto).second);}return trajectory_protos.at(trajectory_id);};std::set<mapping::SubmapId> unfinished_submaps;for (const auto& submap_id_data : GetAllSubmapDataAfterUpdate()) {submap_id_data.id.trajectory_id << " submap_id " << submap_id_data.id.submap_index;proto::Trajectory* trajectory_proto =trajectory(submap_id_data.id.trajectory_id);if (!include_unfinished_submaps &&!submap_id_data.data.submap->insertion_finished()) {// Collect IDs of all unfinished submaps and skip them.unfinished_submaps.insert(submap_id_data.id);continue;}CHECK(submap_id_data.data.submap != nullptr);auto* const submap_proto = trajectory_proto->add_submap();submap_proto->set_submap_index(submap_id_data.id.submap_index);*submap_proto->mutable_pose() =transform::ToProto(submap_id_data.data.pose);}auto constraints_copy = constraintsAfterUpdate();std::set<mapping::NodeId> orphaned_nodes;proto.mutable_constraint()->Reserve(constraints_copy.size());for (auto it = constraints_copy.begin(); it != constraints_copy.end();) {if (!include_unfinished_submaps &&unfinished_submaps.count(it->submap_id) > 0) {// Skip all those constraints that refer to unfinished submaps and// remember the corresponding trajectory nodes as potentially orphaned.orphaned_nodes.insert(it->node_id);it = constraints_copy.erase(it);continue;}*proto.add_constraint() = cartographer::mapping::ToProto(*it);++it;}if (!include_unfinished_submaps) {// Iterate over all constraints and remove trajectory nodes from// 'orphaned_nodes' that are not actually orphaned.for (const auto& constraint : constraints_copy) {orphaned_nodes.erase(constraint.node_id);}}for (const auto& node_id_data : GetTrajectoryNodesAfterUpdate()) {proto::Trajectory* trajectory_proto =trajectory(node_id_data.id.trajectory_id);CHECK(node_id_data.data.constant_data != nullptr);auto* const node_proto = trajectory_proto->add_node();node_proto->set_node_index(node_id_data.id.node_index);node_proto->set_timestamp(common::ToUniversal(node_id_data.data.constant_data->time));*node_proto->mutable_pose() =transform::ToProto(node_id_data.data.global_pose);}auto landmarks_copy = GetLandmarkPosesAfterUpdate();proto.mutable_landmark_poses()->Reserve(landmarks_copy.size());for (const auto& id_pose : landmarks_copy) {auto* landmark_proto = proto.add_landmark_poses();landmark_proto->set_landmark_id(id_pose.first);*landmark_proto->mutable_global_pose() =transform::ToProto(id_pose.second);}return proto;}

打断骨头再接上的代码来了，重写constraints（）函数，在原地图子图的基础上+1,在原地图节点的基础上+2,为什么这么做？+1是为了让子图的id不同，+2不仅是为了让节点id不同，同时为了区分2条拼接路径（这样node会不连续），后端优化检测到不连续的节点就不加入优化（还未修改，但是程序可以运行）

std::vector<PoseGraphInterface::Constraint>
PoseGraph2D::constraintsAfterUpdate() const {std::vector<PoseGraphInterface::Constraint> result;absl::MutexLock locker(&mutex_);int old_map_last_submap_index = -1;int old_map_last_node_index = -1;for (const Constraint& constraint : data_.constraints) {SubmapId submap_id(0, constraint.submap_id.submap_index);NodeId node_id(0, constraint.node_id.node_index);if (constraint.submap_id.trajectory_id == 0 &&constraint.node_id.trajectory_id == 0) {result.push_back(Constraint{constraint.submap_id, constraint.node_id,Constraint::Pose{constraint.pose.zbar_ij *transform::Rigid3d::Rotation(data_.trajectory_nodes.at(constraint.node_id).constant_data->gravity_alignment),constraint.pose.translation_weight,constraint.pose.rotation_weight},constraint.tag});if (old_map_last_submap_index < constraint.submap_id.submap_index) {old_map_last_submap_index = constraint.submap_id.submap_index;}if (old_map_last_node_index < constraint.node_id.node_index) {old_map_last_node_index = constraint.node_id.node_index;}continue;} else {if (constraint.submap_id.trajectory_id == 1001) {int submap_end_index =old_map_last_submap_index;  // data_.submap_data.EndOfTrajectory(0)->id.submap_index;submap_id.submap_index =constraint.submap_id.submap_index + submap_end_index + 1;}if (constraint.node_id.trajectory_id == 1001) {int node_end_index =old_map_last_node_index;  node_id.node_index = constraint.node_id.node_index + node_end_index + 2;}}result.push_back(Constraint{submap_id, node_id,Constraint::Pose{constraint.pose.zbar_ij *transform::Rigid3d::Rotation(data_.trajectory_nodes.at(constraint.node_id).constant_data->gravity_alignment),constraint.pose.translation_weight,constraint.pose.rotation_weight},constraint.tag});}return result;
}

然后自己在ros里面写一个更新服务，就可以在原有地图的基础上，一边定位，一边更新地图（去除老地图），最后把剩余的老地图与更新地图合并，就完成了地图更新，cartographer只能帮各位到这了。
自己不会写代码，感觉是硬伤，要加油补补了

cartographer自动更新地图，2条路径数据合并为1条数据相关推荐

pandas数据合并与重塑_PANDAS 数据合并与重塑（concat篇）
pandas作者Wes McKinney 在[PYTHON FOR DATA ANALYSIS]中对pandas的方方面面都有了一个权威简明的入门级的介绍,但在实际使用过程中,我发现书中的内容还只是冰 ...
phpexcel导出大量数据合并单元格_php 数据导出到excel 2种带有合并单元格的导出...
/** * * execl数据导出*/ function exportOrderExcel($title, $cellName, $data) {$htmlinfo = " ".$ ...
R语言中的横向数据合并merge及纵向数据合并rbind的使用
转载自:https://www.cnblogs.com/awishfullyway/p/6677006.html 我们经常会遇到两个数据框拥有相同的时间或观测值,但这些列却不尽相同.处理的办法就是使用 ...
计算机不支持win10 1809更新,win10更新版本1809后，完全关闭系统自动更新方法，亲测可行...
将win10升级到版本1809后,还有一个附加的" Windows Update Medic Service"服务,它是" Windows Update"服务的 ...
wps关闭自动更新方法
老是弹出更新关不掉烦死了,试了好几种方法,终于成功了 [方法一:删除更新任务计划]此方法不稳定,很可能删了后重新出现此电脑右键 --> 管理 --> 系统工具 --> 任务计划程序 ...
【Python基础】一文搞定pandas的数据合并
作者:来源于读者投稿出品:Python数据之道一文搞定pandas的数据合并在实际处理数据业务需求中,我们经常会遇到这样的需求:将多个表连接起来再进行数据的处理和分析,类似SQL中的连接查询功能 ...
如何将excel三列数据合为一列日期数据
如果要将 Excel 中的三列数据合并为一列日期数据,可以使用以下步骤: 在 Excel 中打开包含需要合并的三列数据的工作表. 选中要合并的三列(或使用鼠标拖动选择). 在 Excel 的" ...
一文搞定Pandas中的数据合并
一文搞定pandas的数据合并在实际处理数据业务需求中,我们经常会遇到这样的需求:将多个表连接起来再进行数据的处理和分析,类似SQL中的连接查询功能. pandas中也提供了几种方法来实现这个功能, ...
一文搞定pandas的数据合并
作者:来源于读者投稿出品:Python数据之道一文搞定pandas的数据合并在实际处理数据业务需求中,我们经常会遇到这样的需求:将多个表连接起来再进行数据的处理和分析,类似SQL中的连接查询功能 ...

cartographer自动更新地图，2条路径数据合并为1条数据

cartographer自动更新地图，2条路径数据合并为1条数据相关推荐

最新文章

热门文章