translation_cost_function.h
Go to the documentation of this file.
1 /*
2  * Copyright 2016 The Cartographer Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
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 Wed Jun 5 2019 21:57:59