acceleration_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_ACCELERATION_COST_FUNCTION_H_
18 #define CARTOGRAPHER_MAPPING_3D_ACCELERATION_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 IMU data and optimized accelerations.
28  public:
29  AccelerationCostFunction(const double scaling_factor,
30  const Eigen::Vector3d& delta_velocity_imu_frame,
31  const double first_delta_time_seconds,
32  const double second_delta_time_seconds)
33  : scaling_factor_(scaling_factor),
34  delta_velocity_imu_frame_(delta_velocity_imu_frame),
35  first_delta_time_seconds_(first_delta_time_seconds),
36  second_delta_time_seconds_(second_delta_time_seconds) {}
37 
40 
41  template <typename T>
42  bool operator()(const T* const middle_rotation, const T* const start_position,
43  const T* const middle_position, const T* const end_position,
44  const T* const gravity_constant, T* residual) const {
45  const Eigen::Matrix<T, 3, 1> imu_delta_velocity =
46  ToEigen(middle_rotation) * delta_velocity_imu_frame_.cast<T>() -
47  *gravity_constant *
49  Eigen::Vector3d::UnitZ())
50  .cast<T>();
51  const Eigen::Matrix<T, 3, 1> start_velocity =
52  (Eigen::Map<const Eigen::Matrix<T, 3, 1>>(middle_position) -
53  Eigen::Map<const Eigen::Matrix<T, 3, 1>>(start_position)) /
55  const Eigen::Matrix<T, 3, 1> end_velocity =
56  (Eigen::Map<const Eigen::Matrix<T, 3, 1>>(end_position) -
57  Eigen::Map<const Eigen::Matrix<T, 3, 1>>(middle_position)) /
59  const Eigen::Matrix<T, 3, 1> delta_velocity = end_velocity - start_velocity;
60 
61  (Eigen::Map<Eigen::Matrix<T, 3, 1>>(residual) =
62  T(scaling_factor_) * (imu_delta_velocity - delta_velocity));
63  return true;
64  }
65 
66  private:
67  template <typename T>
68  static Eigen::Quaternion<T> ToEigen(const T* const quaternion) {
69  return Eigen::Quaternion<T>(quaternion[0], quaternion[1], quaternion[2],
70  quaternion[3]);
71  }
72 
73  const double scaling_factor_;
74  const Eigen::Vector3d delta_velocity_imu_frame_;
77 };
78 
79 } // namespace mapping_3d
80 } // namespace cartographer
81 
82 #endif // CARTOGRAPHER_MAPPING_3D_ACCELERATION_COST_FUNCTION_H_
bool operator()(const T *const middle_rotation, const T *const start_position, const T *const middle_position, const T *const end_position, const T *const gravity_constant, T *residual) const
static Eigen::Quaternion< T > ToEigen(const T *const quaternion)
AccelerationCostFunction(const double scaling_factor, const Eigen::Vector3d &delta_velocity_imu_frame, const double first_delta_time_seconds, const double second_delta_time_seconds)
AccelerationCostFunction & operator=(const AccelerationCostFunction &)=delete


cartographer
Author(s):
autogenerated on Wed Jun 5 2019 21:57:57