i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

-Refactored HypVerification classes (added verification by signatures' similarity), there is no more complicated rejectedHypReason, now it's only true if valide or false if verification fails...

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/branches/0.3/rtabmap@86 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2011-06-09 02:56:31 +00:00
parent c59d461c81
commit 25c2a9f07e
13 changed files with 202 additions and 367 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Parameters.h
View File

@@ -121,7 +121,7 @@ typedef std::pair<const std::string, std::string> ParametersPair;
class RTABMAP_EXP Parameters
{
// Rtabmap parameters
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0); // Simple 0, Epipolar 1
RTABMAP_PARAM(Rtabmap, VhStrategy, int, 0); // None 0, Similarity 1, Epipolar 2
RTABMAP_PARAM(Rtabmap, PublishStats, bool, true); // Publishing statistics
RTABMAP_PARAM(Rtabmap, ReactivationThr, float, 0.0); // Reactivation threshold
RTABMAP_PARAM(Rtabmap, TimeThr, float, 0.7); // Maximum time allowed for the detector (s) (0 means infinity)
@@ -197,6 +197,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM_STR(Bayes, PredictionLC, "0.1 0.24 0.18 0.1 0.04 0.01"); // Prediction of loop closures (Gaussian-like, must be pair size) - Format: {VirtualPlaceProb, LoopClosureProb, BackwardNeighborLvl1, ForwardNeighborLvl1, BackwardNeighborLvl2, ForwardNeighborLvl2, ...}
// Verify hypotheses
RTABMAP_PARAM(Vh, Similarity, float, 0.5); // Minimum similarity to accept an hypothesis
RTABMAP_PARAM(VhEp, MatchCountMin, int, 8); // Minimum of matching visual words pairs to accept the loop hypothesis
RTABMAP_PARAM(VhEp, RansacParam1, float, 3.0); // Fundamental matrix (see cvFindFundamentalMat()): Max distance (in pixels) from the epipolar line for a point to be inlier
RTABMAP_PARAM(VhEp, RansacParam2, float, 0.99); // Fundamental matrix (see cvFindFundamentalMat()): Performance of the RANSAC

6
corelib/include/rtabmap/core/Rtabmap.h
View File

@@ -39,7 +39,7 @@ namespace rtabmap
class Signature;
class VerifyHypotheses;
class HypVerificator;
class Memory;
class BayesFilter;
class SMState;
@@ -60,7 +60,7 @@ public:
kStateDeletingMemory
};
enum VhStrategy {kVhSimple, kVhEpipolar, kVhUndef};
enum VhStrategy {kVhNone, kVhSim, kVhEpipolar, kVhUndef};
static const char * kDefaultIniFileName;
static const char * kDefaultIniFilePath;
@@ -153,7 +153,7 @@ private:
// Abstract classes containing all loop closure
// strategies for a type of signature or configuration.
VerifyHypotheses * _vhStrategy;
HypVerificator * _vhStrategy;
BayesFilter * _bayesFilter;
Memory * _memory;

2
corelib/include/rtabmap/core/RtabmapEvent.h
View File

