12345qiupeng
/
cartographer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Create AutoDiffCostFunction in cost functions. (#718) 

Creating a ceres::AutoDiffCostFunction requires specifying numbers
of residuals and variables, so it is safer to implement this within
the cost functions, which know best.
master
gaschler 2017-11-30 15:41:57 +01:00 committed by GitHub
parent 85bfb888eb
commit 63e901d276
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
14 changed files with 223 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
cartographer/internal/mapping_2d/scan_matching/occupied_space_cost_function.h
View File

@ -33,16 +33,16 @@ namespace scan_matching {
// at pixels with lower values.
class OccupiedSpaceCostFunction {
public:
OccupiedSpaceCostFunction(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
probability_grid_(probability_grid) {}
OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
delete;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid) {
return new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
ceres::DYNAMIC /* residuals */,
3 /* pose variables */>(
new OccupiedSpaceCostFunction(scaling_factor, point_cloud,
probability_grid),
point_cloud.size());
}
template <typename T>
bool operator()(const T* const pose, T* residual) const {
@ -105,6 +105,17 @@ class OccupiedSpaceCostFunction {
const ProbabilityGrid& probability_grid_;
};
OccupiedSpaceCostFunction(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const ProbabilityGrid& probability_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
probability_grid_(probability_grid) {}
OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
delete;
const double scaling_factor_;
const sensor::PointCloud& point_cloud_;
const ProbabilityGrid& probability_grid_;

36
cartographer/internal/mapping_3d/acceleration_cost_function.h
View File

@ -26,17 +26,19 @@ namespace mapping_3d {
// Penalizes differences between IMU data and optimized accelerations.
class AccelerationCostFunction {
public:
AccelerationCostFunction(const double scaling_factor,
const Eigen::Vector3d& delta_velocity_imu_frame,
const double first_delta_time_seconds,
const double second_delta_time_seconds)
: scaling_factor_(scaling_factor),
delta_velocity_imu_frame_(delta_velocity_imu_frame),
first_delta_time_seconds_(first_delta_time_seconds),
second_delta_time_seconds_(second_delta_time_seconds) {}
AccelerationCostFunction(const AccelerationCostFunction&) = delete;
AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor,
const Eigen::Vector3d& delta_velocity_imu_frame,
const double first_delta_time_seconds,
const double second_delta_time_seconds) {
return new ceres::AutoDiffCostFunction<
AccelerationCostFunction, 3 /* residuals */, 4 /* rotation variables */,
3 /* position variables */, 3 /* position variables */,
3 /* position variables */, 1 /* gravity variables */,
4 /* rotation variables */>(new AccelerationCostFunction(
scaling_factor, delta_velocity_imu_frame, first_delta_time_seconds,
second_delta_time_seconds));
}
template <typename T>
bool operator()(const T* const middle_rotation, const T* const start_position,
@ -69,6 +71,18 @@ class AccelerationCostFunction {
}
private:
AccelerationCostFunction(const double scaling_factor,
const Eigen::Vector3d& delta_velocity_imu_frame,
const double first_delta_time_seconds,
const double second_delta_time_seconds)
: scaling_factor_(scaling_factor),
delta_velocity_imu_frame_(delta_velocity_imu_frame),
first_delta_time_seconds_(first_delta_time_seconds),
second_delta_time_seconds_(second_delta_time_seconds) {}
AccelerationCostFunction(const AccelerationCostFunction&) = delete;
AccelerationCostFunction& operator=(const AccelerationCostFunction&) = delete;
template <typename T>
static Eigen::Quaternion<T> ToEigen(const T* const quaternion) {
return Eigen::Quaternion<T>(quaternion[0], quaternion[1], quaternion[2],

23
cartographer/internal/mapping_3d/rotation_cost_function.h
View File

@ -26,13 +26,14 @@ namespace mapping_3d {
// Penalizes differences between IMU data and optimized orientations.
class RotationCostFunction {
public:
RotationCostFunction(const double scaling_factor,
const Eigen::Quaterniond& delta_rotation_imu_frame)
: scaling_factor_(scaling_factor),
delta_rotation_imu_frame_(delta_rotation_imu_frame) {}
RotationCostFunction(const RotationCostFunction&) = delete;
RotationCostFunction& operator=(const RotationCostFunction&) = delete;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor,
const Eigen::Quaterniond& delta_rotation_imu_frame) {
return new ceres::AutoDiffCostFunction<
RotationCostFunction, 3 /* residuals */, 4 /* rotation variables */,
4 /* rotation variables */, 4 /* rotation variables */
>(new RotationCostFunction(scaling_factor, delta_rotation_imu_frame));
}
template <typename T>
bool operator()(const T* const start_rotation, const T* const end_rotation,
@ -54,6 +55,14 @@ class RotationCostFunction {
}
private:
RotationCostFunction(const double scaling_factor,
const Eigen::Quaterniond& delta_rotation_imu_frame)
: scaling_factor_(scaling_factor),
delta_rotation_imu_frame_(delta_rotation_imu_frame) {}
RotationCostFunction(const RotationCostFunction&) = delete;
RotationCostFunction& operator=(const RotationCostFunction&) = delete;
const double scaling_factor_;
const Eigen::Quaterniond delta_rotation_imu_frame_;
};

32
cartographer/internal/mapping_3d/scan_matching/occupied_space_cost_function.h
View File

@ -33,16 +33,15 @@ namespace scan_matching {
// occupied space, i.e. at voxels with lower values.
class OccupiedSpaceCostFunction {
public:
OccupiedSpaceCostFunction(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const HybridGrid& hybrid_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
interpolated_grid_(hybrid_grid) {}
OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
delete;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const sensor::PointCloud& point_cloud,
const HybridGrid& hybrid_grid) {
return new ceres::AutoDiffCostFunction<
OccupiedSpaceCostFunction, ceres::DYNAMIC /* residuals */,
3 /* translation variables */, 4 /* rotation variables */>(
new OccupiedSpaceCostFunction(scaling_factor, point_cloud, hybrid_grid),
point_cloud.size());
}
template <typename T>
bool operator()(const T* const translation, const T* const rotation,
@ -54,6 +53,18 @@ class OccupiedSpaceCostFunction {
return Evaluate(transform, residual);
}
private:
OccupiedSpaceCostFunction(const double scaling_factor,
const sensor::PointCloud& point_cloud,
const HybridGrid& hybrid_grid)
: scaling_factor_(scaling_factor),
point_cloud_(point_cloud),
interpolated_grid_(hybrid_grid) {}
OccupiedSpaceCostFunction(const OccupiedSpaceCostFunction&) = delete;
OccupiedSpaceCostFunction& operator=(const OccupiedSpaceCostFunction&) =
delete;
template <typename T>
bool Evaluate(const transform::Rigid3<T>& transform,
T* const residual) const {
@ -67,7 +78,6 @@ class OccupiedSpaceCostFunction {
return true;
}
private:
const double scaling_factor_;
const sensor::PointCloud& point_cloud_;
const InterpolatedGrid interpolated_grid_;

10
cartographer/mapping_2d/pose_graph/optimization_problem.cc
View File

@ -152,8 +152,7 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(constraint.pose)),
SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose),
// Only loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
@ -187,10 +186,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
const transform::Rigid3d relative_pose =
ComputeRelativePose(trajectory_id, first_node_data, second_node_data);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 3, 3, 3>(
new SpaCostFunction(Constraint::Pose{
relative_pose, options_.consecutive_node_translation_weight(),
options_.consecutive_node_rotation_weight()})),
SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{
relative_pose, options_.consecutive_node_translation_weight(),
options_.consecutive_node_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).data(),
C_nodes.at(second_node_id).data());
}

22
cartographer/mapping_2d/pose_graph/spa_cost_function.h
View File

@ -36,6 +36,21 @@ class SpaCostFunction {
public:
using Constraint = mapping::PoseGraph::Constraint;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const Constraint::Pose& pose) {
return new ceres::AutoDiffCostFunction<SpaCostFunction, 3 /* residuals */,
3 /* pose variables */,
3 /* pose variables */>(
new SpaCostFunction(pose));
}
template <typename T>
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
ComputeScaledError(pose_, c_i, c_j, e);
return true;
}
private:
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
// Computes the error between the node-to-submap alignment 'zbar_ij' and the
@ -71,13 +86,6 @@ class SpaCostFunction {
e[2] = e_ij[2] * T(pose.rotation_weight);
}
template <typename T>
bool operator()(const T* const c_i, const T* const c_j, T* e) const {
ComputeScaledError(pose_, c_i, c_j, e);
return true;
}
private:
const Constraint::Pose pose_;
};

27
cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc
View File

@ -71,26 +71,21 @@ void CeresScanMatcher::Match(const Eigen::Vector2d& target_translation,
ceres::Problem problem;
CHECK_GT(options_.occupied_space_weight(), 0.);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction, ceres::DYNAMIC,
3>(
new OccupiedSpaceCostFunction(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, probability_grid),
point_cloud.size()),
nullptr, ceres_pose_estimate);
OccupiedSpaceCostFunction::CreateAutoDiffCostFunction(
options_.occupied_space_weight() /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, probability_grid),
nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.translation_weight(), 0.);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
new TranslationDeltaCostFunctor(options_.translation_weight(),
target_translation)),
nullptr, ceres_pose_estimate);
TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr /* loss function */, ceres_pose_estimate);
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 1, 3>(
new RotationDeltaCostFunctor(options_.rotation_weight(),
ceres_pose_estimate[2])),
nullptr, ceres_pose_estimate);
RotationDeltaCostFunctor::CreateAutoDiffCostFunction(
options_.rotation_weight(), ceres_pose_estimate[2]),
nullptr /* loss function */, ceres_pose_estimate);
ceres::Solve(ceres_solver_options_, &problem, summary);

20
cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h
View File

@ -27,13 +27,12 @@ namespace scan_matching {
// the solution's distance from 'target_angle'.
class RotationDeltaCostFunctor {
public:
// Constructs a new RotationDeltaCostFunctor for the given 'angle'.
explicit RotationDeltaCostFunctor(const double scaling_factor,
const double target_angle)
: scaling_factor_(scaling_factor), angle_(target_angle) {}
RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const double target_angle) {
return new ceres::AutoDiffCostFunction<
RotationDeltaCostFunctor, 1 /* residuals */, 3 /* pose variables */>(
new RotationDeltaCostFunctor(scaling_factor, target_angle));
}
template <typename T>
bool operator()(const T* const pose, T* residual) const {
@ -42,6 +41,13 @@ class RotationDeltaCostFunctor {
}
private:
explicit RotationDeltaCostFunctor(const double scaling_factor,
const double target_angle)
: scaling_factor_(scaling_factor), angle_(target_angle) {}
RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
const double scaling_factor_;
const double angle_;
};

23
cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h
View File

@ -27,6 +27,21 @@ namespace scan_matching {
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Vector2d& target_translation) {
return new ceres::AutoDiffCostFunction<
TranslationDeltaCostFunctor, 2 /* residuals */, 3 /* pose variables */>(
new TranslationDeltaCostFunctor(scaling_factor, target_translation));
}
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 TranslationDeltaCostFunctor from the given
// 'target_translation' (x, y).
explicit TranslationDeltaCostFunctor(
@ -39,14 +54,6 @@ class TranslationDeltaCostFunctor {
TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =
delete;
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:
const double scaling_factor_;
const double x_;
const double y_;

38
cartographer/mapping_3d/pose_graph/optimization_problem.cc
View File

@ -222,12 +222,11 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
// Add cost functions for intra- and inter-submap constraints.
for (const Constraint& constraint : constraints) {
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(constraint.pose)),
SpaCostFunction::CreateAutoDiffCostFunction(constraint.pose),
// Only loop closure constraints should have a loss function.
constraint.tag == Constraint::INTER_SUBMAP
? new ceres::HuberLoss(options_.huber_scale())
: nullptr,
: nullptr /* loss function */,
C_submaps.at(constraint.submap_id).rotation(),
C_submaps.at(constraint.submap_id).translation(),
C_nodes.at(constraint.node_id).rotation(),
@ -301,13 +300,12 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
result_to_first_center.delta_rotation) *
result_center_to_center.delta_velocity;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
3, 3, 1, 4>(
new AccelerationCostFunction(
options_.acceleration_weight(), delta_velocity,
common::ToSeconds(first_duration),
common::ToSeconds(second_duration))),
nullptr, C_nodes.at(second_node_id).rotation(),
AccelerationCostFunction::CreateAutoDiffCostFunction(
options_.acceleration_weight(), delta_velocity,
common::ToSeconds(first_duration),
common::ToSeconds(second_duration)),
nullptr /* loss function */,
C_nodes.at(second_node_id).rotation(),
C_nodes.at(first_node_id).translation(),
C_nodes.at(second_node_id).translation(),
C_nodes.at(third_node_id).translation(),
@ -315,10 +313,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
trajectory_data.imu_calibration.data());
}
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
new RotationCostFunction(options_.rotation_weight(),
result.delta_rotation)),
nullptr, C_nodes.at(first_node_id).rotation(),
RotationCostFunction::CreateAutoDiffCostFunction(
options_.rotation_weight(), result.delta_rotation),
nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
C_nodes.at(second_node_id).rotation(),
trajectory_data.imu_calibration.data());
}
@ -351,11 +348,9 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
const transform::Rigid3d relative_pose = ComputeRelativePose(
trajectory_id, first_node_data, second_node_data);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(Constraint::Pose{
relative_pose,
options_.consecutive_node_translation_weight(),
options_.consecutive_node_rotation_weight()})),
SpaCostFunction::CreateAutoDiffCostFunction(Constraint::Pose{
relative_pose, options_.consecutive_node_translation_weight(),
options_.consecutive_node_rotation_weight()}),
nullptr /* loss function */, C_nodes.at(first_node_id).rotation(),
C_nodes.at(first_node_id).translation(),
C_nodes.at(second_node_id).rotation(),
@ -406,9 +401,8 @@ void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
}
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
new SpaCostFunction(constraint_pose)),
nullptr, C_fixed_frames.back().rotation(),
SpaCostFunction::CreateAutoDiffCostFunction(constraint_pose),
nullptr /* loss function */, C_fixed_frames.back().rotation(),
C_fixed_frames.back().translation(), C_nodes.at(node_id).rotation(),
C_nodes.at(node_id).translation());
}

