Cartographer源码阅读3D-前端匹配
暴力匹配：RealTimeCorrelativeScanMatcher3D
暴力搜索计算匹配得分，取得最高分对应的位姿。
class RealTimeCorrelativeScanMatcher3D {
public:
explicit RealTimeCorrelativeScanMatcher3D(
const scan_matching::proto::RealTimeCorrelativeScanMatcherOptions&
options);
RealTimeCorrelativeScanMatcher3D(const RealTimeCorrelativeScanMatcher3D&) =
delete;
RealTimeCorrelativeScanMatcher3D& operator=(
const RealTimeCorrelativeScanMatcher3D&) = delete;
// Aligns 'point_cloud' within the 'hybrid_grid' given an
// 'initial_pose_estimate' then updates 'pose_estimate' with the result and
// returns the score.
// 传进来的都是高分辨率的网格和高分辨率点云
float Match(const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const HybridGrid& hybrid_grid,
transform::Rigid3d* pose_estimate) const;
private:
// 生成所有可能解的位姿:x *y *z * rx * ry * rz个
std::vector<transform::Rigid3f> GenerateExhaustiveSearchTransforms(
float resolution, const sensor::PointCloud& point_cloud) const;
// 打分
float ScoreCandidate(const HybridGrid& hybrid_grid,
const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const;
const proto::RealTimeCorrelativeScanMatcherOptions options_;
};
// 入口函数
float RealTimeCorrelativeScanMatcher3D::Match(
const transform::Rigid3d& initial_pose_estimate,
const sensor::PointCloud& point_cloud, const HybridGrid& hybrid_grid,
transform::Rigid3d* pose_estimate) const {
CHECK_NOTNULL(pose_estimate);
float best_score = -1.f;
// 生成所有候选解，并遍历所有的候选解(位姿增量)
for (const transform::Rigid3f& transform : GenerateExhaustiveSearchTransforms(
hybrid_grid.resolution(), point_cloud)) {
// 生成候选解（需要乘以位姿）
const transform::Rigid3f candidate =
initial_pose_estimate.cast<float>() * transform;
// 针对该候选解打分
const float score = ScoreCandidate(
hybrid_grid, sensor::TransformPointCloud(point_cloud, candidate),
transform);
// 最高分迭代
if (score > best_score) {
best_score = score;
*pose_estimate = candidate.cast<double>();
}
}
return best_score;
}
// 生成候选解增量
std::vector<transform::Rigid3f>
RealTimeCorrelativeScanMatcher3D::GenerateExhaustiveSearchTransforms(
const float resolution, const sensor::PointCloud& point_cloud) const {
std::vector<transform::Rigid3f> result;
// 获取线搜索框搜索范围
const int linear_window_size =
common::RoundToInt(options_.linear_search_window() / resolution);
// We set this value to something on the order of resolution to make sure that
// the std::acos() below is defined.
// 得到最大的测距
float max_scan_range = 3.f * resolution;
for (const Eigen::Vector3f& point : point_cloud) {
const float range = point.norm();
max_scan_range = std::max(range, max_scan_range);
}
const float kSafetyMargin = 1.f - 1e-3f;
// 计算最小角度搜索增量，计算方法和2D相同
const float angular_step_size =
kSafetyMargin * std::acos(1.f - common::Pow2(resolution) /
(2.f * common::Pow2(max_scan_range)));
// 计算角度搜索框搜索范围
const int angular_window_size =
common::RoundToInt(options_.angular_search_window() / angular_step_size);
for (int z = -linear_window_size; z <= linear_window_size; ++z) {
for (int y = -linear_window_size; y <= linear_window_size; ++y) {
for (int x = -linear_window_size; x <= linear_window_size; ++x) {
for (int rz = -angular_window_size; rz <= angular_window_size; ++rz) {
for (int ry = -angular_window_size; ry <= angular_window_size; ++ry) {
for (int rx = -angular_window_size; rx <= angular_window_size;
++rx) {
const Eigen::Vector3f angle_axis(rx * angular_step_size,
ry * angular_step_size,
rz * angular_step_size);
// 生成候选解增量
result.emplace_back(
Eigen::Vector3f(x * resolution, y * resolution,
z * resolution),
transform::AngleAxisVectorToRotationQuaternion(angle_axis));
}
}
}
}
}
}
return result;
}
// 打分
float RealTimeCorrelativeScanMatcher3D::ScoreCandidate(
const HybridGrid& hybrid_grid,
const sensor::PointCloud& transformed_point_cloud,
const transform::Rigid3f& transform) const {
float score = 0.f;
// 暴力计算每个点的得分总和
for (const Eigen::Vector3f& point : transformed_point_cloud) {
score += hybrid_grid.GetProbability(hybrid_grid.GetCellIndex(point));
}
// 得到平均分
score /= static_cast<float>(transformed_point_cloud.size());
const float angle = transform::GetAngle(transform);
// 得分乘以权重，权重函数怎么来的，仍未清楚，或许是随意取的
score *=
std::exp(-common::Pow2(transform.translation().norm() *
options_.translation_delta_cost_weight() +
angle * options_.rotation_delta_cost_weight()));
CHECK_GT(score, 0.f);
return score;
}
CeresScanMatcher3D
和2D基本一致，但是多了2个分辨率的地图。
class CeresScanMatcher3D {
public:
explicit CeresScanMatcher3D(const proto::CeresScanMatcherOptions3D& options);
CeresScanMatcher3D(const CeresScanMatcher3D&) = delete;
CeresScanMatcher3D& operator=(const CeresScanMatcher3D&) = delete;
// Aligns 'point_clouds' within the 'hybrid_grids' given an
// 'initial_pose_estimate' and returns a 'pose_estimate' and the solver
// 'summary'.
void Match(const Eigen::Vector3d& target_translation,
const transform::Rigid3d& initial_pose_estimate,
const std::vector<PointCloudAndHybridGridPointers>&
point_clouds_and_hybrid_grids,
transform::Rigid3d* pose_estimate,
ceres::Solver::Summary* summary);
private:
const proto::CeresScanMatcherOptions3D options_;
ceres::Solver::Options ceres_solver_options_;
};
void CeresScanMatcher3D::Match(
const Eigen::Vector3d& target_translation,
const transform::Rigid3d& initial_pose_estimate,
const std::vector<PointCloudAndHybridGridPointers>&
point_clouds_and_hybrid_grids,
transform::Rigid3d* const pose_estimate,
ceres::Solver::Summary* const summary) {
ceres::Problem problem;
optimization::CeresPose ceres_pose(
initial_pose_estimate, nullptr /* translation_parameterization */,
options_.only_optimize_yaw()
// 只优化Yawl角
? std::unique_ptr<ceres::LocalParameterization>(
common::make_unique<ceres::AutoDiffLocalParameterization<
YawOnlyQuaternionPlus, 4, 1>>())
// 优化所有的角度
: std::unique_ptr<ceres::LocalParameterization>(
common::make_unique<ceres::QuaternionParameterization>()),
&problem);
CHECK_EQ(options_.occupied_space_weight_size(),
point_clouds_and_hybrid_grids.size());
// 针对高低分辨率地图，分别添加点的匹配残差
for (size_t i = 0; i != point_clouds_and_hybrid_grids.size(); ++i) {
CHECK_GT(options_.occupied_space_weight(i), 0.);
const sensor::PointCloud& point_cloud =
*point_clouds_and_hybrid_grids[i].first;
const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
problem.AddResidualBlock(
OccupiedSpaceCostFunction3D::CreateAutoDiffCostFunction(
// 高低分辨率地图的匹配权重来自设置参数，并且同一分辨率地图的每个点的权重一样，因为残差是平方的，所以要开根号呀
options_.occupied_space_weight(i) /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, hybrid_grid),
nullptr /* loss function */, ceres_pose.translation(),
ceres_pose.rotation());
}
CHECK_GT(options_.translation_weight(), 0.);
// 添加平移残差
problem.AddResidualBlock(
TranslationDeltaCostFunctor3D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr /* loss function */, ceres_pose.translation());
CHECK_GT(options_.rotation_weight(), 0.);
// 添加旋转残差
problem.AddResidualBlock(
RotationDeltaCostFunctor3D::CreateAutoDiffCostFunction(
options_.rotation_weight(), initial_pose_estimate.rotation()),
nullptr /* loss function */, ceres_pose.rotation());
ceres::Solve(ceres_solver_options_, &problem, summary);
*pose_estimate = ceres_pose.ToRigid();
}
点云残差：
class OccupiedSpaceCostFunction3D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const mapping::HybridGrid& hybrid_grid) {
return new ceres::AutoDiffCostFunction<
OccupiedSpaceCostFunction3D, ceres::DYNAMIC /* residuals */,
3 /* translation variables */, 4 /* rotation variables */>(
new OccupiedSpaceCostFunction3D(scaling_factor, point_cloud,
hybrid_grid),
point_cloud.size());
}
template <typename T>
bool operator()(const T* const translation, const T* const rotation,
T* const residual) const {
const transform::Rigid3<T> transform(
Eigen::Map<const Eigen::Matrix<T, 3, 1>>(translation),
Eigen::Quaternion<T>(rotation[0], rotation[1], rotation[2],
rotation[3]));
return Evaluate(transform, residual);
}
private:
OccupiedSpaceCostFunction3D(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const mapping::HybridGrid& hybrid_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
// 插值网格的构造
interpolated_grid_(hybrid_grid) {}
OccupiedSpaceCostFunction3D(const OccupiedSpaceCostFunction3D&) = delete;
OccupiedSpaceCostFunction3D& operator=(const OccupiedSpaceCostFunction3D&) =
delete;
template <typename T>
bool Evaluate(const transform::Rigid3<T>& transform,
T* const residual) const {
for (size_t i = 0; i < point_cloud_.size(); ++i) {
// 转到网格坐标系下
const Eigen::Matrix<T, 3, 1> world =
transform * point_cloud_[i].cast<T>();
// 插值得到概率
const T probability =
interpolated_grid_.GetProbability(world[0], world[1], world[2]);
// 构建残差
residual[i] = scaling_factor_ * (1. - probability);
}
return true;
}
const double scaling_factor_;
const sensor::PointCloud& point_cloud_;
// 用作插值的网格 和 2D有所不同
const InterpolatedGrid interpolated_grid_;
};
class InterpolatedGrid {
public:
// 构造函数中将网格数据传进来
explicit InterpolatedGrid(const HybridGrid& hybrid_grid)
: hybrid_grid_(hybrid_grid) {}
InterpolatedGrid(const InterpolatedGrid&) = delete;
InterpolatedGrid& operator=(const InterpolatedGrid&) = delete;
// Returns the interpolated probability at (x, y, z) of the HybridGrid
// used to perform the interpolation.
//
// This is a piecewise, continuously differentiable function. We use the
// scalar part of Jet parameters to select our interval below. It is the
// tensor product volume of piecewise cubic polynomials that interpolate
// the values, and have vanishing derivative at the interval boundaries.
template <typename T>
T GetProbability(const T& x, const T& y, const T& z) const {
double x1, y1, z1, x2, y2, z2;
ComputeInterpolationDataPoints(x, y, z, &x1, &y1, &z1, &x2, &y2, &z2);
// 获取低的voxel的index
const Eigen::Array3i index1 =
hybrid_grid_.GetCellIndex(Eigen::Vector3f(x1, y1, z1));
// 获取低的voxel的概率
const double q111 = hybrid_grid_.GetProbability(index1);
// 获取其附近8个voxel的概率，该8个voxel肯定包含坐标点xyz
const double q112 =
hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 0, 1));
const double q121 =
hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 0));
const double q122 =
hybrid_grid_.GetProbability(index1 + Eigen::Array3i(0, 1, 1));
const double q211 =
hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 0));
const double q212 =
hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 0, 1));
const double q221 =
hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 0));
const double q222 =
hybrid_grid_.GetProbability(index1 + Eigen::Array3i(1, 1, 1));
const T normalized_x = (x - x1) / (x2 - x1);
const T normalized_y = (y - y1) / (y2 - y1);
const T normalized_z = (z - z1) / (z2 - z1);
// Compute pow(..., 2) and pow(..., 3). Using pow() here is very expensive.
const T normalized_xx = normalized_x * normalized_x;
const T normalized_xxx = normalized_x * normalized_xx;
const T normalized_yy = normalized_y * normalized_y;
const T normalized_yyy = normalized_y * normalized_yy;
const T normalized_zz = normalized_z * normalized_z;
const T normalized_zzz = normalized_z * normalized_zz;
// 双三次插值，该公式如下
// We first interpolate in z, then y, then x. All 7 times this uses the same
// scheme: A * (2t^3 - 3t^2 + 1) + B * (-2t^3 + 3t^2).
// The first polynomial is 1 at t=0, 0 at t=1, the second polynomial is 0
// at t=0, 1 at t=1. Both polynomials have derivative zero at t=0 and t=1.
const T q11 = (q111 - q112) * normalized_zzz * 2. +
(q112 - q111) * normalized_zz * 3. + q111;
const T q12 = (q121 - q122) * normalized_zzz * 2. +
(q122 - q121) * normalized_zz * 3. + q121;
const T q21 = (q211 - q212) * normalized_zzz * 2. +
(q212 - q211) * normalized_zz * 3. + q211;
const T q22 = (q221 - q222) * normalized_zzz * 2. +
(q222 - q221) * normalized_zz * 3. + q221;
const T q1 = (q11 - q12) * normalized_yyy * 2. +
(q12 - q11) * normalized_yy * 3. + q11;
const T q2 = (q21 - q22) * normalized_yyy * 2. +
(q22 - q21) * normalized_yy * 3. + q21;
return (q1 - q2) * normalized_xxx * 2. + (q2 - q1) * normalized_xx * 3. +
q1;
}
private:
// 计算相邻的两个voxel的中心点
template <typename T>
void ComputeInterpolationDataPoints(const T& x, const T& y, const T& z,
double* x1, double* y1, double* z1,
double* x2, double* y2,
double* z2) const {
const Eigen::Vector3f lower = CenterOfLowerVoxel(x, y, z);
*x1 = lower.x();
*y1 = lower.y();
*z1 = lower.z();
*x2 = lower.x() + hybrid_grid_.resolution();
*y2 = lower.y() + hybrid_grid_.resolution();
*z2 = lower.z() + hybrid_grid_.resolution();
}
// Center of the next lower voxel, i.e., not necessarily the voxel containing
// (x, y, z). For each dimension, the largest voxel index so that the
// corresponding center is at most the given coordinate.
// 计算下一个的voxel的中心点，必须保证center的xyz是小于xyz的
Eigen::Vector3f CenterOfLowerVoxel(const double x, const double y,
const double z) const {
// Center of the cell containing (x, y, z).
Eigen::Vector3f center = hybrid_grid_.GetCenterOfCell(
hybrid_grid_.GetCellIndex(Eigen::Vector3f(x, y, z)));
// Move to the next lower voxel center.
if (center.x() > x) {
center.x() -= hybrid_grid_.resolution();
}
if (center.y() > y) {
center.y() -= hybrid_grid_.resolution();
}
if (center.z() > z) {
center.z() -= hybrid_grid_.resolution();
}
return center;
}
// Uses the scalar part of a Ceres Jet.
template <typename T>
Eigen::Vector3f CenterOfLowerVoxel(const T& jet_x, const T& jet_y,
const T& jet_z) const {
return CenterOfLowerVoxel(jet_x.a, jet_y.a, jet_z.a);
}
const HybridGrid& hybrid_grid_;
};
3D平移残差：
class TranslationDeltaCostFunctor3D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Vector3d& target_translation) {
return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor3D,
3 /* residuals */,
3 /* translation variables */>(
new TranslationDeltaCostFunctor3D(scaling_factor, target_translation));
}
// 平移直接作差
template <typename T>
bool operator()(const T* const translation, T* residual) const {
residual[0] = scaling_factor_ * (translation[0] - x_);
residual[1] = scaling_factor_ * (translation[1] - y_);
residual[2] = scaling_factor_ * (translation[2] - z_);
return true;
}
private:
// Constructs a new TranslationDeltaCostFunctor3D from the given
// 'target_translation'.
explicit TranslationDeltaCostFunctor3D(
const double scaling_factor, const Eigen::Vector3d& target_translation)
: scaling_factor_(scaling_factor),
x_(target_translation.x()),
y_(target_translation.y()),
z_(target_translation.z()) {}
TranslationDeltaCostFunctor3D(const TranslationDeltaCostFunctor3D&) = delete;
TranslationDeltaCostFunctor3D& operator=(
const TranslationDeltaCostFunctor3D&) = delete;
const double scaling_factor_;
const double x_;
const double y_;
const double z_;
};
3D旋转残差：
class RotationDeltaCostFunctor3D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Quaterniond& target_rotation) {
return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor3D,
3 /* residuals */,
4 /* rotation variables */>(
new RotationDeltaCostFunctor3D(scaling_factor, target_rotation));
}
// 旋转得到三个角，并作差
template <typename T>
bool operator()(const T* const rotation_quaternion, T* residual) const {
std::array<T, 4> delta;
common::QuaternionProduct(target_rotation_inverse_, rotation_quaternion,
delta.data());
// Will compute the squared norm of the imaginary component of the delta
// quaternion which is sin(phi/2)^2.
residual[0] = scaling_factor_ * delta[1];
residual[1] = scaling_factor_ * delta[2];
residual[2] = scaling_factor_ * delta[3];
return true;
}
private:
// Constructs a new RotationDeltaCostFunctor3D from the given
// 'target_rotation'.
explicit RotationDeltaCostFunctor3D(const double scaling_factor,
const Eigen::Quaterniond& target_rotation)
: scaling_factor_(scaling_factor) {
// 四元数共轭即为逆（四元数模长为1）
target_rotation_inverse_[0] = target_rotation.w();
target_rotation_inverse_[1] = -target_rotation.x();
target_rotation_inverse_[2] = -target_rotation.y();
target_rotation_inverse_[3] = -target_rotation.z();
}
RotationDeltaCostFunctor3D(const RotationDeltaCostFunctor3D&) = delete;
RotationDeltaCostFunctor3D& operator=(const RotationDeltaCostFunctor3D&) =
delete;
const double scaling_factor_;
double target_rotation_inverse_[4];
};
// 四元数相乘
template <typename T>
inline void QuaternionProduct(const double* const z, const T* const w,
T* const zw) {
zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
}
扫一扫
890
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
