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_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_ 00018 #define CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_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 translating 'translation' to 'target_translation'. 00028 // Cost increases with the solution's distance from 'target_translation'. 00029 class TranslationDeltaCostFunctor3D { 00030 public: 00031 static ceres::CostFunction* CreateAutoDiffCostFunction( 00032 const double scaling_factor, const Eigen::Vector3d& target_translation) { 00033 return new ceres::AutoDiffCostFunction<TranslationDeltaCostFunctor3D, 00034 3 /* residuals */, 00035 3 /* translation variables */>( 00036 new TranslationDeltaCostFunctor3D(scaling_factor, target_translation)); 00037 } 00038 00039 template <typename T> 00040 bool operator()(const T* const translation, T* residual) const { 00041 residual[0] = scaling_factor_ * (translation[0] - x_); 00042 residual[1] = scaling_factor_ * (translation[1] - y_); 00043 residual[2] = scaling_factor_ * (translation[2] - z_); 00044 return true; 00045 } 00046 00047 private: 00048 // Constructs a new TranslationDeltaCostFunctor3D from the given 00049 // 'target_translation'. 00050 explicit TranslationDeltaCostFunctor3D( 00051 const double scaling_factor, const Eigen::Vector3d& target_translation) 00052 : scaling_factor_(scaling_factor), 00053 x_(target_translation.x()), 00054 y_(target_translation.y()), 00055 z_(target_translation.z()) {} 00056 00057 TranslationDeltaCostFunctor3D(const TranslationDeltaCostFunctor3D&) = delete; 00058 TranslationDeltaCostFunctor3D& operator=( 00059 const TranslationDeltaCostFunctor3D&) = delete; 00060 00061 const double scaling_factor_; 00062 const double x_; 00063 const double y_; 00064 const double z_; 00065 }; 00066 00067 } // namespace scan_matching 00068 } // namespace mapping 00069 } // namespace cartographer 00070 00071 #endif // CARTOGRAPHER_MAPPING_INTERNAL_3D_SCAN_MATCHING_TRANSLATION_DELTA_COST_FUNCTOR_3D_H_