本次阅读的源码为 release-1.0 版本的代码

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下载我上传的全套工作空间的代码，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156

cartographer/mapping/internal/2d/pose_graph_2d.cc

AddImuData()

首先通过　global_trajectory_builder.cc　调用　pose_graph_2d.cc　的　AddImuData(), AddOdometryData()

// cartographer/mapping/internal/optimization/optimization_problem_2d.cc
void PoseGraph2D::AddImuData(const int trajectory_id,const sensor::ImuData& imu_data) {common::MutexLocker locker(&mutex_);AddWorkItem([=]() REQUIRES(mutex_) {optimization_problem_->AddImuData(trajectory_id, imu_data);});
}void PoseGraph2D::AddOdometryData(const int trajectory_id,const sensor::OdometryData& odometry_data) {common::MutexLocker locker(&mutex_);AddWorkItem([=]() REQUIRES(mutex_) {optimization_problem_->AddOdometryData(trajectory_id, odometry_data);});
}

AddTrajectoryNode()

再通过　global_trajectory_builder.cc　调用　pose_graph_2d.cc　的　AddNode() --> ComputeConstraintsForNode() --> optimization_problem_->AddTrajectoryNode()

void PoseGraph2D::ComputeConstraintsForNode(const NodeId& node_id,std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,const bool newly_finished_submap) {optimization_problem_->AddTrajectoryNode(matching_id.trajectory_id,optimization::NodeSpec2D{constant_data->time, local_pose_2d,global_pose_2d,constant_data->gravity_alignment});}

AddSubmap()

ComputeConstraintsForNode() --> ComputeConstraint() -->

