rotation_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_ROTATION_COST_FUNCTION_H_
18 #define CARTOGRAPHER_MAPPING_3D_ROTATION_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 orientations.
28  public:
29  RotationCostFunction(const double scaling_factor,
30  const Eigen::Quaterniond& delta_rotation_imu_frame)
31  : scaling_factor_(scaling_factor),
32  delta_rotation_imu_frame_(delta_rotation_imu_frame) {}
33
36
37  template <typename T>
38  bool operator()(const T* const start_rotation, const T* const end_rotation,
39  T* residual) const {
40  const Eigen::Quaternion<T> start(start_rotation[0], start_rotation[1],
41  start_rotation[2], start_rotation[3]);
42  const Eigen::Quaternion<T> end(end_rotation[0], end_rotation[1],
43  end_rotation[2], end_rotation[3]);
44  const Eigen::Quaternion<T> error =
45  end.conjugate() * start * delta_rotation_imu_frame_.cast<T>();
46  residual[0] = scaling_factor_ * error.x();
47  residual[1] = scaling_factor_ * error.y();
48  residual[2] = scaling_factor_ * error.z();
49  return true;
50  }
51
52  private:
53  const double scaling_factor_;
54  const Eigen::Quaterniond delta_rotation_imu_frame_;
55 };
56
57 } // namespace mapping_3d
58 } // namespace cartographer
59
60 #endif // CARTOGRAPHER_MAPPING_3D_ROTATION_COST_FUNCTION_H_
RotationCostFunction & operator=(const RotationCostFunction &)=delete
bool operator()(const T *const start_rotation, const T *const end_rotation, T *residual) const
RotationCostFunction(const double scaling_factor, const Eigen::Quaterniond &delta_rotation_imu_frame)

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