From b0170d1f6f49ead6b1c6423ae2928f0c64660552 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 24 Feb 2017 11:50:29 -0500
Subject: [PATCH] OdometryF2M: generating custom ids for bundle adjustement
 https://github.com/introlab/rtabmap_ros/issues/151

---
 corelib/include/rtabmap/core/OdometryF2M.h | 1 +
 corelib/src/OdometryF2M.cpp                | 8 ++++++++
 2 files changed, 9 insertions(+)

diff --git a/corelib/include/rtabmap/core/OdometryF2M.h b/corelib/include/rtabmap/core/OdometryF2M.h
index c7124ab1..1b28e739 100644
--- a/corelib/include/rtabmap/core/OdometryF2M.h
+++ b/corelib/include/rtabmap/core/OdometryF2M.h
@@ -77,6 +77,7 @@ private:
 	std::multimap<int, Link> bundleLinks_;
 	std::map<int, CameraModel> bundleModels_;
 	std::map<int, int> bundlePoseReferences_;
+	int bundleSeq_;
 	Optimizer * sba_;
 };
 
diff --git a/corelib/src/OdometryF2M.cpp b/corelib/src/OdometryF2M.cpp
index 3980b81e..0bea63eb 100644
--- a/corelib/src/OdometryF2M.cpp
+++ b/corelib/src/OdometryF2M.cpp
@@ -69,6 +69,7 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
 	map_(new Signature(-1)),
 	lastFrame_(new Signature(1)),
+	bundleSeq_(0),
 	sba_(0)
 {
 	UDEBUG("");
@@ -138,6 +139,7 @@ void OdometryF2M::reset(const Transform & initialPose)
 	bundleLinks_.clear();
 	bundleModels_.clear();
 	bundlePoseReferences_.clear();
+	bundleSeq_ = 0;
 }
 
 // return not null transform if odometry is correctly computed
@@ -158,7 +160,10 @@ Transform OdometryF2M::computeTransform(
 	int nFeatures = 0;
 
 	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);
+	data.setId(id);
 
 	if(bundleAdjustment_ > 0 &&
 	   data.cameraModels().size() > 1)
@@ -242,6 +247,9 @@ Transform OdometryF2M::computeTransform(
 						bundleLinks = bundleLinks_;
 						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)));
 						bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));