ScanMatcher.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Simulation, Systems Optimization and Robotics
00013 //       group, TU Darmstadt nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef _scanmatcher_h__
00030 #define _scanmatcher_h__
00031 
00032 #include <Eigen/Geometry>
00033 #include "../scan/DataPointContainer.h"
00034 #include "../util/UtilFunctions.h"
00035 
00036 #include "../util/DrawInterface.h"
00037 #include "../util/HectorDebugInfoInterface.h"
00038 
00039 namespace hectorslam{
00040 
00041 template<typename ConcreteOccGridMapUtil>
00042 class ScanMatcher
00043 {
00044 public:
00045 
00046   ScanMatcher(DrawInterface* drawInterfaceIn = 0, HectorDebugInfoInterface* debugInterfaceIn = 0)
00047     : drawInterface(drawInterfaceIn)
00048     , debugInterface(debugInterfaceIn)
00049   {}
00050 
00051   ~ScanMatcher()
00052   {}
00053 
00054   Eigen::Vector3f matchData(const Eigen::Vector3f& beginEstimateWorld, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataContainer, Eigen::Matrix3f& covMatrix, int maxIterations)
00055   {
00056     if (drawInterface){
00057       drawInterface->setScale(0.05f);
00058       drawInterface->setColor(0.0f,1.0f, 0.0f);
00059       drawInterface->drawArrow(beginEstimateWorld);
00060 
00061       Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));
00062 
00063       drawScan(beginEstimateMap, gridMapUtil, dataContainer);
00064 
00065       drawInterface->setColor(1.0,0.0,0.0);
00066     }
00067 
00068     if (dataContainer.getSize() != 0) {
00069 
00070       Eigen::Vector3f beginEstimateMap(gridMapUtil.getMapCoordsPose(beginEstimateWorld));
00071 
00072       Eigen::Vector3f estimate(beginEstimateMap);
00073 
00074       estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
00075       //bool notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
00076 
00077       /*
00078       const Eigen::Matrix2f& hessian (H.block<2,2>(0,0));
00079 
00080 
00081       Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig(hessian);
00082 
00083       const Eigen::Vector2f& eigValues (eig.eigenvalues());
00084 
00085       float cond = eigValues[1] / eigValues[0];
00086       float determinant = (hessian.determinant());
00087       */
00088       //std::cout << "\n cond: " << cond << " det: " << determinant << "\n";
00089 
00090 
00091       int numIter = maxIterations;
00092 
00093 
00094       for (int i = 0; i < numIter; ++i) {
00095         //std::cout << "\nest:\n" << estimate;
00096 
00097         estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
00098         //notConverged = estimateTransformationLogLh(estimate, gridMapUtil, dataContainer);
00099 
00100         if(drawInterface){
00101           float invNumIterf = 1.0f/static_cast<float> (numIter);
00102           drawInterface->setColor(static_cast<float>(i)*invNumIterf,0.0f, 0.0f);
00103           drawInterface->drawArrow(gridMapUtil.getWorldCoordsPose(estimate));
00104           //drawInterface->drawArrow(Eigen::Vector3f(0.0f, static_cast<float>(i)*0.05, 0.0f));
00105         }
00106 
00107         if(debugInterface){
00108           debugInterface->addHessianMatrix(H);
00109         }
00110       }
00111 
00112       if (drawInterface){
00113         drawInterface->setColor(0.0,0.0,1.0);
00114         drawScan(estimate, gridMapUtil, dataContainer);
00115       }
00116         /*
00117         Eigen::Matrix2f testMat(Eigen::Matrix2f::Identity());
00118         testMat(0,0) = 2.0f;
00119 
00120         float angleWorldCoords = util::toRad(30.0f);
00121         float sinAngle = sin(angleWorldCoords);
00122         float cosAngle = cos(angleWorldCoords);
00123 
00124         Eigen::Matrix2f rotMat;
00125         rotMat << cosAngle, -sinAngle, sinAngle, cosAngle;
00126         Eigen::Matrix2f covarianceRotated (rotMat * testMat * rotMat.transpose());
00127 
00128         drawInterface->setColor(0.0,0.0,1.0,0.5);
00129         drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covarianceRotated);
00130         */
00131 
00132 
00133 
00134         /*
00135         Eigen::Matrix3f covMatMap (gridMapUtil.getCovarianceForPose(estimate, dataContainer));
00136         std::cout << "\nestim:" << estimate;
00137         std::cout << "\ncovMap\n" << covMatMap;
00138         drawInterface->setColor(0.0,0.0,1.0,0.5);
00139 
00140 
00141         Eigen::Matrix3f covMatWorld(gridMapUtil.getCovMatrixWorldCoords(covMatMap));
00142          std::cout << "\ncovWorld\n" << covMatWorld;
00143 
00144         drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covMatMap.block<2,2>(0,0));
00145 
00146         drawInterface->setColor(1.0,0.0,0.0,0.5);
00147         drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()), covMatWorld.block<2,2>(0,0));
00148 
00149         std::cout << "\nH:\n" << H;
00150 
00151         float determinant = H.determinant();
00152         std::cout << "\nH_det: " << determinant;
00153         */
00154 
00155         /*
00156         Eigen::Matrix2f covFromHessian(H.block<2,2>(0,0) * 1.0f);
00157         //std::cout << "\nCovFromHess:\n" << covFromHessian;
00158 
00159         drawInterface->setColor(0.0, 1.0, 0.0, 0.5);
00160         drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()),covFromHessian.inverse());
00161 
00162         Eigen::Matrix3f covFromHessian3d(H * 1.0f);
00163         //std::cout << "\nCovFromHess:\n" << covFromHessian;
00164 
00165         drawInterface->setColor(1.0, 0.0, 0.0, 0.8);
00166         drawInterface->drawCovariance(gridMapUtil.getWorldCoordsPoint(estimate.start<2>()),(covFromHessian3d.inverse()).block<2,2>(0,0));
00167         */
00168 
00169 
00170       estimate[2] = util::normalize_angle(estimate[2]);
00171 
00172       covMatrix = Eigen::Matrix3f::Zero();
00173       //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0).inverse());
00174       //covMatrix.block<2,2>(0,0) = (H.block<2,2>(0,0));
00175 
00176 
00177       /*
00178       covMatrix(0,0) = 1.0/(0.1*0.1);
00179       covMatrix(1,1) = 1.0/(0.1*0.1);
00180       covMatrix(2,2) = 1.0/((M_PI / 18.0f) * (M_PI / 18.0f));
00181       */
00182 
00183 
00184       covMatrix = H;
00185 
00186       return gridMapUtil.getWorldCoordsPose(estimate);
00187     }
00188 
00189     return beginEstimateWorld;
00190   }
00191 
00192 protected:
00193 
00194   bool estimateTransformationLogLh(Eigen::Vector3f& estimate, ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataPoints)
00195   {
00196     gridMapUtil.getCompleteHessianDerivs(estimate, dataPoints, H, dTr);
00197     //std::cout << "\nH\n" << H  << "\n";
00198     //std::cout << "\ndTr\n" << dTr  << "\n";
00199 
00200 
00201     if ((H(0, 0) != 0.0f) && (H(1, 1) != 0.0f)) {
00202 
00203 
00204       //H += Eigen::Matrix3f::Identity() * 1.0f;
00205       Eigen::Vector3f searchDir (H.inverse() * dTr);
00206 
00207       //std::cout << "\nsearchdir\n" << searchDir  << "\n";
00208 
00209       if (searchDir[2] > 0.2f) {
00210         searchDir[2] = 0.2f;
00211         std::cout << "SearchDir angle change too large\n";
00212       } else if (searchDir[2] < -0.2f) {
00213         searchDir[2] = -0.2f;
00214         std::cout << "SearchDir angle change too large\n";
00215       }
00216 
00217       updateEstimatedPose(estimate, searchDir);
00218       return true;
00219     }
00220     return false;
00221   }
00222 
00223   void updateEstimatedPose(Eigen::Vector3f& estimate, const Eigen::Vector3f& change)
00224   {
00225     estimate += change;
00226   }
00227 
00228   void drawScan(const Eigen::Vector3f& pose, const ConcreteOccGridMapUtil& gridMapUtil, const DataContainer& dataContainer)
00229   {
00230     drawInterface->setScale(0.02);
00231 
00232     Eigen::Affine2f transform(gridMapUtil.getTransformForState(pose));
00233 
00234     int size = dataContainer.getSize();
00235     for (int i = 0; i < size; ++i) {
00236       const Eigen::Vector2f& currPoint (dataContainer.getVecEntry(i));
00237       drawInterface->drawPoint(gridMapUtil.getWorldCoordsPoint(transform * currPoint));
00238     }
00239   }
00240 
00241 protected:
00242   Eigen::Vector3f dTr;
00243   Eigen::Matrix3f H;
00244 
00245   DrawInterface* drawInterface;
00246   HectorDebugInfoInterface* debugInterface;
00247 };
00248 
00249 }
00250 
00251 
00252 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Mon Jun 27 2016 04:57:19