当我们把所有的约束都放入了PoseGraph2D 类中的constraints中， 通过
1
2
| optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(),
data_.landmark_nodes);
|
下面是optimizationproblem的定义
1
2
|
std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;
|
OptimizationProblem2D
该类位于 mapping/internal/optimization/optimazation_problem_2d.h中，保存了所有的传感器数据，包括
1
2
3
4
5
6
7
8
9
10
11
| optimization::proto::OptimizationProblemOptions options_;
MapById<NodeId, NodeSpec2D> node_data_;
MapById<SubmapId, SubmapSpec2D> submap_data_;
std::map<std::string, transform::Rigid3d> landmark_data_;
sensor::MapByTime<sensor::ImuData> empty_imu_data_;
sensor::MapByTime<sensor::OdometryData> odometry_data_;
sensor::MapByTime<sensor::FixedFramePoseData> fixed_frame_pose_data_;
std::map<int, PoseGraphInterface::TrajectoryData> trajectory_data_;
|
成员函数
添加数据
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
|
void OptimizationProblem2D::AddOdometryData(
const int trajectory_id, const sensor::OdometryData& odometry_data) {
odometry_data_.Append(trajectory_id, odometry_data);
}
void OptimizationProblem2D::AddFixedFramePoseData(
const int trajectory_id,
const sensor::FixedFramePoseData& fixed_frame_pose_data) {
fixed_frame_pose_data_.Append(trajectory_id, fixed_frame_pose_data);
}
void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
const NodeSpec2D& node_data) {
node_data_.Append(trajectory_id, node_data);
trajectory_data_[trajectory_id];
}
void OptimizationProblem2D::InsertTrajectoryNode(const NodeId& node_id,
const NodeSpec2D& node_data) {
node_data_.Insert(node_id, node_data);
trajectory_data_[node_id.trajectory_id];
}
|
获取数据
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
| const MapById<NodeId, NodeSpec2D>& node_data() const override {
return node_data_;
}
const MapById<SubmapId, SubmapSpec2D>& submap_data() const override {
return submap_data_;
}
const std::map<std::string, transform::Rigid3d>& landmark_data()
const override {
return landmark_data_;
}
const sensor::MapByTime<sensor::ImuData>& imu_data() const override {
return empty_imu_data_;
}
const sensor::MapByTime<sensor::OdometryData>& odometry_data()
const override {
return odometry_data_;
}
|
子图相关
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
|
void OptimizationProblem2D::AddSubmap(
const int trajectory_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Append(trajectory_id, SubmapSpec2D{global_submap_pose});
}
void OptimizationProblem2D::InsertSubmap(
const SubmapId& submap_id, const transform::Rigid2d& global_submap_pose) {
submap_data_.Insert(submap_id, SubmapSpec2D{global_submap_pose});
}
void OptimizationProblem2D::TrimSubmap(const SubmapId& submap_id) {
submap_data_.Trim(submap_id);
}
|
根据里程计数据算出两个节点间的相对坐标变换
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
|
std::unique_ptr<transform::Rigid3d>
OptimizationProblem2D::CalculateOdometryBetweenNodes(
const int trajectory_id, const NodeSpec2D& first_node_data,
const NodeSpec2D& second_node_data) const {
if (odometry_data_.HasTrajectory(trajectory_id)) {
const std::unique_ptr<transform::Rigid3d> first_node_odometry =
InterpolateOdometry(trajectory_id, first_node_data.time);
const std::unique_ptr<transform::Rigid3d> second_node_odometry =
InterpolateOdometry(trajectory_id, second_node_data.time);
if (first_node_odometry != nullptr && second_node_odometry != nullptr) {
transform::Rigid3d relative_odometry =
transform::Rigid3d::Rotation(first_node_data.gravity_alignment) *
first_node_odometry->inverse() * (*second_node_odometry) *
transform::Rigid3d::Rotation(
second_node_data.gravity_alignment.inverse());
return absl::make_unique<transform::Rigid3d>(relative_odometry);
}
}
return nullptr;
}
|
获取里程计数据的位姿
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
|
std::unique_ptr<transform::Rigid3d> OptimizationProblem2D::InterpolateOdometry(
const int trajectory_id, const common::Time time) const {
const auto it = odometry_data_.lower_bound(trajectory_id, time);
if (it == odometry_data_.EndOfTrajectory(trajectory_id)) {
return nullptr;
}
if (it == odometry_data_.BeginOfTrajectory(trajectory_id)) {
if (it->time == time) {
return absl::make_unique<transform::Rigid3d>(it->pose);
}
return nullptr;
}
const auto prev_it = std::prev(it);
return absl::make_unique<transform::Rigid3d>(
Interpolate(transform::TimestampedTransform{prev_it->time, prev_it->pose},
transform::TimestampedTransform{it->time, it->pose}, time)
.transform);
}
|
Solve
1
2
3
4
5
6
7
8
9
10
11
12
13
|
void OptimizationProblem2D::Solve(
const std::vector<Constraint>& constraints,
const std::map<int, PoseGraphInterface::TrajectoryState>&
trajectories_state,
const std::map<std::string, LandmarkNode>& landmark_nodes)
{}
|
成员变量
nodedata
定义为：
1
| MapById<NodeId, NodeSpec2D> node_data_;
|
可以通过函数node_data()获得此数据
1
2
3
| const MapById<NodeId, NodeSpec2D>& node_data() const override {
return node_data_;
}
|
NodeSpec2D 数据结构：
1
2
3
4
5
6
| struct NodeSpec2D {
common::Time time;
transform::Rigid2d local_pose_2d;
transform::Rigid2d global_pose_2d;
Eigen::Quaterniond gravity_alignment;
};
|
nodedata 更新：
1
2
3
4
5
6
|
void OptimizationProblem2D::AddTrajectoryNode(const int trajectory_id,
const NodeSpec2D& node_data) {
node_data_.Append(trajectory_id, node_data);
trajectory_data_[trajectory_id];
}
|
此在节点加入后端后，计算此节点的相关约束的时候进行调用，加入的node_data为此node的global和local的位姿
OptimizationProblem2D对象的使用
打开posegraph_2d.cc，搜索optimization_problem，查看如何使用
添加submap
函数InitializeGlobalSubmapPoses中，返回指定轨迹id下的正处于活跃状态下的子图的SubmapId
这个是一个子图的情况
1
2
3
4
5
| optimization_problem_->AddSubmap(
trajectory_id, transform::Project2D(
ComputeLocalToGlobalTransform(
data_.global_submap_poses_2d, trajectory_id) *
insertion_submaps[0]->local_pose()));
|
这个是有两个活跃子图的情况
1
2
3
4
5
6
7
8
|
optimization_problem_->AddSubmap(
trajectory_id,
first_submap_pose *
constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
constraints::ComputeSubmapPose(*insertion_submaps[1]));
|
添加传感器数据
在向PoseGraph添加传感器数据的时候，
1
2
3
4
| optimization_problem_->AddImuData(trajectory_id, imu_data);
optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
optimization_problem_->AddFixedFramePoseData(trajectory_id,
fixed_frame_pose_data);
|
加入节点数据
在计算节点约束的时候，添加Node的信息加入到优化问题中。
1
2
3
4
5
6
|
optimization_problem_->AddTrajectoryNode(
matching_id.trajectory_id,
optimization::NodeSpec2D{constant_data->time, local_pose_2d,
global_pose_2d,
constant_data->gravity_alignment});
|
对于landmark ，他没有添加到这里，他只是放到了pose_graph_2d中
总结
Node的global位姿
位于PoseGraph2D::AddNode
1
2
3
|
const transform::Rigid3d optimized_pose(
GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
|
submap在global坐标系下的位姿
1
2
3
4
5
6
7
8
9
10
| optimization_problem_->AddSubmap(
trajectory_id, transform::Project2D(
• ComputeLocalToGlobalTransform(
• data_.global_submap_poses_2d, trajectory_id) *
• insertion_submaps[0]->local_pose()));
optimization_problem_->AddSubmap(
trajectory_id,
first_submap_pose *
• constraints::ComputeSubmapPose(*insertion_submaps[0]).inverse() *
• constraints::ComputeSubmapPose(*insertion_submaps[1]));
|
子图在global坐标系下的第一帧的位姿,就是这个子图在local坐标系下的位姿.
之后的子图在global坐标系下的位姿 是通过 第一个子图在global坐标系下的pose 乘以 第一个子图到第二个子图在local坐标系下的位姿变换 得到的.
子图内约束
计算一个节点约束的时候，in PoseGraph2D::ComputeConstraintsForNode，需要添加此Node对所有submap的约束
即submap指向Node的一个约束。这是local坐标系下得到的坐标变换
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
|
for (size_t i = 0; i < insertion_submaps.size(); ++i) {
const SubmapId submap_id = submap_ids[i];
data_.submap_data.at(submap_id).node_ids.emplace(node_id);
const transform::Rigid2d constraint_transform =
constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() *
local_pose_2d;
data_.constraints.push_back(
Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.matcher_translation_weight(),
options_.matcher_rotation_weight()},
Constraint::INTRA_SUBMAP});
}
|
子图间约束
先通过子图在global坐标系下的坐标的逆, 乘以节点在global坐标系下的坐标, 获取子图原点在获取子图原点在 global坐标系下指向节点的相对坐标变换
首先一个先验估计值
1
2
3
4
5
6
|
const transform::Rigid2d initial_relative_pose =
optimization_problem_->submap_data()
.at(submap_id)
.global_pose.inverse() *
optimization_problem_->node_data().at(node_id).global_pose_2d;
|
然后根据子图在local坐标系下的位姿乘以这个坐标变换, 得到节点在local坐标系下的预测位姿
1
2
| const transform::Rigid2d initial_pose =
ComputeSubmapPose(*submap) * initial_relative_pose;
|
再通过分枝定界粗匹配与ceres的精匹配, 对这个节点位姿进行校准, 校准后的位姿还是local坐标系下的
最后, 通过子图在local坐标系下位姿的逆, 乘以这个节点校准后的位姿, 得到子图间约束, 是local坐 标系下, 子图原点指向节点的相对坐标变换.
1
2
| const transform::Rigid2d constraint_transform =
ComputeSubmapPose(*submap).inverse() * pose_estimate;
|
返回
1
2
3
4
5
6
7
|
constraint->reset(new Constraint{submap_id,
node_id,
{transform::Embed3D(constraint_transform),
options_.loop_closure_translation_weight(),
options_.loop_closure_rotation_weight()},
Constraint::INTER_SUBMAP});
|
constraint 为添加到 constraints_ 双端队列的最末尾的数据
最后他返回的数据，将通过ConstraintBuilder2D::RunWhenDoneCallback()函数，把存储子图间约束的数据
1
| std::deque<std::unique_ptr<Constraint>> constraints_
|
全部传入到result中，然后通过调用回调函数PoseGraph2D::HandleWorkQueue(
把数据全部添加到了PoseGraph2D的 data_.constraints中
计算相对位姿的方法总结
- 节点 通过 GetLocalToGlobalTransform * constant_data->local_pose 进行global下位姿的计算
- 子图 通过对前一个子图到后一个子图的坐标变换进行累计, 得到子图在global坐标系下的位姿
- 子图内约束 local坐标系系下, 子图原点指向节点间的坐标变换
- 子图间约束 根据global坐标计算初值, 然后通过分支定界算法粗匹配与ceres的精匹配, 获取校准后的位姿, 最后计算local坐标系系下, 子图原点指向校准后的节点间的坐标变换
