ICP方法
ICP原理
点到线、点到面的ICP原理,具体查看参考书籍。
ICP实现
点到面的ICP
选取5个最近邻,进行局部平面拟合。
bool Icp3d::AlignP2Plane(SE3& init_pose) {
LOG(INFO) << "aligning with point to plane";
assert(target_ != nullptr && source_ != nullptr);
// 整体流程与p2p一致,读者请关注变化部分
SE3 pose = init_pose;
if (!options_.use_initial_translation_) {
pose.translation() = target_center_ - source_center_; // 设置平移初始值
}
std::vector<int> index(source_->points.size());
for (int i = 0; i < index.size(); ++i) {
index[i] = i;
}
std::vector<bool> effect_pts(index.size(), false);
std::vector<Eigen::Matrix<double, 1, 6>> jacobians(index.size());
std::vector<double> errors(index.size());
for (int iter = 0; iter < options_.max_iteration_; ++iter) {
// gauss-newton 迭代
// 最近邻,可以并发
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
auto q = ToVec3d(source_->points[idx]);
Vec3d qs = pose * q; // 转换之后的q
std::vector<int> nn;
kdtree_->GetClosestPoint(ToPointType(qs), nn, 5); // 这里取5个最近邻
if (nn.size() > 3) {
// convert to eigen
std::vector<Vec3d> nn_eigen;
for (int i = 0; i < nn.size(); ++i) {
nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]]));
}
Vec4d n;
if (!math::FitPlane(nn_eigen, n)) {
// 失败的不要
effect_pts[idx] = false;
return;
}
double dis = n.head<3>().dot(qs) + n[3];
if (fabs(dis) > options_.max_plane_distance_) {
// 点离的太远了不要
effect_pts[idx] = false;
return;
}
effect_pts[idx] = true;
// build residual
Eigen::Matrix<double, 1, 6> J;
J.block<1, 3>(0, 0) = -n.head<3>().transpose() * pose.so3().matrix() * SO3::hat(q);
J.block<1, 3>(0, 3) = n.head<3>().transpose();
jacobians[idx] = J;
errors[idx] = dis;
} else {
effect_pts[idx] = false;
}
});
// 累加Hessian和error,计算dx
// 原则上可以用reduce并发,写起来比较麻烦,这里写成accumulate
double total_res = 0;
int effective_num = 0;
auto H_and_err = std::accumulate(
index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()),
[&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre,
int idx) -> std::pair<Mat6d, Vec6d> {
if (!effect_pts[idx]) {
return pre;
} else {
total_res += errors[idx] * errors[idx];
effective_num++;
return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx],
pre.second - jacobians[idx].transpose() * errors[idx]);
}
});
if (effective_num < options_.min_effective_pts_) {
LOG(WARNING) << "effective num too small: " << effective_num;
return false;
}
Mat6d H = H_and_err.first;
Vec6d err = H_and_err.second;
Vec6d dx = H.inverse() * err;
pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
pose.translation() += dx.tail<3>();
// 更新
LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
<< ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm();
if (gt_set_) {
double pose_error = (gt_pose_.inverse() * pose).log().norm();
LOG(INFO) << "iter " << iter << " pose error: " << pose_error;
}
if (dx.norm() < options_.eps_) {
LOG(INFO) << "converged, dx = " << dx.transpose();
break;
}
}
init_pose = pose;
return true;
}
点到线的ICP
bool Icp3d::AlignP2Line(SE3& init_pose) {
LOG(INFO) << "aligning with point to line";
assert(target_ != nullptr && source_ != nullptr);
// 点线与点面基本是完全一样的
SE3 pose = init_pose;
if (options_.use_initial_translation_) {
pose.translation() = target_center_ - source_center_; // 设置平移初始值
LOG(INFO) << "init trans set to " << pose.translation().transpose();
}
std::vector<int> index(source_->points.size());
for (int i = 0; i < index.size(); ++i) {
index[i] = i;
}
std::vector<bool> effect_pts(index.size(), false);
std::vector<Eigen::Matrix<double, 3, 6>> jacobians(index.size());
std::vector<Vec3d> errors(index.size());
for (int iter = 0; iter < options_.max_iteration_; ++iter) {
// gauss-newton 迭代
// 最近邻,可以并发
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
auto q = ToVec3d(source_->points[idx]);
Vec3d qs = pose * q; // 转换之后的q
std::vector<int> nn;
kdtree_->GetClosestPoint(ToPointType(qs), nn, 5); // 这里取5个最近邻
if (nn.size() == 5) {
// convert to eigen
std::vector<Vec3d> nn_eigen;
for (int i = 0; i < 5; ++i) {
nn_eigen.emplace_back(ToVec3d(target_->points[nn[i]]));
}
Vec3d d, p0;
if (!math::FitLine(nn_eigen, p0, d, options_.max_line_distance_)) {
// 失败的不要
effect_pts[idx] = false;
return;
}
Vec3d err = SO3::hat(d) * (qs - p0);
if (err.norm() > options_.max_line_distance_) {
// 点离的太远了不要
effect_pts[idx] = false;
return;
}
effect_pts[idx] = true;
// build residual
Eigen::Matrix<double, 3, 6> J;
J.block<3, 3>(0, 0) = -SO3::hat(d) * pose.so3().matrix() * SO3::hat(q);
J.block<3, 3>(0, 3) = SO3::hat(d);
jacobians[idx] = J;
errors[idx] = err;
} else {
effect_pts[idx] = false;
}
});
// 累加Hessian和error,计算dx
// 原则上可以用reduce并发,写起来比较麻烦,这里写成accumulate
double total_res = 0;
int effective_num = 0;
auto H_and_err = std::accumulate(
index.begin(), index.end(), std::pair<Mat6d, Vec6d>(Mat6d::Zero(), Vec6d::Zero()),
[&jacobians, &errors, &effect_pts, &total_res, &effective_num](const std::pair<Mat6d, Vec6d>& pre,
int idx) -> std::pair<Mat6d, Vec6d> {
if (!effect_pts[idx]) {
return pre;
} else {
total_res += errors[idx].dot(errors[idx]);
effective_num++;
return std::pair<Mat6d, Vec6d>(pre.first + jacobians[idx].transpose() * jacobians[idx],
pre.second - jacobians[idx].transpose() * errors[idx]);
}
});
if (effective_num < options_.min_effective_pts_) {
LOG(WARNING) << "effective num too small: " << effective_num;
return false;
}
Mat6d H = H_and_err.first;
Vec6d err = H_and_err.second;
Vec6d dx = H.inverse() * err;
pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
pose.translation() += dx.tail<3>();
if (gt_set_) {
double pose_error = (gt_pose_.inverse() * pose).log().norm();
LOG(INFO) << "iter " << iter << " pose error: " << pose_error;
}
// 更新
LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
<< ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm();
if (dx.norm() < options_.eps_) {
LOG(INFO) << "converged, dx = " << dx.transpose();
break;
}
}
init_pose = pose;
return true;
}
NDT方法
NDT原理
NDT大致思路:
1、把目标点云按一定分辨率分成若干体素。
2、计算每个体素中的点云高斯分布。
3、配准时,先计算每个点落在哪个体素中,然后建立该点与该体素的均值,方差构成的残差。
4、利用高斯-牛顿法或L-M(Levenberg-Marquardt)方法对估计位姿进行迭代,然后更新位姿估计。
NDT实现
建立体素
NDT类定义
src/ch7/ndt_3d.cc
/**
* 3D 形式的NDT
*/
class Ndt3d {
public:
enum class NearbyType {
CENTER, // 只考虑中心
NEARBY6, // 上下左右前后
};
...
using KeyType = Eigen::Matrix<int, 3, 1>; // 体素的索引
struct VoxelData {
VoxelData() {}
VoxelData(size_t id) { idx_.emplace_back(id); }
std::vector<size_t> idx_; // 点云中点的索引
Vec3d mu_ = Vec3d::Zero(); // 均值
Mat3d sigma_ = Mat3d::Zero(); // 协方差
Mat3d info_ = Mat3d::Zero(); // 协方差之逆
};
...
/// 使用gauss-newton方法进行ndt配准
bool AlignNdt(SE3& init_pose);
private:
void BuildVoxels();
/// 根据最近邻的类型,生成附近网格
void GenerateNearbyGrids();
CloudPtr target_ = nullptr;
CloudPtr source_ = nullptr;
Vec3d target_center_ = Vec3d::Zero();
Vec3d source_center_ = Vec3d::Zero();
SE3 gt_pose_;
bool gt_set_ = false;
Options options_;
std::unordered_map<KeyType, VoxelData, hash_vec<3>> grids_; // 栅格数据
std::vector<KeyType> nearby_grids_; // 附近的栅格
};
构建体素
体素最近邻可以仅使用中心体素,或加上周围的6个体素做最近邻。
体素内部点数大于3,就可计算均值和协方差。
点云稠密,可仅使用中心体素;点云稀疏,可考虑周边体素的情况。
void Ndt3d::BuildVoxels() {
assert(target_ != nullptr);
assert(target_->empty() == false);
grids_.clear();
/// 分配体素
std::vector<size_t> index(target_->size());
std::for_each(index.begin(), index.end(), [idx = 0](size_t& i) mutable { i = idx++; });
std::for_each(index.begin(), index.end(), [this](const size_t& idx) {
Vec3d pt = ToVec3d(target_->points[idx]) * options_.inv_voxel_size_;
auto key = CastToInt(pt);
if (grids_.find(key) == grids_.end()) {
grids_.insert({key, {idx}});
} else {
grids_[key].idx_.emplace_back(idx);
}
});
/// 计算每个体素中的均值和协方差
std::for_each(std::execution::par_unseq, grids_.begin(), grids_.end(), [this](auto& v) {
if (v.second.idx_.size() > options_.min_pts_in_voxel_) {
// 要求至少有3个点
math::ComputeMeanAndCov(v.second.idx_, v.second.mu_, v.second.sigma_,
[this](const size_t& idx) { return ToVec3d(target_->points[idx]); });
// SVD 检查最大与最小奇异值,限制最小奇异值
Eigen::JacobiSVD svd(v.second.sigma_, Eigen::ComputeFullU | Eigen::ComputeFullV);
Vec3d lambda = svd.singularValues();
if (lambda[1] < lambda[0] * 1e-3) {
lambda[1] = lambda[0] * 1e-3;
}
if (lambda[2] < lambda[0] * 1e-3) {
lambda[2] = lambda[0] * 1e-3;
}
Mat3d inv_lambda = Vec3d(1.0 / lambda[0], 1.0 / lambda[1], 1.0 / lambda[2]).asDiagonal();
// v.second.info_ = (v.second.sigma_ + Mat3d::Identity() * 1e-3).inverse(); // 避免出nan
v.second.info_ = svd.matrixV() * inv_lambda * svd.matrixU().transpose();
}
});
/// 删除点数不够的
for (auto iter = grids_.begin(); iter != grids_.end();) {
if (iter->second.idx_.size() > options_.min_pts_in_voxel_) {
iter++;
} else {
iter = grids_.erase(iter);
}
}
}
void Ndt3d::GenerateNearbyGrids() {
if (options_.nearby_type_ == NearbyType::CENTER) {
nearby_grids_.emplace_back(KeyType::Zero());
} else if (options_.nearby_type_ == NearbyType::NEARBY6) {
nearby_grids_ = {KeyType(0, 0, 0), KeyType(-1, 0, 0), KeyType(1, 0, 0), KeyType(0, 1, 0),
KeyType(0, -1, 0), KeyType(0, 0, -1), KeyType(0, 0, 1)};
}
}
配准
bool Ndt3d::AlignNdt(SE3& init_pose) {
LOG(INFO) << "aligning with ndt";
assert(grids_.empty() == false);
SE3 pose = init_pose;
if (options_.remove_centroid_) {
pose.translation() = target_center_ - source_center_; // 设置平移初始值
LOG(INFO) << "init trans set to " << pose.translation().transpose();
}
// 对点的索引,预先生成
int num_residual_per_point = 1;
if (options_.nearby_type_ == NearbyType::NEARBY6) {
num_residual_per_point = 7;
}
std::vector<int> index(source_->points.size());
for (int i = 0; i < index.size(); ++i) {
index[i] = i;
}
// 我们来写一些并发代码
int total_size = index.size() * num_residual_per_point;
for (int iter = 0; iter < options_.max_iteration_; ++iter) {
std::vector<bool> effect_pts(total_size, false);
std::vector<Eigen::Matrix<double, 3, 6>> jacobians(total_size);
std::vector<Vec3d> errors(total_size);
std::vector<Mat3d> infos(total_size);
// gauss-newton 迭代
// 最近邻,可以并发
std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](int idx) {
auto q = ToVec3d(source_->points[idx]);
Vec3d qs = pose * q; // 转换之后的q
// 计算qs所在的栅格以及它的最近邻栅格
Vec3i key = CastToInt(Vec3d(qs * options_.inv_voxel_size_));
for (int i = 0; i < nearby_grids_.size(); ++i) {
auto key_off = key + nearby_grids_[i];
auto it = grids_.find(key_off);
int real_idx = idx * num_residual_per_point + i;
if (it != grids_.end()) {
auto& v = it->second; // voxel
e的相关公式
res的相关公式
J的相关公式
Vec3d e = qs - v.mu_;
// check chi2 th
double res = e.transpose() * v.info_ * e;
if (std::isnan(res) || res > options_.res_outlier_th_) {
effect_pts[real_idx] = false;
continue;
}
// build residual
Eigen::Matrix<double, 3, 6> J;
J.block<3, 3>(0, 0) = -pose.so3().matrix() * SO3::hat(q);
J.block<3, 3>(0, 3) = Mat3d::Identity();
jacobians[real_idx] = J;
errors[real_idx] = e;
infos[real_idx] = v.info_;
effect_pts[real_idx] = true;
} else {
effect_pts[real_idx] = false;
}
}
});
H的计算公式
err的计算公式
由H和err的表达式,结合以下公式(加权最小二乘问题的高斯-牛顿法)
可推出dx的计算方法 dx = H.inverse() * err。
total_res的计算公式
// 累加Hessian和error,计算dx
// 原则上可以用reduce并发,写起来比较麻烦,这里写成accumulate
double total_res = 0;
int effective_num = 0;
Mat6d H = Mat6d::Zero();
Vec6d err = Vec6d::Zero();
for (int idx = 0; idx < effect_pts.size(); ++idx) {
if (!effect_pts[idx]) {
continue;
}
total_res += errors[idx].transpose() * infos[idx] * errors[idx];
// chi2.emplace_back(errors[idx].transpose() * infos[idx] * errors[idx]);
effective_num++;
H += jacobians[idx].transpose() * infos[idx] * jacobians[idx];
err += -jacobians[idx].transpose() * infos[idx] * errors[idx];
}
if (effective_num < options_.min_effective_pts_) {
LOG(WARNING) << "effective num too small: " << effective_num;
return false;
}
Vec6d dx = H.inverse() * err;
pose.so3() = pose.so3() * SO3::exp(dx.head<3>());
pose.translation() += dx.tail<3>();
// 更新
LOG(INFO) << "iter " << iter << " total res: " << total_res << ", eff: " << effective_num
<< ", mean res: " << total_res / effective_num << ", dxn: " << dx.norm()
<< ", dx: " << dx.transpose();
// std::sort(chi2.begin(), chi2.end());
// LOG(INFO) << "chi2 med: " << chi2[chi2.size() / 2] << ", .7: " << chi2[chi2.size() * 0.7]
// << ", .9: " << chi2[chi2.size() * 0.9] << ", max: " << chi2.back();
if (gt_set_) {
double pose_error = (gt_pose_.inverse() * pose).log().norm();
LOG(INFO) << "iter " << iter << " pose error: " << pose_error;
}
if (dx.norm() < options_.eps_) {
LOG(INFO) << "converged, dx = " << dx.transpose();
break;
}
}
init_pose = pose;
return true;
}
参考资料:
1、书籍:《自动驾驶与机器人中的SLAM技术:从理论到实践》
2、代码:https://github.com/gaoxiang12/slam_in_autonomous_driving/tree/master/src/ch7