From 9bfc52d878f29e060dcf63eaf5a6a142a82303b5 Mon Sep 17 00:00:00 2001 From: gaschler Date: Thu, 23 Nov 2017 16:04:49 +0100 Subject: [PATCH] Rename arguments of CostFunctors to target_ (#700) Rename constant targets of cost functors to target_*. Pass only translation to Translation cost functors. --- .../scan_matching/ceres_scan_matcher.cc | 2 +- .../rotation_delta_cost_functor.h | 9 +++--- .../translation_delta_cost_functor.h | 14 ++++----- .../scan_matching/ceres_scan_matcher.cc | 2 +- .../rotation_delta_cost_functor.h | 29 +++++++++---------- .../translation_delta_cost_functor.h | 16 +++++----- 6 files changed, 32 insertions(+), 40 deletions(-) diff --git a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc index d55f0b7..b079366 100644 --- a/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_2d/scan_matching/ceres_scan_matcher.cc @@ -83,7 +83,7 @@ void CeresScanMatcher::Match(const transform::Rigid2d& previous_pose, problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new TranslationDeltaCostFunctor(options_.translation_weight(), - previous_pose)), + previous_pose.translation())), nullptr, ceres_pose_estimate); CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( diff --git a/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h b/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h index ff2a41b..f5a0275 100644 --- a/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h +++ b/cartographer/mapping_2d/scan_matching/rotation_delta_cost_functor.h @@ -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; diff --git a/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h b/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h index 6bfd241..5f3c34a 100644 --- a/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h +++ b/cartographer/mapping_2d/scan_matching/translation_delta_cost_functor.h @@ -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&) = diff --git a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc index c7dbd97..362f5c6 100644 --- a/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc +++ b/cartographer/mapping_3d/scan_matching/ceres_scan_matcher.cc @@ -106,7 +106,7 @@ void CeresScanMatcher::Match(const transform::Rigid3d& previous_pose, problem.AddResidualBlock( new ceres::AutoDiffCostFunction( new TranslationDeltaCostFunctor(options_.translation_weight(), - previous_pose)), + previous_pose.translation())), nullptr, ceres_pose.translation()); CHECK_GT(options_.rotation_weight(), 0.); problem.AddResidualBlock( diff --git a/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h b/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h index 6f271cb..50bdf50 100644 --- a/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h +++ b/cartographer/mapping_3d/scan_matching/rotation_delta_cost_functor.h @@ -20,27 +20,24 @@ #include #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 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 diff --git a/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h b/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h index f7ac8b8..c2e2385 100644 --- a/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h +++ b/cartographer/mapping_3d/scan_matching/translation_delta_cost_functor.h @@ -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&) =