@@ -46,7 +46,7 @@ namespace rtabmap
class RTABMAP_EXP Statistics
{
RTABMAP_STATS(Loop, Closure_id,);
RTABMAP_STATS(Loop, Rejected_reason,);
RTABMAP_STATS(Loop, RejectedHypothesis,);
RTABMAP_STATS(Loop, Highest_hypothesis_id,);
RTABMAP_STATS(Loop, Highest_hypothesis_value,);
RTABMAP_STATS(Loop, Vp_likelihood,);

6
corelib/src/KeypointMemory.cpp
View File

@@ -419,7 +419,7 @@ void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrate
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
std::set<int> pairsIdSet;
VerifyHypothesesEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> >::iterator kpIt = pairs.begin();
std::list<int>::iterator idIt = pairsId.begin();
for(; kpIt != pairs.end() && idIt != pairsId.end(); ++kpIt, ++idIt)
@@ -570,7 +570,7 @@ void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrate
{
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
VerifyHypothesesEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
std::set<int> ignoredIds(pairsId.begin(), pairsId.end());
int dif = maxWords - (int)wordsA.size();
newWords = getMostDescriptiveWords(wordsB, dif, ignoredIds);
@@ -598,7 +598,7 @@ void KeypointMemory::merge(const Signature * from, Signature * to, MergingStrate
}
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
VerifyHypothesesEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
HypVerificatorEpipolarGeo::findPairsDirect(wordsA, wordsB, pairs, pairsId);
std::set<int> ignoredIds(pairsId.begin(), pairsId.end());
int dif = maxWords - (int)wordsB.size();
newWords = getMostDescriptiveWords(wordsA, dif, ignoredIds);

84
corelib/src/Rtabmap.cpp
View File

@@ -299,8 +299,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
vhStrategy = (VhStrategy)std::atoi((*iter).second.c_str());
}
if(!_vhStrategy ||
(vhStrategy!=kVhUndef && ( (vhStrategy == kVhSimple && !dynamic_cast<VerifyHypothesesSimple*>(_vhStrategy)) ||
(vhStrategy == kVhEpipolar && !dynamic_cast<VerifyHypothesesEpipolarGeo*>(_vhStrategy)) ) ))
(vhStrategy!=kVhUndef && ( (vhStrategy == kVhSim && !dynamic_cast<HypVerificatorSim*>(_vhStrategy)) ||
(vhStrategy == kVhEpipolar && !dynamic_cast<HypVerificatorEpipolarGeo*>(_vhStrategy)) ) ))
{
if(_vhStrategy)
{
@@ -310,11 +310,13 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
switch(vhStrategy)
{
case kVhEpipolar:
_vhStrategy = new VerifyHypothesesEpipolarGeo(parameters);
_vhStrategy = new HypVerificatorEpipolarGeo(parameters);
break;
case kVhSim:
_vhStrategy = new HypVerificatorSim(parameters);
break;
case kVhSimple:
default:
_vhStrategy = new VerifyHypothesesSimple(parameters);
_vhStrategy = new HypVerificator(parameters);
break;
}
}
@@ -605,6 +607,7 @@ void Rtabmap::process()
int refId = Memory::kIdInvalid;
float hypothesisRatio = 0.0f; // Only used for statistics
bool rejectedHypothesis = false;
std::map<int, float> likelihood;
std::map<int, float> adjustedLikelihood;
@@ -613,7 +616,6 @@ void Rtabmap::process()
std::list<std::pair<int, float> > reactivateHypotheses;
std::map<int, int> childCount;
int rejectLoopReason = 0;
unsigned int signaturesReactivated = 0;
const Signature * signature = 0;
@@ -819,71 +821,49 @@ void Rtabmap::process()
timeHypothesesCreation = timer.ticks();
ULOGGER_INFO("timeHypothesesCreation=%f",timeHypothesesCreation);
rejectLoopReason = 1; //Default: NOT_ENOUGH_HYPOTHESES
if(hypotheses.size())
{
// Loop closure Threshold
if(!signature->isBadSignature() &&
likelihood.at(hypotheses.front().first) > 0.0f &&
hypotheses.front().second >= _loopThr &&
hypotheses.front().second >= _loopRatio*_highestHypothesisValue &&
_highestHypothesisValue)
if(likelihood.at(hypotheses.front().first) > 0.0f &&
hypotheses.front().second >= _loopThr)
{
//============================================================
// Hypothesis verification for loop closure with geometric
// information (like the epipolar geometry or using the local map
// associated with the signature)
//============================================================
//std::list<int> h;
//h.push_back(hypotheses.front().first);
_lcHypothesisId = hypotheses.front().first;
//_lcHypothesisId = _vhStrategy->verifyHypotheses(h, _memory);
if(hypotheses.front().second >= _loopRatio*_highestHypothesisValue &&
_highestHypothesisValue &&
_vhStrategy->verify(signature, _memory->getSignature(hypotheses.front().first)))
{
_lcHypothesisId = hypotheses.front().first;
}
else
{
rejectedHypothesis = true;
}
timeHypothesesValidation = timer.ticks();
ULOGGER_INFO("timeHypothesesValidation=%f",timeHypothesesValidation);
//rejectLoopReason = 10 + _vhStrategy->getStatus();
// We are tracking the next loop closures,
// reset the retrieval margin
_spreadMargin = 0;
rejectLoopReason = 10; //Accepted
}
if(hypotheses.front().second < _loopThr)
{
rejectLoopReason = 2;
}
//============================================================
// Retrieval id update
//============================================================
int lastReactivatedId = _reactivateId;
// Recalling Threshold
//this->selectHypotheses(posterior, reactivateHypotheses, false);
//if(reactivateHypotheses.front().second >= _remThr)
//{
//Priority to single hypotheses over _remThr
// _reactivateId = reactivateHypotheses.front().first;
//}
//else
//if(_lcHypothesisId)
//{
// _reactivateId = _lcHypothesisId;
//}
//else
//{
//_reactivateId = hypotheses.front().first;
std::list<std::pair<int, float> > hyp;
this->selectHypotheses(posterior, hyp, false);
if(likelihood.at(hyp.front().first) > _remThr)
{
_reactivateId = hyp.front().first;
}
//}
std::list<std::pair<int, float> > hyp;
this->selectHypotheses(posterior, hyp, false);
if(likelihood.at(hyp.front().first) > _remThr)
{
_reactivateId = hyp.front().first;
}
if(hypotheses.front().second < _loopRatio*_highestHypothesisValue)
{
rejectLoopReason = 3;
if(std::find(reactivatedIds.begin(),reactivatedIds.end(), hypotheses.front().first) != reactivatedIds.end())
{
// We are loosing the next loop closures (it
@@ -957,7 +937,7 @@ void Rtabmap::process()
}
// only send actions if rejectLoopReason!=3 (decreasing hypotheses)
if(sLoop && (_actionsSentRejectHyp || rejectLoopReason != 3))
if(sLoop && (_actionsSentRejectHyp || !rejectedHypothesis))
{
// select the actions of the neighbor with the highest
// weight (select the more recent if some have the same weight)
@@ -1115,7 +1095,7 @@ void Rtabmap::process()
}
//Epipolar geometry constraint
stat->addStatistic(Statistics::kLoopRejected_reason(), rejectLoopReason);
stat->addStatistic(Statistics::kLoopRejectedHypothesis(), rejectedHypothesis?1.0f:0);
timeStatsCreation = timer.ticks();
}
else
@@ -1245,7 +1225,7 @@ void Rtabmap::process()
refWordsCount,
dictionarySize,
int(_memory->getWorkingMemSize()),
rejectLoopReason,
rejectedHypothesis?1:0,
processMemoryUsed,
databaseMemoryUsed,
signaturesReactivated,

2
corelib/src/Signature.cpp
View File

@@ -188,7 +188,7 @@ float KeypointSignature::compareTo(const Signature * s) const
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > pairs;
std::list<int> pairsId;
int totalWords = _words.size()>words.size()?_words.size():words.size();
VerifyHypothesesEpipolarGeo::findPairsDirect(words, _words, pairs, pairsId);
HypVerificatorEpipolarGeo::findPairsDirect(words, _words, pairs, pairsId);
similarity = float(pairs.size()) / float(totalWords);

207
corelib/src/VerifyHypotheses.cpp
View File

@@ -20,7 +20,6 @@
#include "VerifyHypotheses.h"
#include "rtabmap/core/Parameters.h"
#include "Signature.h"
#include "Memory.h"
#include <cstdlib>
#include <opencv2/calib3d/calib3d.hpp>
@@ -29,139 +28,65 @@
namespace rtabmap
{
VerifyHypotheses::VerifyHypotheses(const ParametersMap & parameters) :
_status(0)
HypVerificator::HypVerificator(const ParametersMap & parameters)
{
this->parseParameters(parameters);
}
void VerifyHypotheses::parseParameters(const ParametersMap & parameters)
void HypVerificator::parseParameters(const ParametersMap & parameters)
{
}
bool HypVerificator::verify(const Signature * ref, const Signature * hyp)
{
UDEBUG("");
return ref && hyp && !ref->isBadSignature() && !hyp->isBadSignature();
}
/////////////////////////
// VerifyHypothesesSimple
// HypVerificatorSim
/////////////////////////
VerifyHypothesesSimple::VerifyHypothesesSimple(const ParametersMap & parameters) :
VerifyHypotheses(parameters)
HypVerificatorSim::HypVerificatorSim(const ParametersMap & parameters) :
HypVerificator(parameters),
_similarity(Parameters::defaultVhSimilarity())
{
this->parseParameters(parameters);
}
VerifyHypothesesSimple::~VerifyHypothesesSimple()
HypVerificatorSim::~HypVerificatorSim()
{
}
void VerifyHypothesesSimple::parseParameters(const ParametersMap & parameters)
{
VerifyHypotheses::parseParameters(parameters);
}
int VerifyHypothesesSimple::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
{
int hypothesis = 0;
if(!hypotheses.empty())
{
for(std::list<int>::const_iterator i = hypotheses.begin(); i!=hypotheses.end(); ++i)
{
if(*i > 0)
{
hypothesis = *i;
break;
}
}
}
return hypothesis;
}
/////////////////////////
// VerifyHypothesesSignSeq
/////////////////////////
/*VerifyHypothesesSignSeq::VerifyHypothesesSignSeq(const ParametersMap & parameters) :
VerifyHypotheses(parameters),
_seqLength(Parameters::defaultVhEpSeqLength())
{
this->parseParameters(parameters);
}
VerifyHypothesesSignSeq::~VerifyHypothesesSignSeq() {
}
void VerifyHypothesesSignSeq::parseParameters(const ParametersMap & parameters)
void HypVerificatorSim::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kVhEpSeqLength())) != parameters.end())
if((iter=parameters.find(Parameters::kVhSimilarity())) != parameters.end())
{
_seqLength = atoi((*iter).second.c_str());
_similarity = std::atof((*iter).second.c_str());
}
VerifyHypotheses::parseParameters(parameters);
HypVerificator::parseParameters(parameters);
}
int VerifyHypothesesSignSeq::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
bool HypVerificatorSim::verify(const Signature * ref, const Signature * hyp)
{
int hypothesis = 0;
std::map<int, int> hypothesesToKeep;
// update hypotheses
std::map<int, int>::iterator tmp;
for(std::list<int>::const_iterator j=hypotheses.begin(); j!=hypotheses.end(); ++j)
UDEBUG("");
if(ref && hyp)
{
// Add it like a new hypothesis
hypothesesToKeep.insert(std::pair<int, int>(*j, 1)); // NOTE : It will be deleted if an updated hypothesis gives the same id.
// If we have already this hypothesis, just keep it
tmp = _hypotheses.find(*j);
if(tmp != _hypotheses.end())
{
hypothesesToKeep.insert(std::pair<int, int>((*tmp).first, (*tmp).second));
}
// Forward hypothesis
tmp = _hypotheses.find(*j-1);
if(tmp != _hypotheses.end())
{
hypothesesToKeep.insert(std::pair<int, int>(*j, (*tmp).second+1));
}
// Backward hypothesis
tmp = _hypotheses.find(*j+1);
if(tmp != _hypotheses.end())
{
hypothesesToKeep.insert(std::pair<int, int>(*j, (*tmp).second-1));
}
return ref->compareTo(hyp) >= _similarity;
}
_hypotheses = hypothesesToKeep;
// if an hypothesis has at least 3 references, return the id
if(_hypotheses.size()>0)
{
for(std::map<int, int>::iterator i=_hypotheses.begin(); i!=_hypotheses.end(); ++i)
{
if((*i).second > _seqLength)
{
hypothesis = (*i).first;
break; // return the first
}
}
}
return hypothesis;
}*/
return false;
}
/////////////////////////
// VerifyHypothesesEpipolarGeo
// HypVerificatorEpipolarGeo
/////////////////////////
VerifyHypothesesEpipolarGeo::VerifyHypothesesEpipolarGeo(const ParametersMap & parameters) :
VerifyHypotheses(parameters),
HypVerificatorEpipolarGeo::HypVerificatorEpipolarGeo(const ParametersMap & parameters) :
HypVerificator(parameters),
_matchCountMinAccepted(Parameters::defaultVhEpMatchCountMin()),
_ransacParam1(Parameters::defaultVhEpRansacParam1()),
_ransacParam2(Parameters::defaultVhEpRansacParam2())
@@ -169,11 +94,11 @@ VerifyHypothesesEpipolarGeo::VerifyHypothesesEpipolarGeo(const ParametersMap & p
this->parseParameters(parameters);
}
VerifyHypothesesEpipolarGeo::~VerifyHypothesesEpipolarGeo() {
HypVerificatorEpipolarGeo::~HypVerificatorEpipolarGeo() {
}
void VerifyHypothesesEpipolarGeo::parseParameters(const ParametersMap & parameters)
void HypVerificatorEpipolarGeo::parseParameters(const ParametersMap & parameters)
{
ParametersMap::const_iterator iter;
if((iter=parameters.find(Parameters::kVhEpMatchCountMin())) != parameters.end())
@@ -188,66 +113,26 @@ void VerifyHypothesesEpipolarGeo::parseParameters(const ParametersMap & paramete
{
_ransacParam2 = std::atof((*iter).second.c_str());
}
VerifyHypotheses::parseParameters(parameters);
HypVerificator::parseParameters(parameters);
}
void VerifyHypothesesEpipolarGeo::setStatus(int status)
bool HypVerificatorEpipolarGeo::verify(const Signature * ref, const Signature * hyp)
{
// Only set if the status was not set before. This will keep the
// first error (when comparing with many signatures)
if(status == UNDEFINED || this->getStatus() == UNDEFINED || status == ACCEPTED)
UDEBUG("");
const KeypointSignature * ssRef = dynamic_cast<const KeypointSignature *>(ref);
const KeypointSignature * ssHyp = dynamic_cast<const KeypointSignature *>(hyp);
if(ssRef && ssHyp)
{
VerifyHypotheses::setStatus(status);
return doEpipolarGeometry(ssHyp, ssRef);
}
return false;
}
int VerifyHypothesesEpipolarGeo::verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem)
{
ULOGGER_DEBUG("");
int hypothesis = 0;
this->setStatus(UNDEFINED);
if(mem && !hypotheses.empty())
{
const KeypointSignature * ssRef = dynamic_cast<const KeypointSignature *>(mem->getLastSignature());
if(ssRef)
{
unsigned int i=0;
for(std::list<int>::const_iterator iter = hypotheses.begin(); iter!=hypotheses.end(); ++iter)
{
if(*iter > 0)
{
const KeypointSignature * ssHyp = dynamic_cast<const KeypointSignature *>(mem->getSignature(*iter));
if(ssHyp)
{
if(doEpipolarGeometry(ssHyp, ssRef))
{
hypothesis = *iter;
break;
}
}
}
++i;
}
}
}
else if(!mem)
{
this->setStatus(this->MEMORY_IS_NULL);
}
else if(hypotheses.empty())
{
this->setStatus(this->NO_HYPOTHESIS);
}
return hypothesis;
}
bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB)
bool HypVerificatorEpipolarGeo::doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB)
{
if(ssA == 0 || ssB == 0)
{
this->setStatus(this->NULL_MATCHING_SURF_SIGNATURES);
return false;
}
ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
@@ -280,7 +165,6 @@ bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * s
float(pairs.size()) / float(similarities));
if(pairsCount < _matchCountMinAccepted)
{
this->setStatus(this->NOT_ENOUGH_MATCHING_PAIRS);
return false;
}
@@ -452,17 +336,14 @@ bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * s
if(goodCount < _matchCountMinAccepted)
{
this->setStatus(this->EPIPOLAR_CONSTRAINT_FAILED);
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d), min is %d", goodCount, _matchCountMinAccepted);
return false;
}
else
{
this->setStatus(this->ACCEPTED);
return true;
}
}
this->setStatus(this->FUNDAMENTAL_MATRIX_NOT_FOUND);
return false;
}
@@ -470,7 +351,7 @@ bool VerifyHypothesesEpipolarGeo::doEpipolarGeometry(const KeypointSignature * s
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
* realPairsCount = 5
*/
int VerifyHypothesesEpipolarGeo::findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA,
int HypVerificatorEpipolarGeo::findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
std::list<int> & pairsId)
@@ -500,7 +381,7 @@ int VerifyHypothesesEpipolarGeo::findPairsDirect(const std::multimap<int, cv::Ke
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
*/
int VerifyHypothesesEpipolarGeo::findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA,
int HypVerificatorEpipolarGeo::findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
std::list<int> & pairsId)
@@ -531,7 +412,7 @@ int VerifyHypothesesEpipolarGeo::findPairsOne(const std::multimap<int, cv::KeyPo
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
* realPairsCount = 5
*/
int VerifyHypothesesEpipolarGeo::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
int HypVerificatorEpipolarGeo::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB,
std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs,
std::list<int> & pairsId)
@@ -565,7 +446,7 @@ int VerifyHypothesesEpipolarGeo::findPairsAll(const std::multimap<int, cv::KeyPo
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [1 2 4 6]
* return 4
*/
std::list<int> VerifyHypothesesEpipolarGeo::findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA,
std::list<int> HypVerificatorEpipolarGeo::findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA,
const std::multimap<int, cv::KeyPoint> & wordsB)
{
std::list<int> sameIds;
@@ -580,7 +461,7 @@ std::list<int> VerifyHypothesesEpipolarGeo::findSameIds(const std::multimap<int,
return sameIds;
}
int VerifyHypothesesEpipolarGeo::getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB)
int HypVerificatorEpipolarGeo::getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB)
{
const std::list<int> & ids = uUniqueKeys(wordsA);
int total = 0;

73
corelib/src/VerifyHypotheses.h
View File

@@ -32,85 +32,51 @@
namespace rtabmap
{
class Memory;
class Signature;
class RTABMAP_EXP VerifyHypotheses
// return always true, i.e, there is no verification
class RTABMAP_EXP HypVerificator
{
public:
virtual ~VerifyHypotheses() {}
virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem) = 0;
HypVerificator(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificator() {}
virtual bool verify(const Signature * ref, const Signature * hyp);
int getStatus() {return _status;}
virtual void parseParameters(const ParametersMap & parameters);
protected:
VerifyHypotheses(const ParametersMap & parameters = ParametersMap());
virtual void setStatus(int status) {_status = status;}
private:
int _status;
};
/////////////////////////
// VerifyHypothesesSimple
/////////////////////////
class VerifyHypothesesSimple : public VerifyHypotheses {
public:
VerifyHypothesesSimple(const ParametersMap & parameters = ParametersMap());
virtual ~VerifyHypothesesSimple();
virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
virtual void parseParameters(const ParametersMap & parameters);
};
/////////////////////////
// VerifyHypothesesSignSeq
// HypVerificatorSim
/////////////////////////
/*class VerifyHypothesesSignSeq : public VerifyHypotheses {
class HypVerificatorSim : public HypVerificator {
public:
VerifyHypothesesSignSeq(const ParametersMap & parameters = ParametersMap());
virtual ~VerifyHypothesesSignSeq();
virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
HypVerificatorSim(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificatorSim();
virtual bool verify(const Signature * ref, const Signature * hyp);
virtual void parseParameters(const ParametersMap & parameters);
private:
std::map<int, int> _hypotheses;
int _seqLength;
};*/
float _similarity;
};
/////////////////////////
// VerifyHypothesesEpipolarGeo
// HypVerificatorEpipolarGeo
/////////////////////////
class KeypointSignature;
class RTABMAP_EXP VerifyHypothesesEpipolarGeo : public VerifyHypotheses
class RTABMAP_EXP HypVerificatorEpipolarGeo : public HypVerificator
{
public:
enum STATUS
{
UNDEFINED,
ACCEPTED,
NO_HYPOTHESIS,
MEMORY_IS_NULL,
NOT_ENOUGH_MATCHING_PAIRS,
EPIPOLAR_CONSTRAINT_FAILED,
NULL_MATCHING_SURF_SIGNATURES,
FUNDAMENTAL_MATRIX_NOT_FOUND
};
public:
static int findPairsOne(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
static int findPairsDirect(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
static int findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB, std::list<std::pair<cv::KeyPoint, cv::KeyPoint> > & pairs, std::list<int> & pairsId);
static std::list<int> findSameIds(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
public:
VerifyHypothesesEpipolarGeo(const ParametersMap & parameters = ParametersMap());
virtual ~VerifyHypothesesEpipolarGeo();
virtual int verifyHypotheses(const std::list<int> & hypotheses, const Memory * mem);
HypVerificatorEpipolarGeo(const ParametersMap & parameters = ParametersMap());
virtual ~HypVerificatorEpipolarGeo();
virtual bool verify(const Signature * ref, const Signature * hyp);
virtual void parseParameters(const ParametersMap & parameters);
int getTotalSimilarities(const std::multimap<int, cv::KeyPoint> & wordsA, const std::multimap<int, cv::KeyPoint> & wordsB);
@@ -123,9 +89,6 @@ public:
void setRansacParam1(double ransacParam1) {_ransacParam1 = ransacParam1;}
void setRansacParam2(double ransacParam2) {_ransacParam2 = ransacParam2;}
protected:
virtual void setStatus(int status);
private:
bool doEpipolarGeometry(const KeypointSignature * ssA, const KeypointSignature * ssB);

2
corelib/tests/Tests.cpp
View File

@@ -468,7 +468,7 @@ void Tests::testVerifyHypotheses()
wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(5, 5, 1)));
wordsB.insert(std::pair<int, cv::KeyPoint>(6, cv::KeyPoint(6, 6, 1)));
int total = VerifyHypothesesEpipolarGeo::findPairsAll(wordsA, wordsB, pairs, pairsId);
int total = HypVerificatorEpipolarGeo::findPairsAll(wordsA, wordsB, pairs, pairsId);
//printf("[%d,%d]\n", total, (int)pairs.size());
CPPUNIT_ASSERT(total == 5);