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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cartographer/mapping/pose_graph/landmark_cost_function.h → cartographer/mapping_3d/pose_graph/landmark_cost_function.h
View File

@ -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 <typename T>
@ -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<T, 3> 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_

13
cartographer/mapping/pose_graph/landmark_cost_function_test.cc → cartographer/mapping_3d/pose_graph/landmark_cost_function_test.cc
View File

@ -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<double, 4> prev_node_rotation{{1., 0., 0., 0.}};
std::array<double, 3> prev_node_translation{{0., 0., 0.}};
@ -62,5 +67,5 @@ TEST(LandmarkCostFunctionTest, SmokeTest) {
} // namespace
} // namespace pose_graph
} // namespace mapping
} // namespace mapping_3d
} // namespace cartographer