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

Parameters renamed: "OdomLocalMap" group is now "OdomF2M" for Frame to Map odometry. Visual registration feature matching: using guess transform to limit the radius of correspondences "Vis/CorGuessWinSize=16"

Browse Source
This commit is contained in:
matlabbe
2016-02-22 16:13:14 -05:00
parent 0d68f74d80
commit ccc4b4a6c2
22 changed files with 844 additions and 638 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CMakeLists.txt
View File

@@ -57,7 +57,7 @@ SET(SRC_FILES
Odometry.cpp
OdometryThread.cpp
OdometryLocalMap.cpp
OdometryF2M.cpp
OdometryMono.cpp
OdometryF2F.cpp

1
corelib/src/Memory.cpp
View File

@@ -2056,7 +2056,6 @@ Transform Memory::computeTransform(
if(fromS && toS)
{
UWARN("%d=%d %d=%d", fromId, fromS->sensorData().cameraModels().size(), toId, toS->sensorData().cameraModels().size());
// make sure we have all data needed
if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
(_registrationPipeline->isScanRequired()) ||

4
corelib/src/Odometry.cpp
View File

@@ -25,9 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/OdometryF2M.h>
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryF2F.h"
#include "rtabmap/core/OdometryLocalMap.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
@@ -54,7 +54,7 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
odometry = new OdometryF2F(parameters);
break;
default:
odometry = new OdometryLocalMap(parameters);
odometry = new OdometryF2M(parameters);
type = Odometry::kTypeLocalMap;
break;
}

35
corelib/src/OdometryF2F.cpp
View File

@@ -37,12 +37,12 @@ namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomF2FKeyFrameThr()),
guessFromMotion_(Parameters::defaultOdomF2FGuessMotion()),
guessFromMotion_(Parameters::defaultOdomGuessMotion()),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomF2FKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2FGuessMotion(), guessFromMotion_);
Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
}
OdometryF2F::~OdometryF2F()
@@ -130,25 +130,28 @@ Transform OdometryF2F::computeTransform(
if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_)
{
UDEBUG("Update key frame");
Signature newRefFrame(data);
int features = 0;
if(registrationPipeline_->getMinVisualCorrespondences()>0)
int features = newFrame.sensorData().keypoints().size();
if(features == 0)
{
newFrame = Signature(data);
// this will generate features only for the first frame
Signature dummy;
registrationPipeline_->computeTransformationMod(
newRefFrame,
newFrame,
dummy);
features = (int)newRefFrame.getWords().size();
features = (int)newFrame.sensorData().keypoints().size();
}
if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
(registrationPipeline_->getMinGeometryCorrespondencesRatio()==0.0f ||
(newRefFrame.sensorData().laserScanRaw().cols &&
(newRefFrame.sensorData().laserScanMaxPts() == 0 || float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
(newFrame.sensorData().laserScanRaw().cols &&
(newFrame.sensorData().laserScanMaxPts() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
{
refFrame_ = newRefFrame;
refFrame_ = newFrame;
refFrame_.setWords(std::multimap<int, cv::KeyPoint>());
refFrame_.setWords3(std::multimap<int, cv::Point3f>());
refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());
//reset motion
motionSinceLastKeyFrame_.setIdentity();
@@ -160,13 +163,13 @@ Transform OdometryF2F::computeTransform(
UWARN("Too low 2D features (%d), keeping last key frame...", features);
}
if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newRefFrame.sensorData().laserScanRaw().cols==0)
if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanRaw().cols==0)
{
UWARN("Too low scan points (%d), keeping last key frame...", newRefFrame.sensorData().laserScanRaw().cols);
UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().cols);
}
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newRefFrame.sensorData().laserScanMaxPts() != 0 && float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanMaxPts() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
{
UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
}
}
}
@@ -189,7 +192,7 @@ Transform OdometryF2F::computeTransform(
timer.elapsed(),
output.isNull()?"true":"false",
(int)regInfo.inliers,
(int)refFrame_.getWords().size(),
(int)refFrame_.sensorData().keypoints().size(),
!output.isNull()?"true":"false");
return output;

328
corelib/src/OdometryF2M.cpp Normal file
View File

@@ -0,0 +1,328 @@
/*
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.
*/
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/VisualWord.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include <opencv2/calib3d/calib3d.hpp>
#include <rtabmap/core/OdometryF2M.h>
#if _MSC_VER
#define ISFINITE(value) _finite(value)
#else
#define ISFINITE(value) std::isfinite(value)
#endif
namespace rtabmap {
OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Odometry(parameters),
maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
regVis_(new RegistrationVis(parameters)),
map_(new Signature(-1))
{
UDEBUG("");
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
if(!fixedMapPath_.empty())
{
UINFO("Init odometry from a fixed database: \"%s\"", fixedMapPath_.c_str());
// init the local map with a all 3D features contained in the database
ParametersMap customParameters;
customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
Memory memory(customParameters);
if(!memory.init(fixedMapPath_, false, ParametersMap()))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
else
{
// get the graph
std::map<int, int> ids = memory.getNeighborsId(memory.getLastSignatureId(), 0, -1);
std::map<int, Transform> poses;
std::multimap<int, Link> links;
memory.getMetricConstraints(uKeysSet(ids), poses, links, true);
if(poses.size())
{
//optimize the graph
Optimizer * optimizer = Optimizer::create(parameters);
std::map<int, Transform> optimizedPoses = optimizer->optimize(poses.begin()->first, poses, links);
delete optimizer;
std::multimap<int, cv::Point3f> words3D;
std::multimap<int, cv::Mat> wordsDescriptors;
// fill the local map
for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
posesIter!=optimizedPoses.end();
++posesIter)
{
const Signature * s = memory.getSignature(posesIter->first);
if(s)
{
// Transform 3D points accordingly to pose and add them to local map
for(std::multimap<int, cv::Point3f>::const_iterator pointsIter=s->getWords3().begin();
pointsIter!=s->getWords3().end();
++pointsIter)
{
if(!uContains(words3D, pointsIter->first))
{
words3D.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
if(s->getWordsDescriptors().size() == s->getWords3().size())
{
UASSERT(uContains(s->getWordsDescriptors(), pointsIter->first));
wordsDescriptors.insert(std::make_pair(pointsIter->first, s->getWordsDescriptors().find(pointsIter->first)->second));
}
else // load descriptor from dictionary
{
UASSERT(memory.getVWDictionary()->getWord(pointsIter->first) != 0);
wordsDescriptors.insert(std::make_pair(pointsIter->first, memory.getVWDictionary()->getWord(pointsIter->first)->getDescriptor()));
}
}
}
}
}
UASSERT(words3D.size() == wordsDescriptors.size());
map_->setWords3(words3D);
map_->setWordsDescriptors(wordsDescriptors);
}
else
{
UERROR("No pose loaded from database \"%s\"", fixedMapPath_.c_str());
}
}
if((int)map_->getWords3().size() < regVis_->getMinInliers() || map_->getWords3().size() == 0)
{
UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
fixedMapPath_.c_str(), (int)map_->getWords3().size());
}
}
}
OdometryF2M::~OdometryF2M()
{
delete map_;
UDEBUG("");
}
void OdometryF2M::reset(const Transform & initialPose)
{
if(fixedMapPath_.empty())
{
Odometry::reset(initialPose);
map_->sensorData() = SensorData();
}
else
{
UWARN("Odometry cannot be reset when a fixed local map is set.");
}
}
const std::multimap<int, cv::Point3f> & OdometryF2M::getLocalMap() const
{
return map_->getWords3();
}
// return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(info)
{
info->type = 0;
}
RegistrationInfo regInfo;
int nFeatures = 0;
// Generate keypoints from the new data
if(data.isValid())
{
Signature newSignature(data);
if(map_->getWords3().size() && newSignature.sensorData().isValid())
{
Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
Transform transform = regVis_->computeTransformationMod(*map_, newSignature, guess, &regInfo);
if(!transform.isNull())
{
// make it incremental
transform = this->getPose().inverse() * transform;
}
else if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UWARN("Unknown registration error");
}
if(fixedMapPath_.empty())
{
output = transform;
int added = 0;
int removed = 0;
// update local map
std::multimap<int, cv::Point3f> mapPoints = map_->getWords3();
std::multimap<int, cv::Mat> mapDescriptors = map_->getWordsDescriptors();
Transform t = this->getPose()*output;
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT(newSignature.getWordsDescriptors().size() == newSignature.getWords3().size());
std::list<int> newIds = uUniqueKeys(newSignature.getWordsDescriptors());
for(std::list<int>::iterator iter=newIds.begin(); iter!=newIds.end(); ++iter)
{
if(mapPoints.find(*iter) == mapPoints.end())
{
mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(newSignature.getWords3().find(*iter)->second, t)));
mapDescriptors.insert(std::make_pair(*iter, newSignature.getWordsDescriptors().find(*iter)->second));
++added;
}
}
// remove words in map if max size is reached
if(mapPoints.size() > maximumMapSize_)
{
// remove oldest first, keep matched features
std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
std::multimap<int, cv::Mat>::iterator iterMapWords = mapDescriptors.begin();
for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
{
if(matches.find(iter->first) == matches.end())
{
iter = mapPoints.erase(iter);
iterMapWords = mapDescriptors.erase(iterMapWords);
++removed;
}
else
{
++iter;
++iterMapWords;
}
}
}
map_->setWords3(mapPoints);
map_->setWordsDescriptors(mapDescriptors);
UINFO("Updated map: %d added %d removed (new map size=%d)", added, removed, (int)mapPoints.size());
}
else
{
// fixed local map, don't update with the new signature
output = transform;
}
}
else
{
// just generate keypoints for the new signature
Signature dummy;
regVis_->computeTransformationMod(
newSignature,
dummy);
if(fixedMapPath_.empty() && (int)newSignature.getWords3().size() >= regVis_->getMinInliers())
{
output.setIdentity();
// a very high variance tells that the new pose is not linked with the previous one
regInfo.variance = 9999;
Transform t = this->getPose(); // initial pose may be not identity...
std::multimap<int, cv::Point3f> transformedPoints;
for(std::multimap<int, cv::Point3f>::const_iterator iter = newSignature.getWords3().begin(); iter!=newSignature.getWords3().end(); ++iter)
{
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
}
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(newSignature.getWordsDescriptors());
map_->sensorData().setCameraModels(newSignature.sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(newSignature.sensorData().stereoCameraModel());
}
}
map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
if(this->isInfoDataFilled() && info)
{
info->words = newSignature.getWords();
}
}
if(info)
{
info->variance = regInfo.variance;
info->inliers = regInfo.inliers;
info->matches = regInfo.matches;
info->features = nFeatures;
info->localMapSize = (int)map_->getWords3().size();
if(this->isInfoDataFilled())
{
info->wordMatches = regInfo.matchesIDs;
info->wordInliers = regInfo.inliersIDs;
}
}
UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d",
timer.elapsed(),
output.isNull()?"true":"false",
nFeatures,
regInfo.inliers,
regInfo.matches,
regInfo.variance,
(int)map_->getWords3().size());
return output;
}
} // namespace rtabmap

