Rename arguments of CostFunctors to target_ (#700)

Rename constant targets of cost functors to target_*.
Pass only translation to Translation cost functors.
master
gaschler 2017-11-23 16:04:49 +01:00 committed by Wally B. Feed
parent 38eeb17164
commit 9bfc52d878
6 changed files with 32 additions and 40 deletions

View File

@ -83,7 +83,7 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose,
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 2, 3>(
new TranslationDeltaCostFunctor(options_.translation_weight(),
previous_pose)),
previous_pose.translation())),
nullptr, ceres_pose_estimate);
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(

View File

@ -18,20 +18,19 @@
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_H_
#include "Eigen/Core"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace mapping_2d {
namespace scan_matching {
// Computes the cost of rotating the initial pose estimate. Cost increases with
// the solution's distance from the initial estimate.
// Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with
// 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 angle)
: scaling_factor_(scaling_factor), angle_(angle) {}
const double target_angle)
: scaling_factor_(scaling_factor), angle_(target_angle) {}
RotationDeltaCostFunctor(const RotationDeltaCostFunctor&) = delete;
RotationDeltaCostFunctor& operator=(const RotationDeltaCostFunctor&) = delete;

View File

@ -18,24 +18,22 @@
#define CARTOGRAPHER_MAPPING_2D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
#include "Eigen/Core"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace mapping_2d {
namespace scan_matching {
// Computes the cost of translating the initial pose estimate. Cost increases
// with the solution's distance from the initial estimate.
// Computes the cost of translating 'pose' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor {
public:
// Constructs a new TranslationDeltaCostFunctor from the given
// 'initial_pose_estimate' (x, y, theta).
// 'target_translation' (x, y).
explicit TranslationDeltaCostFunctor(
const double scaling_factor,
const transform::Rigid2d& initial_pose_estimate)
const double scaling_factor, const Eigen::Vector2d& target_translation)
: scaling_factor_(scaling_factor),
x_(initial_pose_estimate.translation().x()),
y_(initial_pose_estimate.translation().y()) {}
x_(target_translation.x()),
y_(target_translation.y()) {}
TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete;
TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =

View File

@ -106,7 +106,7 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose,
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor, 3, 3>(
new TranslationDeltaCostFunctor(options_.translation_weight(),
previous_pose)),
previous_pose.translation())),
nullptr, ceres_pose.translation());
CHECK_GT(options_.rotation_weight(), 0.);
problem.AddResidualBlock(

View File

@ -20,27 +20,24 @@
#include <cmath>
#include "Eigen/Core"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/rotation.h"
namespace cartographer {
namespace mapping_3d {
namespace scan_matching {
// Computes the cost of rotating the pose estimate. Cost increases with the
// solution's distance from the rotation estimate.
// Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'.
// Cost increases with the solution's distance from 'target_rotation'.
class RotationDeltaCostFunctor {
public:
// Constructs a new RotationDeltaCostFunctor from the given
// 'rotation_estimate'.
// Constructs a new RotationDeltaCostFunctor from the given 'target_rotation'.
explicit RotationDeltaCostFunctor(const double scaling_factor,
const Eigen::Quaterniond& initial_rotation)
const Eigen::Quaterniond& target_rotation)
: scaling_factor_(scaling_factor) {
initial_rotation_inverse_[0] = initial_rotation.w();
initial_rotation_inverse_[1] = -initial_rotation.x();
initial_rotation_inverse_[2] = -initial_rotation.y();
initial_rotation_inverse_[3] = -initial_rotation.z();
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;
@ -49,10 +46,10 @@ class RotationDeltaCostFunctor {
template <typename T>
bool operator()(const T* const rotation_quaternion, T* residual) const {
T delta[4];
T initial_rotation_inverse[4] = {
T(initial_rotation_inverse_[0]), T(initial_rotation_inverse_[1]),
T(initial_rotation_inverse_[2]), T(initial_rotation_inverse_[3])};
ceres::QuaternionProduct(initial_rotation_inverse, rotation_quaternion,
T target_rotation_inverse[4] = {
T(target_rotation_inverse_[0]), T(target_rotation_inverse_[1]),
T(target_rotation_inverse_[2]), T(target_rotation_inverse_[3])};
ceres::QuaternionProduct(target_rotation_inverse, rotation_quaternion,
delta);
// Will compute the squared norm of the imaginary component of the delta
// quaternion which is sin(phi/2)^2.
@ -64,7 +61,7 @@ class RotationDeltaCostFunctor {
private:
const double scaling_factor_;
double initial_rotation_inverse_[4];
double target_rotation_inverse_[4];
};
} // namespace scan_matching

View File

@ -18,25 +18,23 @@
#define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
#include "Eigen/Core"
#include "cartographer/transform/rigid_transform.h"
namespace cartographer {
namespace mapping_3d {
namespace scan_matching {
// Computes the cost of translating the initial pose estimate. Cost increases
// with the solution's distance from the initial estimate.
// Computes the cost of translating 'translation' to 'target_translation'.
// Cost increases with the solution's distance from 'target_translation'.
class TranslationDeltaCostFunctor {
public:
// Constructs a new TranslationDeltaCostFunctor from the given
// 'initial_pose_estimate'.
// 'target_translation'.
explicit TranslationDeltaCostFunctor(
const double scaling_factor,
const transform::Rigid3d& initial_pose_estimate)
const double scaling_factor, const Eigen::Vector3d& target_translation)
: scaling_factor_(scaling_factor),
x_(initial_pose_estimate.translation().x()),
y_(initial_pose_estimate.translation().y()),
z_(initial_pose_estimate.translation().z()) {}
x_(target_translation.x()),
y_(target_translation.y()),
z_(target_translation.z()) {}
TranslationDeltaCostFunctor(const TranslationDeltaCostFunctor&) = delete;
TranslationDeltaCostFunctor& operator=(const TranslationDeltaCostFunctor&) =