一、使用步骤
第一步 声明CostFunctor
class MyScalarCostFunctor {
MyScalarCostFunctor(double k): k_(k) {}
template <typename T>
bool operator()(const T* const x , const T* const y, T* e) const {
// (k - x^T * y)^2, 平 方 由 ceres自 动 添 加
e[0] = k_ - x[0] * y[0] - x[1] * y[1];
return true;
}
private:
double k_;
};
第二步 调用AutoDiffCostFunction函数
第三步 添加残差块
ceres::Problem problem;
problem.AddResidualBlock(cost_function);
第四步 进行求解
ceres::Solver::Summary summary;
ceres::Solve(ceres_solver_options_, &problem, summary);
std::cout << summary.BriefReport() << std::endl; // summary.FullReport()
二、cere 使用实例
/**
* @brief 基于Ceres的扫描匹配
*
* @param[in] target_translation 预测出来的先验位置, 只有xy
* @param[in] initial_pose_estimate (校正后的)先验位姿, 有xy与theta
* @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
* @param[in] grid 用于匹配的栅格地图
* @param[out] pose_estimate 优化之后的位姿
* @param[out] summary
*/
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
const transform::Rigid2d& initial_pose_estimate,
const sensor::PointCloud& point_cloud,
const Grid2D& grid,
transform::Rigid2d* const pose_estimate,
ceres::Solver::Summary* const summary) const
{
double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
initial_pose_estimate.translation().y(),
initial_pose_estimate.rotation().angle()};
ceres::Problem problem;
CHECK_GT(options_.occupied_space_weight(), 0.);
// 地图部分的残差
problem.AddResidualBlock(
OccupiedSpaceCostFunction2D::CreateAutoDiffCostFunction(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, grid),
nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.translation_weight(), 0.);
// 平移的残差
problem.AddResidualBlock(
TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.rotation_weight(), 0.);
// 旋转的残差, 固定了角度不变
problem.AddResidualBlock(
RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
options_.rotation_weight(), ceres_pose_estimate[2]),
nullptr /* loss function */, ceres_pose_estimate);
ceres::Solve(ceres_solver_options_, &problem, summary);
*pose_estimate = transform::Rigid2d(
{ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}
1.地图部分的残差
OccupiedSpaceCostFunction2D得定义：
occupied_space_cost_function_2d.h
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_OCCUPIED_SPACE_COST_FUNCTION_2D_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/mapping/2d/grid_2d.h"
#include "cartographer/mapping/probability_values.h"
#include "cartographer/sensor/point_cloud.h"
#include "ceres/ceres.h"
#include "ceres/cubic_interpolation.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
// Computes a cost for matching the 'point_cloud' to the 'grid' with
// a 'pose'. The cost increases with poorer correspondence of the grid and the
// point observation (e.g. points falling into less occupied space).
class OccupiedSpaceCostFunction2D {
private:
static constexpr int kPadding = INT_MAX / 4;
class GridArrayAdapter {
public:
enum { DATA_DIMENSION = 1 };
explicit GridArrayAdapter(const Grid2D& grid) : grid_(grid) {}
void GetValue(const int row, const int column, double* const value) const {
if (row < kPadding || column < kPadding || row >= NumRows() - kPadding ||
column >= NumCols() - kPadding) {
*value = kMaxCorrespondenceCost;
} else {
*value = static_cast<double>(grid_.GetCorrespondenceCost(
Eigen::Array2i(column - kPadding, row - kPadding)));
}
}
int NumRows() const {
return grid_.limits().cell_limits().num_y_cells + 2 * kPadding;
}
int NumCols() const {
return grid_.limits().cell_limits().num_x_cells + 2 * kPadding;
}
private:
const Grid2D& grid_;
};
OccupiedSpaceCostFunction2D(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const Grid2D& grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
grid_(grid) {}
OccupiedSpaceCostFunction2D(const OccupiedSpaceCostFunction2D&) = delete;
OccupiedSpaceCostFunction2D& operator=(const OccupiedSpaceCostFunction2D&) =
delete;
const double scaling_factor_;
const sensor::PointCloud& point_cloud_;
const Grid2D& grid_;
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const Grid2D& grid) {
return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction2D,
ceres::DYNAMIC /* residuals */,
3 /* pose variables */>(
new OccupiedSpaceCostFunction2D(scaling_factor, point_cloud, grid),
point_cloud.size()); // 比固定残差维度的 多了一个参数
}
template <typename T>
bool operator()(const T* const pose, T* residual) const {
Eigen::Matrix<T, 2, 1> translation(pose[0], pose[1]);
Eigen::Rotation2D<T> rotation(pose[2]);
Eigen::Matrix<T, 2, 2> rotation_matrix = rotation.toRotationMatrix();
Eigen::Matrix<T, 3, 3> transform;
transform << rotation_matrix, translation, T(0.), T(0.), T(1.);
const GridArrayAdapter adapter(grid_);
ceres::BiCubicInterpolator<GridArrayAdapter> interpolator(adapter);
const MapLimits& limits = grid_.limits();
for (size_t i = 0; i < point_cloud_.size(); ++i)
{
// Note that this is a 2D point. The third component is a scaling factor.
const Eigen::Matrix<T, 3, 1> point((T(point_cloud_[i].x())),
(T(point_cloud_[i].y())), T(1.));
// 根据预测位姿对单个点进行坐标变换
const Eigen::Matrix<T, 3, 1> world = transform * point;
获取三次插值之后的栅格free值, Evaluate函数内部调用了GetValue函数
interpolator.Evaluate(
(limits.max().x() - world[0]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding),
(limits.max().y() - world[1]) / limits.resolution() - 0.5 +
static_cast<double>(kPadding),
&residual[i]);
// free值越小, 表示占用的概率越大
residual[i] = scaling_factor_ * residual[i];
}
return true;
}
};
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer
2.平移的残差
TranslationDeltaCostFunctor2D得定义：
translation_delta_cost_functor_2d.h
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_
#include "Eigen/Core"
#include "ceres/ceres.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
// Computes the cost of translating 'pose' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor2D {
public:
// 静态成员函数, 返回CostFunction
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Vector2d& target_translation) {
return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor2D,
2 /* residuals */,
3 /* pose variables */>(
new TranslationDeltaCostFunctor2D(scaling_factor, target_translation));
}
// 平移量残差的计算, (pose[0] - x_)的平方ceres会自动加上
template <typename T>
bool operator()(const T* const pose, T* residual) const {
residual[0] = scaling_factor_ * (pose[0] - x_);
residual[1] = scaling_factor_ * (pose[1] - y_);
return true;
}
private:
// Constructs a new TranslationDeltaCostFunctor2D from the given
// 'target_translation' (x, y).
explicit TranslationDeltaCostFunctor2D(
const double scaling_factor, const Eigen::Vector2d& target_translation)
: scaling_factor_(scaling_factor),
x_(target_translation.x()),
y_(target_translation.y()) {}
TranslationDeltaCostFunctor2D(const TranslationDeltaCostFunctor2D&) = delete;
TranslationDeltaCostFunctor2D& operator=(
const TranslationDeltaCostFunctor2D&) = delete;
const double scaling_factor_;
const double x_;
const double y_;
};
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_2D_H_
3.旋转的残差
RotationDeltaCostFunctor2D成本函数得定义：
rotation_delta_cost_functor_2d.h
#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_
#include "Eigen/Core"
#include "ceres/ceres.h"
namespace cartographer {
namespace mapping {
namespace scan_matching {
// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with
// the solution's distance from 'target_angle'.
class RotationDeltaCostFunctor2D {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const double target_angle) {
return new ceres::AutoDiffCostFunction<
RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>(
new RotationDeltaCostFunctor2D(scaling_factor, target_angle));
}
// 旋转量残差的计算
template <typename T>
bool operator()(const T* const pose, T* residual) const {
residual[0] = scaling_factor_ * (pose[2] - angle_);
return true;
}
private:
explicit RotationDeltaCostFunctor2D(const double scaling_factor,
const double target_angle)
: scaling_factor_(scaling_factor), angle_(target_angle) {}
RotationDeltaCostFunctor2D(const RotationDeltaCostFunctor2D&) = delete;
RotationDeltaCostFunctor2D& operator=(const RotationDeltaCostFunctor2D&) =
delete;
const double scaling_factor_;
const double angle_;
};
} // namespace scan_matching
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_
三、总结
CeresScanMatcher2D::Match 优化的是物理坐标, 不是像素坐标, 所以坐标是连续的.
可能有问题的点
平移和旋转的残差项是逼近于先验位姿的, 当先验位姿不准确时会产生问题
可能的改进建议
先将地图的权重调大, 平移旋转的权重调小, 如 1000, 1, 1, 或者 100, 1, 1
调参没有作用的时候可以将平移和旋转的残差项注释掉
390
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