395
corelib/src/OdometryLocalMap.cpp
View File

@@ -1,395 +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.
*/
#include "rtabmap/core/OdometryLocalMap.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include <opencv2/calib3d/calib3d.hpp>
#if _MSC_VER
#define ISFINITE(value) _finite(value)
#else
#define ISFINITE(value) std::isfinite(value)
#endif
namespace rtabmap {
OdometryLocalMap::OdometryLocalMap(const ParametersMap & parameters) :
Odometry(parameters),
localHistoryMaxSize_(Parameters::defaultOdomLocalMapHistorySize()),
fixedLocalMapPath_(Parameters::defaultOdomLocalMapFixedMapPath()),
memory_(0),
regVis_(new RegistrationVis(parameters))
{
UDEBUG("");
Parameters::parse(parameters, Parameters::kOdomLocalMapHistorySize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomLocalMapFixedMapPath(), fixedLocalMapPath_);
ParametersMap customParameters;
float minDepth = Parameters::defaultVisMinDepth();
float maxDepth = Parameters::defaultVisMaxDepth();
std::string roi = Parameters::defaultVisRoiRatios();
bool useDepthAsMask = Parameters::defaultVisUseDepthAsMask();
Parameters::parse(parameters, Parameters::kVisMinDepth(), minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), roi);
Parameters::parse(parameters, Parameters::kVisUseDepthAsMask(), useDepthAsMask);
customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(minDepth)));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), roi));
customParameters.insert(ParametersPair(Parameters::kMemUseDepthAsMask(), uBool2Str(useDepthAsMask)));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false"));
int nn = Parameters::defaultVisCorNNType();
float nndr = Parameters::defaultVisCorNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisCorNNType(), nn);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(maxFeatures)));
// Memory's stereo parameters, copy from Odometry
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
int subPixIterations = Parameters::defaultVisSubPixIterations();
double subPixEps = Parameters::defaultVisSubPixEps();
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(Parameters::isFeatureParameter(iter->first) ||
group.compare("Stereo") == 0)
{
customParameters.insert(*iter);
}
}
if(fixedLocalMapPath_.empty())
{
memory_ = new Memory(customParameters);
if(!memory_->init("", false, ParametersMap()))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
}
else
{
UINFO("Init odometry from a fixed database: \"%s\"", fixedLocalMapPath_.c_str());
// init the local map with a all 3D features contained in the database
customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
memory_ = new Memory(customParameters);
if(!memory_->init(fixedLocalMapPath_, false, ParametersMap()))
{
UERROR("Error initializing the memory for BOW Odometry.");
}
else
{
// get the graph
std::map<int, int> ids = memory_->getNeighborsId(memory_->getLastSignatureId(), 0, -1);
std::map<int, Transform> poses;
std::multimap<int, Link> links;
memory_->getMetricConstraints(uKeysSet(ids), poses, links, true);
if(poses.size())
{
//optimize the graph
Optimizer * optimizer = Optimizer::create(parameters);
std::map<int, Transform> optimizedPoses = optimizer->optimize(poses.begin()->first, poses, links);
delete optimizer;
// fill the local map
for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
posesIter!=optimizedPoses.end();
++posesIter)
{
const Signature * s = memory_->getSignature(posesIter->first);
if(s)
{
// Transform 3D points accordingly to pose and add them to local map
const std::multimap<int, cv::Point3f> & words3D = s->getWords3();
for(std::multimap<int, cv::Point3f>::const_iterator pointsIter=words3D.begin();
pointsIter!=words3D.end();
++pointsIter)
{
if(!uContains(localMap_, pointsIter->first))
{
localMap_.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
}
}
}
}
}
else
{
UERROR("No pose loaded from database \"%s\"", fixedLocalMapPath_.c_str());
}
}
if((int)localMap_.size() < regVis_->getMinInliers() || localMap_.size() == 0)
{
UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
fixedLocalMapPath_.c_str(), (int)localMap_.size());
}
}
}
OdometryLocalMap::~OdometryLocalMap()
{
delete memory_;
UDEBUG("");
}
void OdometryLocalMap::reset(const Transform & initialPose)
{
if(fixedLocalMapPath_.empty())
{
Odometry::reset(initialPose);
memory_->init("", false, ParametersMap());
localMap_.clear();
}
else
{
UWARN("Odometry cannot be reset when a fixed local map is set.");
}
}
// return not null transform if odometry is correctly computed
Transform OdometryLocalMap::computeTransform(
const SensorData & data,
OdometryInfo * info)
{
UTimer timer;
Transform output;
if(info)
{
info->type = 0;
}
RegistrationInfo regInfo;
int nFeatures = 0;
if(memory_->update(data))
{
const Signature * newSignature = memory_->getLastWorkingSignature();
if(newSignature)
{
nFeatures = (int)newSignature->getWords().size();
if(this->isInfoDataFilled() && info)
{
info->words = newSignature->getWords();
}
}
if(localMap_.size() && newSignature)
{
Transform transform;
if((int)localMap_.size() >= regVis_->getMinInliers() &&
(int)newSignature->getWords().size()>=regVis_->getMinInliers())
{
Transform t;
Signature tmpLocalMap(-1);
tmpLocalMap.setWords3(localMap_);
t = regVis_->computeTransformation(tmpLocalMap, *newSignature, this->getPose(), &regInfo);
if(!t.isNull())
{
// make it incremental
transform = this->getPose().inverse() * t;
}
else if(!regInfo.rejectedMsg.empty())
{
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
}
else
{
UWARN("Unknown registration error");
}
}
else if((int)newSignature->getWords().size()<regVis_->getMinInliers())
{
UWARN("New signature has too low extracted features (%d < %d)", (int)newSignature->getWords().size(), regVis_->getMinInliers());
}
else
{
UWARN("Local map too small!? (%d < %d)", (int)localMap_.size(), regVis_->getMinInliers());
}
if(transform.isNull())
{
memory_->deleteLocation(newSignature->id());
}
else if(fixedLocalMapPath_.empty())
{
output = transform;
// remove words if history max size is reached
while(localMap_.size() && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>1)
{
int nodeId = *memory_->getStMem().begin();
std::list<int> removedPts;
memory_->deleteLocation(nodeId, &removedPts);
for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
{
localMap_.erase(*iter);
}
}
if(localHistoryMaxSize_ == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
{
UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
(int)localMap_.size(), localHistoryMaxSize_, (int)newSignature->getWords3().size());
}
// update local map
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
Transform t = this->getPose()*output;
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
// Only add unique words not in local map
if(newSignature->getWords3().count(*iter) == 1)
{
// keep old word
if(localMap_.find(*iter) == localMap_.end())
{
const cv::Point3f & pt = newSignature->getWords3().find(*iter)->second;
if(util3d::isFinite(pt))
{
cv::Point3f pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
}
}
else
{
localMap_.erase(*iter);
}
}
}
else
{
// fixed local map, just delete the new signature
output = transform;
memory_->deleteLocation(newSignature->id());
}
}
else if(newSignature)
{
int count = 0;
std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
if(fixedLocalMapPath_.empty() && (int)uniques.size() >= regVis_->getMinInliers())
{
output.setIdentity();
// a very high variance tells that the new pose is not linked with the previous one
regInfo.variance = 9999;
Transform t = this->getPose(); // initial pose maybe not identity...
for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
{
// Only add unique words
if(newSignature->getWords3().count(*iter) == 1)
{
const cv::Point3f & pt = newSignature->getWords3().find(*iter)->second;
if(util3d::isFinite(pt))
{
cv::Point3f pt2 = util3d::transformPoint(pt, t);
localMap_.insert(std::make_pair(*iter, pt2));
}
else
{
++count;
}
}
}
}
else
{
// not enough features, just delete it
memory_->deleteLocation(newSignature->id());
}
UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
}
memory_->emptyTrash();
}
if(info)
{
info->variance = regInfo.variance;
info->inliers = regInfo.inliers;
info->matches = regInfo.matches;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
if(this->isInfoDataFilled())
{
info->wordMatches = regInfo.matchesIDs;
info->wordInliers = regInfo.inliersIDs;
}
}
UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d dict=%d nodes=%d",
timer.elapsed(),
output.isNull()?"true":"false",
nFeatures,
regInfo.inliers,
regInfo.matches,
regInfo.variance,
(int)localMap_.size(),
(int)memory_->getVWDictionary()->getVisualWords().size(),
(int)memory_->getStMem().size());
return output;
}
} // namespace rtabmap

