开源代码：kiss-icp安装与运行 ubuntu20.04+ros noetic
前言
kiss-icp为一种纯激光雷达里程计方法，适配多种数据集
源码地址：https://github.com/PRBonn/kiss-icp
一、环境
ubuntu20.04
ROS noetic
二、复现步骤
2.1 创建工作空间
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws
catkin_make
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc2.2 克隆代码
cd ~/catkin_ws/src
git clone https://github.com/PRBonn/kiss-icp
cd ..
catkin_make2.3 数据集
该算法适用于多种激光雷达，包括Velodyne、Ouster、Livox，可使用公开数据集或自行采集数据。
Livox官方数据集：https://www.livoxtech.com/cn/dataset
LIO-SAM Public Dataset：https://drive.google.com/drive/folders/1gJHwfdHCRdjP7vuT556pv8atqrCJPbUq
2.4 运行kiss-icp
roslaunch kiss_icp odometry.launch topic:=
rosbag play *.bag 2.5 建图
campus_small_dataset.bag：
三、框架
1、使用等速模型进行纠偏
2、使用体素图模型对点云进行采样
3、寻找对应点时引入自适应阈值
4、使用哈希表寻找帧到地图的对应点关系
5、执行ICP优化，得到位姿估计
四、代码解读
4.1 配置文件
存于~/src/kiss-icp/config中，包括advanced.yaml（示例）和basic.yaml
#basic.yaml
out_dir: "results"
data:
deskew: False
preprocess: True
max_range: 100.0 # can be also changed in the CLI
min_range: 5.0
mapping:
max_points_per_voxel: 20
adaptive_threshold:
initial_threshold: 2.0
min_motion_th: 0.14.2 核心代码
主要分为pipeline中的KissICP.cpp和core中的Deskew.cpp、Preprocessing.cpp、Registration.cpp、Threshold.cpp和VoxelHashMap.cpp
4.2.1 KissICP.cpp
#include "KissICP.hpp"
#include
#include
#include
#include "kiss_icp/core/Deskew.hpp"
#include "kiss_icp/core/Preprocessing.hpp"
#include "kiss_icp/core/Registration.hpp"
#include "kiss_icp/core/VoxelHashMap.hpp"
namespace kiss_icp::pipeline {
//对带有时间戳的输入点云数据进行去畸变操作
//frame：一帧点云数据；timestamps：点云数据时间戳
//返回不带时间戳的去畸变后的点云
KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector &frame,
const std::vector ×tamps) {
const auto &deskew_frame = [&]() -> std::vector {
if (!config_.deskew) return frame;
// TODO(Nacho) Add some asserts here to sanitize the timestamps
// If not enough poses for the estimation, do not de-skew
const size_t N = poses().size();
if (N <= 2) return frame;
// Estimate linear and angular velocities
const auto &start_pose = poses_[N - 2];
const auto &finish_pose = poses_[N - 1];
return DeSkewScan(frame, timestamps, start_pose, finish_pose);
}();
return RegisterFrame(deskew_frame);
}
//对去畸变后的点云数据进行处理
KissICP::Vector3dVectorTuple KissICP::RegisterFrame(const std::vector &frame) {
// Preprocess the input cloud
// 去除阈值外的点，处理完的点云存入cropped_frame
const auto &cropped_frame = Preprocess(frame, config_.max_range, config_.min_range);
// Voxelize体素化
// source存入两次下采样结果，用于ICP配准
// frame_downsample存入一次下采样结果
const auto &[source, frame_downsample] = Voxelize(cropped_frame);
// Get motion prediction and adaptive_threshold
const double sigma = GetAdaptiveThreshold();
// Compute initial_guess for ICP
// 计算倒数第二个位姿和最后一个位姿之间的相对位姿变换
const auto prediction = GetPredictionModel();
// 如果poses_不为空，返回容器中的最后一个位姿；为空，返回单位位姿，即没有旋转和平移的初始位姿
const auto last_pose = !poses_.empty() ? poses_.back() : Sophus::SE3d();
const auto initial_guess = last_pose * prediction;
// Run icp
// 进行当前帧相对于局部地图的配准，将配准后的新位姿赋值给new_pose
const Sophus::SE3d new_pose = kiss_icp::RegisterFrame(source,
local_map_,
initial_guess,
3.0 * sigma,
sigma / 3.0);
const auto model_deviation = initial_guess.inverse() * new_pose;
//根据配准结果自适应调整算法的阈值
adaptive_threshold_.UpdateModelDeviation(model_deviation);
//将配准后的新帧位姿new_pose与局部地图local_map_进行更新，VoxelHashMap中
local_map_.Update(frame_downsample, new_pose);
//将配准后的新帧位姿new_pose添加到一个保存所有帧位姿的容器poses_中
poses_.push_back(new_pose);
//返回包含原始帧frame和配准后的帧source
return {frame, source};
}
KissICP::Vector3dVectorTuple KissICP::Voxelize(const std::vector &frame) const {
const auto voxel_size = config_.voxel_size;
//一次下采样
const auto frame_downsample = kiss_icp::VoxelDownsample(frame, voxel_size * 0.5);
//两次下采样
const auto source = kiss_icp::VoxelDownsample(frame_downsample, voxel_size * 1.5);
return {source, frame_downsample};
}
//判断是否有足够的运动
//如果没有足够的运动，则返回预先设定的初始阈值；如果有足够的运动，则返回自适应阈值
double KissICP::GetAdaptiveThreshold() {
if (!HasMoved()) {
return config_.initial_threshold;
}
return adaptive_threshold_.ComputeThreshold();
}
Sophus::SE3d KissICP::GetPredictionModel() const {
Sophus::SE3d pred = Sophus::SE3d();
const size_t N = poses_.size();
//N < 2，直接返回单位位姿
if (N < 2) return pred;
return poses_[N - 2].inverse() * poses_[N - 1];
}
//判断位姿序列中的首尾两个位姿之间的运动量是否足够大
bool KissICP::HasMoved() {
if (poses_.empty()) return false;
// poses_.front().inverse() * poses_.back()：两个位姿之间的相对位姿变换
// ！！.translation().norm()：获取相对位姿变换的平移部分的范数，即表示两个位姿之间的运动量
const double motion = (poses_.front().inverse() * poses_.back()).translation().norm();
return motion > 5.0 * config_.min_motion_th;
}
} // namespace kiss_icp::pipeline
4.2.2 Deskew.cpp
对原始点云进行运动补偿
#include "Deskew.hpp"
#include
#include
#include
#include
namespace {
/// TODO(Nacho) Explain what is the very important meaning of this param
constexpr double mid_pose_timestamp{0.5};
} // namespace
namespace kiss_icp {
std::vector DeSkewScan(const std::vector &frame,
const std::vector ×tamps,
const Sophus::SE3d &start_pose,
const Sophus::SE3d &finish_pose) {
//计算起始位姿和结束位姿之间的位姿差异，使用.log()将其转换为自然对数(e)形式，以便后续插值操作
const auto delta_pose = (start_pose.inverse() * finish_pose).log();
//创建一个和输入frame同样大小的空向量，用于存储去畸变后的点云数据
std::vector corrected_frame(frame.size());
//并行循环，对输入点云数据进行遍历并执行去畸变操作
tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
//计算时间戳在timestamps[i]处对应的位姿变换
//？？？？？mid_pose_timestamp为什么取0.5
const auto motion = Sophus::SE3d::exp((timestamps[i] - mid_pose_timestamp) * delta_pose);
corrected_frame[i] = motion * frame[i];
});
return corrected_frame;
}
} // namespace kiss_icp 4.2.3 Preprocessing.cpp
包括对点云数据进行下采样及滤除范围外的点
#include "Preprocessing.hpp"
#include
#include
#include
#include
#include
#include
#include
//定义VoxelHash，使用了名为Voxel的结构体和名为VoxelHash的哈希函数
//将Voxel结构体对象映射为哈希值
namespace {
using Voxel = Eigen::Vector3i;
struct VoxelHash {
size_t operator()(const Voxel &voxel) const {
const uint32_t *vec = reinterpret_cast(voxel.data());
return ((1 << 20) - 1) & (vec[0] * 73856093 ^ vec[1] * 19349663 ^ vec[2] * 83492791);
}
};
} // namespace
//VoxelDownsample函数，用于对输入的点云数据 进行体素下采样操作
namespace kiss_icp {
std::vector VoxelDownsample(const std::vector &frame,
double voxel_size) {
//定义名为grid的robin_map数据结构，用于存储体素化后的点云数据
tsl::robin_map grid;
//预分配哈希表grid的存储空间
grid.reserve(frame.size());
//遍历输入的点云数据frame中的每个点point
for (const auto &point : frame) {
//计算体素索引voxel
const auto voxel = Voxel((point / voxel_size).cast());
//如果体素索引voxel在哈希表grid中不存在，将该体素索引voxel作为键，将对应的点云数据point作为值，插入到哈希表中
if (grid.contains(voxel)) continue;
grid.insert({voxel, point});
}
std::vector frame_dowsampled;
frame_dowsampled.reserve(grid.size());
for (const auto &[voxel, point] : grid) {
(void)voxel;
//将体素化后的点云数据point添加到frame_downsampled向量中
frame_dowsampled.emplace_back(point);
}
return frame_dowsampled;
}
std::vector Preprocess(const std::vector &frame,
double max_range,
double min_range) {
std::vector inliers;
//遍历输入的点云数据frame，将满足特定条件的点复制到向量inliers中
std::copy_if(frame.cbegin(), frame.cend(), std::back_inserter(inliers), [&](const auto &pt) {
//计算当前点pt的范数，即点的欧几里得距离
const double norm = pt.norm();
return norm < max_range && norm > min_range;
});
return inliers;
}
std::vector CorrectKITTIScan(const std::vector &frame) {
constexpr double VERTICAL_ANGLE_OFFSET = (0.205 * M_PI) / 180.0;
std::vector corrected_frame(frame.size());
tbb::parallel_for(size_t(0), frame.size(), [&](size_t i) {
const auto &pt = frame[i];
const Eigen::Vector3d rotationVector = pt.cross(Eigen::Vector3d(0., 0., 1.));
corrected_frame[i] =
Eigen::AngleAxisd(VERTICAL_ANGLE_OFFSET, rotationVector.normalized()) * pt;
});
return corrected_frame;
}
} // namespace kiss_icp 4.2.4 Threshold.cpp
计算自适应阈值
#include "Threshold.hpp"
#include
#include
namespace {
double ComputeModelError(const Sophus::SE3d &model_deviation, double max_range) {
//根据旋转矩阵计算旋转角度theta
const double theta = Eigen::AngleAxisd(model_deviation.rotationMatrix()).angle();
//计算旋转偏差
const double delta_rot = 2.0 * max_range * std::sin(theta / 2.0);
//计算平移偏差
const double delta_trans = model_deviation.translation().norm();
//将旋转偏差和平移偏差相加，得到模型相对于参考位姿的配准误差
return delta_trans + delta_rot;
}
} // namespace
namespace kiss_icp {
//根据收集的有足够运动的样本计算自适应阈值
double AdaptiveThreshold::ComputeThreshold() {
double model_error = ComputeModelError(model_deviation_, max_range_);
if (model_error > min_motion_th_) {
model_error_sse2_ += model_error * model_error;
num_samples_++;
}
if (num_samples_ < 1) {
return initial_threshold_;
}
return std::sqrt(model_error_sse2_ / num_samples_);
}
} // namespace kiss_icp 4.2.5 Registration.cpp
进行帧与局部地图的配准，得到新位姿
#include "Registration.hpp"
#include
#include
#include
#include
#include
#include
#include
namespace Eigen {
using Matrix6d = Eigen::Matrix;
using Matrix3_6d = Eigen::Matrix;
using Vector6d = Eigen::Matrix;
} // namespace Eigen
namespace {
inline double square(double x) { return x * x; }
struct ResultTuple {
ResultTuple() {
JTJ.setZero();
JTr.setZero();
}
ResultTuple operator+(const ResultTuple &other) {
this->JTJ += other.JTJ;
this->JTr += other.JTr;
return *this;
}
Eigen::Matrix6d JTJ;
Eigen::Vector6d JTr;
};
//对点云数据进行坐标变换
void TransformPoints(const Sophus::SE3d &T, std::vector &points) {
std::transform(points.cbegin(), points.cend(), points.begin(),
[&](const auto &point) { return T * point; });
}
//将源点云src对齐到目标点云tgt，得到位姿估计结果estimation
Sophus::SE3d AlignClouds(const std::vector &source,
const std::vector &target,
double th) {
//计算每个点对应的雅克比矩阵Jacobian和残差residual
auto compute_jacobian_and_residual = [&](auto i) {
const Eigen::Vector3d residual = source[i] - target[i];
Eigen::Matrix3_6d J_r;//定义6x6的矩阵J_r，用于存储雅克比矩阵，对应李群SE(3)的六维切空间
//给雅克比矩阵的左上角3x3子矩阵赋值为3x3的单位矩阵，即Identity
J_r.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity();
//给雅克比矩阵的右上角3x3子矩阵赋值
//hat操作：将source[i]向量转换为反对称矩阵
J_r.block<3, 3>(0, 3) = -1.0 * Sophus::SO3d::hat(source[i]);
return std::make_tuple(J_r, residual);
};
//并行化函数，将点云的范围分成若干块，使用多个线程同时计算不同块的雅克比矩阵和残差，并将结果进行累积
const auto &[JTJ, JTr] = tbb::parallel_reduce(
// Range
tbb::blocked_range{0, source.size()},
// Identity
ResultTuple(),
// 1st Lambda: Parallel computation
// 接受blocked_range对象r作为输入，包含了该线程负责处理的点云索引范围
// ResultTuple J用于存储当前线程内部的雅克比矩阵和残差的私有结果，使用引用&以便在并行计算中对其进行累积
[&](const tbb::blocked_range &r, ResultTuple J) -> ResultTuple {
//计算每个点的加权系数，根据残差的大小决定权重
auto Weight = [&](double residual2) { return square(th) / square(th + residual2); };
auto &[JTJ_private, JTr_private] = J;
for (auto i = r.begin(); i < r.end(); ++i) {
const auto &[J_r, residual] = compute_jacobian_and_residual(i);
const double w = Weight(residual.squaredNorm());
JTJ_private.noalias() += J_r.transpose() * w * J_r;
JTr_private.noalias() += J_r.transpose() * w * residual;
}
return J;
},
// 2nd Lambda: Parallel reduction of the private Jacboians
// 将不同线程的私有结果进行累积，得到最终的JTJ和JTr
[&](ResultTuple a, const ResultTuple &b) -> ResultTuple { return a + b; });
// 计算线性方程组的最小二乘解，使用Cholesky分解ldlt()求解线性系统
const Eigen::Vector6d x = JTJ.ldlt().solve(-JTr);
// 使用Sophus库计算了6维向量x的指数映射，得到SE(3)群中的一个元素
return Sophus::SE3d::exp(x);
}
constexpr int MAX_NUM_ITERATIONS_ = 500; //迭代最大次数
constexpr double ESTIMATION_THRESHOLD_ = 0.0001;
} // namespace
namespace kiss_icp {
Sophus::SE3d RegisterFrame(const std::vector &frame,
const VoxelHashMap &voxel_map,
const Sophus::SE3d &initial_guess,
double max_correspondence_distance,
double kernel) {
if (voxel_map.Empty()) return initial_guess;
// Equation (9)
// 使用预测模型将点云数据转换到全局坐标系
std::vector source = frame;
TransformPoints(initial_guess, source);
// ICP迭代
Sophus::SE3d T_icp = Sophus::SE3d();//表示ICP迭代过程中的累积位姿变换
for (int j = 0; j < MAX_NUM_ITERATIONS_; ++j) {
// Equation (10)
// 获取当前迭代步骤中源点云source和目标点云tgt之间的对应点对
const auto &[src, tgt] = voxel_map.GetCorrespondences(source, max_correspondence_distance);
// Equation (11)
// 将源点云src对齐到目标点云tgt，得到位姿估计结果estimation
auto estimation = AlignClouds(src, tgt, kernel);
// Equation (12)
// 根据位姿估计结果estimation对源点云source进行坐标变换
TransformPoints(estimation, source);
// Update iterations
T_icp = estimation * T_icp;
// Termination criteria
if (estimation.log().norm() < ESTIMATION_THRESHOLD_) break;
}
// Spit the final transformation
return T_icp * initial_guess;
}
} // namespace kiss_icp 4.2.6 VoxelHashMap.cpp
包括了大部分的体素图处理步骤
#include "VoxelHashMap.hpp"
#include
#include
#include
#include
#include
#include
#include
#include
// This parameters are not intended to be changed, therefore we do not expose it
namespace {
struct ResultTuple {
ResultTuple(std::size_t n) {
source.reserve(n);
target.reserve(n);
}
std::vector source;//存储点云数据中的源点
std::vector target;//存储点云数据中的目标点
};
} // namespace
namespace kiss_icp {
//获取当前迭代步骤中源点云source和目标点云tgt之间的对应点对
VoxelHashMap::Vector3dVectorTuple VoxelHashMap::GetCorrespondences(
const Vector3dVector &points, double max_correspondance_distance) const {
// Lambda Function to obtain the KNN of one point, maybe refactor
// 创建Lambda函数GetClosestNeighboor，用于获取给定点point的最近邻点
auto GetClosestNeighboor = [&](const Eigen::Vector3d &point) {
//将点point的x坐标除以voxel_size_得到的浮点数转换为整数kx，用于确定点所在的x轴格子的索引
auto kx = static_cast(point[0] / voxel_size_);
auto ky = static_cast(point[1] / voxel_size_);
auto kz = static_cast(point[2] / voxel_size_);
std::vector voxels;//用于存储与point相邻的格子（体素）的索引
voxels.reserve(27); //为voxels容器预留27个元素的空间，在三维空间中，一个点的3x3x3邻域一共有27个格子
for (int i = kx - 1; i < kx + 1 + 1; ++i) {
for (int j = ky - 1; j < ky + 1 + 1; ++j) {
for (int k = kz - 1; k < kz + 1 + 1; ++k) {
voxels.emplace_back(i, j, k);//构成与点point相邻的所有格子
}
}
}
using Vector3dVector = std::vector;
Vector3dVector neighboors;
neighboors.reserve(27 * max_points_per_voxel_);//为neighboors容器预留空间，预计容器中可能存储的点数
//遍历voxels容器中的每个相邻体素
std::for_each(voxels.cbegin(), voxels.cend(), [&](const auto &voxel) {
auto search = map_.find(voxel);//在map_中查找指定的相邻体素voxel
if (search != map_.end()) {
//获取相邻体素voxel对应的点云数据，存储在points变量中
const auto &points = search->second.points;
if (!points.empty()) {
//遍历相邻体素voxel对应的点云数据points，并将其中的每个点添加到neighboors容器中
for (const auto &point : points) {
neighboors.emplace_back(point);
}
}
}
});
Eigen::Vector3d closest_neighbor;
double closest_distance2 = std::numeric_limits::max();
//遍历neighboors容器中的每个点，查找最近的邻点
std::for_each(neighboors.cbegin(), neighboors.cend(), [&](const auto &neighbor) {
double distance = (neighbor - point).squaredNorm();
if (distance < closest_distance2) {
closest_neighbor = neighbor;
closest_distance2 = distance;
}
});
return closest_neighbor;
};
using points_iterator = std::vector::const_iterator;
//对点云数据进行并行化处理，并将结果存储在source和target中
const auto [source, target] = tbb::parallel_reduce(
// Range 要处理的点云数据的范围
tbb::blocked_range{points.cbegin(), points.cend()},
// Identity
ResultTuple(points.size()),
// 1st lambda: Parallel computation
// 并行计算点云数据中每个点的最近邻点对
[max_correspondance_distance, &GetClosestNeighboor](
const tbb::blocked_range &r, ResultTuple res) -> ResultTuple {
auto &[src, tgt] = res;//存储每个点与其最近邻点的对应关系
src.reserve(r.size());
tgt.reserve(r.size());
//遍历范围r中的每个点
for (const auto &point : r) {
Eigen::Vector3d closest_neighboors = GetClosestNeighboor(point);
if ((closest_neighboors - point).norm() < max_correspondance_distance) {
src.emplace_back(point);
tgt.emplace_back(closest_neighboors);
}
}
return res;//包含了每个点与其最近邻点的对应关系
},
// 2nd lambda: Parallel reduction
// 并行化合并
[](ResultTuple a, const ResultTuple &b) -> ResultTuple {
auto &[src, tgt] = a;
const auto &[srcp, tgtp] = b;
//将第二个结果对象b中的srcp容器中的数据移动插入到src容器的末尾
src.insert(src.end(), //
std::make_move_iterator(srcp.begin()), std::make_move_iterator(srcp.end()));
//将第二个结果对象b中的tgtp容器中的数据移动插入到tgt容器的末尾
tgt.insert(tgt.end(), //
std::make_move_iterator(tgtp.begin()), std::make_move_iterator(tgtp.end()));
return a; //返回合并后的结果对象a
});
//返回合并后的源点和目标点
return std::make_tuple(source, target);
}
std::vector VoxelHashMap::Pointcloud() const {
std::vector points;
points.reserve(max_points_per_voxel_ * map_.size());
for (const auto &[voxel, voxel_block] : map_) {
(void)voxel;
for (const auto &point : voxel_block.points) {
points.push_back(point);
}
}
return points;
}
// 更新体素哈希图，保持最新的点云数据
void VoxelHashMap::Update(const Vector3dVector &points, const Eigen::Vector3d &origin) {
AddPoints(points);
RemovePointsFarFromLocation(origin);
}
//对点云数据进行坐标系转换
void VoxelHashMap::Update(const Vector3dVector &points, const Sophus::SE3d &pose) {
Vector3dVector points_transformed(points.size());
std::transform(points.cbegin(), points.cend(), points_transformed.begin(),
[&](const auto &point) { return pose * point; });
//获取位姿pose的平移向量（原点？），并将其存储在origin
const Eigen::Vector3d &origin = pose.translation();
Update(points_transformed, origin);
}
// 通过遍历一组3D点，将它们根据体素坐标添加到体素哈希映射中
// 如果已经存在相同体素坐标的体素，则将点添加到对应的体素块中，否则创建一个新的体素块并插入到哈希映射中
void VoxelHashMap::AddPoints(const std::vector &points) {
std::for_each(points.cbegin(), points.cend(), [&](const auto &point) {
auto voxel = Voxel((point / voxel_size_).template cast());
//在map_中查找具有体素坐标voxel的键值对
//如果找到了匹配的体素，则返回指向该体素坐标的迭代器，否则返回map_.end()
auto search = map_.find(voxel);
if (search != map_.end()) {
auto &voxel_block = search.value();
voxel_block.AddPoint(point);
} else {
map_.insert({voxel, VoxelBlock{{point}, max_points_per_voxel_}});
}
});
}
//移除距离原点origin很远的点云数据
void VoxelHashMap::RemovePointsFarFromLocation(const Eigen::Vector3d &origin) {
for (const auto &[voxel, voxel_block] : map_) {
//从当前迭代的体素块中获取存储的点云数据，pt代表该体素块中的第一个点
const auto &pt = voxel_block.points.front();
const auto max_distance2 = max_distance_ * max_distance_;
if ((pt - origin).squaredNorm() > (max_distance2)) {
map_.erase(voxel);
}
}
}
} // namespace kiss_icp 4.3 其余代码
metrics文件夹中的Metrics.cpp，用于计算KITTI数据集姿态估计的误差指标
#include "Metrics.hpp"
#include
#include
#include
#include
#include
#include
// All this not so beatifull C++ functions are taken from kitti-dev-kit
namespace {
double lengths[] = {100, 200, 300, 400, 500, 600, 700, 800};
int32_t num_lengths = 8;
struct errors {
int32_t first_frame;
double r_err;
double t_err;
double len;
double speed;
errors(int32_t first_frame, double r_err, double t_err, double len, double speed)
: first_frame(first_frame), r_err(r_err), t_err(t_err), len(len), speed(speed) {}
};
std::vector TrajectoryDistances(const std::vector &poses) {
std::vector dist;
dist.push_back(0);
for (uint32_t i = 1; i < poses.size(); i++) {
const Eigen::Matrix4d &P1 = poses[i - 1];
const Eigen::Matrix4d &P2 = poses[i];
double dx = P1(0, 3) - P2(0, 3);
double dy = P1(1, 3) - P2(1, 3);
double dz = P1(2, 3) - P2(2, 3);
dist.push_back(dist[i - 1] + std::sqrt(dx * dx + dy * dy + dz * dz));
}
return dist;
}
int32_t LastFrameFromSegmentLength(const std::vector &dist,
int32_t first_frame,
double len) {
for (uint32_t i = first_frame; i < dist.size(); i++) {
if (dist[i] > dist[first_frame] + len) {
return i;
}
}
return -1;
}
double RotationError(const Eigen::Matrix4d &pose_error) {
double a = pose_error(0, 0);
double b = pose_error(1, 1);
double c = pose_error(2, 2);
double d = 0.5 * (a + b + c - 1.0);
return std::acos(std::max(std::min(d, 1.0), -1.0));
}
double TranslationError(const Eigen::Matrix4d &pose_error) {
double dx = pose_error(0, 3);
double dy = pose_error(1, 3);
double dz = pose_error(2, 3);
return std::sqrt(dx * dx + dy * dy + dz * dz);
}
std::vector CalcSequenceErrors(const std::vector &poses_gt,
const std::vector &poses_result) {
// error vector
std::vector err;
// parameters
int32_t step_size = 10; // every second
// pre-compute distances (from ground truth as reference)
std::vector dist = TrajectoryDistances(poses_gt);
// for all start positions do
for (uint32_t first_frame = 0; first_frame < poses_gt.size(); first_frame += step_size) {
// for all segment lengths do
for (int32_t i = 0; i < num_lengths; i++) {
// current length
double len = lengths[i];
// compute last frame
int32_t last_frame = LastFrameFromSegmentLength(dist, first_frame, len);
// continue, if sequence not long enough
if (last_frame == -1) {
continue;
}
// compute rotational and translational errors
Eigen::Matrix4d pose_delta_gt = poses_gt[first_frame].inverse() * poses_gt[last_frame];
Eigen::Matrix4d pose_delta_result =
poses_result[first_frame].inverse() * poses_result[last_frame];
Eigen::Matrix4d pose_error = pose_delta_result.inverse() * pose_delta_gt;
double r_err = RotationError(pose_error);
double t_err = TranslationError(pose_error);
// compute speed
auto num_frames = static_cast(last_frame - first_frame + 1);
double speed = len / (0.1 * num_frames);
// write to file
err.emplace_back(first_frame, r_err / len, t_err / len, len, speed);
}
}
// return error vector
return err;
}
} // namespace
// 用于计算KITTI数据集姿态估计的误差指标
// SeqError：平均位移误差和平均旋转误差
// AbsoluteTrajectoryError：绝对姿态误差（Absolute Trajectory Error, ATE）
namespace kiss_icp::metrics {
std::tuple SeqError(const std::vector &poses_gt,
const std::vector &poses_result) {
std::vector err = CalcSequenceErrors(poses_gt, poses_result);
double t_err = 0;
double r_err = 0;
for (const auto &it : err) {
t_err += it.t_err;
r_err += it.r_err;
}
double avg_trans_error = 100.0 * (t_err / static_cast(err.size()));
double avg_rot_error = 100.0 * (r_err / static_cast(err.size())) / 3.14 * 180.0;
return std::make_tuple(avg_trans_error, avg_rot_error);
}
std::tuple AbsoluteTrajectoryError(const std::vector &poses_gt,
const std::vector &poses_result) {
assert(poses_gt.size() == poses_result.size() &&
"AbsoluteTrajectoryError| Different number of poses in ground truth and estimate");
Eigen::MatrixXd source(3, poses_gt.size());
Eigen::MatrixXd target(3, poses_gt.size());
const size_t num_poses = poses_gt.size();
// Align the two trajectories using SVD-ICP (Umeyama algorithm)
for (size_t i = 0; i < num_poses; ++i) {
source.block<3, 1>(0, i) = poses_result[i].block<3, 1>(0, 3);
target.block<3, 1>(0, i) = poses_gt[i].block<3, 1>(0, 3);
}
const Eigen::Matrix4d T_align_trajectories = Eigen::umeyama(source, target, false);
// ATE computation
double ATE_rot = 0, ATE_trans = 0;
for (size_t j = 0; j < num_poses; ++j) {
// Apply alignement matrix
const Eigen::Matrix4d T_estimate = T_align_trajectories * poses_result[j];
const Eigen::Matrix4d &T_ground_truth = poses_gt[j];
// Compute error in translation and rotation matrix (ungly)
const Eigen::Matrix3d delta_R =
T_ground_truth.block<3, 3>(0, 0) * T_estimate.block<3, 3>(0, 0).transpose();
const Eigen::Vector3d delta_t =
T_ground_truth.block<3, 1>(0, 3) - delta_R * T_estimate.block<3, 1>(0, 3);
// Get angular error
const double theta = Eigen::AngleAxisd(delta_R).angle();
// Sum of Squares
ATE_rot += theta * theta;
ATE_trans += delta_t.squaredNorm();
}
// Get the RMSE
ATE_rot /= static_cast(num_poses);
ATE_trans /= static_cast(num_poses);
return std::make_tuple(std::sqrt(ATE_rot), std::sqrt(ATE_trans));
}
} // namespace kiss_icp::metrics 五、不足
由于为纯激光雷达里程计，受z方向上的累积误差以及俯仰角的抖动误差影响，在长距离的Velodyne数据集上的建图效果不太理想。后续可以加入IMU数据及回环检测模块提升建图精度。