00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CVisualOdometryStereo_H
00029 #define CVisualOdometryStereo_H
00030
00031 #include <mrpt/slam/CObservationStereoImages.h>
00032 #include <mrpt/poses/CPose3DPDFGaussian.h>
00033 #include <mrpt/poses/CPose3DPDFParticles.h>
00034 #include <mrpt/poses/CPose3D.h>
00035 #include <mrpt/slam/CMetricMap.h>
00036 #include <mrpt/slam/CObservationVisualLandmarks.h>
00037 #include <mrpt/scan_matching.h>
00038
00039 #include <mrpt/vision/CFeatureExtraction.h>
00040 #include <mrpt/vision/utils.h>
00041
00042 #include <mrpt/math/CMatrix.h>
00043 #include <mrpt/utils/CMRPTImage.h>
00044 #include <mrpt/gui/CDisplayWindow.h>
00045
00046 #define VISUALODOMETRY_USES_GUI 1
00047
00048 namespace mrpt
00049 {
00050 namespace vision
00051 {
00054 struct MRPTDLLIMPEXP TOdometryOptions : public mrpt::utils::CLoadableOptions
00055 {
00056 enum TCov_Computation_Method
00057 {
00058 TCov_Linear = -1,
00059 TCov_UT,
00060 TCov_Particles
00061 };
00062
00063
00064 TCov_Computation_Method covMethod;
00065 unsigned int minNumFeatures;
00066 double minDispX, minDispY, minSetDist;
00067 bool VERBOSE, PLOT_IMAGES, SAVE_FILES;
00068
00069 TOdometryOptions( );
00070
00073 void loadFromConfigFile(
00074 const mrpt::utils::CConfigFileBase &source,
00075 const std::string §ion);
00076
00079 void dumpToTextStream(mrpt::utils::CStream &out);
00080
00081 };
00082
00089 class MRPTDLLIMPEXP CVisualOdometryStereo
00090 {
00091
00092 public:
00093
00096 CVisualOdometryStereo();
00097
00100 virtual ~CVisualOdometryStereo();
00101
00104 void reset();
00105
00111 bool process( const mrpt::slam::CObservationStereoImages &inObs, mrpt::poses::CPose3DPDFGaussian &outEst );
00112
00113 vision::TStereoSystemParams stereoParams;
00114 vision::TMatchingOptions matchingOptions;
00115 vision::TOdometryOptions odometryOptions;
00116
00117 unsigned int nIter;
00118
00119 protected:
00120
00121 bool firstObs;
00122
00123 vision::CMatchedFeatureList mfList;
00124 vision::CFeatureExtraction fExtract;
00125 mrpt::slam::CObservationVisualLandmarks Prev_cloud;
00126 utils::CMRPTImage imgL, imgR;
00127
00128 vision::CFeatureList leftFeats, rightFeats;
00129
00130 #if VISUALODOMETRY_USES_GUI
00131 gui::CDisplayWindow *wind1,*wind2;
00132 #endif
00133
00137 void computeRTCovariance(
00138 const mrpt::poses::CPose3D &meanPose,
00139 const mrpt::slam::CMetricMap::TMatchingPairList &list,
00140 const mrpt::slam::CObservationVisualLandmarks &Pre_Cloud,
00141 const mrpt::slam::CObservationVisualLandmarks &Cur_Cloud,
00142 math::CMatrixD &RTCov );
00143
00147 void storeMatchedFeatureList(
00148 const CMatchedFeatureList &mfList );
00149
00153 void addMatches(
00154 const CMatchedFeatureList &mfList );
00155
00159 void getMatchedList(
00160 CMatchedFeatureList &mfList );
00161
00162
00163 };
00164
00165 }
00166 }
00167
00168 #endif