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

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

Browse Source
This commit is contained in:
matlabbe
2018-02-02 13:12:14 -05:00
parent 5d2912baa1
commit 8ca3bca810
3 changed files with 60 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/include/rtabmap/core/BayesFilter.h
View File

@@ -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:

79
corelib/src/BayesFilter.cpp
View File

@@ -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

8
tools/Reprocess/main.cpp
View File

@@ -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
}
}