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

OdometryF2M: generating custom ids for bundle adjustement https://github.com/introlab/rtabmap_ros/issues/151

Browse Source
This commit is contained in:
matlabbe
2017-02-24 11:50:29 -05:00
parent 45ca3500c6
commit b0170d1f6f
2 changed files with 9 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/OdometryF2M.h
+1
View File
@@ -77,6 +77,7 @@ private:
std::multimap<int, Link> bundleLinks_; std::multimap<int, Link> bundleLinks_;
std::map<int, CameraModel> bundleModels_; std::map<int, CameraModel> bundleModels_;
std::map<int, int> bundlePoseReferences_; std::map<int, int> bundlePoseReferences_;
int bundleSeq_;
Optimizer * sba_; Optimizer * sba_;
}; };
corelib/src/OdometryF2M.cpp
+8
View File
@@ -69,6 +69,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()), bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
map_(new Signature(-1)), map_(new Signature(-1)),
lastFrame_(new Signature(1)), lastFrame_(new Signature(1)),
bundleSeq_(0),
sba_(0) sba_(0)
{ {
UDEBUG(""); UDEBUG("");
@@ -138,6 +139,7 @@ void OdometryF2M::reset(const Transform & initialPose)
bundleLinks_.clear(); bundleLinks_.clear();
bundleModels_.clear(); bundleModels_.clear();
bundlePoseReferences_.clear(); bundlePoseReferences_.clear();
bundleSeq_ = 0;
} }
// return not null transform if odometry is correctly computed // return not null transform if odometry is correctly computed
@@ -158,7 +160,10 @@ Transform OdometryF2M::computeTransform(
int nFeatures = 0; int nFeatures = 0;
delete lastFrame_; delete lastFrame_;
int id = data.id();
data.setId(++bundleSeq_); // generate our own unique ids, to make sure they are correctly set
lastFrame_ = new Signature(data); lastFrame_ = new Signature(data);
data.setId(id);
if(bundleAdjustment_ > 0 && if(bundleAdjustment_ > 0 &&
data.cameraModels().size() > 1) data.cameraModels().size() > 1)
@@ -242,6 +247,9 @@ Transform OdometryF2M::computeTransform(
bundleLinks = bundleLinks_; bundleLinks = bundleLinks_;
bundleModels = bundleModels_; bundleModels = bundleModels_;
UASSERT_MSG(bundlePoses.find(lastFrame_->id()) == bundlePoses.end(),
uFormat("Frame %d already added! Make sure the input frames have unique IDs!", lastFrame_->id()).c_str());
bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.varianceAng, regInfo.varianceLin))); bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.varianceAng, regInfo.varianceLin)));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform)); bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));