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_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ 00018 #define CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_ 00019 00020 #include "Eigen/Core" 00021 #include "ceres/ceres.h" 00022 00023 namespace cartographer { 00024 namespace mapping { 00025 namespace scan_matching { 00026 00027 // Computes the cost of rotating 'pose' to 'target_angle'. Cost increases with 00028 // the solution's distance from 'target_angle'. 00029 class RotationDeltaCostFunctor2D { 00030 public: 00031 static ceres::CostFunction* CreateAutoDiffCostFunction( 00032 const double scaling_factor, const double target_angle) { 00033 return new ceres::AutoDiffCostFunction< 00034 RotationDeltaCostFunctor2D, 1 /* residuals */, 3 /* pose variables */>( 00035 new RotationDeltaCostFunctor2D(scaling_factor, target_angle)); 00036 } 00037 00038 template <typename T> 00039 bool operator()(const T* const pose, T* residual) const { 00040 residual[0] = scaling_factor_ * (pose[2] - angle_); 00041 return true; 00042 } 00043 00044 private: 00045 explicit RotationDeltaCostFunctor2D(const double scaling_factor, 00046 const double target_angle) 00047 : scaling_factor_(scaling_factor), angle_(target_angle) {} 00048 00049 RotationDeltaCostFunctor2D(const RotationDeltaCostFunctor2D&) = delete; 00050 RotationDeltaCostFunctor2D& operator=(const RotationDeltaCostFunctor2D&) = 00051 delete; 00052 00053 const double scaling_factor_; 00054 const double angle_; 00055 }; 00056 00057 } // namespace scan_matching 00058 } // namespace mapping 00059 } // namespace cartographer 00060 00061 #endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_SCAN_MATCHING_ROTATION_DELTA_COST_FUNCTOR_2D_H_