std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(const int trajectory_id, const common::Time time,const std::vector<std::shared_ptr<const Submap2D>>& insertion_submaps) {CHECK(!insertion_submaps.empty());const auto& submap_data = optimization_problem_->submap_data();if (insertion_submaps.size() == 1) {// If we don't already have an entry for the first submap, add one.if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0) {if (initial_trajectory_poses_.count(trajectory_id) > 0) {trajectory_connectivity_state_.Connect(trajectory_id,initial_trajectory_poses_.at(trajectory_id).to_trajectory_id, time);}optimization_problem_->AddSubmap(trajectory_id,transform::Project2D(ComputeLocalToGlobalTransform(global_submap_poses_, trajectory_id) *insertion_submaps[0]->local_pose()));}CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));const SubmapId submap_id{trajectory_id, 0};CHECK(submap_data_.at(submap_id).submap == insertion_submaps.front());return {submap_id};}CHECK_EQ(2, insertion_submaps.size());const auto end_it = submap_data.EndOfTrajectory(trajectory_id);CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);const SubmapId last_submap_id = std::prev(end_it)->id;if (submap_data_.at(last_submap_id).submap == insertion_submaps.front()) {// In this case, 'last_submap_id' is the ID of// 'insertions_submaps.front()' and 'insertions_submaps.back()' is new.const auto& first_submap_pose = submap_data.at(last_submap_id).global_pose;optimization_problem_->AddSubmap(trajectory_id,first_submap_pose *constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *constraints::ComputeSubmapPose(*insertion_submaps[1]));return {last_submap_id,SubmapId{trajectory_id, last_submap_id.submap_index + 1}};}CHECK(submap_data_.at(last_submap_id).submap == insertion_submaps.back());const SubmapId front_submap_id{trajectory_id,last_submap_id.submap_index - 1};CHECK(submap_data_.at(front_submap_id).submap == insertion_submaps.front());return {front_submap_id, last_submap_id};
}

Solve()

ComputeConstraintsForNode() --> DispatchOptimization() --> HandleWorkQueue() --> RunOptimization() -->

optimization_problem_->Solve()

void PoseGraph2D::DispatchOptimization() {run_loop_closure_ = true;// If there is a 'work_queue_' already, some other thread will take care.if (work_queue_ == nullptr) {work_queue_ = common::make_unique<std::deque<std::function<void()>>>();constraint_builder_.WhenDone(std::bind(&PoseGraph2D::HandleWorkQueue, this, std::placeholders::_1));}
}void PoseGraph2D::HandleWorkQueue(const constraints::ConstraintBuilder2D::Result& result) {{common::MutexLocker locker(&mutex_);constraints_.insert(constraints_.end(), result.begin(), result.end());}RunOptimization();// ...}void PoseGraph2D::RunOptimization() {if (optimization_problem_->submap_data().empty()) {return;}// No other thread is accessing the optimization_problem_, constraints_,// frozen_trajectories_ and landmark_nodes_ when executing the Solve. Solve is// time consuming, so not taking the mutex before Solve to avoid blocking// foreground processing.optimization_problem_->Solve(constraints_, frozen_trajectories_,landmark_nodes_);// ...
}

cartographer/mapping/internal/optimization/optimization_problem_2d.cc

struct NodeSpec2D {common::Time time;transform::Rigid2d local_pose_2d;transform::Rigid2d global_pose_2d;Eigen::Quaterniond gravity_alignment;
};struct SubmapSpec2D {transform::Rigid2d global_pose;
};

void OptimizationProblem2D::Solve(const std::vector<Constraint>& constraints,const std::set<int>& frozen_trajectories,const std::map<std::string, LandmarkNode>& landmark_nodes) {if (node_data_.empty()) {// Nothing to optimize.return;}ceres::Problem::Options problem_options;ceres::Problem problem(problem_options);// Set the starting point.// TODO(hrapp): Move ceres data into SubmapSpec.MapById<SubmapId, std::array<double, 3>> C_submaps;MapById<NodeId, std::array<double, 3>> C_nodes;std::map<std::string, CeresPose> C_landmarks;bool first_submap = true;bool freeze_landmarks = !frozen_trajectories.empty();for (const auto& submap_id_data : submap_data_) {const bool frozen =frozen_trajectories.count(submap_id_data.id.trajectory_id) != 0;C_submaps.Insert(submap_id_data.id,FromPose(submap_id_data.data.global_pose));problem.AddParameterBlock(C_submaps.at(submap_id_data.id).data(), 3);if (first_submap || frozen) {first_submap = false;// Fix the pose of the first submap or all submaps of a frozen// trajectory.problem.SetParameterBlockConstant(C_submaps.at(submap_id_data.id).data());}}for (const auto& node_id_data : node_data_) {const bool frozen =frozen_trajectories.count(node_id_data.id.trajectory_id) != 0;C_nodes.Insert(node_id_data.id, FromPose(node_id_data.data.global_pose_2d));problem.AddParameterBlock(C_nodes.at(node_id_data.id).data(), 3);if (frozen) {problem.SetParameterBlockConstant(C_nodes.at(node_id_data.id).data());}}// Add cost functions for intra- and inter-submap constraints.for (const Constraint& constraint : constraints) {problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(constraint.pose),// Only loop closure constraints should have a loss function.constraint.tag == Constraint::INTER_SUBMAP? new ceres::HuberLoss(options_.huber_scale()): nullptr,C_submaps.at(constraint.submap_id).data(),C_nodes.at(constraint.node_id).data());}// Add cost functions for landmarks.AddLandmarkCostFunctions(landmark_nodes, freeze_landmarks, node_data_,&C_nodes, &C_landmarks, &problem);// Add penalties for violating odometry or changes between consecutive nodes// if odometry is not available.for (auto node_it = node_data_.begin(); node_it != node_data_.end();) {const int trajectory_id = node_it->id.trajectory_id;const auto trajectory_end = node_data_.EndOfTrajectory(trajectory_id);if (frozen_trajectories.count(trajectory_id) != 0) {node_it = trajectory_end;continue;}auto prev_node_it = node_it;for (++node_it; node_it != trajectory_end; ++node_it) {const NodeId first_node_id = prev_node_it->id;const NodeSpec2D& first_node_data = prev_node_it->data;prev_node_it = node_it;const NodeId second_node_id = node_it->id;const NodeSpec2D& second_node_data = node_it->data;if (second_node_id.node_index != first_node_id.node_index + 1) {continue;}// Add a relative pose constraint based on the odometry (if available).std::unique_ptr<transform::Rigid3d> relative_odometry =CalculateOdometryBetweenNodes(trajectory_id, first_node_data,second_node_data);if (relative_odometry != nullptr) {problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(Constraint::Pose{*relative_odometry, options_.odometry_translation_weight(),options_.odometry_rotation_weight()}),nullptr /* loss function */, C_nodes.at(first_node_id).data(),C_nodes.at(second_node_id).data());}// Add a relative pose constraint based on consecutive local SLAM poses.const transform::Rigid3d relative_local_slam_pose =transform::Embed3D(first_node_data.local_pose_2d.inverse() *second_node_data.local_pose_2d);problem.AddResidualBlock(CreateAutoDiffSpaCostFunction(Constraint::Pose{relative_local_slam_pose,options_.local_slam_pose_translation_weight(),options_.local_slam_pose_rotation_weight()}),nullptr /* loss function */, C_nodes.at(first_node_id).data(),C_nodes.at(second_node_id).data());}}// Solve.ceres::Solver::Summary summary;ceres::Solve(common::CreateCeresSolverOptions(options_.ceres_solver_options()),&problem, &summary);if (options_.log_solver_summary()) {LOG(INFO) << summary.FullReport();}// Store the result.for (const auto& C_submap_id_data : C_submaps) {submap_data_.at(C_submap_id_data.id).global_pose =ToPose(C_submap_id_data.data);}for (const auto& C_node_id_data : C_nodes) {node_data_.at(C_node_id_data.id).global_pose_2d =ToPose(C_node_id_data.data);}for (const auto& C_landmark : C_landmarks) {landmark_data_[C_landmark.first] = C_landmark.second.ToRigid();}
}

CostFunction

ceres::CostFunction* CreateAutoDiffSpaCostFunction(const PoseGraphInterface::Constraint::Pose& observed_relative_pose) {return new ceres::AutoDiffCostFunction<SpaCostFunction2D, 3 /* residuals */,3 /* start pose variables */,3 /* end pose variables */>(new SpaCostFunction2D(observed_relative_pose));
}class SpaCostFunction2D {public:explicit SpaCostFunction2D(const PoseGraphInterface::Constraint::Pose& observed_relative_pose): observed_relative_pose_(observed_relative_pose) {}template <typename T>bool operator()(const T* const start_pose, const T* const end_pose,T* e) const {const std::array<T, 3> error =ScaleError(ComputeUnscaledError(transform::Project2D(observed_relative_pose_.zbar_ij),start_pose, end_pose),observed_relative_pose_.translation_weight,observed_relative_pose_.rotation_weight);std::copy(std::begin(error), std::end(error), e);return true;}private:const PoseGraphInterface::Constraint::Pose observed_relative_pose_;
};

references

1 https://blog.csdn.net/xiaoma_bk/article/details/82392295 --- cartographer 子图和节点的优化

2 https://blog.csdn.net/MyArrow/article/details/82909148 --- Cartographer SPA残差

cartographer探秘第四章之代码解析（六） --- 后端优化 --- 优化求解相关推荐

学习笔记-第十四章恶意代码分析实战
第十四章恶意代码的网络特征 1.网络应对措施. 网络行为的基本属性包括IP地址,TCP端口,以及流量内容等,网络和安全设备可以利用它们,来提供网络应对措施.根据IP地址和端口,防火墙和路由器可以限 ...
第四章 PX4-Pixhawk-MPU6000传感器驱动解析
第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解析步骤同样可以对L3GD20(3轴陀螺仪).HMC58 ...
第四章MPU6000传感器驱动解析
版权声明:本文为博主原创文章,未经博主允许不得转载. 第四章MPU6000传感器驱动解析 Mpu6000是一个3轴加速度和3轴陀螺仪传感器,这一章节我们将对MPU6000这个传感器进行解析,依照这个解 ...
CodeCombat代码全记录（Python学习利器）--安息之云山峰（第四章）代码1
我们已经有了前三章的基础了,到了第四章,你会发现提示少之又少了.这时,我们大都要靠自己了!!!加油!!如果你没有思路,请仔细看下装备的说明,毕竟到了这关,你需要购买好多其他的新装备,而新装备中存在了新 ...
Paxos算法之旅（四）zookeeper代码解析--转载
ZooKeeper是近期比较热门的一个类Paxos实现.也是一个逐渐得到广泛应用的开源的分布式锁服务实现.被认为是Chubby的开源版,虽然具体实现有很多差异.ZooKeeper概要的介绍可以看官方文 ...
一个基于JRTPLIB的轻量级RTSP客户端(myRTSPClient)——收流篇：（四）example代码解析...
--------------------更新2018.08.20------------------- 添加http_tunnel_example.cpp作为RtspOverHttp示例程序. --- ...
视觉SLAM十四讲CH10代码解析及课后习题详解
g2o_viewer问题解决在进行位姿图优化时候,如果出现g2o_viewer: command not found,说明你的g2o_viewer并没有安装上,打开你之前安装的g2o文件夹,打开bi ...
视觉SLAM十四讲CH8代码解析及课后习题详解
第一版的代码: direct_semidense.cpp #include <iostream> #include <fstream> #include <list> ...
CE教程第四章《代码寻找》
步骤 5: 代码寻找 (密码=888899) 有时一些东西的保存位置在你重新开始游戏时会改变,甚至是在你玩的时候也会变,在这种情况下,你用2步仍然能做出可以用的内存列表. 在这一步会说明怎样用寻找代 ...
经典卷积模型(四)GoogLeNet-Inception(V3)代码解析
Inception-V3 网络主干依旧由Inception.辅助分类器构成,其中Inception有六类. BasicConv2d 基本卷积模块 BasicConv2d为带BN+relu的卷积. cl ...

cartographer探秘第四章之代码解析（六） --- 后端优化 --- 优化求解