00001 /* 00002 * Copyright 2016 The Cartographer Authors 00003 * 00004 * Licensed under the Apache License, Version 2.0 (the "License"); 00005 * you may not use this file except in compliance with the License. 00006 * You may obtain a copy of the License at 00007 * 00008 * http://www.apache.org/licenses/LICENSE-2.0 00009 * 00010 * Unless required by applicable law or agreed to in writing, software 00011 * distributed under the License is distributed on an "AS IS" BASIS, 00012 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 00013 * See the License for the specific language governing permissions and 00014 * limitations under the License. 00015 */ 00016 00017 #ifndef CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_ 00018 #define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_ 00019 00020 #include <cmath> 00021 00022 #include "Eigen/Core" 00023 #include "cartographer/common/math.h" 00024 #include "ceres/ceres.h" 00025 #include "ceres/rotation.h" 00026 00027 namespace cartographer { 00028 namespace mapping { 00029 namespace scan_matching { 00030 00031 // Computes the cost of rotating 'rotation_quaternion' to 'target_rotation'. 00032 // Cost increases with the solution's distance from 'target_rotation'. 00033 class RotationDeltaCostFunctor3D { 00034 public: 00035 static ceres::CostFunction* CreateAutoDiffCostFunction( 00036 const double scaling_factor, const Eigen::Quaterniond& target_rotation) { 00037 return new ceres::AutoDiffCostFunction<RotationDeltaCostFunctor3D, 00038 3 /* residuals */, 00039 4 /* rotation variables */>( 00040 new RotationDeltaCostFunctor3D(scaling_factor, target_rotation)); 00041 } 00042 00043 template <typename T> 00044 bool operator()(const T* const rotation_quaternion, T* residual) const { 00045 std::array<T, 4> delta; 00046 common::QuaternionProduct(target_rotation_inverse_, rotation_quaternion, 00047 delta.data()); 00048 // Will compute the squared norm of the imaginary component of the delta 00049 // quaternion which is sin(phi/2)^2. 00050 residual[0] = scaling_factor_ * delta[1]; 00051 residual[1] = scaling_factor_ * delta[2]; 00052 residual[2] = scaling_factor_ * delta[3]; 00053 return true; 00054 } 00055 00056 private: 00057 // Constructs a new RotationDeltaCostFunctor3D from the given 00058 // 'target_rotation'. 00059 explicit RotationDeltaCostFunctor3D(const double scaling_factor, 00060 const Eigen::Quaterniond& target_rotation) 00061 : scaling_factor_(scaling_factor) { 00062 target_rotation_inverse_[0] = target_rotation.w(); 00063 target_rotation_inverse_[1] = -target_rotation.x(); 00064 target_rotation_inverse_[2] = -target_rotation.y(); 00065 target_rotation_inverse_[3] = -target_rotation.z(); 00066 } 00067 00068 RotationDeltaCostFunctor3D(const RotationDeltaCostFunctor3D&) = delete; 00069 RotationDeltaCostFunctor3D& operator=(const RotationDeltaCostFunctor3D&) = 00070 delete; 00071 00072 const double scaling_factor_; 00073 double target_rotation_inverse_[4]; 00074 }; 00075 00076 } // namespace scan_matching 00077 } // namespace mapping 00078 } // namespace cartographer 00079 00080 #endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_3D_H_