14
corelib/src/OdometryMono.cpp
View File

@@ -59,7 +59,7 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
pnpReprojError_(Parameters::defaultVisPnPReprojError()),
pnpFlags_(Parameters::defaultVisPnPFlags()),
pnpRefineIterations_(Parameters::defaultVisPnPRefineIterations()),
localHistoryMaxSize_(Parameters::defaultOdomLocalMapHistorySize()),
localHistoryMaxSize_(Parameters::defaultOdomF2MMaxSize()),
initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
minTranslation_(Parameters::defaultOdomMonoMinTranslation()),
@@ -77,7 +77,7 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), pnpReprojError_);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), pnpFlags_);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), pnpRefineIterations_);
Parameters::parse(parameters, Parameters::kOdomLocalMapHistorySize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
@@ -129,15 +129,7 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
std::string group = uSplit(iter->first, '/').front();
if(group.compare("SURF") == 0 ||
group.compare("SIFT") == 0 ||
group.compare("BRIEF") == 0 ||
group.compare("FAST") == 0 ||
group.compare("ORB") == 0 ||
group.compare("FREAK") == 0 ||
group.compare("GFTT") == 0 ||
group.compare("BRISK") == 0)
if(Parameters::isFeatureParameter(iter->first))
{
customParameters.insert(*iter);
}

4
corelib/src/OdometryThread.cpp
View File

@@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/OdometryF2M.h>
#include "rtabmap/core/OdometryThread.h"
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/OdometryMono.h"
#include "rtabmap/core/OdometryLocalMap.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/OdometryEvent.h"
@@ -103,7 +103,7 @@ void OdometryThread::mainLoop()
void OdometryThread::addData(const SensorData & data)
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryLocalMap*>(_odometry) == 0)
if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryF2M*>(_odometry) == 0)
{
if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
{

13
corelib/src/Parameters.cpp
View File

@@ -140,11 +140,16 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.11.2
removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion())));
// 0.11.0
removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize", std::make_pair(true, Parameters::kOdomLocalMapHistorySize())));
removedParameters_.insert(std::make_pair("OdomBow/FixedLocalMapPath", std::make_pair(true, Parameters::kOdomLocalMapFixedMapPath())));
removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
removedParameters_.insert(std::make_pair("OdomBow/FixedLocalMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
removedParameters_.insert(std::make_pair("OdomFlow/KeyFrameThr", std::make_pair(true, Parameters::kOdomF2FKeyFrameThr())));
removedParameters_.insert(std::make_pair("OdomFlow/GuessMotion", std::make_pair(true, Parameters::kOdomF2FGuessMotion())));
removedParameters_.insert(std::make_pair("OdomFlow/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion())));
removedParameters_.insert(std::make_pair("Kp/WordsPerImage", std::make_pair(true, Parameters::kKpMaxFeatures())));
@@ -248,7 +253,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(false, "")));
removedParameters_.insert(std::make_pair("Odom/Type", std::make_pair(true, Parameters::kVisFeatureType())));
removedParameters_.insert(std::make_pair("Odom/MaxWords", std::make_pair(true, Parameters::kVisMaxFeatures())));
removedParameters_.insert(std::make_pair("Odom/LocalHistory", std::make_pair(true, Parameters::kOdomLocalMapHistorySize())));
removedParameters_.insert(std::make_pair("Odom/LocalHistory", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
removedParameters_.insert(std::make_pair("Odom/NearestNeighbor", std::make_pair(true, Parameters::kVisCorNNType())));
removedParameters_.insert(std::make_pair("Odom/NNDR", std::make_pair(true, Parameters::kVisCorNNDR())));
}

380
corelib/src/RegistrationVis.cpp
View File

@@ -40,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UMath.h>
#include <rtflann/flann.hpp>
namespace rtabmap {
RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
@@ -59,6 +61,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_flowIterations(Parameters::defaultVisCorFlowIterations()),
_flowEps(Parameters::defaultVisCorFlowEps()),
_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
_nndr(Parameters::defaultVisCorNNDR()),
_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
_useDepthAsMask(Parameters::defaultVisUseDepthAsMask())
{
_featureParameters = Parameters::getDefaultParameters();
@@ -95,6 +99,8 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
Parameters::parse(parameters, Parameters::kVisUseDepthAsMask(), _useDepthAsMask);
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
@@ -157,10 +163,15 @@ RegistrationVis::~RegistrationVis()
{
}
Feature2D * RegistrationVis::createFeatureDetector() const
{
return Feature2D::create(_featureParameters);
}
Transform RegistrationVis::computeTransformationImpl(
Signature & fromSignature,
Signature & toSignature,
Transform guess, // guess is only used by Optical Flow correspondences (flowMaxLevel is set to 0 when guess is used)
Transform guess, // (flowMaxLevel is set to 0 when guess is used)
RegistrationInfo & info) const
{
UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
@@ -212,7 +223,8 @@ Transform RegistrationVis::computeTransformationImpl(
{
UDEBUG("");
// just some checks to make sure that input data are ok
UASSERT((fromSignature.getWords().empty() && fromSignature.getWords3().empty())||
UASSERT(fromSignature.getWords().empty() ||
fromSignature.getWords3().empty() ||
(fromSignature.getWords().size() == fromSignature.getWords3().size()));
UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
@@ -224,38 +236,43 @@ Transform RegistrationVis::computeTransformationImpl(
toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
toSignature.sensorData().descriptors().rows == 0 ||
toSignature.getWordsDescriptors().size() == 0);
UASSERT(fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
UASSERT(fromSignature.sensorData().imageRaw().empty() ||
fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
fromSignature.sensorData().imageRaw().type() == CV_8UC3);
UASSERT(toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
UASSERT(toSignature.sensorData().imageRaw().empty() ||
toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
toSignature.sensorData().imageRaw().type() == CV_8UC3);
Feature2D * detector = Feature2D::create(_featureParameters);
Feature2D * detector = createFeatureDetector();
std::vector<cv::KeyPoint> kptsFrom;
if(fromSignature.getWords().empty())
{
if(fromSignature.sensorData().keypoints().empty())
{
if(fromSignature.sensorData().imageRaw().channels() > 1)
if(!fromSignature.sensorData().imageRaw().empty())
{
cv::Mat tmp;
cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
fromSignature.sensorData().setImageRaw(tmp);
}
cv::Mat depthMask;
if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty())
{
if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
if(fromSignature.sensorData().imageRaw().channels() > 1)
{
depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
cv::Mat tmp;
cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
fromSignature.sensorData().setImageRaw(tmp);
}
}
kptsFrom = detector->generateKeypoints(
fromSignature.sensorData().imageRaw(),
depthMask);
cv::Mat depthMask;
if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty())
{
if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
{
depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
}
}
kptsFrom = detector->generateKeypoints(
fromSignature.sensorData().imageRaw(),
depthMask);
}
}
else
{
@@ -271,6 +288,8 @@ Transform RegistrationVis::computeTransformationImpl(
std::multimap<int, cv::KeyPoint> wordsTo;
std::multimap<int, cv::Point3f> words3From;
std::multimap<int, cv::Point3f> words3To;
std::multimap<int, cv::Mat> wordsDescFrom;
std::multimap<int, cv::Mat> wordsDescTo;
if(_correspondencesApproach == 1) //Optical Flow
{
UDEBUG("");
@@ -325,7 +344,6 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<unsigned char> status;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
int winSize = _flowWinSize;
cv::calcOpticalFlowPyrLK(
fromSignature.sensorData().imageRaw(),
toSignature.sensorData().imageRaw(),
@@ -333,7 +351,7 @@ Transform RegistrationVis::computeTransformationImpl(
cornersTo,
status,
err,
cv::Size(winSize, winSize),
cv::Size(_flowWinSize, _flowWinSize),
guessSet?0:_flowMaxLevel,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
@@ -440,30 +458,28 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
UDEBUG("kptsTo=%d", (int)kptsTo.size());
cv::Mat descriptorsFrom;
if(kptsFrom.size())
if((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
fromSignature.getWordsDescriptors().size() == (int)kptsFrom.size())
{
if(fromSignature.getWordsDescriptors().size() == (int)kptsFrom.size())
descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
fromSignature.getWordsDescriptors().begin()->second.cols,
fromSignature.getWordsDescriptors().begin()->second.type());
int i=0;
for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
iter!=fromSignature.getWordsDescriptors().end();
++iter, ++i)
{
descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
fromSignature.getWordsDescriptors().begin()->second.cols,
fromSignature.getWordsDescriptors().begin()->second.type());
int i=0;
for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
iter!=fromSignature.getWordsDescriptors().end();
++iter, ++i)
{
iter->second.copyTo(descriptorsFrom.row(i));
}
}
else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
{
descriptorsFrom = fromSignature.sensorData().descriptors();
}
else if(!fromSignature.sensorData().imageRaw().empty())
{
descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
iter->second.copyTo(descriptorsFrom.row(i));
}
}
else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
{
descriptorsFrom = fromSignature.sensorData().descriptors();
}
else if(!fromSignature.sensorData().imageRaw().empty())
{
descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
}
cv::Mat descriptorsTo;
if(kptsTo.size())
@@ -513,57 +529,239 @@ Transform RegistrationVis::computeTransformationImpl(
kptsTo3D = uValues(toSignature.getWords3());
}
// We have all data we need here, so match using the vocabulary
UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
VWDictionary dictionary(_featureParameters);
std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
std::list<int> toWordIds;
UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
if(descriptorsTo.rows)
{
dictionary.update();
toWordIds = dictionary.addNewWords(descriptorsTo, 2);
}
dictionary.clear(false);
std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
UASSERT(kptsFrom3D.size() == kptsFrom.size());
UASSERT(fromWordIds.size() == kptsFrom.size());
int i=0;
for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
{
if(fromWordIdsSet.count(*iter) == 1)
{
wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
}
++i;
}
UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
UASSERT(toWordIds.size() == kptsTo.size());
i=0;
for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
{
if(toWordIdsSet.count(*iter) == 1)
{
wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
}
}
++i;
}
//remove doubles
fromSignature.sensorData().setFeatures(kptsFrom, descriptorsFrom);
toSignature.sensorData().setFeatures(kptsTo, descriptorsTo);
UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
// We have all data we need here, so match!
if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
{
// If guess is set, limit the search of matches using optical flow window size
bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet)
{
UDEBUG("");
UASSERT(kptsTo.size() == descriptorsTo.rows);
// Use guess to project 3D "from" keypoints into "to" image
std::vector<cv::Point2f> cornersProjected;
if(kptsFrom3D.size() && (guessSet || kptsFrom.size()==0))
{
if(fromSignature.sensorData().cameraModels().size() > 1)
{
UFATAL("Radius feature matching is not supported for multiple cameras.");
}
Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
Transform guessCameraRef = (guess * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersProjected);
}
else if(kptsFrom.size())
{
cv::KeyPoint::convert(kptsFrom, cornersProjected);
}
UDEBUG("");
// For each projected feature guess of "from" in "to", find its matching feature in
// the radius around the projected guess.
// TODO: do cross-check?
if(cornersProjected.size())
{
// Create kd-tree for keypoints "to"
std::vector<cv::Point2f> pointsTo;
cv::KeyPoint::convert(kptsTo, pointsTo);
rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
rtflann::Index<rtflann::L2<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
index.buildIndex();
std::vector< std::vector<size_t> > indices;
std::vector<std::vector<float> > dists;
float radius = (float)_guessWinSize; // pixels
rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
UASSERT((int)indices.size() == cornersProjectedMat.rows);
UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
UASSERT(cornersProjectedMat.rows == descriptorsFrom.rows);
UASSERT(kptsFrom.empty() || cornersProjectedMat.rows == (int)kptsFrom.size());
UASSERT(kptsFrom3D.empty() || cornersProjectedMat.rows == (int)kptsFrom3D.size());
UDEBUG("");
// Process results (Nearest Neighbor Distance Ratio)
int notMatchedUniqueId = cornersProjectedMat.rows;
std::set<int> addedWordsTo;
for(int i = 0; i < cornersProjectedMat.rows; ++i)
{
int matchedIndex = -1;
if(indices[i].size() >= 2)
{
cv::Mat descriptors(indices[i].size(), descriptorsTo.cols, descriptorsTo.type());
for(unsigned int j=0; j<indices[i].size(); ++j)
{
descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(j));
addedWordsTo.insert(indices[i].at(j));
}
std::vector<std::vector<cv::DMatch> > matches;
cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
matcher.knnMatch(descriptorsFrom.row(i), descriptors, matches, 2);
UASSERT(matches.size() == 1);
UASSERT(matches[0].size() == 2);
if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
{
matchedIndex = indices[i].at(matches[0].at(0).trainIdx);
}
}
else if(indices[i].size() == 1)
{
matchedIndex = indices[i].at(0);
}
if(matchedIndex >= 0)
{
if(kptsFrom.size())
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
}
if(kptsFrom3D.size())
{
words3From.insert(std::make_pair(i, kptsFrom3D[i]));
}
wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
wordsTo.insert(std::make_pair(i, kptsTo[matchedIndex]));
wordsDescTo.insert(std::make_pair(i, descriptorsTo.row(matchedIndex)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(i, kptsTo3D[matchedIndex]));
}
}
else
{
// gen fake ids
if(kptsFrom.size())
{
wordsFrom.insert(std::make_pair(notMatchedUniqueId, kptsFrom[i]));
}
if(kptsFrom3D.size())
{
words3From.insert(std::make_pair(notMatchedUniqueId, kptsFrom3D[i]));
}
wordsDescFrom.insert(std::make_pair(notMatchedUniqueId, descriptorsFrom.row(i)));
++notMatchedUniqueId;
}
}
UDEBUG("addedWordsTo=%d, kptsTo=%d, wordsTo=%d", (int)addedWordsTo.size(), (int)kptsTo.size(), (int)wordsTo.size());
// create fake ids for not matched words from "to"
for(unsigned int i=0; i<kptsTo.size(); ++i)
{
if(addedWordsTo.find(i) == addedWordsTo.end())
{
wordsTo.insert(std::make_pair(notMatchedUniqueId, kptsTo[i]));
wordsDescTo.insert(std::make_pair(notMatchedUniqueId, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(notMatchedUniqueId, kptsTo3D[i]));
}
++notMatchedUniqueId;
}
}
}
UDEBUG("");
}
else
{
UDEBUG("");
// match between all descriptors
VWDictionary dictionary(_featureParameters);
std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
std::list<int> toWordIds;
if(descriptorsTo.rows)
{
dictionary.update();
toWordIds = dictionary.addNewWords(descriptorsTo, 2);
}
dictionary.clear(false);
std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
UASSERT(kptsFrom.empty() || fromWordIds.size() == kptsFrom.size());
UASSERT(kptsFrom3D.empty() || fromWordIds.size() == kptsFrom3D.size());
UASSERT(fromWordIds.size() == descriptorsFrom.rows);
int i=0;
for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
{
if(fromWordIdsSet.count(*iter) == 1)
{
if(kptsFrom.size())
{
wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
}
if(kptsFrom3D.size())
{
words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
}
wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
}
++i;
}
UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
UASSERT(toWordIds.size() == kptsTo.size());
UASSERT(toWordIds.size() == descriptorsTo.rows);
i=0;
for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
{
if(toWordIdsSet.count(*iter) == 1)
{
wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
}
}
++i;
}
}
}
else if(descriptorsFrom.rows)
{
//just create fake words
UASSERT(kptsFrom.size() == descriptorsFrom.rows);
UASSERT(words3From.empty() || kptsFrom.size() == words3From.size());
for(unsigned int i=0; i<kptsFrom.size(); ++i)
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
if(kptsFrom3D.size())
{
words3From.insert(std::make_pair(i, kptsFrom3D[i]));
}
}
}
}
fromSignature.setWords(wordsFrom);
fromSignature.setWords3(words3From);
fromSignature.setWordsDescriptors(wordsDescFrom);
toSignature.setWords(wordsTo);
toSignature.setWords3(words3To);
toSignature.setWordsDescriptors(wordsDescTo);
delete detector;
}
@@ -700,7 +898,7 @@ Transform RegistrationVis::computeTransformationImpl(
_PnPFlags,
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
uMultimapToMapUnique(signatureA->getWords3()),
uMultimapToMapUnique(signatureB->getWords3()),
varianceFromInliersCount()?0:&variances[dir],
&matchesV,
&inliersV);
@@ -708,8 +906,8 @@ Transform RegistrationVis::computeTransformationImpl(
matches[dir] = matchesV;
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
(int)inliers[dir].size(), _minInliers, signatureA->id(), signatureB->id());
msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
(int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
UINFO(msg.c_str());
}
}

51
corelib/src/VWDictionary.cpp
View File

@@ -61,7 +61,8 @@ public:
nextIndex_(0),
featuresType_(0),
featuresDim_(0),
isLSH_(false)
isLSH_(false),
useDistanceL1_(false)
{
}
virtual ~FlannIndex()
@@ -1099,8 +1100,6 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
{
UTimer timer;
timer.start();
std::vector<int> resultIds(vws.size(), 0);
unsigned int k=2; // k nearest neighbor
if(_visualWords.size() && vws.size())
{
@@ -1110,20 +1109,15 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
if(dim != (*vws.begin())->getDescriptor().cols)
{
UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", (*vws.begin())->getDescriptor().cols, dim);
return resultIds;
return std::vector<int>(vws.size(), 0);
}
if(type != (*vws.begin())->getDescriptor().type())
{
UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", (*vws.begin())->getDescriptor().type(), type);
return resultIds;
return std::vector<int>(vws.size(), 0);
}
std::vector<std::vector<cv::DMatch> > matches;
bool bruteForce = false;
cv::Mat results;
cv::Mat dists;
// fill the request matrix
int index = 0;
VisualWord * vw;
@@ -1139,10 +1133,43 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
}
ULOGGER_DEBUG("Preparation time = %fs", timer.ticks());
return findNN(query);
}
return std::vector<int>(vws.size(), 0);
}
std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
{
UTimer timer;
timer.start();
std::vector<int> resultIds(query.rows, 0);
unsigned int k=2; // k nearest neighbor
if(_visualWords.size() && query.rows)
{
int dim = _visualWords.begin()->second->getDescriptor().cols;
int type = _visualWords.begin()->second->getDescriptor().type();
if(dim != query.cols)
{
UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", query.cols, dim);
return resultIds;
}
if(type != query.type())
{
UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", query.type(), type);
return resultIds;
}
std::vector<std::vector<cv::DMatch> > matches;
bool bruteForce = false;
cv::Mat results;
cv::Mat dists;
if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
{
//Find nearest neighbors
UDEBUG("newPts.total()=%d ", query.total());
UDEBUG("query.rows=%d ", query.rows);
if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
{
@@ -1231,7 +1258,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
}
ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks());
for(unsigned int i=0; i<vws.size(); ++i)
for(unsigned int i=0; i<query.rows; ++i)
{
std::multimap<float, int> fullResults; // Contains results from the kd-tree search [and the naive search in new words]
if(!bruteForce && dists.cols)