28
cartographer/mapping_3d/pose_graph/spa_cost_function.h
View File

@ -36,6 +36,24 @@ class SpaCostFunction {
public:
using Constraint = mapping::PoseGraph::Constraint;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const Constraint::Pose& pose) {
return new ceres::AutoDiffCostFunction<
SpaCostFunction, 6 /* residuals */, 4 /* rotation variables */,
3 /* translation variables */, 4 /* rotation variables */,
3 /* translation variables */>(new SpaCostFunction(pose));
}
template <typename T>
bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
const T* const c_j_rotation, const T* const c_j_translation,
T* const e) const {
ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation,
c_j_translation, e);
return true;
}
private:
explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
// Computes the error between the node-to-submap alignment 'zbar_ij' and the
@ -90,16 +108,6 @@ class SpaCostFunction {
}
}
template <typename T>
bool operator()(const T* const c_i_rotation, const T* const c_i_translation,
const T* const c_j_rotation, const T* const c_j_translation,
T* const e) const {
ComputeScaledError(pose_, c_i_rotation, c_i_translation, c_j_rotation,
c_j_translation, e);
return true;
}
private:
const Constraint::Pose pose_;
};

28
cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc
View File

@ -93,27 +93,23 @@ void CeresScanMatcher::Match(const Eigen::Vector3d& target_translation,
*point_clouds_and_hybrid_grids[i].first;
const HybridGrid& hybrid_grid = *point_clouds_and_hybrid_grids[i].second;
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<OccupiedSpaceCostFunction,
ceres::DYNAMIC, 3, 4>(
new OccupiedSpaceCostFunction(
options_.occupied_space_weight(i) /
std::sqrt(static_cast<double>(point_cloud.size())),
point_cloud, hybrid_grid),
point_cloud.size()),
nullptr, ceres_pose.translation(), ceres_pose.rotation());
OccupiedSpaceCostFunction::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(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 3, 3>(
new TranslationDeltaCostFunctor(options_.translation_weight(),
target_translation)),
nullptr, ceres_pose.translation());
TranslationDeltaCostFunctor::CreateAutoDiffCostFunction(
options_.translation_weight(), target_translation),
nullptr /* loss function */, ceres_pose.translation());
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor, 3, 4>(
new RotationDeltaCostFunctor(options_.rotation_weight(),
initial_pose_estimate.rotation())),
nullptr, ceres_pose.rotation());
RotationDeltaCostFunctor::CreateAutoDiffCostFunction(
options_.rotation_weight(), initial_pose_estimate.rotation()),
nullptr /* loss function */, ceres_pose.rotation());
ceres::Solve(ceres_solver_options_, &problem, summary);

