Move 3D landmark cost function to mapping_3d/. (#870)

[RFC=0011](https://github.com/googlecartographer/rfcs/blob/master/text/0011-landmarks.md)
master
Alexander Belyaev 2018-01-31 18:40:10 +01:00 committed by GitHub
parent a749d28a67
commit ab890a8e15
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 25 additions and 15 deletions

View File

@ -14,20 +14,21 @@
* limitations under the License. * limitations under the License.
*/ */
#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ #ifndef CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ #define CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_
#include "Eigen/Core" #include "Eigen/Core"
#include "Eigen/Geometry" #include "Eigen/Geometry"
#include "cartographer/mapping/pose_graph/cost_helpers.h" #include "cartographer/mapping/pose_graph/cost_helpers.h"
#include "cartographer/mapping/pose_graph_interface.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/rigid_transform.h"
#include "cartographer/transform/transform.h" #include "cartographer/transform/transform.h"
#include "ceres/ceres.h" #include "ceres/ceres.h"
#include "ceres/jet.h" #include "ceres/jet.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping_3d {
namespace pose_graph { namespace pose_graph {
// Cost function measuring the weighted error between the observed pose given by // Cost function measuring the weighted error between the observed pose given by
@ -38,8 +39,8 @@ class LandmarkCostFunction {
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
static ceres::CostFunction* CreateAutoDiffCostFunction( static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation, common::Time prev_node_time, const LandmarkObservation& observation, const NodeData& prev_node,
common::Time next_node_time) { const NodeData& next_node) {
return new ceres::AutoDiffCostFunction< return new ceres::AutoDiffCostFunction<
LandmarkCostFunction, 6 /* residuals */, LandmarkCostFunction, 6 /* residuals */,
4 /* previous node rotation variables */, 4 /* previous node rotation variables */,
@ -48,7 +49,7 @@ class LandmarkCostFunction {
3 /* next node translation variables */, 3 /* next node translation variables */,
4 /* landmark rotation variables */, 4 /* landmark rotation variables */,
3 /* landmark translation variables */>( 3 /* landmark translation variables */>(
new LandmarkCostFunction(observation, prev_node_time, next_node_time)); new LandmarkCostFunction(observation, prev_node, next_node));
} }
template <typename T> template <typename T>
@ -58,6 +59,10 @@ class LandmarkCostFunction {
const T* const next_node_translation, const T* const next_node_translation,
const T* const landmark_rotation, const T* const landmark_rotation,
const T* const landmark_translation, T* const e) const { 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<T, 3> interpolated_pose_translation{ const std::array<T, 3> interpolated_pose_translation{
{prev_node_translation[0] + {prev_node_translation[0] +
interpolation_parameter_ * interpolation_parameter_ *
@ -83,14 +88,14 @@ class LandmarkCostFunction {
private: private:
LandmarkCostFunction(const LandmarkObservation& observation, 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_( : landmark_to_tracking_transform_(
observation.landmark_to_tracking_transform), observation.landmark_to_tracking_transform),
translation_weight_(observation.translation_weight), translation_weight_(observation.translation_weight),
rotation_weight_(observation.rotation_weight), rotation_weight_(observation.rotation_weight),
interpolation_parameter_( interpolation_parameter_(
common::ToSeconds(observation.time - prev_node_time) / common::ToSeconds(observation.time - prev_node.time) /
common::ToSeconds(next_node_time - prev_node_time)) {} common::ToSeconds(next_node.time - prev_node.time)) {}
const transform::Rigid3d landmark_to_tracking_transform_; const transform::Rigid3d landmark_to_tracking_transform_;
const double translation_weight_; const double translation_weight_;
@ -99,7 +104,7 @@ class LandmarkCostFunction {
}; };
} // namespace pose_graph } // namespace pose_graph
} // namespace mapping } // namespace mapping_3d
} // namespace cartographer } // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_ #endif // CARTOGRAPHER_MAPPING_3D_POSE_GRAPH_LANDMARK_COST_FUNCTION_H_

View File

@ -14,14 +14,14 @@
* limitations under the License. * 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 "cartographer/transform/rigid_transform.h"
#include "gmock/gmock.h" #include "gmock/gmock.h"
#include "gtest/gtest.h" #include "gtest/gtest.h"
namespace cartographer { namespace cartographer {
namespace mapping { namespace mapping_3d {
namespace pose_graph { namespace pose_graph {
namespace { namespace {
@ -32,6 +32,11 @@ using LandmarkObservation =
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation; mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
TEST(LandmarkCostFunctionTest, SmokeTest) { 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( auto* cost_function = LandmarkCostFunction::CreateAutoDiffCostFunction(
LandmarkObservation{ LandmarkObservation{
0 /* trajectory ID */, 0 /* trajectory ID */,
@ -40,7 +45,7 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
1. /* translation_weight */, 1. /* translation_weight */,
2. /* rotation_weight */, 2. /* rotation_weight */,
}, },
common::FromUniversal(0), common::FromUniversal(10)); prev_node, next_node);
std::array<double, 4> prev_node_rotation{{1., 0., 0., 0.}}; std::array<double, 4> prev_node_rotation{{1., 0., 0., 0.}};
std::array<double, 3> prev_node_translation{{0., 0., 0.}}; std::array<double, 3> prev_node_translation{{0., 0., 0.}};
@ -62,5 +67,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
} // namespace } // namespace
} // namespace pose_graph } // namespace pose_graph
} // namespace mapping } // namespace mapping_3d
} // namespace cartographer } // namespace cartographer