mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Moved some common include files to public headers folder
This commit is contained in:
matlabbe
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "BayesFilter.h"
|
||||
#include "rtabmap/core/BayesFilter.h"
|
||||
#include "rtabmap/core/Memory.h"
|
||||
#include "rtabmap/core/Signature.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
|
||||
@@ -1,87 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef BAYESFILTER_H_
|
||||
#define BAYESFILTER_H_
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <list>
|
||||
#include <set>
|
||||
#include "rtabmap/utilite/UEventsHandler.h"
|
||||
#include "rtabmap/core/Parameters.h"
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
class Memory;
|
||||
class Signature;
|
||||
|
||||
class RTABMAP_EXP BayesFilter
|
||||
{
|
||||
public:
|
||||
BayesFilter(const ParametersMap & parameters = ParametersMap());
|
||||
virtual ~BayesFilter();
|
||||
virtual void parseParameters(const ParametersMap & parameters);
|
||||
const std::map<int, float> & computePosterior(const Memory * memory, const std::map<int, float> & likelihood);
|
||||
void reset();
|
||||
|
||||
//setters
|
||||
void setPredictionLC(const std::string & prediction);
|
||||
|
||||
//getters
|
||||
const std::map<int, float> & getPosterior() const {return _posterior;}
|
||||
float getVirtualPlacePrior() const {return _virtualPlacePrior;}
|
||||
const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
|
||||
std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
|
||||
|
||||
cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
|
||||
|
||||
private:
|
||||
cv::Mat updatePrediction(const cv::Mat & oldPrediction,
|
||||
const Memory * memory,
|
||||
const std::vector<int> & oldIds,
|
||||
const std::vector<int> & newIds) const;
|
||||
void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
|
||||
float addNeighborProb(cv::Mat & prediction,
|
||||
unsigned int col,
|
||||
const std::map<int, int> & neighbors,
|
||||
const std::map<int, int> & idToIndexMap) const;
|
||||
void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
|
||||
|
||||
private:
|
||||
std::map<int, float> _posterior;
|
||||
cv::Mat _prediction;
|
||||
float _virtualPlacePrior;
|
||||
std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
|
||||
bool _fullPredictionUpdate;
|
||||
float _totalPredictionLCValues;
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
#endif /* BAYESFILTER_H_ */
|
||||
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/core/DBDriver.h"
|
||||
|
||||
#include "rtabmap/core/Signature.h"
|
||||
#include "VisualWord.h"
|
||||
#include "rtabmap/core/VisualWord.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "rtabmap/utilite/UMath.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
|
||||
@@ -28,7 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "DBDriverSqlite3.h"
|
||||
|
||||
#include "rtabmap/core/Signature.h"
|
||||
#include "VisualWord.h"
|
||||
#include "rtabmap/core/VisualWord.h"
|
||||
#include "rtabmap/core/VWDictionary.h"
|
||||
#include "rtabmap/core/util3d.h"
|
||||
#include "rtabmap/core/Compression.h"
|
||||
|
||||
@@ -38,7 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/core/RtabmapEvent.h"
|
||||
#include "rtabmap/core/VWDictionary.h"
|
||||
#include <rtabmap/core/EpipolarGeometry.h>
|
||||
#include "VisualWord.h"
|
||||
#include "rtabmap/core/VisualWord.h"
|
||||
#include "rtabmap/core/Features2d.h"
|
||||
#include "DBDriverSqlite3.h"
|
||||
#include "rtabmap/core/util3d_features.h"
|
||||
|
||||
@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rtabmap/utilite/UTimer.h"
|
||||
#include "rtabmap/utilite/UConversion.h"
|
||||
#include "ParticleFilter.h"
|
||||
#include "rtabmap/core/ParticleFilter.h"
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
|
||||
@@ -1,177 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef PARTICLEFILTER_H_
|
||||
#define PARTICLEFILTER_H_
|
||||
|
||||
#include <rtabmap/utilite/UMath.h>
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
|
||||
|
||||
namespace rtabmap {
|
||||
|
||||
// taken from http://www.developpez.net/forums/d544518/c-cpp/c/equivalent-randn-matlab-c/
|
||||
#define TWOPI (6.2831853071795864769252867665590057683943387987502) /* 2 * pi */
|
||||
|
||||
/*
|
||||
RAND is a macro which returns a pseudo-random numbers from a uniform
|
||||
distribution on the interval [0 1]
|
||||
*/
|
||||
#define RAND (rand())/((double) RAND_MAX)
|
||||
|
||||
/*
|
||||
RANDN is a macro which returns a pseudo-random numbers from a normal
|
||||
distribution with mean zero and standard deviation one. This macro uses Box
|
||||
Muller's algorithm
|
||||
*/
|
||||
#define RANDN (sqrt(-2.0*log(RAND))*cos(TWOPI*RAND))
|
||||
|
||||
std::vector<double> cumSum(const std::vector<double> & v)
|
||||
{
|
||||
std::vector<double> cum(v.size());
|
||||
double sum = 0;
|
||||
for(unsigned int i=0; i<v.size(); ++i)
|
||||
{
|
||||
cum[i] = v[i] + sum;
|
||||
sum += v[i];
|
||||
}
|
||||
return cum;
|
||||
}
|
||||
|
||||
std::vector<double> resample(const std::vector<double> & p, // particles
|
||||
const std::vector<double> & w, // weights
|
||||
bool normalizeWeights = false)
|
||||
{
|
||||
std::vector<double> np; //new particles
|
||||
if(p.size() != w.size() || p.size() == 0)
|
||||
{
|
||||
UERROR("particles (%d) and weights (%d) are not the same size", p.size(), w.size());
|
||||
return np;
|
||||
}
|
||||
|
||||
std::vector<double> cs;
|
||||
if(normalizeWeights)
|
||||
{
|
||||
double wSum = uSum(w);
|
||||
std::vector<double> wNorm(w.size());
|
||||
for(unsigned int i=0; i<w.size(); ++i)
|
||||
{
|
||||
wNorm[i] = w[i]/wSum;
|
||||
}
|
||||
cs = cumSum(wNorm); // cumulative sum
|
||||
}
|
||||
else
|
||||
{
|
||||
cs = cumSum(w); // cumulative sum
|
||||
}
|
||||
for(unsigned int j=0; j<cs.size(); ++j)
|
||||
{
|
||||
cs[j]/=cs.back();
|
||||
}
|
||||
|
||||
np.resize(p.size());
|
||||
for(unsigned int i=0; i<np.size(); ++i)
|
||||
{
|
||||
unsigned int index = 0;
|
||||
double randnum = RAND;
|
||||
for(unsigned int j=0; j<cs.size(); ++j)
|
||||
{
|
||||
if(randnum < cs[j])
|
||||
{
|
||||
index = j;
|
||||
break;
|
||||
}
|
||||
}
|
||||
np[i] = p[index];
|
||||
}
|
||||
return np;
|
||||
}
|
||||
|
||||
|
||||
class ParticleFilter
|
||||
{
|
||||
public:
|
||||
ParticleFilter(unsigned int nParticles = 200,
|
||||
double noise = 0.1,
|
||||
double lambda = 10.0,
|
||||
double initValue = 0.0) :
|
||||
noise_(noise),
|
||||
lambda_(lambda)
|
||||
{
|
||||
particles_.resize(nParticles, initValue);
|
||||
}
|
||||
|
||||
void init(double initValue = 0.0f)
|
||||
{
|
||||
particles_ = std::vector<double>(particles_.size(), initValue);
|
||||
}
|
||||
|
||||
double filter(double val)
|
||||
{
|
||||
std::vector<double> weights(particles_.size(), 1);
|
||||
double sumWeights = 0;
|
||||
for(unsigned int i=0; i<particles_.size(); ++i)
|
||||
{
|
||||
// add noise to particle
|
||||
particles_[i] += noise_ * RANDN;
|
||||
|
||||
// compute weight
|
||||
double dist = fabs(particles_[i] - val);
|
||||
//dist = sqrt(dist*dist);
|
||||
double w = exp(-lambda_*dist);
|
||||
if(uIsFinite(w) && w > 0)
|
||||
{
|
||||
weights[i] = w;
|
||||
}
|
||||
sumWeights += weights[i];
|
||||
}
|
||||
|
||||
|
||||
//normalize and compute estimated value
|
||||
double value =0.0;
|
||||
for(unsigned int i=0; i<weights.size(); ++i)
|
||||
{
|
||||
weights[i] /= sumWeights;
|
||||
value += weights[i] * particles_[i];
|
||||
}
|
||||
|
||||
//resample the particles
|
||||
particles_ = resample(particles_, weights, false);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
private:
|
||||
std::vector<double> particles_;
|
||||
double noise_;
|
||||
double lambda_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
#endif /* PARTICLEFILTER_H_ */
|
||||
@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
|
||||
#include "rtabmap/core/Memory.h"
|
||||
#include "rtabmap/core/VWDictionary.h"
|
||||
#include "BayesFilter.h"
|
||||
#include "rtabmap/core/BayesFilter.h"
|
||||
|
||||
#include <rtabmap/utilite/ULogger.h>
|
||||
#include <rtabmap/utilite/UFile.h>
|
||||
|
||||
@@ -26,7 +26,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "rtabmap/core/VWDictionary.h"
|
||||
#include "VisualWord.h"
|
||||
#include "rtabmap/core/VisualWord.h"
|
||||
|
||||
#include "rtabmap/core/Signature.h"
|
||||
#include "rtabmap/core/DBDriver.h"
|
||||
|
||||
@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include "VisualWord.h"
|
||||
#include "rtabmap/core/VisualWord.h"
|
||||
#include "rtabmap/utilite/ULogger.h"
|
||||
#include "rtabmap/utilite/UStl.h"
|
||||
|
||||
|
||||
@@ -1,65 +0,0 @@
|
||||
/*
|
||||
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
|
||||
|
||||
#include <opencv2/core/core.hpp>
|
||||
#include <map>
|
||||
|
||||
namespace rtabmap
|
||||
{
|
||||
|
||||
class RTABMAP_EXP VisualWord
|
||||
{
|
||||
public:
|
||||
VisualWord(int id, const cv::Mat & descriptor, int signatureId = 0);
|
||||
~VisualWord();
|
||||
|
||||
void addRef(int signatureId);
|
||||
int removeAllRef(int signatureId);
|
||||
|
||||
int getTotalReferences() const {return _totalReferences;}
|
||||
int id() const {return _id;}
|
||||
const cv::Mat & getDescriptor() const {return _descriptor;}
|
||||
const std::map<int, int> & getReferences() const {return _references;} // (signature id , occurrence in the signature)
|
||||
|
||||
bool isSaved() const {return _saved;}
|
||||
void setSaved(bool saved) {_saved = saved;}
|
||||
|
||||
private:
|
||||
int _id;
|
||||
cv::Mat _descriptor;
|
||||
bool _saved; // If it's saved to db
|
||||
|
||||
int _totalReferences;
|
||||
std::map<int, int> _references; // (signature id , occurrence in the signature)
|
||||
std::map<int, int> _oldReferences; // (signature id , occurrence in the signature)
|
||||
};
|
||||
|
||||
} // namespace rtabmap
|
||||
Reference in New Issue
Block a user