From ab890a8e159743b6f4b719a4cdd0f65ba5bb4534 Mon Sep 17 00:00:00 2001 From: Alexander Belyaev <32522095+pifon2a@users.noreply.github.com> Date: Wed, 31 Jan 2018 18:40:10 +0100 Subject: [PATCH] Move 3D landmark cost function to mapping_3d/. (#870) [RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md) --- .../pose_graph/landmark_cost_function.h | 27 +++++++++++-------- .../pose_graph/landmark_cost_function_test.cc | 13 ++++++--- 2 files changed, 25 insertions(+), 15 deletions(-) rename cartographer/{mapping => mapping_3d}/pose_graph/landmark_cost_function.h (81%) rename cartographer/{mapping => mapping_3d}/pose_graph/landmark_cost_function_test.cc (88%) diff --git a/cartographer/mapping/pose_graph/landmark_cost_function.h b/cartographer/mapping_3d/pose_graph/landmark_cost_function.h similarity index 81% rename from cartographer/mapping/pose_graph/landmark_cost_function.h rename to cartographer/mapping_3d/pose_graph/landmark_cost_function.h index ab3736d..46a028f 100644 --- a/cartographer/mapping/pose_graph/landmark_cost_function.h +++ b/cartographer/mapping_3d/pose_graph/landmark_cost_function.h @@ -14,20 +14,21 @@ * limitations under the License. */ -#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ -#define CARTOGRAPHER_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ +#ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ +#define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ #include "Eigen/Core" #include "Eigen/Geometry" #include "cartographer/mapping/pose_graph/cost_helpers.h" #include "cartographer/mapping/pose_graph_interface.h" +#include "cartographer/mapping_3d/pose_graph/optimization_problem.h" #include "cartographer/transform/rigid_transform.h" #include "cartographer/transform/transform.h" #include "ceres/ceres.h" #include "ceres/jet.h" namespace cartographer { -namespace mapping { +namespace mapping_3d { namespace pose_graph { // Cost function measuring the weighted error between the observed pose given by @@ -38,8 +39,8 @@ class LandmarkCostFunction { mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; static ceres::CostFunction* CreateAutoDiffCostFunction( - const LandmarkObservation& observation, common::Time prev_node_time, - common::Time next_node_time) { + const LandmarkObservation& observation, const NodeData& prev_node, + const NodeData& next_node) { return new ceres::AutoDiffCostFunction< LandmarkCostFunction, 6 /* residuals */, 4 /* previous node rotation variables */, @@ -48,7 +49,7 @@ class LandmarkCostFunction { 3 /* next node translation variables */, 4 /* landmark rotation variables */, 3 /* landmark translation variables */>( - new LandmarkCostFunction(observation, prev_node_time, next_node_time)); + new LandmarkCostFunction(observation, prev_node, next_node)); } template @@ -58,6 +59,10 @@ class LandmarkCostFunction { const T* const next_node_translation, const T* const landmark_rotation, const T* const landmark_translation, T* const e) const { + using mapping::pose_graph::ComputeUnscaledError; + using mapping::pose_graph::ScaleError; + using mapping::pose_graph::SlerpQuaternions; + const std::array interpolated_pose_translation{ {prev_node_translation[0] + interpolation_parameter_ * @@ -83,14 +88,14 @@ class LandmarkCostFunction { private: LandmarkCostFunction(const LandmarkObservation& observation, - common::Time prev_node_time, common::Time next_node_time) + const NodeData& prev_node, const NodeData& next_node) : landmark_to_tracking_transform_( observation.landmark_to_tracking_transform), translation_weight_(observation.translation_weight), rotation_weight_(observation.rotation_weight), interpolation_parameter_( - common::ToSeconds(observation.time - prev_node_time) / - common::ToSeconds(next_node_time - prev_node_time)) {} + common::ToSeconds(observation.time - prev_node.time) / + common::ToSeconds(next_node.time - prev_node.time)) {} const transform::Rigid3d landmark_to_tracking_transform_; const double translation_weight_; @@ -99,7 +104,7 @@ class LandmarkCostFunction { }; } // namespace pose_graph -} // namespace mapping +} // namespace mapping_3d } // namespace cartographer -#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ +#endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ diff --git a/cartographer/mapping/pose_graph/landmark_cost_function_test.cc b/cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc similarity index 88% rename from cartographer/mapping/pose_graph/landmark_cost_function_test.cc rename to cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc index 85242b0..47cfc08 100644 --- a/cartographer/mapping/pose_graph/landmark_cost_function_test.cc +++ b/cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc @@ -14,14 +14,14 @@ * limitations under the License. */ -#include "cartographer/mapping/pose_graph/landmark_cost_function.h" +#include "cartographer/mapping_3d/pose_graph/landmark_cost_function.h" #include "cartographer/transform/rigid_transform.h" #include "gmock/gmock.h" #include "gtest/gtest.h" namespace cartographer { -namespace mapping { +namespace mapping_3d { namespace pose_graph { namespace { @@ -32,6 +32,11 @@ using LandmarkObservation = mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; TEST(LandmarkCostFunctionTest, SmokeTest) { + NodeData prev_node; + prev_node.time = common::FromUniversal(0); + NodeData next_node; + next_node.time = common::FromUniversal(10); + auto* cost_function = LandmarkCostFunction::CreateAutoDiffCostFunction( LandmarkObservation{ 0 /* trajectory ID */, @@ -40,7 +45,7 @@ TEST(LandmarkCostFunctionTest, SmokeTest) { 1. /* translation_weight */, 2. /* rotation_weight */, }, - common::FromUniversal(0), common::FromUniversal(10)); + prev_node, next_node); std::array prev_node_rotation{{1., 0., 0., 0.}}; std::array prev_node_translation{{0., 0., 0.}}; @@ -62,5 +67,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) { } // namespace } // namespace pose_graph -} // namespace mapping +} // namespace mapping_3d } // namespace cartographer