Fixed build error in rtabmap-reprocess without octomap. Fixed non c++11 build for BayesFilter.

corelib/include/rtabmap/core/BayesFilter.h

@@ -33,7 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <opencv2/core/core.hpp>
 #include <list>
 #include <set>
-#include <unordered_map>
 #include "rtabmap/utilite/UEventsHandler.h"
 #include "rtabmap/core/Parameters.h"
 
@@ -68,10 +67,6 @@ private:
 			const std::vector<int> & oldIds,
 			const std::vector<int> & newIds);
 	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
-	float addNeighborProb(cv::Mat & prediction,
-			unsigned int col,
-			const std::map<int, int> & neighbors,
-			const std::unordered_map<int, int> & idToIndex) const;
 	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
 
 private:

corelib/src/BayesFilter.cpp

@@ -31,7 +31,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Parameters.h"
 #include <iostream>
 #include <set>
+#if __cplusplus >= 201103L
+#include <unordered_map>
 #include <unordered_set>
+#endif
 
 #include "rtabmap/utilite/UtiLite.h"
 
@@ -222,6 +225,42 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
 	return _posterior;
 }
 
+float addNeighborProb(cv::Mat & prediction,
+			unsigned int col,
+			const std::map<int, int> & neighbors,
+			const std::vector<double> & predictionLC,
+#if __cplusplus >= 201103L
+			const std::unordered_map<int, int> & idToIndex
+#else
+			const std::map<int, int> & idToIndex
+#endif
+			)
+{
+	UASSERT(col < (unsigned int)prediction.cols &&
+			col < (unsigned int)prediction.rows);
+
+	float sum=0.0f;
+	float * dataPtr = (float*)prediction.data;
+	for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
+	{
+		if(iter->first>=0)
+		{
+#if __cplusplus >= 201103L
+			std::unordered_map<int, int>::const_iterator jter = idToIndex.find(iter->first);
+#else
+			std::map<int, int>::const_iterator jter = idToIndex.find(iter->first);
+#endif
+			if(jter != idToIndex.end())
+			{
+				UASSERT((iter->second+1) < (int)predictionLC.size());
+				sum += dataPtr[col + jter->second*prediction.cols] = predictionLC[iter->second+1];
+			}
+		}
+	}
+	return sum;
+}
+
+
 cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
 {
 	if(!_fullPredictionUpdate && !_prediction.empty())
@@ -239,8 +278,12 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 	UTimer timerGlobal;
 	timerGlobal.start();
 
+#if __cplusplus >= 201103L
 	std::unordered_map<int,int> idToIndexMap;
 	idToIndexMap.reserve(ids.size());
+#else
+	std::map<int,int> idToIndexMap;
+#endif
 	for(unsigned int i=0; i<ids.size(); ++i)
 	{
 		if(ids[i]>0)
@@ -308,7 +351,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 
 					float sum = 0.0f; // sum values added
 					int index = idToIndexMap.at(*iter);
-					sum += this->addNeighborProb(prediction, index, neighbors, idToIndexMap);
+					sum += addNeighborProb(prediction, index, neighbors, _predictionLC, idToIndexMap);
 					idsDone.insert(*iter);
 					this->normalize(prediction, index, sum, ids[0]<0);
 				}
@@ -439,11 +482,19 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	UDEBUG("time creating prediction = %fs", timer.restart());
 
 	// Create id to index maps
+#if __cplusplus >= 201103L
 	std::unordered_set<int> oldIdsSet(oldIds.begin(), oldIds.end());
+#else
+	std::set<int> oldIdsSet(oldIds.begin(), oldIds.end());
+#endif
 	UDEBUG("time creating old ids set = %fs", timer.restart());
 
+#if __cplusplus >= 201103L
 	std::unordered_map<int,int> newIdToIndexMap;
 	newIdToIndexMap.reserve(newIds.size());
+#else
+	std::map<int,int> newIdToIndexMap;
+#endif
 	for(unsigned int i=0; i<newIds.size(); ++i)
 	{
 		if(newIds[i]>0)
@@ -511,7 +562,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 			}
 			const std::map<int, int> & neighbors = _neighborsIndex.at(newIds[i]);
 
-			float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
+			float sum = addNeighborProb(prediction, i, neighbors, _predictionLC, newIdToIndexMap);
 			this->normalize(prediction, i, sum, newIds[0]<0);
 			++added;
 			int count = 0;
@@ -548,7 +599,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 				const std::map<int, int> & neighbors = kter->second;
 				e1+=t1.ticks();
 
-				float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
+				float sum = addNeighborProb(prediction, index, neighbors, _predictionLC, newIdToIndexMap);
 				e3+=t1.ticks();
 
 				this->normalize(prediction, index, sum, newIds[0]<0);
@@ -640,26 +691,4 @@ void BayesFilter::updatePosterior(const Memory * memory, const std::vector<int>
 	_posterior = newPosterior;
 }
 
-float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map<int, int> & neighbors, const std::unordered_map<int, int> & idToIndex) const
-{
-	UASSERT(col < (unsigned int)prediction.cols &&
-			col < (unsigned int)prediction.rows);
-
-	float sum=0.0f;
-	float * dataPtr = (float*)prediction.data;
-	for(std::map<int, int>::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter)
-	{
-		if(iter->first>=0)
-		{
-			std::unordered_map<int, int>::const_iterator jter = idToIndex.find(iter->first);
-			if(jter != idToIndex.end())
-			{
-				sum += dataPtr[col + jter->second*prediction.cols] = _predictionLC[iter->second+1];
-			}
-		}
-	}
-	return sum;
-}
-
-
 } // namespace rtabmap

