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

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 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:
virtual Transform computeTransform(SensorData & data, OdometryInfo * info = 0);
@@ -54,6 +55,7 @@ private:
RegistrationVis * regVis_;
Signature * map_;
Signature * lastFrame_;
};
}

47
corelib/src/OdometryF2M.cpp
View File

@@ -56,7 +56,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
regVis_(new RegistrationVis(parameters)),
map_(new Signature(-1))
map_(new Signature(-1)),
lastFrame_(new Signature(1))
{
UDEBUG("");
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
@@ -144,6 +145,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
OdometryF2M::~OdometryF2M()
{
delete map_;
delete lastFrame_;
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
Transform OdometryF2M::computeTransform(
SensorData & data,
@@ -182,16 +179,18 @@ Transform OdometryF2M::computeTransform(
RegistrationInfo regInfo;
int nFeatures = 0;
delete lastFrame_;
lastFrame_ = new Signature(data);
// Generate keypoints from the new data
if(data.isValid())
if(lastFrame_->sensorData().isValid())
{
Signature newSignature(data);
if(map_->getWords3().size() && newSignature.sensorData().isValid())
if(map_->getWords3().size() && lastFrame_->sensorData().isValid())
{
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())
{
@@ -219,14 +218,14 @@ Transform OdometryF2M::computeTransform(
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());
UASSERT(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size());
std::list<int> newIds = uUniqueKeys(lastFrame_->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));
mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(lastFrame_->getWords3().find(*iter)->second, t)));
mapDescriptors.insert(std::make_pair(*iter, lastFrame_->getWordsDescriptors().find(*iter)->second));
++added;
}
}
@@ -270,12 +269,12 @@ Transform OdometryF2M::computeTransform(
// just generate keypoints for the new signature
Signature dummy;
regVis_->computeTransformationMod(
newSignature,
*lastFrame_,
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();
// 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...
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)));
}
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(newSignature.getWordsDescriptors());
map_->sensorData().setCameraModels(newSignature.sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(newSignature.sensorData().stereoCameraModel());
map_->setWordsDescriptors(lastFrame_->getWordsDescriptors());
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
}
}
map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
nFeatures = newSignature.getWords().size();
nFeatures = lastFrame_->getWords().size();
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
bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet)
if(guessSet && _guessWinSize > 0)
{
UDEBUG("");
UASSERT((int)kptsTo.size() == descriptorsTo.rows);