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

OdomF2M: fixed initial 3D points transformed

Browse Source
This commit is contained in:
matlabbe
2016-03-09 16:32:16 -05:00
parent b634b34a65
commit 49b5514319
1 changed files with 24 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
corelib/src/OdometryF2M.cpp
View File

@@ -169,11 +169,12 @@ OdometryF2M::~OdometryF2M()
void OdometryF2M::reset(const Transform & initialPose) void OdometryF2M::reset(const Transform & initialPose)
{ {
Odometry::reset(initialPose);
*lastFrame_ = Signature(1);
if(fixedMapPath_.empty()) if(fixedMapPath_.empty())
{ {
Odometry::reset(initialPose);
*map_ = Signature(-1); *map_ = Signature(-1);
*lastFrame_ = Signature(1);
} }
else else
{ {
@@ -432,12 +433,29 @@ Transform OdometryF2M::computeTransform(
{ {
// update local map // update local map
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str()); UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size()); UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
map_->setWords(lastFrame_->getWords()); std::multimap<int, cv::KeyPoint> words;
map_->setWords3(lastFrame_->getWords3()); std::multimap<int, cv::Point3f> transformedPoints;
map_->setWordsDescriptors(lastFrame_->getWordsDescriptors()); std::multimap<int, cv::Mat> descriptors;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
iter!=lastFrame_->getWords3().end();
++iter,++descIter,++wordsIter)
{
if(util3d::isFinite(iter->second))
{
words.insert(*wordsIter);
transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
descriptors.insert(*descIter);
}
}
map_->setWords(words);
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(descriptors);
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels()); map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel()); map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
} }