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

Add a 'cost_helpers' library. (#862) 

* Add a 'cost_helpers' library.

* Change naming and add comments.
master
Alexander Belyaev 2018-01-30 22:12:25 +01:00 committed by GitHub
parent 93568641f9
commit 708e7fc57d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 144 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
cartographer/mapping/pose_graph/cost_helpers.h Normal file
View File

@ -0,0 +1,55 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_
#include "Eigen/Core"
#include "Eigen/Geometry"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
namespace cartographer {
namespace mapping {
namespace pose_graph {
// Computes the error between the given relative pose and the difference of
// poses 'start' and 'end' which are both in an arbitrary common frame.
//
// 'start' and 'end' poses have the format [x, y, rotation].
template <typename T>
static std::array<T, 3> ComputeUnscaledError2d(
const transform::Rigid2d& relative_pose, const T* const start,
const T* const end);
// Computes the error between the given relative pose and the difference of
// poses 'start' and 'end' which are both in an arbitrary common frame.
//
// 'start' and 'end' translation has the format [x, y, z].
// 'start' and 'end' rotation are quaternions in the format [w, n_1, n_2, n_3].
template <typename T>
static std::array<T, 6> ComputeUnscaledError3d(
const transform::Rigid3d& relative_pose, const T* const start_rotation,
const T* const start_translation, const T* const end_rotation,
const T* const end_translation);
} // namespace pose_graph
} // namespace mapping
} // namespace cartographer
#include "cartographer/mapping/pose_graph/cost_helpers_impl.h"
#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_H_

76
cartographer/mapping/pose_graph/cost_helpers_impl.h Normal file
View File

@ -0,0 +1,76 @@
/*
* Copyright 2018 The Cartographer Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_
#define CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_
namespace cartographer {
namespace mapping {
namespace pose_graph {
template <typename T>
static std::array<T, 3> ComputeUnscaledError2d(
const transform::Rigid2d& relative_pose, const T* const start,
const T* const end) {
const T cos_theta_i = cos(start[2]);
const T sin_theta_i = sin(start[2]);
const T delta_x = end[0] - start[0];
const T delta_y = end[1] - start[1];
const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y,
-sin_theta_i * delta_x + cos_theta_i * delta_y,
end[2] - start[2]};
return {{T(relative_pose.translation().x()) - h[0],
T(relative_pose.translation().y()) - h[1],
common::NormalizeAngleDifference(
T(relative_pose.rotation().angle()) - h[2])}};
}
template <typename T>
static std::array<T, 6> ComputeUnscaledError3d(
const transform::Rigid3d& relative_pose, const T* const start_rotation,
const T* const start_translation, const T* const end_rotation,
const T* const end_translation) {
const Eigen::Quaternion<T> R_i_inverse(start_rotation[0], -start_rotation[1],
-start_rotation[2],
-start_rotation[3]);
const Eigen::Matrix<T, 3, 1> delta(end_translation[0] - start_translation[0],
end_translation[1] - start_translation[1],
end_translation[2] - start_translation[2]);
const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
const Eigen::Quaternion<T> h_rotation_inverse =
Eigen::Quaternion<T>(end_rotation[0], -end_rotation[1], -end_rotation[2],
-end_rotation[3]) *
Eigen::Quaternion<T>(start_rotation[0], start_rotation[1],
start_rotation[2], start_rotation[3]);
const Eigen::Matrix<T, 3, 1> angle_axis_difference =
transform::RotationQuaternionToAngleAxisVector(
h_rotation_inverse * relative_pose.rotation().cast<T>());
return {{T(relative_pose.translation().x()) - h_translation[0],
T(relative_pose.translation().y()) - h_translation[1],
T(relative_pose.translation().z()) - h_translation[2],
angle_axis_difference[0], angle_axis_difference[1],
angle_axis_difference[2]}};
}
} // namespace pose_graph
} // namespace mapping
} // namespace cartographer
#endif // CARTOGRAPHER_MAPPING_POSE_GRAPH_COST_HELPERS_IMPL_H_

14
cartographer/mapping/pose_graph/landmark_cost_function.h
View File

@ -19,8 +19,8 @@
#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/spa_cost_function.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
@ -62,7 +62,7 @@ std::array<T, 4> SlerpQuaternions(const T* const prev_rotation,
class LandmarkCostFunction {
public:
using LandmarkObservation =
mapping::PoseGraph::LandmarkNode::LandmarkObservation;
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
static ceres::CostFunction* CreateAutoDiffCostFunction(
const LandmarkObservation& observation, common::Time prev_node_time,
@ -99,13 +99,9 @@ class LandmarkCostFunction {
std::array<T, 4> interpolated_pose_rotation = SlerpQuaternions(
prev_node_rotation, next_node_rotation, T(interpolation_parameter_));
// TODO(pifon2a): Move functions common for all cost functions outside of
// SpaCostFunction scope.
const std::array<T, 6> unscaled_error =
mapping_3d::pose_graph::SpaCostFunction::ComputeUnscaledError(
landmark_to_tracking_transform_, interpolated_pose_rotation.data(),
interpolated_pose_translation, landmark_rotation,
landmark_translation);
const std::array<T, 6> unscaled_error = ComputeUnscaledError3d(
landmark_to_tracking_transform_, interpolated_pose_rotation.data(),
interpolated_pose_translation, landmark_rotation, landmark_translation);
e[0] = T(translation_weight_) * unscaled_error[0];
e[1] = T(translation_weight_) * unscaled_error[1];

2
cartographer/mapping/pose_graph/landmark_cost_function_test.cc
View File

@ -29,7 +29,7 @@ using ::testing::DoubleEq;
using ::testing::ElementsAre;
using LandmarkObservation =
mapping::PoseGraph::LandmarkNode::LandmarkObservation;
mapping::PoseGraphInterface::LandmarkNode::LandmarkObservation;
TEST(LandmarkCostFunctionTest, SmokeTest) {
auto* cost_function = LandmarkCostFunction::CreateAutoDiffCostFunction(

25
cartographer/mapping_2d/pose_graph/spa_cost_function.h
View File

@ -23,6 +23,7 @@
#include "Eigen/Geometry"
#include "cartographer/common/math.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph/cost_helpers.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
@ -50,34 +51,14 @@ class SpaCostFunction {
return true;
}
// Computes the error between the node-to-submap alignment 'zbar_ij' and the
// difference of submap pose 'c_i' and node pose 'c_j' which are both in an
// arbitrary common frame.
template <typename T>
static std::array<T, 3> ComputeUnscaledError(
const transform::Rigid2d& zbar_ij, const T* const c_i,
const T* const c_j) {
const T cos_theta_i = cos(c_i[2]);
const T sin_theta_i = sin(c_i[2]);
const T delta_x = c_j[0] - c_i[0];
const T delta_y = c_j[1] - c_i[1];
const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y,
-sin_theta_i * delta_x + cos_theta_i * delta_y,
c_j[2] - c_i[2]};
return {{T(zbar_ij.translation().x()) - h[0],
T(zbar_ij.translation().y()) - h[1],
common::NormalizeAngleDifference(T(zbar_ij.rotation().angle()) -
h[2])}};
}
// Computes the error scaled by 'translation_weight' and 'rotation_weight',
// storing it in 'e'.
template <typename T>
static void ComputeScaledError(const Constraint::Pose& pose,
const T* const c_i, const T* const c_j,
T* const e) {
const std::array<T, 3> e_ij =
ComputeUnscaledError(transform::Project2D(pose.zbar_ij), c_i, c_j);
const std::array<T, 3> e_ij = mapping::pose_graph::ComputeUnscaledError2d(
transform::Project2D(pose.zbar_ij), c_i, c_j);
e[0] = e_ij[0] * T(pose.translation_weight);
e[1] = e_ij[1] * T(pose.translation_weight);
e[2] = e_ij[2] * T(pose.rotation_weight);

40
cartographer/mapping_3d/pose_graph/spa_cost_function.h
View File

@ -23,6 +23,7 @@
#include "Eigen/Geometry"
#include "cartographer/common/math.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph/cost_helpers.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
@ -53,39 +54,6 @@ class SpaCostFunction {
return true;
}
// Computes the error between the node-to-submap alignment 'zbar_ij' and the
// difference of submap pose 'c_i' and node pose 'c_j' which are both in an
// arbitrary common frame.
template <typename T>
static std::array<T, 6> ComputeUnscaledError(
const transform::Rigid3d& zbar_ij, const T* const c_i_rotation,
const T* const c_i_translation, const T* const c_j_rotation,
const T* const c_j_translation) {
const Eigen::Quaternion<T> R_i_inverse(c_i_rotation[0], -c_i_rotation[1],
-c_i_rotation[2], -c_i_rotation[3]);
const Eigen::Matrix<T, 3, 1> delta(c_j_translation[0] - c_i_translation[0],
c_j_translation[1] - c_i_translation[1],
c_j_translation[2] - c_i_translation[2]);
const Eigen::Matrix<T, 3, 1> h_translation = R_i_inverse * delta;
const Eigen::Quaternion<T> h_rotation_inverse =
Eigen::Quaternion<T>(c_j_rotation[0], -c_j_rotation[1],
-c_j_rotation[2], -c_j_rotation[3]) *
Eigen::Quaternion<T>(c_i_rotation[0], c_i_rotation[1], c_i_rotation[2],
c_i_rotation[3]);
const Eigen::Matrix<T, 3, 1> angle_axis_difference =
transform::RotationQuaternionToAngleAxisVector(
h_rotation_inverse * zbar_ij.rotation().cast<T>());
return {{T(zbar_ij.translation().x()) - h_translation[0],
T(zbar_ij.translation().y()) - h_translation[1],
T(zbar_ij.translation().z()) - h_translation[2],
angle_axis_difference[0], angle_axis_difference[1],
angle_axis_difference[2]}};
}
// Computes the error scaled by 'translation_weight' and 'rotation_weight',
// storing it in 'e'.
template <typename T>
@ -94,9 +62,9 @@ class SpaCostFunction {
const T* const c_i_translation,
const T* const c_j_rotation,
const T* const c_j_translation, T* const e) {
const std::array<T, 6> e_ij =
ComputeUnscaledError(pose.zbar_ij, c_i_rotation, c_i_translation,
c_j_rotation, c_j_translation);
const std::array<T, 6> e_ij = mapping::pose_graph::ComputeUnscaledError3d(
pose.zbar_ij, c_i_rotation, c_i_translation, c_j_rotation,
c_j_translation);
for (int ij : {0, 1, 2}) {
e[ij] = e_ij[ij] * T(pose.translation_weight);
}