30
cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h
View File

@ -30,19 +30,14 @@ namespace scan_matching {
// Cost increases with the solution's distance from 'target_rotation'.
class RotationDeltaCostFunctor {
public:
// Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'.
explicit RotationDeltaCostFunctor(const double scaling_factor,
const Eigen::Quaterniond& target_rotation)
: scaling_factor_(scaling_factor) {
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();
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Quaterniond& target_rotation) {
return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor,
3 /* residuals */,
4 /* rotation variables */>(
new RotationDeltaCostFunctor(scaling_factor, target_rotation));
}
RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
template <typename T>
bool operator()(const T* const rotation_quaternion, T* residual) const {
T delta[4];
@ -60,6 +55,19 @@ class RotationDeltaCostFunctor {
}
private:
// Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'.
explicit RotationDeltaCostFunctor(const double scaling_factor,
const Eigen::Quaterniond& target_rotation)
: scaling_factor_(scaling_factor) {
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();
}
RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;
const double scaling_factor_;
double target_rotation_inverse_[4];
};

26
cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h
View File

@ -27,6 +27,23 @@ namespace scan_matching {
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor {
public:
static ceres::CostFunction* CreateAutoDiffCostFunction(
const double scaling_factor, const Eigen::Vector3d& target_translation) {
return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor,
3 /* residuals */,
3 /* translation variables */>(
new TranslationDeltaCostFunctor(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 TranslationDeltaCostFunctor from the given
// 'target_translation'.
explicit TranslationDeltaCostFunctor(
@ -40,15 +57,6 @@ class TranslationDeltaCostFunctor {
TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =
delete;
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:
const double scaling_factor_;
const double x_;
const double y_;