HectorDebugInfoProvider.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 HECTOR_DEBUG_INFO_PROVIDER_H__
00030 #define HECTOR_DEBUG_INFO_PROVIDER_H__
00031 
00032 #include "util/HectorDebugInfoInterface.h"
00033 #include "util/UtilFunctions.h"
00034 
00035 #include "ros/ros.h"
00036 
00037 #include "hector_mapping/HectorDebugInfo.h"
00038 
00039 
00040 class HectorDebugInfoProvider : public HectorDebugInfoInterface
00041 {
00042 public:
00043 
00044   HectorDebugInfoProvider()
00045   {
00046     ros::NodeHandle nh_;
00047 
00048     debugInfoPublisher_ = nh_.advertise<hector_mapping::HectorDebugInfo>("hector_debug_info", 50, true);
00049   };
00050 
00051   virtual void sendAndResetData()
00052   {
00053     debugInfoPublisher_.publish(debugInfo);
00054     debugInfo.iterData.clear();
00055   }
00056 
00057 
00058   virtual void addHessianMatrix(const Eigen::Matrix3f& hessian)
00059   {
00060     hector_mapping::HectorIterData iterData;
00061 
00062     for (int i=0; i < 9; ++i){
00063       iterData.hessian[i] = static_cast<double>(hessian.data()[i]);
00064       iterData.determinant = hessian.determinant();
00065 
00066       Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eig(hessian);
00067 
00068       const Eigen::Vector3f& eigValues (eig.eigenvalues());
00069       iterData.conditionNum = eigValues[2] / eigValues[0];
00070 
00071 
00072       iterData.determinant2d = hessian.block<2,2>(0,0).determinant();
00073       Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> eig2d(hessian.block<2,2>(0,0));
00074 
00075       const Eigen::Vector2f& eigValues2d (eig2d.eigenvalues());
00076       iterData.conditionNum2d = eigValues2d[1] / eigValues2d[0];
00077     }
00078 
00079     debugInfo.iterData.push_back(iterData);
00080   }
00081 
00082   virtual void addPoseLikelihood(float lh)
00083   {
00084 
00085   }
00086 
00087 
00088   hector_mapping::HectorDebugInfo debugInfo;
00089 
00090   ros::Publisher debugInfoPublisher_;
00091 
00092 };
00093 
00094 #endif


hector_mapping
Author(s): Stefan Kohlbrecher
autogenerated on Wed Aug 26 2015 11:45:03