当前位置:网站首页>cartographer_optimization_problem_2d
cartographer_optimization_problem_2d
2022-06-26 05:09:00 【古路】
cartographer_optimization_problem_2d
0.引言
主要业务逻辑回到mapping/internal/2d/pose_graph_2d.cc/PoseGraph2D::HandleWorkQueue-->RunOptimization()
中。具体的优化这一部分就是在optimization
中进行实现。
前面计算约束结果为:
1.设置待优化变量
- 子图的global_pose
- 节点的global_pose_2d
2.残差项
2.1.第一种残差-scan_match
将节点与子图原点在global坐标系下的相对位姿 与 约束 的差值作为残差项
- 第一种坐标变换: 节点与子图原点在global坐标系下的坐标变换(预测位姿)
- 第二种坐标变换: 子图内约束与子图间约束(测量位姿)
残差计算代码:
/** * @brief 2d 根据SPA论文里的公式求残差 * * 计算残差: * T12 = T1.inverse() * T2 * [R1.inverse * R2, R1.inverse * (t2 -t1)] * [0 , 1 ] * * @param[in] relative_pose * @param[in] start * @param[in] end * @return std::array<T, 3> */
template <typename T>
static std::array<T, 3> ComputeUnscaledError(
const transform::Rigid2d& relative_pose, const T* const start,
const T* const end) {
// 旋转矩阵R
const T cos_theta_i = cos(start[2]);
const T sin_theta_i = sin(start[2]);
const T delta_x = end[0] - start[0]; // t2 -t1
const T delta_y = end[1] - start[1];
const T h[3] = {
cos_theta_i * delta_x + sin_theta_i * delta_y, // R.inverse * (t2 -t1)
-sin_theta_i * delta_x + cos_theta_i * delta_y,
end[2] - start[2]};
return {
{
T(relative_pose.translation().x()) - h[0],
T(relative_pose.translation().y()) - h[1],
common::NormalizeAngleDifference(
T(relative_pose.rotation().angle()) - h[2])}};
}
2.2.第二种残差-Landmark
landmark数据 与 通过2个节点位姿插值出来的相对位姿 的差值作为残差项
- 第一种坐标变换: landmark数据的时间在2个节点位姿中插值出来的位姿(预测位姿)
- 第二种坐标变换: landmark数据中的landmark_to_tracking_transform_(测量位姿)
struct LandmarkNode {
// landmark数据是相对于tracking_frame的相对坐标变换
struct LandmarkObservation {
int trajectory_id;
common::Time time;
transform::Rigid3d landmark_to_tracking_transform;
double translation_weight;
double rotation_weight;
};
// 同一时刻可能会观测到多个landmark数据
std::vector<LandmarkObservation> landmark_observations;
// 这帧数据对应的tracking_frame在global坐标系下的位姿
absl::optional<transform::Rigid3d> global_landmark_pose;
bool frozen = false;
};
主要是node <--> landmark
之间的约束:
// Step: landmark数据 与 通过2个节点位姿插值出来的相对位姿 的差值作为残差项
AddLandmarkCostFunctions(landmark_nodes, node_data_, &C_nodes, &C_landmarks,
&problem, options_.huber_scale());
略.
2.3.第三种残差-Odometer
节点与节点间在global坐标系下的相对坐标变换 与 通过里程计数据插值出的相对坐标变换 的差值作为残差项
- 第一种坐标变换: 相邻2个节点间在global坐标系下的坐标变换
- 第二种坐标变换: 再分别计算这2个节点的时间在里程计数据队列中插值得到的2个里程计位姿, 计算这2个里程计位姿间的坐标变换
2.4.第四种残差-local_global
节点与节点间在global坐标系下的相对坐标变换 与 相邻2个节点在local坐标系下的相对坐标变换 的差值作为残差项
- 第一种坐标变换: 相邻2个节间在global坐标系下的坐标变换
- 第二种坐标变换: 相邻2个节点在local坐标系下的坐标变换
2.5.第五种残差-gps
节点与gps坐标系原点在global坐标系下的相对坐标变换 与 通过gps数据进行插值得到的相对坐标变换 的差值作为残差项
- 第一种坐标变换: 节点对应的时刻在gps数据中插值得到的gps相对于gps坐标系原点的位姿
- 第二种坐标变换: 节点在global坐标系下 与 gps坐标系原点 的相对坐标变换
3.理论参考
边栏推荐
- 6.1 - 6.2 公鑰密碼學簡介
- Happy New Year!
- Use fill and fill in Matplotlib_ Between fill the blank area between functions
- Learn from small samples and run to the sea of stars
- Generalized linear model (logistic regression, Poisson regression)
- 【quartz】从数据库中读取配置实现动态定时任务
- Codeforces Round #800 (Div. 2)
- 5. <tag-栈和常规问题>补充: lt.946. 验证栈序列(同剑指 Offer 31. 栈的压入、弹出序列)
- [unity3d] human computer interaction input
- [latex] error type summary (hold the change)
猜你喜欢
Codeforces Round #800 (Div. 2)
torchvision_transform(图像增强)
Multipass Chinese document - remote use of multipass
Learn from small samples and run to the sea of stars
[unity3d] rigid body component
YOLOV5超参数设置与数据增强解析
Classic theory: detailed explanation of three handshakes and four waves of TCP protocol
Briefly describe the pitfalls of mobile IM development: architecture design, communication protocol and client
Final review of brain and cognitive science
LeetCode 19. Delete the penultimate node of the linked list
随机推荐
YOLOV5超参数设置与数据增强解析
File upload and security dog
0622 horse palm fell 9%
【Unity3D】碰撞体组件Collider
Schematic diagram of UWB ultra high precision positioning system
Astype conversion data type
Record a circular reference problem
图像翻译/GAN:Unsupervised Image-to-Image Translation with Self-Attention Networks基于自我注意网络的无监督图像到图像的翻译
Comment enregistrer une image dans une applet Wechat
UWB超高精度定位系统原理图
ECCV 2020 double champion team, take you to conquer target detection on the 7th
Illustration of ONEFLOW's learning rate adjustment strategy
[unity3d] collider assembly
YOLOv5-6.0的一些参数设置和特征图可视化
递归遍历目录结构和树状展现
C# 39. string类型和byte[]类型相互转换(实测)
Codeforces Round #802 (Div. 2)(A-D)
Cookie and session Basics
线程优先级
Multipass Chinese documents - improve mount performance