tools/Reprocess/main.cpp

@@ -229,7 +229,9 @@ int main(int argc, char * argv[])
 				globalMapStats.clear();
 				double timeUpdateInit = 0.0;
 				double timeUpdateGrid = 0.0;
+#ifdef RTABMAP_OCTOMAP
 				double timeUpdateOctoMap = 0.0;
+#endif
 				const rtabmap::Statistics & stats = rtabmap.getStatistics();
 				UTimer t;
 				if(stats.poses().size() && stats.getSignatures().size())
@@ -254,7 +256,6 @@ int main(int argc, char * argv[])
 						{
 							cv::Mat ground, obstacles;
 							stats.getSignatures().find(id)->second.sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles);
-							const cv::Point3f & viewpoint = stats.getSignatures().find(id)->second.sensorData().gridViewPoint();
 
 							timeUpdateInit = t.ticks();
 
@@ -267,6 +268,7 @@ int main(int argc, char * argv[])
 #ifdef RTABMAP_OCTOMAP
 							if(updateOctoMap)
 							{
+								const cv::Point3f & viewpoint = stats.getSignatures().find(id)->second.sensorData().gridViewPoint();
 								octomap.addToCache(id, ground, obstacles, viewpoint);
 								octomap.update(stats.poses());
 								timeUpdateOctoMap = t.ticks() + timeUpdateInit;
@@ -276,6 +278,8 @@ int main(int argc, char * argv[])
 					}
 				}
 
+				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
+#ifdef RTABMAP_OCTOMAP
 				//Simulate publishing
 				double timePub2dOctoMap = 0.0;
 				double timePub3dOctoMap = 0.0;
@@ -291,10 +295,10 @@ int main(int argc, char * argv[])
 					timePub3dOctoMap = t.ticks();
 				}
 
-				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/GridUpdate/ms"), timeUpdateGrid*1000.0f));
 				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapUpdate/ms"), timeUpdateOctoMap*1000.0f));
 				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctoMapProjection/ms"), timePub2dOctoMap*1000.0f));
 				globalMapStats.insert(std::make_pair(std::string("GlobalGrid/OctomapToCloud/ms"), timePub3dOctoMap*1000.0f));
+#endif
 			}
 		}