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

OodmetryF2M: Added getter to last frame

Browse Source
This commit is contained in:
matlabbe
2016-02-23 11:46:07 -05:00
parent 9e446aab7a
commit 061211b1b6
3 changed files with 27 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/include/rtabmap/core/OdometryF2M.h
View File

@@ -42,7 +42,8 @@ public:
virtual ~OdometryF2M(); virtual ~OdometryF2M();
virtual void reset(const Transform & initialPose = Transform::getIdentity()); virtual void reset(const Transform & initialPose = Transform::getIdentity());
const std::multimap<int, cv::Point3f> & getLocalMap() const; const Signature & getMap() const {return *map_;}
const Signature & getLastFrame() const {return *lastFrame_;}
private: private:
virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0); virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
@@ -54,6 +55,7 @@ private:
RegistrationVis * regVis_; RegistrationVis * regVis_;
Signature * map_; Signature * map_;
Signature * lastFrame_;
}; };
} }

47
corelib/src/OdometryF2M.cpp
View File

@@ -56,7 +56,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
maximumMapSize_(Parameters::defaultOdomF2MMaxSize()), maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()), fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
regVis_(new RegistrationVis(parameters)), regVis_(new RegistrationVis(parameters)),
map_(new Signature(-1)) map_(new Signature(-1)),
lastFrame_(new Signature(1))
{ {
UDEBUG(""); UDEBUG("");
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_); Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
@@ -144,6 +145,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
OdometryF2M::~OdometryF2M() OdometryF2M::~OdometryF2M()
{ {
delete map_; delete map_;
delete lastFrame_;
UDEBUG(""); UDEBUG("");
} }
@@ -161,11 +163,6 @@ void OdometryF2M::reset(const Transform & initialPose)
} }
} }
const std::multimap<int, cv::Point3f> & OdometryF2M::getLocalMap() const
{
return map_->getWords3();
}
// return not null transform if odometry is correctly computed // return not null transform if odometry is correctly computed
Transform OdometryF2M::computeTransform( Transform OdometryF2M::computeTransform(
SensorData & data, SensorData & data,
@@ -182,16 +179,18 @@ Transform OdometryF2M::computeTransform(
RegistrationInfo regInfo; RegistrationInfo regInfo;
int nFeatures = 0; int nFeatures = 0;
delete lastFrame_;
lastFrame_ = new Signature(data);
// Generate keypoints from the new data // Generate keypoints from the new data
if(data.isValid()) if(lastFrame_->sensorData().isValid())
{ {
Signature newSignature(data); if(map_->getWords3().size() && lastFrame_->sensorData().isValid())
if(map_->getWords3().size() && newSignature.sensorData().isValid())
{ {
Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform(); Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
Transform transform = regVis_->computeTransformationMod(*map_, newSignature, guess, &regInfo); Transform transform = regVis_->computeTransformationMod(*map_, *lastFrame_, guess, &regInfo);
data.setFeatures(newSignature.sensorData().keypoints(), newSignature.sensorData().descriptors()); data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
if(!transform.isNull()) if(!transform.isNull())
{ {
@@ -219,14 +218,14 @@ Transform OdometryF2M::computeTransform(
std::multimap<int, cv::Mat> mapDescriptors = map_->getWordsDescriptors(); std::multimap<int, cv::Mat> mapDescriptors = map_->getWordsDescriptors();
Transform t = this->getPose()*output; Transform t = this->getPose()*output;
UASSERT(mapPoints.size() == mapDescriptors.size()); UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT(newSignature.getWordsDescriptors().size() == newSignature.getWords3().size()); UASSERT(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size());
std::list<int> newIds = uUniqueKeys(newSignature.getWordsDescriptors()); std::list<int> newIds = uUniqueKeys(lastFrame_->getWordsDescriptors());
for(std::list<int>::iterator iter=newIds.begin(); iter!=newIds.end(); ++iter) for(std::list<int>::iterator iter=newIds.begin(); iter!=newIds.end(); ++iter)
{ {
if(mapPoints.find(*iter) == mapPoints.end()) if(mapPoints.find(*iter) == mapPoints.end())
{ {
mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(newSignature.getWords3().find(*iter)->second, t))); mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(lastFrame_->getWords3().find(*iter)->second, t)));
mapDescriptors.insert(std::make_pair(*iter, newSignature.getWordsDescriptors().find(*iter)->second)); mapDescriptors.insert(std::make_pair(*iter, lastFrame_->getWordsDescriptors().find(*iter)->second));
++added; ++added;
} }
} }
@@ -270,12 +269,12 @@ Transform OdometryF2M::computeTransform(
// just generate keypoints for the new signature // just generate keypoints for the new signature
Signature dummy; Signature dummy;
regVis_->computeTransformationMod( regVis_->computeTransformationMod(
newSignature, *lastFrame_,
dummy); dummy);
data.setFeatures(newSignature.sensorData().keypoints(), newSignature.sensorData().descriptors()); data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
if(fixedMapPath_.empty() && (int)newSignature.getWords3().size() >= regVis_->getMinInliers()) if(fixedMapPath_.empty() && (int)lastFrame_->getWords3().size() >= regVis_->getMinInliers())
{ {
output.setIdentity(); output.setIdentity();
// a very high variance tells that the new pose is not linked with the previous one // a very high variance tells that the new pose is not linked with the previous one
@@ -283,24 +282,24 @@ Transform OdometryF2M::computeTransform(
Transform t = this->getPose(); // initial pose may be not identity... Transform t = this->getPose(); // initial pose may be not identity...
std::multimap<int, cv::Point3f> transformedPoints; std::multimap<int, cv::Point3f> transformedPoints;
for(std::multimap<int, cv::Point3f>::const_iterator iter = newSignature.getWords3().begin(); iter!=newSignature.getWords3().end(); ++iter) for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter)
{ {
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t))); transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
} }
map_->setWords3(transformedPoints); map_->setWords3(transformedPoints);
map_->setWordsDescriptors(newSignature.getWordsDescriptors()); map_->setWordsDescriptors(lastFrame_->getWordsDescriptors());
map_->sensorData().setCameraModels(newSignature.sensorData().cameraModels()); map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(newSignature.sensorData().stereoCameraModel()); map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
} }
} }
map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
nFeatures = newSignature.getWords().size(); nFeatures = lastFrame_->getWords().size();
if(this->isInfoDataFilled() && info) if(this->isInfoDataFilled() && info)
{ {
info->words = newSignature.getWords(); info->words = lastFrame_->getWords();
} }
} }

2
corelib/src/RegistrationVis.cpp
View File

@@ -540,7 +540,7 @@ Transform RegistrationVis::computeTransformationImpl(
{ {
// If guess is set, limit the search of matches using optical flow window size // If guess is set, limit the search of matches using optical flow window size
bool guessSet = !guess.isIdentity() && !guess.isNull(); bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet) if(guessSet && _guessWinSize > 0)
{ {
UDEBUG(""); UDEBUG("");
UASSERT((int)kptsTo.size() == descriptorsTo.rows); UASSERT((int)kptsTo.size() == descriptorsTo.rows);