mapping_2d/sparse_pose_graph/spa_cost_function.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
9  *
10  * Unless required by applicable law or agreed to in writing, software
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16
17 #ifndef CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_
18 #define CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_
19
20 #include <array>
21
22 #include "Eigen/Core"
23 #include "Eigen/Geometry"
28 #include "ceres/ceres.h"
29 #include "ceres/jet.h"
30
31 namespace cartographer {
32 namespace mapping_2d {
33 namespace sparse_pose_graph {
34
36  public:
38
39  explicit SpaCostFunction(const Constraint::Pose& pose) : pose_(pose) {}
40
41  // Computes the error between the scan-to-submap alignment 'zbar_ij' and the
42  // difference of submap pose 'c_i' and scan pose 'c_j' which are both in an
43  // arbitrary common frame.
44  template <typename T>
45  static std::array<T, 3> ComputeUnscaledError(
46  const transform::Rigid2d& zbar_ij, const T* const c_i,
47  const T* const c_j) {
48  const T cos_theta_i = cos(c_i[2]);
49  const T sin_theta_i = sin(c_i[2]);
50  const T delta_x = c_j[0] - c_i[0];
51  const T delta_y = c_j[1] - c_i[1];
52  const T h[3] = {cos_theta_i * delta_x + sin_theta_i * delta_y,
53  -sin_theta_i * delta_x + cos_theta_i * delta_y,
54  c_j[2] - c_i[2]};
55  return {{T(zbar_ij.translation().x()) - h[0],
56  T(zbar_ij.translation().y()) - h[1],
57  common::NormalizeAngleDifference(T(zbar_ij.rotation().angle()) -
58  h[2])}};
59  }
60
61  // Computes the error scaled by 'translation_weight' and 'rotation_weight',
62  // storing it in 'e'.
63  template <typename T>
65  const T* const c_i, const T* const c_j,
66  T* const e) {
67  const std::array<T, 3> e_ij =
69  e[0] = e_ij[0] * T(pose.translation_weight);
70  e[1] = e_ij[1] * T(pose.translation_weight);
71  e[2] = e_ij[2] * T(pose.rotation_weight);
72  }
73
74  template <typename T>
75  bool operator()(const T* const c_i, const T* const c_j, T* e) const {
76  ComputeScaledError(pose_, c_i, c_j, e);
77  return true;
78  }
79
80  private:
82 };
83
84 } // namespace sparse_pose_graph
85 } // namespace mapping_2d
86 } // namespace cartographer
87
88 #endif // CARTOGRAPHER_MAPPING_2D_SPARSE_POSE_GRAPH_SPA_COST_FUNCTION_H_
const Vector & translation() const
static std::array< T, 3 > ComputeUnscaledError(const transform::Rigid2d &zbar_ij, const T *const c_i, const T *const c_j)
transform::Rigid3d pose
Rigid2< T > Project2D(const Rigid3< T > &transform)
Definition: transform.h:103
T NormalizeAngleDifference(T difference)
Definition: math.h:62
bool operator()(const T *const c_i, const T *const c_j, T *e) const
static void ComputeScaledError(const Constraint::Pose &pose, const T *const c_i, const T *const c_j, T *const e)

cartographer
Author(s):
autogenerated on Mon Jun 10 2019 12:51:39