translation_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_3D_TRANSLATION_COST_FUNCTION_H_
18 #define CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_
19
20 #include "Eigen/Core"
21 #include "Eigen/Geometry"
22
23 namespace cartographer {
24 namespace mapping_3d {
25
26 // Penalizes differences between velocity and change in position.
28  public:
29  TranslationCostFunction(const double scaling_factor,
30  const double delta_time_seconds)
31  : scaling_factor_(scaling_factor),
32  delta_time_seconds_(delta_time_seconds) {}
33
36
37  template <typename T>
38  bool operator()(const T* const start_translation,
39  const T* const end_translation, const T* const velocity,
40  T* residual) const {
41  const T delta_x = end_translation[0] - start_translation[0];
42  const T delta_y = end_translation[1] - start_translation[1];
43  const T delta_z = end_translation[2] - start_translation[2];
44
45  residual[0] =
46  scaling_factor_ * (delta_x - velocity[0] * delta_time_seconds_);
47  residual[1] =
48  scaling_factor_ * (delta_y - velocity[1] * delta_time_seconds_);
49  residual[2] =
50  scaling_factor_ * (delta_z - velocity[2] * delta_time_seconds_);
51  return true;
52  }
53
54  private:
55  const double scaling_factor_;
56  const double delta_time_seconds_;
57 };
58
59 } // namespace mapping_3d
60 } // namespace cartographer
61
62 #endif // CARTOGRAPHER_MAPPING_3D_TRANSLATION_COST_FUNCTION_H_
TranslationCostFunction & operator=(const TranslationCostFunction &)=delete
bool operator()(const T *const start_translation, const T *const end_translation, const T *const velocity, T *residual) const
TranslationCostFunction(const double scaling_factor, const double delta_time_seconds)

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