ndt_matcher_d2d_feature.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, AASS Research Center, Orebro University.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of AASS Research Center nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  */
00035 #ifndef NDTMATCHERFEATUREF2F_HH
00036 #define NDTMATCHERFEATUREF2F_HH
00037 
00038 #include <ndt_registration/ndt_matcher_d2d.h>
00039 namespace lslgeneric
00040 {
00044 template <typename PointSource, typename PointTarget>
00045 class NDTMatcherFeatureD2D : public lslgeneric::NDTMatcherD2D<PointSource,PointTarget>
00046 {
00047 public:
00048     NDTMatcherFeatureD2D(const std::vector<std::pair<int, int> > &corr, double trimFactor = 1.) : _corr(corr), _trimFactor(trimFactor)
00049     {
00050         _goodCorr.resize(corr.size());
00051         std::fill(_goodCorr.begin(), _goodCorr.end(), true);
00052     }
00053 
00072     bool covariance( lslgeneric::NDTMap<PointTarget>& target,
00073                      lslgeneric::NDTMap<PointSource>& source,
00074                      Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
00075                      Eigen::Matrix<double,6,6> &cov
00076                    );
00077 
00078     //compute the score of a point cloud to an NDT
00079     virtual double scoreNDT(std::vector<lslgeneric::NDTCell<PointSource>*> &source,
00080                             lslgeneric::NDTMap<PointTarget> &target,
00081                             Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T);
00082 
00083     //compute the score gradient & hessian of a point cloud + transformation to an NDT
00084     // input: moving, fixed, tr, computeHessian
00085     //output: score_gradient, Hessian
00086     virtual double derivativesNDT(
00087             const std::vector<NDTCell<PointSource>*> &sourceNDT,
00088             const NDTMap<PointTarget> &targetNDT,
00089             Eigen::MatrixXd &score_gradient,
00090             Eigen::MatrixXd &Hessian,
00091             bool computeHessian
00092     );
00093 #if 0
00094     virtual bool update_gradient_hessian(
00095         Eigen::Matrix<double,6,1> &score_gradient,
00096         Eigen::Matrix<double,6,6> &Hessian,
00097 
00098         Eigen::Vector3d &m1,
00099         Eigen::Matrix3d &C1);
00100 #endif
00101 
00102     using NDTMatcherD2D<PointSource,PointTarget>::Jest;
00103     using NDTMatcherD2D<PointSource,PointTarget>::Hest;
00104     using NDTMatcherD2D<PointSource,PointTarget>::Zest;
00105     using NDTMatcherD2D<PointSource,PointTarget>::ZHest;
00106     using NDTMatcherD2D<PointSource,PointTarget>::lfd1;
00107     using NDTMatcherD2D<PointSource,PointTarget>::lfd2;
00108     using NDTMatcherD2D<PointSource,PointTarget>::normalizeAngle;
00109     using NDTMatcherD2D<PointSource,PointTarget>::NUMBER_OF_ACTIVE_CELLS;
00110 protected:
00111     const std::vector<std::pair<int, int> > & _corr;
00112     double _trimFactor;
00113     std::vector<bool> _goodCorr;
00114 };
00115 } // namespace
00116 
00117 #include <ndt_registration/impl/ndt_matcher_d2d_feature.hpp>
00118 
00119 #endif


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:30