From 8ca3bca8106f2347bba572215d467add854f8494 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 2 Feb 2018 13:12:14 -0500 Subject: [PATCH] Fixed build error in rtabmap-reprocess without octomap. Fixed non c++11 build for BayesFilter. --- corelib/include/rtabmap/core/BayesFilter.h | 5 -- corelib/src/BayesFilter.cpp | 79 +++++++++++++++------- tools/Reprocess/main.cpp | 8 ++- 3 files changed, 60 insertions(+), 32 deletions(-) diff --git a/corelib/include/rtabmap/core/BayesFilter.h b/corelib/include/rtabmap/core/BayesFilter.h index 4596c629..e67f53f0 100644 --- a/corelib/include/rtabmap/core/BayesFilter.h +++ b/corelib/include/rtabmap/core/BayesFilter.h @@ -33,7 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include #include "rtabmap/utilite/UEventsHandler.h" #include "rtabmap/core/Parameters.h" @@ -68,10 +67,6 @@ private: const std::vector & oldIds, const std::vector & newIds); void updatePosterior(const Memory * memory, const std::vector & likelihoodIds); - float addNeighborProb(cv::Mat & prediction, - unsigned int col, - const std::map & neighbors, - const std::unordered_map & idToIndex) const; void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const; private: diff --git a/corelib/src/BayesFilter.cpp b/corelib/src/BayesFilter.cpp index d47a3e2e..1b89eca4 100644 --- a/corelib/src/BayesFilter.cpp +++ b/corelib/src/BayesFilter.cpp @@ -31,7 +31,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/Parameters.h" #include #include +#if __cplusplus >= 201103L +#include #include +#endif #include "rtabmap/utilite/UtiLite.h" @@ -222,6 +225,42 @@ const std::map & BayesFilter::computePosterior(const Memory * memory return _posterior; } +float addNeighborProb(cv::Mat & prediction, + unsigned int col, + const std::map & neighbors, + const std::vector & predictionLC, +#if __cplusplus >= 201103L + const std::unordered_map & idToIndex +#else + const std::map & 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::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) + { + if(iter->first>=0) + { +#if __cplusplus >= 201103L + std::unordered_map::const_iterator jter = idToIndex.find(iter->first); +#else + std::map::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 & 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 idToIndexMap; idToIndexMap.reserve(ids.size()); +#else + std::map idToIndexMap; +#endif for(unsigned int i=0; i0) @@ -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 oldIdsSet(oldIds.begin(), oldIds.end()); +#else + std::set oldIdsSet(oldIds.begin(), oldIds.end()); +#endif UDEBUG("time creating old ids set = %fs", timer.restart()); +#if __cplusplus >= 201103L std::unordered_map newIdToIndexMap; newIdToIndexMap.reserve(newIds.size()); +#else + std::map newIdToIndexMap; +#endif for(unsigned int i=0; i0) @@ -511,7 +562,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction, } const std::map & 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 & 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 _posterior = newPosterior; } -float BayesFilter::addNeighborProb(cv::Mat & prediction, unsigned int col, const std::map & neighbors, const std::unordered_map & 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::const_iterator iter=neighbors.begin(); iter!=neighbors.end(); ++iter) - { - if(iter->first>=0) - { - std::unordered_map::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 diff --git a/tools/Reprocess/main.cpp b/tools/Reprocess/main.cpp index 9793af91..b25885ed 100644 --- a/tools/Reprocess/main.cpp +++ b/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 } }