05 初始位姿估计
PoseExtrapolator
类PoseExtrapolator的初始化
// 将IMU数据加入到Extrapolator中
void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
InitializeExtrapolator(imu_data.time);
extrapolator_->AddImuData(imu_data);
}
如果使用IMU，即lua文件中的use_imu_data()设置为true。当IMU数据队列第一帧进入LocalTrajectoryBuilder2D类的成员函数AddImuData，会调用另一个成员函数InitializeExtrapolator，将第一帧的时间传入来初始化类PoseExtrapolator。（后续都使用这种情况进行分析）
// 如果不用imu, 就在雷达这初始化位姿推测器
if (!options_.use_imu_data()) {
InitializeExtrapolator(time);
}
如果不使用IMU，即lua文件中的use_imu_data()设置为false。此时类PoseExtrapolator就用第一次雷达点云数据时间同步后的time来进行初始化。
如果没有对lua文件中options_.pose_extrapolator_options().constant_velocity().pose_queue_duration()和options_.pose_extrapolator_options().constant_velocity().imu_gravity_time_constant()这两个参数进行修改，那么将使用两个的默认值，0.001和10来对进行构造。
// 如果Extrapolator没有初始化就进行初始化
void LocalTrajectoryBuilder2D::InitializeExtrapolator(const common::Time time) {
// 如果已经初始化过了就直接返回
if (extrapolator_ != nullptr) {
return;
}
// 注意 use_imu_based为true就会报错
CHECK(!options_.pose_extrapolator_options().use_imu_based());
// TODO(gaschler): Consider using InitializeWithImu as 3D does.
// 初始化位姿推测器
extrapolator_ = absl::make_unique<PoseExtrapolator>(
::cartographer::common::FromSeconds(options_.pose_extrapolator_options()
.constant_velocity()
.pose_queue_duration()), // 0.001s
options_.pose_extrapolator_options()
.constant_velocity()
.imu_gravity_time_constant()); // 10
// 添加初始位姿
extrapolator_->AddPose(time, transform::Rigid3d::Identity());
}
调用了构造函数后，类PoseExtrapolator目前已经被赋值的成员变量如下
| 类PoseExtrapolator的成员变量 | 现在的值 |
|---|---|
pose_queue_duration_ |
0.001 |
gravity_time_constant_ |
10 |
cached_extrapolated_pose_ |
|
linear_velocity_from_poses_ |
Eigen::Vector3d::Zero() |
angular_velocity_from_poses_ |
Eigen::Vector3d::Zero() |
linear_velocity_from_odometry_ |
Eigen::Vector3d::Zero() |
angular_velocity_from_odometry_ |
Eigen::Vector3d::Zero() |
以上变量前三个是构造函数赋值，后面的是头文件中有默认值
初始化完成后这里还调用了一个类PoseExtrapolator的成员函数AddPose
PoseExtrapolator::AddPose
// 将扫描匹配后的pose加入到pose队列中,计算线速度与角速度,并将imu_tracker_的状态更新到time时刻
void PoseExtrapolator::AddPose(const common::Time time,
const transform::Rigid3d& pose) {
// 如果imu_tracker_没有初始化就先进行初始化
//(ty：按正常的流程来走，第一次执行到这里的时候，imu_data_应该为空，
// 因为这个AddPose函数执行完之后才第一次朝imu_data_添加imu数据)
if (imu_tracker_ == nullptr) {
common::Time tracker_start = time;
if (!imu_data_.empty()) {
tracker_start = std::min(tracker_start, imu_data_.front().time);
}
// imu_tracker_的初始化
//(ty：这里的tracker_start就是第一帧IMU数据的time，
// gravity_time_constant_ = 10)
imu_tracker_ =
absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
}
// 在timed_pose_queue_中保存pose
//（第一次添加的time是第一帧imu数据的time，pose是[0,0,0]）
timed_pose_queue_.push_back(TimedPose{time, pose});
// 保持pose队列中第二个pose的时间要大于 time - pose_queue_duration_
while (timed_pose_queue_.size() > 2 && // timed_pose_queue_最少是2个数据
timed_pose_queue_[1].time <= time - pose_queue_duration_) {
timed_pose_queue_.pop_front();
}
// 根据加入的pose计算线速度与角速度
UpdateVelocitiesFromPoses();
// 将imu_tracker_更新到time时刻
AdvanceImuTracker(time, imu_tracker_.get());
// pose队列更新了,之前imu及里程计数据已经过时了
// 因为pose是匹配的结果,之前的imu及里程计数据是用于预测的,现在结果都有了,之前的用于预测的数据肯定不需要了
TrimImuData();
TrimOdometryData();
// 用于根据里程计数据计算线速度时姿态的预测
odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
// 用于位姿预测时的姿态预测
extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}
第一次调用该函数，传入的参数是第一帧IMU数据的时间，以及一个[0,0,0]的坐标转换关系
按正常的流程来走，第一次执行到这里的时候，PoseExtrapolator类中用来存储IMU数据队列的成员变量imu_data_应该为空，因为添加IMU数据的成员函数AddImuData第一次被调用是在第一次执行完成员函数AddPose之后！
-
首先对类
ImuTracker进行了构造，传入的参数是10和第一帧IMU数据的time-
类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_第一帧IMU数据的time last_linear_acceleration_time_common::Time::min() orientation_Eigen::Quaterniond::Identity() gravity_vector_Eigen::Vector3d::UnitZ() imu_angular_velocity_Eigen::Vector3d::Zero()
-
-
向成员变量
timed_pose_queue_队列中添加第一个元素，其size变为1类PoseExtrapolator的成员变量 现在的值 timed_pose_queue_{第一帧imu数据的time， -
由于
timed_pose_queue_的size小于2，因此进不去while循环 -
调用成员函数
UpdateVelocitiesFromPoses，进入该函数第一步就有一个判断if (timed_pose_queue_.size() < 2) { // We need two poses to estimate velocities. return; }因此这个函数第一次调用会马上返回，不作任何处理
-
调用成员函数
AdvanceImuTracker，将第一帧IMU的time和刚刚创建好的ImuTracker传入-
首先检查传入的
time是否大于等于imu_tracker->time()，这里表示的意思是只能预测imu_tracker->time()之后的某个时间戳，而不能预测之前时间，这也符合逻辑，如果time小于imu_tracker->time()，说明IMU传感器的时序不是从小到大 -
前面提到，
imu_data_目前为空，所以进入if语句-
调用类ImuTracker的成员函数
Advance，传入第一帧IMU数据的time-
CHECK_LE(time_, time);首先检查ImuTracker的成员变量
time_是否小于等于传入进来的time，这个意义和上一步一样！ -
const double delta_t = common::ToSeconds(time - time_);获取这一帧的IMU数据与上一帧IMU的数据的时间差
delta_t，值等于传入的time（这一帧的time)和time_（上一帧的time）的差值由于
time_和传入的time都表示第一帧IMU的time，两者相等，所以第一次delta_t为0 -
// 上一时刻的角速度乘以时间,得到当前时刻相对于上一时刻的预测的姿态变化量,再转换成四元数 const Eigen::Quaterniond rotation = transform::AngleAxisVectorToRotationQuaternion( Eigen::Vector3d(imu_angular_velocity_ * delta_t));通过上一帧的角速度乘以
delta_t，得到一个姿态变化量，将其转换四元数rotation由于刚开始
imu_angular_velocity_为Eigen::Vector3d::Zero()，且delta_t也为0，所以rotation为[0,0,0,1] -
// 使用上一时刻的姿态 orientation_ 乘以姿态变化量, 得到当前时刻的预测出的姿态 orientation_ = (orientation_ * rotation).normalized();将上一步得到的姿态变化量作用于上一帧的姿态上，预测出这一帧的姿态
orientation_将[0,0,0,1]作用于之前的姿态上，不会发生任何变化，所以第一次
orientation_保持不变，为默认值[0,0,0,1] -
// 根据预测出的姿态变化量,预测旋转后的线性加速度的值 gravity_vector_ = rotation.conjugate() * gravity_vector_;不要被
gravity_vector_变量名给迷惑，它不是记录重力方向的，是用它和orientation_的乘积！即orientation_*gravity_vector_= [0,0,k]（k为任意的正实数）（当k为非正数的时候，说明你的车已经与地面的夹角大于等于90度了）因为初始化的时候初始化的时候
orientation_为[0,0,0,1]，而gravity_vector_为[0,0,1]，两者的乘积就为[0,0,1]，每当rotation作用于orientation_上，就会用rotation相反的方向去作用于gravity_vector_，始终保持orientation_*gravity_vector_= [0,0,k] -
// 更新时间 time_ = time;将这一帧的时间赋给time_，当下次执行到这里的时候，这一帧就变成上一帧！
第一次time_被赋值为第一帧IMU数据的时间
-
-
调用类ImuTracker的成员函数
AddImuLinearAccelerationObservation，第一次传入的是一个固定的加速度[0,0,1]，后面都是传入实际的IMU数据的加速度-
const double delta_t = last_linear_acceleration_time_ > common::Time::min() ? common::ToSeconds(time_ - last_linear_acceleration_time_) : std::numeric_limits<double>::infinity();这个
delta_t和刚刚调用的成员函数Advance中的delta_t表示的意义一样！即获取这一帧的IMU数据与上一帧IMU的数据的时间差delta_t不同的是，第一次运行到这里，
Advance中的delta_t被赋值的是0，而这个delta_t是无穷大！因为last_linear_acceleration_time_的值刚开始还是默认的common::Time::min()，所以第一次delta_t被赋值的是std::numeric_limits<double>::infinity()，表示正无穷大！ -
last_linear_acceleration_time_ = time_;将这一帧的IMU的time赋值给
last_linear_acceleration_time_，因为当下一次执行这个函数，这一帧的IMU的time就变成上一帧了，依此循环，所以也解释delta_t为什么是两帧IMU时间之差类ImuTracker的成员变量 现在的值 last_linear_acceleration_time_第一帧IMU数据的time -
// Step: 2 求alpha, alpha=1-e^(-delta_t/10) // delta_t越大, alpha越大 const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_); gravity_vector_ = (1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;这里使用的是一个RC低通滤波器原理，简单的理解就是利用
alpha来决定这一帧的IMU数据中加速度的权重，alpha越大，这一帧加速度的权重就越大，换句话说，就越相信这一帧的加速度。由于
alpha是delta_t决定的，delta_t越大，alpha越大，所以这里总的来说，就是delta_t的大小决定了这一帧加速度的权重。可以理解为因为有噪声的存在, 时间差越大，之前的gravity_vector_的权重就应该越小越好，反之最新的加速度权重就越大越好！第一次执行到这里的，由于
delta_t是无穷大，所以alpha为1，完全相信传入进来的imu_linear_acceleration，即[0,0,1]。gravity_vector_的初始值也是[0,0,1]，然而这里得到最新的gravity_vector_还是[0,0,1] -
const Eigen::Quaterniond rotation = FromTwoVectors( gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ()); orientation_ = (orientation_ * rotation).normalized();在成员函数
Advance提过，是用gravity_vector_和orientation_的乘积来维护重力方向的！在上一步，由于向量的加减，这一帧的IMU加速度会让
gravity_vector_发生变化（尽管第一次执行到上一步没有改变gravity_vector_的值，但是后面传入的IMU真实加速度肯定会对这个值进行一定修正），而orientation_还保持原来的值，这就导致目前gravity_vector_和orientation_的乘积不再是[0,0,k]，这肯定是不行的！所以这里操作其实就在维护gravity_vector_和orientation_的乘积不变！其实这里的
orientation_.conjugate() * Eigen::Vector3d::UnitZ()不就是在没有被上一步修改之前的gravity_vector_吗？所以首先求出上一步修正后的gravity_vector_和没有修正前的gravity_vector_之间的姿态变化量rotation。这里还要注意一点，FromTwoVectors函数返回的是第一个参数转向第二个参数的旋转量！所以是修正后转成未修正前的
rotation，不能把参数互换！否则会导致gravity_vector_和orientation_的乘积是[0,0,k]将这个姿态变化量作用于未被修改的
orientation_，就继续维持gravity_vector_和orientation_的乘积是[0,0,k]第一次执行到这里，由于修改后的
gravity_vector和修改前的gravity_vector都是[0,0,1]，因此两者之间的旋转量rotation是[0,0,0,1]，将其作用于orientation_也不会变化，orientation_还是保持[0,0,0,1]不变 -
CHECK_GT((orientation_ * gravity_vector_).z(), 0.); CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);最后这里cartographer自己也会检查
orientation_ * gravity_vector_的值是否正确。按前面所说，代码始终都在维持orientation_和gravity_vector_的乘积为[0,0,k]（k为正实数），所以第一步先检查这个z轴，也就是k是否是大于0；然后将乘积归一化后（z轴从k变成1）再取z轴，检查是否大于0.99。显然，这里肯定是满足的。到这里其实就可以总结一下，
orientation_的物理意义其实就是记录的在三维空间中，将tracking_frame旋转到与local slam坐标系方向一致（可以理解成垂直于重力加速度方向的一个水平面，即一个二维坐标系）的旋转量。即通过orientation_就可以将tracking_frame坐标系在三维空间上旋转成与local slam坐标系方向一致。当然，如果想将tracking_frame坐标转换到local slam坐标系上还需要加上tracking_frame坐标系原点相对于local slam坐标系原点的坐标即可！后续在
LocalTrajectoryBuilder2D::AddRangeData中，从PoseExtrapolator返回的就是一帧点云中每个点转换到local slam上的位姿（返回一个rigid3d类）因为我们的小车可能上坡或者下坡，再或者地面颠簸等，tracking_frame都会时不时与水平面形成夹角，如果我们不将其转换到local slam这个固定的水平面坐标系上，后续去点云畸变、体素滤波等，点云的(x,y,z)都得乘以当时角度的正弦值，小车角度变一次就得转换一次，这不是给自己找事？举个最简单例子，体素滤波有个max_z和min_z的参数，这两个参数是针对距离你tracking_frame竖直方向上的高度。如果小车和地面有夹角，那岂不是你的z轴是不是也应该乘以对应的正弦值才行？
-
-
调用类ImuTracker的成员函数
AddImuAngularVelocityObservation，如果使用了里程计，就传入angular_velocity_from_odometry_，没有使用就传入angular_velocity_from_poses_，第一次传入的值都是[0,0,0]-
imu_angular_velocity_ = imu_angular_velocity;这个成员函数就一个赋值操作，将传入的IMU角速度
mu_angular_velocity赋值给类ImuTraker成员变量imu_angular_velocity_。第一次传入进来的是[0,0,0]，所以imu_angular_velocity_是[0,0,0]
-
执行到这里，第一次调用成员函数
PoseExtrapolator::AdvanceImuTracker就结束了，返回到PoseExtrapolator::AddPose中继续进行成员函数TrimImuData() -
-
继续调用成员函数
PoseExtrapolator::TrimImuData()对数据队列imu_data_进行修剪
// 修剪imu的数据队列,丢掉过时的imu数据 void PoseExtrapolator::TrimImuData() { // 保持imu队列中第二个数据的时间要大于最后一个位姿的时间, imu_date_最少是1个 while (imu_data_.size() > 1 && !timed_pose_queue_.empty() && imu_data_[1].time <= timed_pose_queue_.back().time) { imu_data_.pop_front(); } }满足以下三个条件就对删除数据队列
imu_data_中第一个数据，直到有一个条件不满足就退出- 队列
imu_data_中的IMU数据至少有1个 timed_pose_queue_有数据- 数据队列中第二个IMU数据的time要小于等于最后一个位姿pose的time
第一次调用该函数，
imu_data_还是空的，显然第一个条件就不满足，直接退出！-
调用成员函数
PoseExtrapolator::TrimImuData()对数据队列odometry_data_进行修剪，处理同IMU数据 -
最后两步就是将之前构造好的
imu_tracker_分别对odometry_imu_tracker_和extrapolation_imu_tracker_构造，这里就说明odometry_imu_tracker_和extrapolation_imu_tracker_初始时和imu_tracker_一模一样！
执行到这里，第一次调用成员函数
PoseExtrapolator::AddPose就结束了执行完一次后，更新一下类
PoseExtrapolator和类ImuTracker的成员变量类PoseExtrapolator的成员变量 现在的值 pose_queue_duration_0.001 gravity_time_constant_10 cached_extrapolated_pose_linear_velocity_from_poses_Eigen::Vector3d::Zero() angular_velocity_from_poses_Eigen::Vector3d::Zero() linear_velocity_from_odometry_Eigen::Vector3d::Zero() angular_velocity_from_odometry_Eigen::Vector3d::Zero() timed_pose_queue_{第一帧imu数据的time， 类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_第一帧IMU数据的time last_linear_acceleration_time_第一帧IMU数据的time orientation_Eigen::Quaterniond::Identity() gravity_vector_Eigen::Vector3d::UnitZ() imu_angular_velocity_Eigen::Vector3d::Zero() -
// 将IMU数据加入到Extrapolator中
void LocalTrajectoryBuilder2D::AddImuData(const sensor::ImuData& imu_data) {
CHECK(options_.use_imu_data()) << "An unexpected IMU packet was added.";
InitializeExtrapolator(imu_data.time);
extrapolator_->AddImuData(imu_data);
}
当函数InitializeExtrapolator(imu_data.time)完以后，也就是以上所有流程都走完了，extrapolator_才第一次调用了添加IMU数据的函数AddImuData
// 向imu数据队列中添加imu数据,并进行数据队列的修剪
void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
CHECK(timed_pose_queue_.empty() ||
imu_data.time >= timed_pose_queue_.back().time);
imu_data_.push_back(imu_data);
TrimImuData();
}
- 因为IMU队列中，规定了数据队列中第二个IMU数据的time要大于最后一个位姿pose的time，所以传进来最新IMU数据的time也应该至少大于等于最后一个位姿pose的time
当这里，cartographer初始位姿估计中所有的类就初始化完成，当下一次调用的时间是第一次处理激光雷达数据的时候，即类LocalTrajectoryBuilder2D的成员函数AddRangeData中。
即使激光雷达先打开也没用，因为当extrapolator_没有被构造时，LocalTrajectoryBuilder2D::AddRangeData只会进行进行多个雷达点云数据的时间同步，而后就返回了！
if (extrapolator_ == nullptr) {
// Until we've initialized the extrapolator with our first IMU message, we
// cannot compute the orientation of the rangefinder.
LOG(INFO) << "Extrapolator not yet initialized.";
return nullptr;
}
因为IMU的频率都远大于激光雷达的频率，所以一般当第一帧IMU数据到来后进行以上所有步骤的初始化，而后还来了好多帧IMU数据，而后才是第一帧激光雷达数据，此时IMU队列中已经有多个数据了。
之前我想到一种最极限的一种情况就是，IMU第一帧刚初始化完成，第二帧IMU数据都还没到来时，激光雷达的数据就来了，即此时IMU队列中就只有一帧数据，但这种情况可能不行！
// 计算第一个点的时间
const common::Time time_first_point =
time +
common::FromSeconds(synchronized_data.ranges.front().point_time.time);
// 只有在extrapolator_初始化时, GetLastPoseTime()是common::Time::min()
//（ty：只要imu启动了，就会调用LocalTrajectoryBuilder2D::AddImuData对extrapolator_初始化)
if (time_first_point < extrapolator_->GetLastPoseTime()) {
LOG(INFO) << "Extrapolator is still initializing.";
return nullptr;
}
因为激光雷达的time是表示最后一个点的时间戳，所以time_first_point 表示的就是这段点云中第一个点的时间戳，是小于time的，而extrapolator_->GetLastPoseTime()返回的就是第一帧IMU数据的时间戳，所以这里要求点云段的时间段要小于0.01s（假设IMU是100HZ的频率）才能满足！
综上所述，为了方便叙述，我假设第一次成功调用（那种提前返回nullptr不算）LocalTrajectoryBuilder2D::AddRangeData时，IMU队列中已经有三个IMU数据，而激光雷达的第一个点云和最后一个点云的time都正好在第三帧IMU数据和第四帧IMU数据之间！
// 预测得到每一个时间点的位姿
for (const auto& range : synchronized_data.ranges) {
common::Time time_point = time + common::FromSeconds(range.point_time.time);
// 如果该时间比上次预测位姿的时间还要早,说明这个点的时间戳往回走了, 就报错
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
// 一个循环只报一次错
if (!warned) {
LOG(ERROR)
<< "Timestamp of individual range data point jumps backwards from "
<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
warned = true;
}
time_point = extrapolator_->GetLastExtrapolatedTime();
}
// Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
range_data_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
}
第二次执行初始化那一大堆函数就是在最后这里，这里的time_point就是激光雷达这一段点云，从第一个点依次到最后一个点的时间戳！按之前假设，都在第三帧IMU数据和第四帧IMU数据之间！
调用类PoseExtrapolator的成员函数ExtrapolatePose
// 预测得到time时刻 tracking frame 在 local slam 坐标系下的位姿
transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) {
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
CHECK_GE(time, newest_timed_pose.time);
// 如果本次预测时间与上次计算时间相同 就不再重复计算
if (cached_extrapolated_pose_.time != time) {
// 预测tracking frame在local slam坐标系下time时刻的位置
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
// 预测tracking frame在local slam坐标系下time时刻的姿态
const Eigen::Quaterniond rotation =
newest_timed_pose.pose.rotation() *
ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
cached_extrapolated_pose_ =
TimedPose{time, transform::Rigid3d{translation, rotation}};
}
return cached_extrapolated_pose_.pose;
}
同样还是逐句分析
-
const TimedPose& newest_timed_pose = timed_pose_queue_.back(); CHECK_GE(time, newest_timed_pose.time);按之前的分析，这里
newest_timed_pose就是初始化时存入的值{第一帧imu数据的time，{[0,0,0],[0,0,0,1]}}检查要预测的时间是否是在得到pose之后，因为往得到pose之前的time推测没有任何意义！
传入的time是位于第三帧IMU数据的time之后，所以满足
-
// 如果本次预测时间与上次计算时间相同 就不再重复计算 if (cached_extrapolated_pose_.time != time) { // 预测tracking frame在local slam坐标系下time时刻的位置 const Eigen::Vector3d translation = ExtrapolateTranslation(time) + newest_timed_pose.pose.translation(); // 预测tracking frame在local slam坐标系下time时刻的姿态 const Eigen::Quaterniond rotation = newest_timed_pose.pose.rotation() * ExtrapolateRotation(time, extrapolation_imu_tracker_.get()); cached_extrapolated_pose_ = TimedPose{time, transform::Rigid3d{translation, rotation}}; }这里的
cached_extrapolated_pose_可以理解成拿来存储预测某一时刻time对应从 tracking frame 在 local slam 坐标系下的位姿pose这个if条件语句是为了防止重复计算，如果传来的time和预测到的time都相等了，这就是所需要的pose，直接返回
进入if执行第一句代码计算
translation，表示预测tracking frame在local slam坐标系下time时刻的位置，会调用成员函数ExtrapolateTranslation，传入第一个点云的time-
// 返回从最后一个位姿的时间 到time时刻 的tracking frame在local slam坐标系下的平移量 Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) { const TimedPose& newest_timed_pose = timed_pose_queue_.back(); const double extrapolation_delta = common::ToSeconds(time - newest_timed_pose.time); // 使用tracking frame 在 local slam坐标系下的线速度 乘以时间 得到平移量的预测 // 如果不使用里程计就使用通过pose计算出的线速度 if (odometry_data_.size() < 2) { return extrapolation_delta * linear_velocity_from_poses_; } // 如果使用里程计就使用通过里程计计算出的线速度 return extrapolation_delta * linear_velocity_from_odometry_; }这里面的
newest_timed_pose还是{第一帧imu数据的time，{[0,0,0],[0,0,0,1]}}extrapolation_delta表示第一个点云的time与第一帧IMU数据的time之间的差值如果不使用里程计就使用通过pose计算出的线速度来计算位移，目前
linear_velocity_from_poses_为[0,0,0]，所以函数返回值也是[0,0,0] -
newest_timed_pose.pose.translation()的值是[0,0,0]，加上函数返回值[0,0,0]，得到的translation为[0,0,0]
进入if执行第二句代码计算
rotation，表示预测tracking frame在local slam坐标系下time时刻的姿态，会调用成员函数ExtrapolateRotation，传入第一个点云的time和一个类ImuTracker的指针。-
// 计算从imu_tracker到time时刻的姿态变化量 Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation( const common::Time time, ImuTracker* const imu_tracker) const { CHECK_GE(time, imu_tracker->time()); // 更新imu_tracker的状态到time时刻 AdvanceImuTracker(time, imu_tracker); // 通过imu_tracker_获取上一次位姿校准时的姿态 /* 这里要注意，一个是imu_tracker_，一个是imu_tracker imu_tracker_是成员变量，在这里表示保存的上一帧位姿 imu_tracker是函数参数，在这里表示保存的这一时刻的位姿 */ const Eigen::Quaterniond last_orientation = imu_tracker_->orientation(); // 求取上一帧到当前时刻预测出的姿态变化量：上一帧姿态四元数的逆 乘以 当前时刻预测出来的姿态四元数 return last_orientation.inverse() * imu_tracker->orientation(); }首先检查传入的time要大于等于目前ImuTracker已经预测到的time
马上调用成员函数
AdvanceImuTracker-
if (imu_data_.empty() || time < imu_data_.front().time)初始化的时候虽然调用过该函数，但是初始化调用只进入了这个if后就返回了，这里不一样！这里不会进入if！这里
imu_data_已经有三个数据了，传入的time是第三帧IMU数据以后的time，显然这个if就满足了！正常情况下，只有初始化才能进入这个if -
// imu_tracker的时间比imu数据队列中第一个数据的时间早, 就先预测到imu数据队列中第一个数据的时间 if (imu_tracker->time() < imu_data_.front().time) { // Advance to the beginning of 'imu_data_'. imu_tracker->Advance(imu_data_.front().time); }这里表示如果imu_tracker的时间比IMU数据队列中第一个数据的时间都还要早,那就先预测到imu数据队列中第一个数据的时间
这里
imu_tracker->time()和imu_data_.front().time都是第一帧IMU数据的time，因此不满足if -
// 在imu数据队列中找到第一个时间上 大于等于 imu_tracker->time() 的数据的索引 auto it = std::lower_bound( imu_data_.begin(), imu_data_.end(), imu_tracker->time(), [](const sensor::ImuData& imu_data, const common::Time& time) { return imu_data.time < time; });这是C++11的语法，放在这里表示在IMU数据队列中找到第一个时间上大于等于
imu_tracker->time()的数据的索引。这里的it是第一帧IMU数据的索引！
-
// 然后依次对imu数据进行预测, 以及添加观测, 直到imu_data_的时间大于等于time截止 while (it != imu_data_.end() && it->time < time) { // 预测出当前时刻的姿态与重力方向 imu_tracker->Advance(it->time); // 根据线速度的观测,更新重力的方向,并根据重力的方向对上一时刻预测的姿态进行校准 imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration); // 更新角速度观测 imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity); ++it; }这里依次对IMU队列中的数据进行预测, 以及添加观测, 直到
imu_data_的时间大于等于time截止根据我之前的假设，这里while要执行三次，分别传入第一帧、第二帧、第三帧IMU数据的time，加速度，角速度，每次执行完对应函数的类ImuTracker成员变量对应的值如下：
将第一帧IMU数据的time传入
ImuTracker::Advance，所得到的变量（由于初始化的时候介绍每个变量的意义，这里直接写出调用这个函数后的成员变量的值）类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_第一帧IMU数据的time last_linear_acceleration_time_第一帧IMU数据的time orientation_Eigen::Quaterniond::Identity() gravity_vector_Eigen::Vector3d::UnitZ() imu_angular_velocity_Eigen::Vector3d::Zero() 将第一帧IMU数据的加速度传入
ImuTracker::AddImuLinearAccelerationObservation，所得到的变量类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_第一帧IMU数据的time last_linear_acceleration_time_第一帧IMU数据的time orientation_Eigen::Quaterniond::Identity() gravity_vector_Eigen::Vector3d::UnitZ() imu_angular_velocity_Eigen::Vector3d::Zero() 将第一帧IMU数据的角速度传入
ImuTracker::AddImuAngularVelocityObservation，所得到的变量类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_第一帧IMU数据的time last_linear_acceleration_time_第一帧IMU数据的time orientation_Eigen::Quaterniond::Identity() gravity_vector_Eigen::Vector3d::UnitZ() imu_angular_velocity_第一帧IMU数据的角速度 将第二帧IMU数据的time传入
ImuTracker::Advance，所得到的变量(t1表示第一帧IMU数据的time，t2表示第二帧IMU数据的time，以此类推)
(a1表示第一帧IMU数据的加速度，a2表示第二帧IMU数据的加速度，以此类推)
(w1表示第一帧IMU数据的角速度，w2表示第二帧IMU数据的角速度，以此类推)
(r1表示第一帧IMU数据最新的
orientation_，r2表示第二帧IMU数据最新的orientation_，以此类推)(g1表示第一帧IMU数据最新的
gravity_vector_，g2表示第二帧IMU数据最新的gravity_vector_，以此类推)类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_t2 last_linear_acceleration_time_t1 orientation_r2 = w1*(t2-t1) gravity_vector_g2 = [0,0,1]*(r2.conjugate()) imu_angular_velocity_w1 r2* g2 = [0, 0, k](k为任意大于0的实数)将第二帧IMU数据的加速度传入
ImuTracker::AddImuLinearAccelerationObservation，为了方便表示该函数中IMU加速度对gravity_vector_的修正，把修正后gravity_vector_到修正前的gravity_vector_的旋转量rotation都固定为k，而修正前的gravity_vector_到修正后的gravity_vector_则为-k。所得到的变量类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_t2 last_linear_acceleration_time_t2 orientation_r2 = r2 * k gravity_vector_g2 = g2 * (-k) imu_angular_velocity_w1 r2 * g2 = [0, 0, k](k为任意大于0的实数)将第二帧IMU数据的角速度传入
ImuTracker::AddImuAngularVelocityObservation，所得到的变量类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_t2 last_linear_acceleration_time_t2 orientation_r2 gravity_vector_g2 imu_angular_velocity_w2 将第三帧IMU数据的time传入
ImuTracker::Advance，所得到的变量类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_t3 last_linear_acceleration_time_t2 orientation_r3 = r2 * w2(t3-t2) gravity_vector_g3 = g2 * (w2(t3-t2).conjugate()) imu_angular_velocity_w2 r3 * g3 = [0, 0, k](k为任意大于0的实数)将第三帧IMU数据的加速度传入
ImuTracker::AddImuLinearAccelerationObservation，所得到的变量类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_t3 last_linear_acceleration_time_t3 orientation_r3 = r3 * k gravity_vector_g3 = g3 * (-k) imu_angular_velocity_w2 r3 * g3 = [0, 0, k](k为任意大于0的实数)将第三帧IMU数据的角速度传入
ImuTracker::AddImuAngularVelocityObservation，所得到的变量类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_t3 last_linear_acceleration_time_t3 orientation_r3 = r3 * k gravity_vector_g3 = g3 * (-k) imu_angular_velocity_w3 -
// 最后将imu_tracker的状态预测到time时刻 imu_tracker->Advance(time);此时只是预测到IMU数据队列中第三个IMU数据的time，而传入的time是在第三帧和第四帧之间，所以最后将
imu_tracker预测到传入的time时刻，即第一个点云的时间戳！所得到的变量（用
time_first_point表示点云中第一个点的time，rt表示第一个点的orientation_，gt表示第一个点的gravity_vector_类ImuTracker的成员变量 现在的值 imu_gravity_time_constant_10 time_点云的第一个点的time（用time_first_point表示） last_linear_acceleration_time_t3 orientation_rt = r3 * w3(time_first_point - t3) gravity_vector_gt = g3 * (w3(time_first_point - t3).conjugate()) imu_angular_velocity_w3 rt * gt = [0, 0, k](k为任意大于0的实数)
-
-
执行到这里，就返回到
PoseExtrapolator::ExtrapolateRotation继续执行// 通过imu_tracker_获取上一次位姿校准时的姿态 /* 这里要注意，一个是imu_tracker_，一个是imu_tracker imu_tracker_是成员变量，在这里表示保存的上一帧位姿 imu_tracker是函数参数，在这里表示保存的这一时刻的位姿 */ const Eigen::Quaterniond last_orientation = imu_tracker_->orientation(); return last_orientation.inverse() * imu_tracker->orientation();这里有两个
ImuTracker，一个是形参传入的extrapolation_imu_tracker_，在函数中为imu_tracker，也就是刚刚更新预测的ImuTracker，所以它的orientation()表示此刻time，tracking_frame到local slam坐标系的旋转量另一个
ImuTracker是最开始初始化的成员变量imu_tracker_，就是用这个成员变量对extrapolation_imu_tracker_进行的构造！它的orientation()表示上一时刻time，tracking_frame到local slam坐标系的旋转量所以最后将上一时刻time1的旋转量的逆加在这一时刻time2的旋转量上，就可以得到time1到time2这段时间内，tracking_frame在local slam坐标系下的一个姿态变化量！
这里返回的就是IMU第一帧数据的time到点云中第一个点的time时间内，tracking_frame在local slam坐标系下的姿态变化量
-
-
执行到这里，就返回到
PoseExtrapolator::ExtrapolatePose继续执行// 预测tracking frame在local slam坐标系下time时刻的姿态 const Eigen::Quaterniond rotation = newest_timed_pose.pose.rotation() * ExtrapolateRotation(time, extrapolation_imu_tracker_.get());newest_timed_pose.pose.rotation()表示上一时刻time，tracking_frame到local slam坐标系的旋转量，所以这里的rotation表示预测的这一时刻time，tracking_frame在local slam坐标系下time时刻的姿态 -
cached_extrapolated_pose_ = TimedPose{time, transform::Rigid3d{translation, rotation}};用
cached_extrapolated_pose_记录最新预测出来的time和pose -
return cached_extrapolated_pose_.pose;最后返回预测出time时刻下的pose
执行到这里，就返回到LocalTrajectoryBuilder2D::AddRangeData函数中，预测在一帧点云数据中第一个点对应的time下，此刻tracking_frame 在 local slam坐标系下的位姿，后面一帧雷达数据中所有点的处理都一样！
// 预测得到每一个时间点的位姿
for (const auto& range : synchronized_data.ranges) {
common::Time time_point = time + common::FromSeconds(range.point_time.time);
// 如果该时间比上次预测位姿的时间还要早,说明这个点的时间戳往回走了, 就报错
if (time_point < extrapolator_->GetLastExtrapolatedTime()) {
// 一个循环只报一次错
if (!warned) {
LOG(ERROR)
<< "Timestamp of individual range data point jumps backwards from "
<< extrapolator_->GetLastExtrapolatedTime() << " to " << time_point;
warned = true;
}
time_point = extrapolator_->GetLastExtrapolatedTime();
}
// Step: 2 预测出 每个点的时间戳时刻, tracking frame 在 local slam 坐标系下的位姿
range_data_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
}
所以range_data_poses最后存储了一帧点云中每个点相对于上一个帧点云的这段时间内，tracking_frame相对与local slam坐标系的位姿变化！
为什么会费尽心思将tracking_frame坐标系转换到local slam坐标系下，而且记录一帧点云中每个点对应time时刻，tracking_frame相对于local slam坐标系的位姿？
个人理解：从tracking_frame坐标系转换到一个固定的local slam坐标系是因为如果不转换的话，在运动的时候会产生点云畸变！记录一帧点云中每个点对应的time时刻，tracking_frame相对于local slam坐标系的位姿是为了消除掉每个点在运动时产生的点云畸变！
如果不将点云从tracking_frame坐标系转换到一个固定坐标系下，假设激光雷达的频率是10HZ，相当于在0.1s内扫描的点，都属于同一帧点云，在这同一帧点云当中的每一个点都应该相对于同一个tracking_frame坐标系的原点坐标。小车静止的时候确实没问题，但当小车运动起来，tracking_frame坐标系原点是时时刻刻在移动的，就会产生畸变！
假设激光雷达是逆时针旋转，从t1时刻开始新的一帧扫描，小车朝着y轴方向行驶，黑色的直线表示实际的墙体
理想情况下，在运动过程中点云应该呈现黑色点的样子，实际上呈现的是红色的样式，因为tracking_frame的原点在一帧点云扫描过程中发生了变化！
例如图中的t2时刻（还在一帧点云时间内)，小车朝y方向前进了L，实际第9个点到O2的距离会被误以为是到O1的距离，也就导致了该点向前偏移L，也就是图中红色点的位置！
所以cartographer处理的办法就是将一帧中雷达点云的每一个点，本来是以tracking_frame坐标系下激光雷达的中心点（这个点的坐标值是在urdf中设置的激光雷达相对于tracking_frame的坐标）为原点的数据，都统一的转换到一个固定的local slam坐标系下，激光雷达的中心为原点的点云数据。
例如上图中，要将点云数据在运动过程中都以O1为中心。在local slam固定坐标系下，通过
range_data_poses[8]就可以获得tracking_frame坐标系在t2到t1这段时间内的姿态变化量，将其作用于在此刻第9个点相对于O2的坐标，就可以准确得将O2时刻的第9个点转换到以O1为原点的坐标系下！这样就完美解决了点云运动畸变的问题！
接着回到LocalTrajectoryBuilder2D::AddRangeData继续进行执行
if (num_accumulated_ == 0) {
// 'accumulated_range_data_.origin' is uninitialized until the last
// accumulation.
accumulated_range_data_ = sensor::Range Data{{}, {}, {}};
}
这个num_accumulated就是记录几帧有效点云数据，用户通过lua文件可以设置几帧有效点云进行一次扫描匹配！进行扫描匹配前会将这个值重新置为0。
这里得有个提前认知：
从Laserscan传过来的点云数据，以激光雷达中心为原点是一个平面；在tracking_frame坐标系下，z轴的值是等于激光雷达原点相对于tracking_frame坐标系的高度，是在urdf中设置的！
因为tracking_frame相对于local slam可能有旋转量，如果这个旋转量为[0,0,0,1]，那么点云数据其实就是一个平行于local slam坐标系的一个平面！高度为激光雷达原点到local slam原点的距离！
接下来就是最关键的一步：
// 对每个数据点进行处理
for (size_t i = 0; i < synchronized_data.ranges.size(); ++i) {
// 获取在tracking frame 下点的坐标
const sensor::TimedRangefinderPoint& hit =
synchronized_data.ranges[i].point_time;
// 将点云的origins坐标转到 local slam 坐标系下
//(ty：将激光雷达的坐标原点在tracking_frame转换到local slam坐标系)
const Eigen::Vector3f origin_in_local =
range_data_poses[i] *
synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
// Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标
//
sensor::RangefinderPoint hit_in_local =
range_data_poses[i] * sensor::ToRangefinderPoint(hit);
// 计算这个点的距离, 这里用的是去畸变之后的点的距离
const Eigen::Vector3f delta = hit_in_local.position - origin_in_local;
const float range = delta.norm();
// param: min_range max_range
if (range >= options_.min_range()) {
if (range <= options_.max_range()) {
// 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标
accumulated_range_data_.returns.push_back(hit_in_local);
} else {
// Step: 4 超过max_range时的处理: 用一个距离进行替代, 并放入misses里
hit_in_local.position =
origin_in_local +
// param: missing_data_ray_length, 是个比例, 不是距离
options_.missing_data_ray_length() / range * delta;
accumulated_range_data_.misses.push_back(hit_in_local);
}
}
} // end for
-
这里使用的点云数据类型是
TimedPointCloudOriginData，每个变量的详细解释如下// 时间同步后的点云 struct TimedPointCloudOriginData { struct RangeMeasurement { TimedRangefinderPoint point_time; // 带时间戳的单个数据点的坐标 xyz float intensity; // 强度值（ty：默认为0） size_t origin_index; // （ty：属于哪个激光雷达。假设有两个激光雷达，该值等于0 表示第一个，等于1表示第二个。因为每个激光雷达中 心相对于tracking_frame的位置不同，所以 origins不 同，需要用下标记录，当然如果使用一个激光雷达该 值就是0） }; common::Time time; // 点云的时间（ty：最后一个点的时间戳） std::vector<Eigen::Vector3f> origins; //激光雷达中心相对于tracking_frame原点的坐标 // （ty：时间同步后的点云可能不光只有一个激光雷达的数据，origins记录单个激光雷达转换到tracking_frame的变换关系，不同的激光雷达origins不同，通过一帧点云中每个点的origin_index，区分这个点合并前是属于哪个激光雷达的点云数据，从而存储的origins就是每个点对应激光雷达转换tracking_frame的变换关系） std::vector<RangeMeasurement> ranges; // 数据点的集合 }; -
// 获取在tracking frame 下点的坐标 const sensor::TimedRangefinderPoint& hit = synchronized_data.ranges[i].point_time;hit是在一帧点云数据中，每个点在tracking_frame坐标系下的坐标与时间戳，也就是之前画图举例的O2到第9个点的位移。 -
// 将点云的origins坐标转到 local slam 坐标系下 //(ty：将激光雷达的坐标原点在tracking_frame转换到local slam坐标系) const Eigen::Vector3f origin_in_local = range_data_poses[i] * synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);因为时间同步后的点云以time大小重新进行了排序，所以cartographer用
synchronized_data.ranges[i].origin_index记录了每个点是属于合并前哪个激光雷达的点云数据。- 如果只使用一个激光雷达，那么
synchronized_data.ranges[i].origin_index全部都为0 - 如果使用n个的激光雷达，那么
synchronized_data.ranges[i].origin_index范围是[0, n-1]
将一帧点云中，某个点的time时刻下，预测tracking_frame坐标系到local slam坐标系下的位姿变化作用在原来激光雷达中心相对于tracking_frame坐标系下的坐标，就可以获得激光雷达中心在local slam坐标系下的坐标
origin_in_local，也就是之前画图举例的O1O2。 - 如果只使用一个激光雷达，那么
-
// Step: 3 运动畸变的去除, 将相对于tracking_frame的hit坐标 转成 local坐标系下的坐标 sensor::RangefinderPoint hit_in_local = range_data_poses[i] * sensor::ToRangefinderPoint(hit);同理，将一帧点云中，某个点的time时刻下，预测tracking_frame到local slam坐标系下的位姿变化作用在原来这个点在tracking_frame坐标系下的坐标，就可以得到去掉点云畸变以后的坐标信息，也就是之前画图举例O1到第9个点的位移。
-
// 计算这个点的距离, 这里用的是去畸变之后的点的距离 const Eigen::Vector3f delta = hit_in_local.position - origin_in_local; const float range = delta.norm();hit_in_local.position是local slam坐标系下一帧点云数据中，该点到local slam原点的坐标origin_in_local是local slam坐标系下，该点的激光雷达原点到local slam原点的坐标两者的差值就是在local坐标系下，该点到激光雷达原点的坐标
前面提过，这里点云数据以激光雷达为原点就是一个平面
所以
range就很好理解，就是这个平面内，距离激光雷达原点的距离 -
// param: min_range max_range if (range >= options_.min_range()) { if (range <= options_.max_range()) { // 在这里可以看到, returns里保存的是local slam下的去畸变之后的点的坐标 accumulated_range_data_.returns.push_back(hit_in_local); } else { // Step: 4 超过max_range时的处理: 用一个距离进行替代, 并放入misses里 hit_in_local.position = origin_in_local + // param: missing_data_ray_length, 是个比例, 不是距离 options_.missing_data_ray_length() / range * delta; accumulated_range_data_.misses.push_back(hit_in_local); } }options_.min_range()和range <= options_.max_range()都是在lua文件里设置的可以看到，范围小于
options_.min_range()的点，cartographer直接丢弃了accumulated_range_data_的是结构体RangeData，定义如下：/** * @brief local_slam_data中存储所有雷达点云的数据结构 * * @param origin 点云的原点在local坐标系下的坐标 * @param returns 所有雷达数据点在local坐标系下的坐标, 记为returns, 也就是hit * @param misses 是在光线方向上未检测到返回的点(nan, inf等等)或超过最大配置距离的点 */ struct RangeData { Eigen::Vector3f origin; PointCloud returns; PointCloud misses; // local坐标系下的坐标 };当
range在options_.min_range()和options_.max_range()以内，将点存入到returns中accumulated_range_data_.returns中当range大于
options_.max_range()，delta/range其实就是将delta这个向量的x，y值作归一化，再乘以一个lua文件中设置的参数options_.missing_data_ray_length()，获取了一个新的x，y的值，最后将其加上激光雷达原点到local slam原点的坐标得到自定义的一个值hit_in_local.position，将其存入accumulated_range_data_.misses中// 有一帧有效的数据了 ++num_accumulated_;都执行到这里，相当于这一帧点云是符合扫描匹配的要求！将对应的
num_accumulated_加1// param: num_accumulated_range_data 几帧有效的点云数据进行一次扫描匹配 if (num_accumulated_ >= options_.num_accumulated_range_data()) { // 计算2次有效点云数据的的时间差 const common::Time current_sensor_time = synchronized_data.time; absl::optional<common::Duration> sensor_duration; if (last_sensor_time_.has_value()) { sensor_duration = current_sensor_time - last_sensor_time_.value(); } last_sensor_time_ = current_sensor_time; // 重置变量 num_accumulated_ = 0; // 获取机器人当前姿态 const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( extrapolator_->EstimateGravityOrientation(time)); // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time // 'time'. // 以最后一个点的时间戳估计出的坐标为这帧数据的原点 accumulated_range_data_.origin = range_data_poses.back().translation(); return AddAccumulatedRangeData( time, // 将点云变换到local原点处, 且姿态为0 TransformToGravityAlignedFrameAndFilter( gravity_alignment.cast<float>() * range_data_poses.back().inverse(), accumulated_range_data_), gravity_alignment, sensor_duration); }-
options_.num_accumulated_range_data()是lua文件中设置的，默认是有一帧有效点云就进行一次扫描匹配如果有两个不同的激光雷达，这里要设置成大于等于2才行
-
// 计算2次有效点云数据的的时间差 const common::Time current_sensor_time = synchronized_data.time; absl::optional<common::Duration> sensor_duration; if (last_sensor_time_.has_value()) { sensor_duration = current_sensor_time - last_sensor_time_.value(); } last_sensor_time_ = current_sensor_time; // 重置变量 num_accumulated_ = 0;获取这次进行扫描匹配的时间
current_sensor_time的与上次进行扫描匹配的时间last_sensor_time_的时间差sensor_duration将
num_accumulated_重置，以便于重复记录有效的点云帧 -
// 获取机器人当前姿态 const transform::Rigid3d gravity_alignment = transform::Rigid3d::Rotation( extrapolator_->EstimateGravityOrientation(time)); // TODO(gaschler): This assumes that 'range_data_poses.back()' is at time // 'time'. // 以最后一个点的时间戳估计出的坐标为这帧数据的原点 accumulated_range_data_.origin = range_data_poses.back().translation();这里传入的
time是这一帧点云最后一个点的time，所以extrapolator_->EstimateGravityOrientation(time)返回的是最后一个点对应的time下，tracking_frame坐标系相对于local slam坐标系的旋转量rotation。static Rigid3 Rotation(const Quaternion& rotation) { return Rigid3(Vector::Zero(), rotation); }通过
transform::Rigid3d::Rotation将rotation转换成Rigid3类gravity_alignment。gravity_alignment对应的translation是[0,0,0]，gravity_alignment对应的rotation是最后一个点对应的time下，tracking_frame坐标系相对于local slam坐标系的旋转量。range_data_poses.back().translation()是这一帧点云最后一个点的time下，对应的tracking_frame坐标系在local slam坐标系的坐标，将其存入到accumulated_range_data_.origin，这样accumulated_range_data_的赋值就全部完成了！ -
return AddAccumulatedRangeData( time, // 将点云变换到local原点处, 且姿态为0 TransformToGravityAlignedFrameAndFilter( gravity_alignment.cast<float>() * range_data_poses.back().inverse(), accumulated_range_data_), gravity_alignment, sensor_duration); }这里最重要的就是传入了一个
gravity_alignment.cast<float>() * range_data_poses.back().inverse()-
range_data_poses.back()就是取得一帧点云中最后一个点time下，tracking_frame坐标系相对于local slam坐标系的位姿，这里对其求逆// T = [R t] T^-1 = [R^-1 -R^-1 * t] // [0 1] [0 1 ] // R是旋转矩阵, 特殊正交群, 所以R^-1 = R^T Rigid3 inverse() const { const Quaternion rotation = rotation_.conjugate(); const Vector translation = -(rotation * translation_); return Rigid3(translation, rotation); }所获得的新的Rigid3表示在一帧点云中最后一个time下，local slam坐标系相对于tracking_frame坐标系的位姿，对应的translation是local slam坐标系原点在tracking_frame坐标系原点下的相对坐标，对应的rotation是local slam坐标系相对于tracking_frame坐标系的旋转量。
前面提到，
gravity_alignment对应的translation是[0,0,0]，gravity_alignment对应的rotation是tracking_frame坐标系相对于local slam坐标系的旋转量。// lhs是全局坐标系下的位姿, rhs是全局坐标系下的坐姿变动量 // lhs.rotation() * rhs.translation() + lhs.translation() 的意思是 // 将 rhs 转换成 lhs自身坐标系下的位姿变动量 再与lhs的坐标相加 // 得到 lhs 在全局坐标系下的新的位姿 template <typename FloatType> Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs, const Rigid3<FloatType>& rhs) { return Rigid3<FloatType>( lhs.rotation() * rhs.translation() + lhs.translation(), (lhs.rotation() * rhs.rotation()).normalized()); }gravity_alignment.cast<float>() * range_data_poses.back().inverse()获得的Rigid3的旋转量取模运算后对应是rotation就是[0,0,0,1]，此刻tracking_frame坐标系与local slam坐标系之间通过平移就可以重合，而translation对应的就是这个平移关系，是local slam坐标系原点相对于tracking_frame坐标系！前面也提过，当旋转量为[0,0,0,1]，现在点云数据其实就是一个平行于local slam坐标系的一个平面！高度为激光雷达原点到local slam原点的距离！
-
-
浙公网安备 33010602011771号