mapping_3d/scan_matching/translation_delta_cost_functor.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_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
18 #define CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
19
20 #include "Eigen/Core"
22
23 namespace cartographer {
24 namespace mapping_3d {
25 namespace scan_matching {
26
27 // Computes the cost of translating the initial pose estimate. Cost increases
28 // with the solution's distance from the initial estimate.
30  public:
31  // Constructs a new TranslationDeltaCostFunctor from the given
32  // 'initial_pose_estimate'.
34  const double scaling_factor,
35  const transform::Rigid3d& initial_pose_estimate)
36  : scaling_factor_(scaling_factor),
37  x_(initial_pose_estimate.translation().x()),
38  y_(initial_pose_estimate.translation().y()),
39  z_(initial_pose_estimate.translation().z()) {}
40
43  delete;
44
45  template <typename T>
46  bool operator()(const T* const translation, T* residual) const {
47  residual[0] = scaling_factor_ * (translation[0] - x_);
48  residual[1] = scaling_factor_ * (translation[1] - y_);
49  residual[2] = scaling_factor_ * (translation[2] - z_);
50  return true;
51  }
52
53  private:
54  const double scaling_factor_;
55  const double x_;
56  const double y_;
57  const double z_;
58 };
59
60 } // namespace scan_matching
61 } // namespace mapping_3d
62 } // namespace cartographer
63
64 #endif // CARTOGRAPHER_MAPPING_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_H_
TranslationDeltaCostFunctor & operator=(const TranslationDeltaCostFunctor &)=delete
bool operator()(const T *const translation, T *residual) const
TranslationDeltaCostFunctor(const double scaling_factor, const transform::Rigid3d &initial_pose_estimate)

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