From b4507e027d2d6360e8f7b6bafa5ca8e935e92330 Mon Sep 17 00:00:00 2001 From: Adrian Bauer Date: Tue, 8 Nov 2016 12:26:14 +0100 Subject: [PATCH 01/14] multi-threading support --- openslam_gmapping | 1 + 1 file changed, 1 insertion(+) create mode 160000 openslam_gmapping diff --git a/openslam_gmapping b/openslam_gmapping new file mode 160000 index 0000000..c716f01 --- /dev/null +++ b/openslam_gmapping @@ -0,0 +1 @@ +Subproject commit c716f0192131029b31a49554f8c11353d29819a5 From 3c4d4656180b316607aa9b0cc0ca2f32461c0841 Mon Sep 17 00:00:00 2001 From: Adrian Bauer Date: Tue, 8 Nov 2016 12:34:56 +0100 Subject: [PATCH 02/14] test --- CMakeLists.txt | 1 + gridfastslam/CMakeLists (copy).txt | 9 + gridfastslam/CMakeLists.txt | 12 +- gridfastslam/CMakeLists.txt~ | 20 + gridfastslam/gridslamprocessor.cpp~ | 518 ++++++++++++++++++ .../gmapping/gridfastslam/gridslamprocessor.h | 6 +- .../gridfastslam/gridslamprocessor.hxx | 46 +- .../gridfastslam/gridslamprocessor.hxx~ | 217 ++++++++ .../gridfastslam/gridslamprocessor.h~ | 339 ++++++++++++ openslam_gmapping | 1 - 10 files changed, 1162 insertions(+), 7 deletions(-) create mode 100644 gridfastslam/CMakeLists (copy).txt create mode 100644 gridfastslam/CMakeLists.txt~ create mode 100644 gridfastslam/gridslamprocessor.cpp~ create mode 100644 include/gmapping/gridfastslam/gridslamprocessor.hxx~ create mode 100644 include/gmapping/gridfastslam/gridslamprocessor.h~ delete mode 160000 openslam_gmapping diff --git a/CMakeLists.txt b/CMakeLists.txt index 17f2b55..bd83aeb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin) catkin_package( INCLUDE_DIRS include LIBRARIES gridfastslam scanmatcher sensor_base sensor_range sensor_odometry utils + DEPENDS OpenMP ) include_directories(include) diff --git a/gridfastslam/CMakeLists (copy).txt b/gridfastslam/CMakeLists (copy).txt new file mode 100644 index 0000000..b3e7f24 --- /dev/null +++ b/gridfastslam/CMakeLists (copy).txt @@ -0,0 +1,9 @@ +add_library(gridfastslam + gfsreader.cpp + gridslamprocessor.cpp + gridslamprocessor_tree.cpp + motionmodel.cpp +) +target_link_libraries(gridfastslam scanmatcher sensor_range) + +install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/gridfastslam/CMakeLists.txt b/gridfastslam/CMakeLists.txt index b3e7f24..0be45c8 100644 --- a/gridfastslam/CMakeLists.txt +++ b/gridfastslam/CMakeLists.txt @@ -1,9 +1,19 @@ +find_package(OpenMP REQUIRED) +if(OPENMP_FOUND) + message(STATUS "OPENMP FOUND") + set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) # or if you use C: ${OpenMP_C_FLAGS} + set(OpenMP_LIBS gomp) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +endif() + add_library(gridfastslam gfsreader.cpp gridslamprocessor.cpp gridslamprocessor_tree.cpp motionmodel.cpp ) -target_link_libraries(gridfastslam scanmatcher sensor_range) + +target_link_libraries(gridfastslam scanmatcher sensor_range ${OpenMP_LIBS}) install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/gridfastslam/CMakeLists.txt~ b/gridfastslam/CMakeLists.txt~ new file mode 100644 index 0000000..6b2dd69 --- /dev/null +++ b/gridfastslam/CMakeLists.txt~ @@ -0,0 +1,20 @@ +find_package(OpenMP REQUIRED) +if(OPENMP_FOUND) + message(STATUS "OPENMP FOUND") + set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) # or if you use C: ${OpenMP_C_FLAGS} + set(OpenMP_LIBS gomp) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") +endif() + + +add_library(gridfastslam + gfsreader.cpp + gridslamprocessor.cpp + gridslamprocessor_tree.cpp + motionmodel.cpp +) + +target_link_libraries(gridfastslam scanmatcher sensor_range ${OpenMP_LIBS}) + +install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/gridfastslam/gridslamprocessor.cpp~ b/gridfastslam/gridslamprocessor.cpp~ new file mode 100644 index 0000000..b95ccdd --- /dev/null +++ b/gridfastslam/gridslamprocessor.cpp~ @@ -0,0 +1,518 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define MAP_CONSISTENCY_CHECK +//#define GENERATE_TRAJECTORIES + +namespace GMapping { + +const double m_distanceThresholdCheck = 20; + +using namespace std; + + GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){ + + period_ = 5.0; + m_obsSigmaGain=1; + m_resampleThreshold=0.5; + m_minimumScore=0.; + } + + GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) + :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout){ + + period_ = 5.0; + + m_obsSigmaGain=gsp.m_obsSigmaGain; + m_resampleThreshold=gsp.m_resampleThreshold; + m_minimumScore=gsp.m_minimumScore; + + m_beams=gsp.m_beams; + m_indexes=gsp.m_indexes; + m_motionModel=gsp.m_motionModel; + m_resampleThreshold=gsp.m_resampleThreshold; + m_matcher=gsp.m_matcher; + + m_count=gsp.m_count; + m_readingCount=gsp.m_readingCount; + m_lastPartPose=gsp.m_lastPartPose; + m_pose=gsp.m_pose; + m_odoPose=gsp.m_odoPose; + m_linearDistance=gsp.m_linearDistance; + m_angularDistance=gsp.m_angularDistance; + m_neff=gsp.m_neff; + + cerr << "FILTER COPY CONSTRUCTOR" << endl; + cerr << "m_odoPose=" << m_odoPose.x << " " < >::reference* const, int> PointerMap; + PointerMap pmap; + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + const ScanMatcherMap& m1(it->map); + const HierarchicalArray2D& h1(m1.storage()); + for (int x=0; x >& a1(h1.m_cells[x][y]); + if (a1.m_reference){ + PointerMap::iterator f=pmap.find(a1.m_reference); + if (f==pmap.end()) + pmap.insert(make_pair(a1.m_reference, 1)); + else + f->second++; + } + } + } + } + cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; + for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) + assert(it->first->shares==(unsigned int)it->second); + + cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; +# endif + GridSlamProcessor* cloned=new GridSlamProcessor(*this); + +# ifdef MAP_CONSISTENCY_CHECK + cerr << __PRETTY_FUNCTION__ << ": trajectories end" << endl; + cerr << __PRETTY_FUNCTION__ << ": performing afterclone_fit_test" << endl; + ParticleVector::const_iterator jt=cloned->m_particles.begin(); + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + const ScanMatcherMap& m1(it->map); + const ScanMatcherMap& m2(jt->map); + const HierarchicalArray2D& h1(m1.storage()); + const HierarchicalArray2D& h2(m2.storage()); + jt++; + for (int x=0; x >& a1(h1.m_cells[x][y]); + const autoptr< Array2D >& a2(h2.m_cells[x][y]); + assert(a1.m_reference==a2.m_reference); + assert((!a1.m_reference) || !(a1.m_reference->shares%2)); + } + } + } + cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; +# endif + return cloned; +} + + GridSlamProcessor::~GridSlamProcessor(){ + cerr << __PRETTY_FUNCTION__ << ": Start" << endl; + cerr << __PRETTY_FUNCTION__ << ": Deleting tree" << endl; + for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ +#ifdef TREE_CONSISTENCY_CHECK + TNode* node=it->node; + while(node) + node=node->parent; + cerr << "@" << endl; +#endif + if (it->node) + delete it->node; + //cout << "l=" << it->weight<< endl; + } + +# ifdef MAP_CONSISTENCY_CHECK + cerr << __PRETTY_FUNCTION__ << ": performing predestruction_fit_test" << endl; + typedef std::map >::reference* const, int> PointerMap; + PointerMap pmap; + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + const ScanMatcherMap& m1(it->map); + const HierarchicalArray2D& h1(m1.storage()); + for (int x=0; x >& a1(h1.m_cells[x][y]); + if (a1.m_reference){ + PointerMap::iterator f=pmap.find(a1.m_reference); + if (f==pmap.end()) + pmap.insert(make_pair(a1.m_reference, 1)); + else + f->second++; + } + } + } + } + cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; + for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) + assert(it->first->shares>=(unsigned int)it->second); + cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; +# endif + } + + + + void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, + int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){ + m_obsSigmaGain=likelihoodGain; + m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip); + if (m_infoStream) + m_infoStream << " -maxUrange "<< urange + << " -maxUrange "<< range + << " -sigma "<< sigma + << " -kernelSize "<< kernsize + << " -lstep " << lopt + << " -lobsGain " << m_obsSigmaGain + << " -astep " << aopt << endl; + + + } + +void GridSlamProcessor::setMotionModelParameters +(double srr, double srt, double str, double stt){ + m_motionModel.srr=srr; + m_motionModel.srt=srt; + m_motionModel.str=str; + m_motionModel.stt=stt; + + if (m_infoStream) + m_infoStream << " -srr "<< srr << " -srt "<< srt + << " -str "<< str << " -stt "<< stt << endl; + +} + + void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){ + m_linearThresholdDistance=linear; + m_angularThresholdDistance=angular; + m_resampleThreshold=resampleThreshold; + if (m_infoStream) + m_infoStream << " -linearUpdate " << linear + << " -angularUpdate "<< angular + << " -resampleThreshold " << m_resampleThreshold << endl; + } + + //HERE STARTS THE BEEF + + GridSlamProcessor::Particle::Particle(const ScanMatcherMap& m): + map(m), pose(0,0,0), weight(0), weightSum(0), gweight(0), previousIndex(0){ + node=0; + } + + + void GridSlamProcessor::setSensorMap(const SensorMap& smap){ + + /* + Construct the angle table for the sensor + + FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot + */ + + SensorMap::const_iterator laser_it=smap.find(std::string("FLASER")); + if (laser_it==smap.end()){ + cerr << "Attempting to load the new carmen log format" << endl; + laser_it=smap.find(std::string("ROBOTLASER1")); + assert(laser_it!=smap.end()); + } + const RangeSensor* rangeSensor=dynamic_cast((laser_it->second)); + assert(rangeSensor && rangeSensor->beams().size()); + + m_beams=static_cast(rangeSensor->beams().size()); + double* angles=new double[rangeSensor->beams().size()]; + for (unsigned int i=0; ibeams()[i].pose.theta; + } + m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose()); + delete [] angles; + } + + void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){ + m_xmin=xmin; + m_ymin=ymin; + m_xmax=xmax; + m_ymax=ymax; + m_delta=delta; + if (m_infoStream) + m_infoStream + << " -xmin "<< m_xmin + << " -xmax "<< m_xmax + << " -ymin "<< m_ymin + << " -ymax "<< m_ymax + << " -delta "<< m_delta + << " -particles "<< size << endl; + + + m_particles.clear(); + TNode* node=new TNode(initialPose, 0, 0, 0); + ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta); + for (unsigned int i=0; i(o.getSensor()); + if (os && os->isIdeal() && m_outputStream){ + m_outputStream << setiosflags(ios::fixed) << setprecision(3); + m_outputStream << "SIMULATOR_POS " << o.getPose().x << " " << o.getPose().y << " " ; + m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " << o.getTime() << endl; + } + } + + + bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){ + + /**retireve the position from the reading, and compute the odometry*/ + OrientedPoint relPose=reading.getPose(); + if (!m_count){ + m_lastPartPose=m_odoPose=relPose; + } + + //write the state of the reading and update all the particles using the motion model + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + OrientedPoint& pose(it->pose); + pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose); + } + + // update the output file + if (m_outputStream.is_open()){ + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << "ODOM "; + m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " "; + m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " "; + m_outputStream << reading.getTime(); + m_outputStream << endl; + } + if (m_outputStream.is_open()){ + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << "ODO_UPDATE "<< m_particles.size() << " "; + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + OrientedPoint& pose(it->pose); + m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; + m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; + } + m_outputStream << reading.getTime(); + m_outputStream << endl; + } + + //invoke the callback + onOdometryUpdate(); + + + // accumulate the robot translation and rotation + OrientedPoint move=relPose-m_odoPose; + move.theta=atan2(sin(move.theta), cos(move.theta)); + m_linearDistance+=sqrt(move*move); + m_angularDistance+=fabs(move.theta); + + // if the robot jumps throw a warning + if (m_linearDistance>m_distanceThresholdCheck){ + cerr << "***********************************************************************" << endl; + cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl; + cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl; + cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y + << " " <=m_linearThresholdDistance + || m_angularDistance>=m_angularThresholdDistance + || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){ + last_update_time_ = reading.getTime(); + + if (m_outputStream.is_open()){ + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << "FRAME " << m_readingCount; + m_outputStream << " " << m_linearDistance; + m_outputStream << " " << m_angularDistance << endl; + } + + if (m_infoStream) + m_infoStream << "update frame " << m_readingCount << endl + << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl; + + + cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y + << " " << reading.getPose().theta << endl; + + + //this is for converting the reading in a scan-matcher feedable form + assert(reading.size()==m_beams); + double * plainReading = new double[m_beams]; + for(unsigned int i=0; i(reading.getSensor()), + reading.getTime()); + + if (m_count>0){ + scanMatch(plainReading); + if (m_outputStream.is_open()){ + m_outputStream << "LASER_READING "<< reading.size() << " "; + m_outputStream << setiosflags(ios::fixed) << setprecision(2); + for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){ + m_outputStream << *b << " "; + } + OrientedPoint p=reading.getPose(); + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl; + m_outputStream << "SM_UPDATE "<< m_particles.size() << " "; + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + const OrientedPoint& pose=it->pose; + m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; + m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; + } + m_outputStream << endl; + } + onScanmatchUpdate(); + + updateTreeWeights(false); + + if (m_infoStream){ + m_infoStream << "neff= " << m_neff << endl; + } + if (m_outputStream.is_open()){ + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << "NEFF " << m_neff << endl; + } + resample(plainReading, adaptParticles, reading_copy); + + } else { + m_infoStream << "Registering First Scan"<< endl; + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + m_matcher.invalidateActiveArea(); + m_matcher.computeActiveArea(it->map, it->pose, plainReading); + m_matcher.registerScan(it->map, it->pose, plainReading); + + // cyr: not needed anymore, particles refer to the root in the beginning! + TNode* node=new TNode(it->pose, 0., it->node, 0); + //node->reading=0; + node->reading = reading_copy; + it->node=node; + + } + } + // cerr << "Tree: normalizing, resetting and propagating weights at the end..." ; + updateTreeWeights(false); + // cerr << ".done!" <previousPose=it->pose; + } + + } + if (m_outputStream.is_open()) + m_outputStream << flush; + m_readingCount++; + return processed; + } + + + std::ofstream& GridSlamProcessor::outputStream(){ + return m_outputStream; + } + + std::ostream& GridSlamProcessor::infoStream(){ + return m_infoStream; + } + + + int GridSlamProcessor::getBestParticleIndex() const{ + unsigned int bi=0; + double bw=-std::numeric_limits::max(); + for (unsigned int i=0; i #include "motionmodel.h" +#include + namespace GMapping { @@ -243,6 +245,7 @@ namespace GMapping { /**minimum score for considering the outcome of the scanmatching good*/ PARAM_SET_GET(double, minimumScore, protected, public, public); + protected: /**Copy constructor*/ GridSlamProcessor(const GridSlamProcessor& gsp); @@ -305,11 +308,10 @@ namespace GMapping { // stream in which to write the messages std::ostream& m_infoStream; - // the functions below performs side effect on the internal structure, //should be called only inside the processScan method private: - + std::vector durations; /**scanmatches all the particles*/ inline void scanMatch(const double *plainReading); /**normalizes the particle weights*/ diff --git a/include/gmapping/gridfastslam/gridslamprocessor.hxx b/include/gmapping/gridfastslam/gridslamprocessor.hxx index 8a8b7a4..fa1cf75 100644 --- a/include/gmapping/gridfastslam/gridslamprocessor.hxx +++ b/include/gmapping/gridfastslam/gridslamprocessor.hxx @@ -8,15 +8,55 @@ If the scan matching fails, the particle gets a default likelihood.*/ inline void GridSlamProcessor::scanMatch(const double* plainReading){ // sample a new pose from each scan in the reference - + double avg_duration = 0; double sumScore=0; - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + double start = omp_get_wtime(); + +#pragma omp parallel for reduction(+:sumScore) + for (int i = 0; i < m_particles.size(); i++){ + OrientedPoint corrected; + double score, l, s; + score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading); + // it->pose=corrected; + // m_infoStream << i << std::endl; + if (score>m_minimumScore){ + m_particles[i].pose=corrected; + // m_infoStream << "Pose was corrected!" << std::endl; + } else { + if (m_infoStream){ + m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l < 0) avg_duration /= durations.size(); + + m_infoStream << "Scan matching took: " << avg_duration << std::endl; + /*for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++, i++){ OrientedPoint corrected; double score, l, s; score=m_matcher.optimize(corrected, it->map, it->pose, plainReading); // it->pose=corrected; + // m_infoStream << i << std::endl; if (score>m_minimumScore){ it->pose=corrected; + // m_infoStream << "Pose was corrected!" << std::endl; } else { if (m_infoStream){ m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <map, it->pose, plainReading); - } + }*/ if (m_infoStream) m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl; } diff --git a/include/gmapping/gridfastslam/gridslamprocessor.hxx~ b/include/gmapping/gridfastslam/gridslamprocessor.hxx~ new file mode 100644 index 0000000..b2c318a --- /dev/null +++ b/include/gmapping/gridfastslam/gridslamprocessor.hxx~ @@ -0,0 +1,217 @@ + +#ifdef MACOSX +// This is to overcome a possible bug in Apple's GCC. +#define isnan(x) (x==FP_NAN) +#endif + +/**Just scan match every single particle. +If the scan matching fails, the particle gets a default likelihood.*/ +inline void GridSlamProcessor::scanMatch(const double* plainReading){ + // sample a new pose from each scan in the reference + double avg_duration = 0; + double sumScore=0; + double start = omp_get_wtime(); + +//#pragma omp parallel for reduction(+:sumScore) + for (int i = 0; i < m_particles.size(); i++){ + OrientedPoint corrected; + double score, l, s; + score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading); + // it->pose=corrected; + // m_infoStream << i << std::endl; + if (score>m_minimumScore){ + m_particles[i].pose=corrected; + // m_infoStream << "Pose was corrected!" << std::endl; + } else { + if (m_infoStream){ + m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l < 0) avg_duration /= durations.size(); + + m_infoStream << "Scan matching took: " << avg_duration << std::endl; + /*for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++, i++){ + OrientedPoint corrected; + double score, l, s; + score=m_matcher.optimize(corrected, it->map, it->pose, plainReading); + // it->pose=corrected; + // m_infoStream << i << std::endl; + if (score>m_minimumScore){ + it->pose=corrected; + // m_infoStream << "Pose was corrected!" << std::endl; + } else { + if (m_infoStream){ + m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <map, it->pose, plainReading); + sumScore+=score; + it->weight+=l; + it->weightSum+=l; + + //set up the selective copy of the active area + //by detaching the areas that will be updated + m_matcher.invalidateActiveArea(); + m_matcher.computeActiveArea(it->map, it->pose, plainReading); + }*/ + if (m_infoStream) + m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl; +} + +inline void GridSlamProcessor::normalize(){ + //normalize the log m_weights + double gain=1./(m_obsSigmaGain*m_particles.size()); + double lmax= -std::numeric_limits::max(); + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + lmax=it->weight>lmax?it->weight:lmax; + } + //cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl; + + m_weights.clear(); + double wcum=0; + m_neff=0; + for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + m_weights.push_back(exp(gain*(it->weight-lmax))); + wcum+=m_weights.back(); + //cout << "l=" << it->weight<< endl; + } + + m_neff=0; + for (std::vector::iterator it=m_weights.begin(); it!=m_weights.end(); it++){ + *it=*it/wcum; + double w=*it; + m_neff+=w*w; + } + m_neff=1./m_neff; + +} + +inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){ + + bool hasResampled = false; + + TNodeVector oldGeneration; + for (unsigned int i=0; i resampler; + m_indexes=resampler.resampleIndexes(m_weights, adaptSize); + + if (m_outputStream.is_open()){ + m_outputStream << "RESAMPLE "<< m_indexes.size() << " "; + for (std::vector::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){ + m_outputStream << *it << " "; + } + m_outputStream << std::endl; + } + + onResampleUpdate(); + //BEGIN: BUILDING TREE + ParticleVector temp; + unsigned int j=0; + std::vector deletedParticles; //this is for deleteing the particles which have been resampled away. + + // cerr << "Existing Nodes:" ; + for (unsigned int i=0; i" << m_indexes[i] << "B("<childs <<") "; + node=new TNode(p.pose, 0, oldNode, 0); + //node->reading=0; + node->reading=reading; + // cerr << "A("<parent->childs <<") " <setWeight(0); + m_matcher.invalidateActiveArea(); + m_matcher.registerScan(it->map, it->pose, plainReading); + m_particles.push_back(*it); + } + std::cerr << " Done" <pose, 0.0, *node_it, 0); + + //node->reading=0; + node->reading=reading; + it->node=node; + + //END: BUILDING TREE + m_matcher.invalidateActiveArea(); + m_matcher.registerScan(it->map, it->pose, plainReading); + it->previousIndex=index; + index++; + node_it++; + + } + std::cerr << "Done" < +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "motionmodel.h" + +#include + + +namespace GMapping { + + /**This class defines the basic GridFastSLAM algorithm. It + implements a rao blackwellized particle filter. Each particle + has its own map and robot pose.
This implementation works + as follows: each time a new pair odometry/laser reading is + received, the particle's robot pose is updated according to the + motion model. This pose is subsequently used for initalizing a + scan matching algorithm. The scanmatcher performs a local + optimization for each particle. It is initialized with the + pose drawn from the motion model, and the pose is corrected + according to the each particle map.
+ In order to avoid unnecessary computation the filter state is updated + only when the robot moves more than a given threshold. + */ + class GridSlamProcessor{ + public: + + + /**This class defines the the node of reversed tree in which the trajectories are stored. + Each node of a tree has a pointer to its parent and a counter indicating the number of childs of a node. + The tree is updated in a way consistent with the operation performed on the particles. + */ + struct TNode{ + /**Constructs a node of the trajectory tree. + @param pose: the pose of the robot in the trajectory + @param weight: the weight of the particle at that point in the trajectory + @param accWeight: the cumulative weight of the particle + @param parent: the parent node in the tree + @param childs: the number of childs + */ + TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0); + + /**Destroys a tree node, and consistently updates the tree. If a node whose parent has only one child is deleted, + also the parent node is deleted. This because the parent will not be reacheable anymore in the trajectory tree.*/ + ~TNode(); + + /**The pose of the robot*/ + OrientedPoint pose; + + /**The weight of the particle*/ + double weight; + + /**The sum of all the particle weights in the previous part of the trajectory*/ + double accWeight; + + double gweight; + + + /**The parent*/ + TNode* parent; + + /**The range reading to which this node is associated*/ + const RangeReading* reading; + + /**The number of childs*/ + unsigned int childs; + + /**counter in visiting the node (internally used)*/ + mutable unsigned int visitCounter; + + /**visit flag (internally used)*/ + mutable bool flag; + }; + + typedef std::vector TNodeVector; + typedef std::deque TNodeDeque; + + /**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/ + struct Particle{ + /**constructs a particle, given a map + @param map: the particle map + */ + Particle(const ScanMatcherMap& map); + + /** @returns the weight of a particle */ + inline operator double() const {return weight;} + /** @returns the pose of a particle */ + inline operator OrientedPoint() const {return pose;} + /** sets the weight of a particle + @param w the weight + */ + inline void setWeight(double w) {weight=w;} + /** The map */ + ScanMatcherMap map; + /** The pose of the robot */ + OrientedPoint pose; + + /** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */ + OrientedPoint previousPose; + + /** The weight of the particle */ + double weight; + + /** The cumulative weight of the particle */ + double weightSum; + + double gweight; + + /** The index of the previous particle in the trajectory tree */ + int previousIndex; + + /** Entry to the trajectory tree */ + TNode* node; + }; + + + typedef std::vector ParticleVector; + + /** Constructs a GridSlamProcessor, initialized with the default parameters */ + GridSlamProcessor(); + + /** Constructs a GridSlamProcessor, whose output is routed to a stream. + @param infoStr: the output stream + */ + GridSlamProcessor(std::ostream& infoStr); + + /** @returns a deep copy of the grid slam processor with all the internal structures. + */ + GridSlamProcessor* clone() const; + + /**Deleted the gridslamprocessor*/ + virtual ~GridSlamProcessor(); + + //methods for accessing the parameters + void setSensorMap(const SensorMap& smap); + void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, + OrientedPoint initialPose=OrientedPoint(0,0,0)); + void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, + int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0); + void setMotionModelParameters(double srr, double srt, double str, double stt); + void setUpdateDistances(double linear, double angular, double resampleThreshold); + void setUpdatePeriod(double p) {period_=p;} + + //the "core" algorithm + void processTruePos(const OdometryReading& odometry); + bool processScan(const RangeReading & reading, int adaptParticles=0); + + /**This method copies the state of the filter in a tree. + The tree is represented through reversed pointers (each node has a pointer to its parent). + The leafs are stored in a vector, whose size is the same as the number of particles. + @returns the leafs of the tree + */ + TNodeVector getTrajectories() const; + void integrateScanSequence(TNode* node); + + /**the scanmatcher algorithm*/ + ScanMatcher m_matcher; + /**the stream used for writing the output of the algorithm*/ + std::ofstream& outputStream(); + /**the stream used for writing the info/debug messages*/ + std::ostream& infoStream(); + /**@returns the particles*/ + inline const ParticleVector& getParticles() const {return m_particles; } + + inline const std::vector& getIndexes() const{return m_indexes; } + int getBestParticleIndex() const; + //callbacks + virtual void onOdometryUpdate(); + virtual void onResampleUpdate(); + virtual void onScanmatchUpdate(); + + //accessor methods + /**the maxrange of the laser to consider */ + MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public); + + /**the maximum usable range of the laser. A beam is cropped to this value. [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public); + + /**The sigma used by the greedy endpoint matching. [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public); + + /**The sigma of a beam used for likelihood computation [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public); + + /**The kernel in which to look for a correspondence[scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, int, kernelSize, protected, public, public); + + /**The optimization step in rotation [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public); + + /**The optimization step in translation [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public); + + /**The number of iterations of the scanmatcher [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public); + + /**the beams to skip for computing the likelihood (consider a beam every likelihoodSkip) [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public); + + /**translational sampling range for the likelihood [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public); + + /**angular sampling range for the likelihood [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public); + + /**translational sampling range for the likelihood [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public); + + /**angular sampling step for the likelihood [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public); + + /**generate an accupancy grid map [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public); + + /**enlarge the map when the robot goes out of the boundaries [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public); + + /**pose of the laser wrt the robot [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public); + + + /**odometry error in translation as a function of translation (rho/rho) [motionmodel]*/ + STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public); + + /**odometry error in translation as a function of rotation (rho/theta) [motionmodel]*/ + STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public); + + /**odometry error in rotation as a function of translation (theta/rho) [motionmodel]*/ + STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public); + + /**odometry error in rotation as a function of rotation (theta/theta) [motionmodel]*/ + STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public); + + /**minimum score for considering the outcome of the scanmatching good*/ + PARAM_SET_GET(double, minimumScore, protected, public, public); + + + protected: + /**Copy constructor*/ + GridSlamProcessor(const GridSlamProcessor& gsp); + + /**the laser beams*/ + unsigned int m_beams; + double last_update_time_; + double period_; + + /**the particles*/ + ParticleVector m_particles; + + /**the particle indexes after resampling (internally used)*/ + std::vector m_indexes; + + /**the particle weights (internally used)*/ + std::vector m_weights; + + /**the motion model*/ + MotionModel m_motionModel; + + /**this sets the neff based resampling threshold*/ + PARAM_SET_GET(double, resampleThreshold, protected, public, public); + + //state + int m_count, m_readingCount; + OrientedPoint m_lastPartPose; + OrientedPoint m_odoPose; + OrientedPoint m_pose; + double m_linearDistance, m_angularDistance; + PARAM_GET(double, neff, protected, public); + + //processing parameters (size of the map) + PARAM_GET(double, xmin, protected, public); + PARAM_GET(double, ymin, protected, public); + PARAM_GET(double, xmax, protected, public); + PARAM_GET(double, ymax, protected, public); + //processing parameters (resolution of the map) + PARAM_GET(double, delta, protected, public); + + //registration score (if a scan score is above this threshold it is registered in the map) + PARAM_SET_GET(double, regScore, protected, public, public); + //registration score (if a scan score is below this threshold a scan matching failure is reported) + PARAM_SET_GET(double, critScore, protected, public, public); + //registration score maximum move allowed between consecutive scans + PARAM_SET_GET(double, maxMove, protected, public, public); + + //process a scan each time the robot translates of linearThresholdDistance + PARAM_SET_GET(double, linearThresholdDistance, protected, public, public); + + //process a scan each time the robot rotates more than angularThresholdDistance + PARAM_SET_GET(double, angularThresholdDistance, protected, public, public); + + //smoothing factor for the likelihood + PARAM_SET_GET(double, obsSigmaGain, protected, public, public); + + //stream in which to write the gfs file + std::ofstream m_outputStream; + + // stream in which to write the messages + std::ostream& m_infoStream; + + // the functions below performs side effect on the internal structure, + //should be called only inside the processScan method + private: + + /**scanmatches all the particles*/ + inline void scanMatch(const double *plainReading); + /**normalizes the particle weights*/ + inline void normalize(); + + // return if a resampling occured or not + inline bool resample(const double* plainReading, int adaptParticles, + const RangeReading* rr=0); + + //tree utilities + + void updateTreeWeights(bool weightsAlreadyNormalized = false); + void resetTree(); + double propagateWeights(); + + }; + +typedef std::multimap TNodeMultimap; + + +#include "gridslamprocessor.hxx" + +}; + +#endif diff --git a/openslam_gmapping b/openslam_gmapping deleted file mode 160000 index c716f01..0000000 --- a/openslam_gmapping +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c716f0192131029b31a49554f8c11353d29819a5 From 93e79ba17ea542fce1993e97ccd20f30f4f66295 Mon Sep 17 00:00:00 2001 From: Adrian Bauer Date: Tue, 8 Nov 2016 12:47:12 +0100 Subject: [PATCH 03/14] added multi-threaded scan matching --- .../gridfastslam/gridslamprocessor.hxx | 36 ++++--------------- .../gridfastslam/gridslamprocessor.hxx~ | 2 +- 2 files changed, 7 insertions(+), 31 deletions(-) diff --git a/include/gmapping/gridfastslam/gridslamprocessor.hxx b/include/gmapping/gridfastslam/gridslamprocessor.hxx index fa1cf75..4745a60 100644 --- a/include/gmapping/gridfastslam/gridslamprocessor.hxx +++ b/include/gmapping/gridfastslam/gridslamprocessor.hxx @@ -16,9 +16,9 @@ inline void GridSlamProcessor::scanMatch(const double* plainReading){ for (int i = 0; i < m_particles.size(); i++){ OrientedPoint corrected; double score, l, s; + score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading); // it->pose=corrected; - // m_infoStream << i << std::endl; if (score>m_minimumScore){ m_particles[i].pose=corrected; // m_infoStream << "Pose was corrected!" << std::endl; @@ -40,6 +40,7 @@ inline void GridSlamProcessor::scanMatch(const double* plainReading){ m_matcher.invalidateActiveArea(); m_matcher.computeActiveArea(m_particles[i].map, m_particles[i].pose, plainReading); } + double duration = omp_get_wtime() - start; durations.push_back(duration); for(int i = 0; i < durations.size(); i++) { @@ -47,36 +48,11 @@ inline void GridSlamProcessor::scanMatch(const double* plainReading){ } if(durations.size() > 0) avg_duration /= durations.size(); - m_infoStream << "Scan matching took: " << avg_duration << std::endl; - /*for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++, i++){ - OrientedPoint corrected; - double score, l, s; - score=m_matcher.optimize(corrected, it->map, it->pose, plainReading); - // it->pose=corrected; - // m_infoStream << i << std::endl; - if (score>m_minimumScore){ - it->pose=corrected; - // m_infoStream << "Pose was corrected!" << std::endl; - } else { - if (m_infoStream){ - m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <map, it->pose, plainReading); - sumScore+=score; - it->weight+=l; - it->weightSum+=l; - - //set up the selective copy of the active area - //by detaching the areas that will be updated - m_matcher.invalidateActiveArea(); - m_matcher.computeActiveArea(it->map, it->pose, plainReading); - }*/ - if (m_infoStream) + + if (m_infoStream) { m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl; + m_infoStream << "Average Scan Matching Time=" << avg_duration << "s" << std::endl; + } } inline void GridSlamProcessor::normalize(){ diff --git a/include/gmapping/gridfastslam/gridslamprocessor.hxx~ b/include/gmapping/gridfastslam/gridslamprocessor.hxx~ index b2c318a..fa1cf75 100644 --- a/include/gmapping/gridfastslam/gridslamprocessor.hxx~ +++ b/include/gmapping/gridfastslam/gridslamprocessor.hxx~ @@ -12,7 +12,7 @@ inline void GridSlamProcessor::scanMatch(const double* plainReading){ double sumScore=0; double start = omp_get_wtime(); -//#pragma omp parallel for reduction(+:sumScore) +#pragma omp parallel for reduction(+:sumScore) for (int i = 0; i < m_particles.size(); i++){ OrientedPoint corrected; double score, l, s; From 824a57bbeb97d3b4dcc381b356bdc4447b8f9c7f Mon Sep 17 00:00:00 2001 From: Adrian Bauer Date: Tue, 8 Nov 2016 12:49:14 +0100 Subject: [PATCH 04/14] added multi-threaded scan matching --- gridfastslam/CMakeLists (copy).txt | 9 - gridfastslam/CMakeLists.txt~ | 20 -- gridfastslam/gridslamprocessor.cpp~ | 518 ---------------------------- 3 files changed, 547 deletions(-) delete mode 100644 gridfastslam/CMakeLists (copy).txt delete mode 100644 gridfastslam/CMakeLists.txt~ delete mode 100644 gridfastslam/gridslamprocessor.cpp~ diff --git a/gridfastslam/CMakeLists (copy).txt b/gridfastslam/CMakeLists (copy).txt deleted file mode 100644 index b3e7f24..0000000 --- a/gridfastslam/CMakeLists (copy).txt +++ /dev/null @@ -1,9 +0,0 @@ -add_library(gridfastslam - gfsreader.cpp - gridslamprocessor.cpp - gridslamprocessor_tree.cpp - motionmodel.cpp -) -target_link_libraries(gridfastslam scanmatcher sensor_range) - -install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/gridfastslam/CMakeLists.txt~ b/gridfastslam/CMakeLists.txt~ deleted file mode 100644 index 6b2dd69..0000000 --- a/gridfastslam/CMakeLists.txt~ +++ /dev/null @@ -1,20 +0,0 @@ -find_package(OpenMP REQUIRED) -if(OPENMP_FOUND) - message(STATUS "OPENMP FOUND") - set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) # or if you use C: ${OpenMP_C_FLAGS} - set(OpenMP_LIBS gomp) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") -endif() - - -add_library(gridfastslam - gfsreader.cpp - gridslamprocessor.cpp - gridslamprocessor_tree.cpp - motionmodel.cpp -) - -target_link_libraries(gridfastslam scanmatcher sensor_range ${OpenMP_LIBS}) - -install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) diff --git a/gridfastslam/gridslamprocessor.cpp~ b/gridfastslam/gridslamprocessor.cpp~ deleted file mode 100644 index b95ccdd..0000000 --- a/gridfastslam/gridslamprocessor.cpp~ +++ /dev/null @@ -1,518 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include - -//#define MAP_CONSISTENCY_CHECK -//#define GENERATE_TRAJECTORIES - -namespace GMapping { - -const double m_distanceThresholdCheck = 20; - -using namespace std; - - GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){ - - period_ = 5.0; - m_obsSigmaGain=1; - m_resampleThreshold=0.5; - m_minimumScore=0.; - } - - GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) - :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout){ - - period_ = 5.0; - - m_obsSigmaGain=gsp.m_obsSigmaGain; - m_resampleThreshold=gsp.m_resampleThreshold; - m_minimumScore=gsp.m_minimumScore; - - m_beams=gsp.m_beams; - m_indexes=gsp.m_indexes; - m_motionModel=gsp.m_motionModel; - m_resampleThreshold=gsp.m_resampleThreshold; - m_matcher=gsp.m_matcher; - - m_count=gsp.m_count; - m_readingCount=gsp.m_readingCount; - m_lastPartPose=gsp.m_lastPartPose; - m_pose=gsp.m_pose; - m_odoPose=gsp.m_odoPose; - m_linearDistance=gsp.m_linearDistance; - m_angularDistance=gsp.m_angularDistance; - m_neff=gsp.m_neff; - - cerr << "FILTER COPY CONSTRUCTOR" << endl; - cerr << "m_odoPose=" << m_odoPose.x << " " < >::reference* const, int> PointerMap; - PointerMap pmap; - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - const ScanMatcherMap& m1(it->map); - const HierarchicalArray2D& h1(m1.storage()); - for (int x=0; x >& a1(h1.m_cells[x][y]); - if (a1.m_reference){ - PointerMap::iterator f=pmap.find(a1.m_reference); - if (f==pmap.end()) - pmap.insert(make_pair(a1.m_reference, 1)); - else - f->second++; - } - } - } - } - cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; - for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) - assert(it->first->shares==(unsigned int)it->second); - - cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; -# endif - GridSlamProcessor* cloned=new GridSlamProcessor(*this); - -# ifdef MAP_CONSISTENCY_CHECK - cerr << __PRETTY_FUNCTION__ << ": trajectories end" << endl; - cerr << __PRETTY_FUNCTION__ << ": performing afterclone_fit_test" << endl; - ParticleVector::const_iterator jt=cloned->m_particles.begin(); - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - const ScanMatcherMap& m1(it->map); - const ScanMatcherMap& m2(jt->map); - const HierarchicalArray2D& h1(m1.storage()); - const HierarchicalArray2D& h2(m2.storage()); - jt++; - for (int x=0; x >& a1(h1.m_cells[x][y]); - const autoptr< Array2D >& a2(h2.m_cells[x][y]); - assert(a1.m_reference==a2.m_reference); - assert((!a1.m_reference) || !(a1.m_reference->shares%2)); - } - } - } - cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; -# endif - return cloned; -} - - GridSlamProcessor::~GridSlamProcessor(){ - cerr << __PRETTY_FUNCTION__ << ": Start" << endl; - cerr << __PRETTY_FUNCTION__ << ": Deleting tree" << endl; - for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ -#ifdef TREE_CONSISTENCY_CHECK - TNode* node=it->node; - while(node) - node=node->parent; - cerr << "@" << endl; -#endif - if (it->node) - delete it->node; - //cout << "l=" << it->weight<< endl; - } - -# ifdef MAP_CONSISTENCY_CHECK - cerr << __PRETTY_FUNCTION__ << ": performing predestruction_fit_test" << endl; - typedef std::map >::reference* const, int> PointerMap; - PointerMap pmap; - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - const ScanMatcherMap& m1(it->map); - const HierarchicalArray2D& h1(m1.storage()); - for (int x=0; x >& a1(h1.m_cells[x][y]); - if (a1.m_reference){ - PointerMap::iterator f=pmap.find(a1.m_reference); - if (f==pmap.end()) - pmap.insert(make_pair(a1.m_reference, 1)); - else - f->second++; - } - } - } - } - cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; - for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) - assert(it->first->shares>=(unsigned int)it->second); - cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; -# endif - } - - - - void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, - int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){ - m_obsSigmaGain=likelihoodGain; - m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip); - if (m_infoStream) - m_infoStream << " -maxUrange "<< urange - << " -maxUrange "<< range - << " -sigma "<< sigma - << " -kernelSize "<< kernsize - << " -lstep " << lopt - << " -lobsGain " << m_obsSigmaGain - << " -astep " << aopt << endl; - - - } - -void GridSlamProcessor::setMotionModelParameters -(double srr, double srt, double str, double stt){ - m_motionModel.srr=srr; - m_motionModel.srt=srt; - m_motionModel.str=str; - m_motionModel.stt=stt; - - if (m_infoStream) - m_infoStream << " -srr "<< srr << " -srt "<< srt - << " -str "<< str << " -stt "<< stt << endl; - -} - - void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){ - m_linearThresholdDistance=linear; - m_angularThresholdDistance=angular; - m_resampleThreshold=resampleThreshold; - if (m_infoStream) - m_infoStream << " -linearUpdate " << linear - << " -angularUpdate "<< angular - << " -resampleThreshold " << m_resampleThreshold << endl; - } - - //HERE STARTS THE BEEF - - GridSlamProcessor::Particle::Particle(const ScanMatcherMap& m): - map(m), pose(0,0,0), weight(0), weightSum(0), gweight(0), previousIndex(0){ - node=0; - } - - - void GridSlamProcessor::setSensorMap(const SensorMap& smap){ - - /* - Construct the angle table for the sensor - - FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot - */ - - SensorMap::const_iterator laser_it=smap.find(std::string("FLASER")); - if (laser_it==smap.end()){ - cerr << "Attempting to load the new carmen log format" << endl; - laser_it=smap.find(std::string("ROBOTLASER1")); - assert(laser_it!=smap.end()); - } - const RangeSensor* rangeSensor=dynamic_cast((laser_it->second)); - assert(rangeSensor && rangeSensor->beams().size()); - - m_beams=static_cast(rangeSensor->beams().size()); - double* angles=new double[rangeSensor->beams().size()]; - for (unsigned int i=0; ibeams()[i].pose.theta; - } - m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose()); - delete [] angles; - } - - void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){ - m_xmin=xmin; - m_ymin=ymin; - m_xmax=xmax; - m_ymax=ymax; - m_delta=delta; - if (m_infoStream) - m_infoStream - << " -xmin "<< m_xmin - << " -xmax "<< m_xmax - << " -ymin "<< m_ymin - << " -ymax "<< m_ymax - << " -delta "<< m_delta - << " -particles "<< size << endl; - - - m_particles.clear(); - TNode* node=new TNode(initialPose, 0, 0, 0); - ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta); - for (unsigned int i=0; i(o.getSensor()); - if (os && os->isIdeal() && m_outputStream){ - m_outputStream << setiosflags(ios::fixed) << setprecision(3); - m_outputStream << "SIMULATOR_POS " << o.getPose().x << " " << o.getPose().y << " " ; - m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " << o.getTime() << endl; - } - } - - - bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){ - - /**retireve the position from the reading, and compute the odometry*/ - OrientedPoint relPose=reading.getPose(); - if (!m_count){ - m_lastPartPose=m_odoPose=relPose; - } - - //write the state of the reading and update all the particles using the motion model - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - OrientedPoint& pose(it->pose); - pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose); - } - - // update the output file - if (m_outputStream.is_open()){ - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << "ODOM "; - m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " "; - m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " "; - m_outputStream << reading.getTime(); - m_outputStream << endl; - } - if (m_outputStream.is_open()){ - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << "ODO_UPDATE "<< m_particles.size() << " "; - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - OrientedPoint& pose(it->pose); - m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; - m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; - } - m_outputStream << reading.getTime(); - m_outputStream << endl; - } - - //invoke the callback - onOdometryUpdate(); - - - // accumulate the robot translation and rotation - OrientedPoint move=relPose-m_odoPose; - move.theta=atan2(sin(move.theta), cos(move.theta)); - m_linearDistance+=sqrt(move*move); - m_angularDistance+=fabs(move.theta); - - // if the robot jumps throw a warning - if (m_linearDistance>m_distanceThresholdCheck){ - cerr << "***********************************************************************" << endl; - cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl; - cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl; - cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y - << " " <=m_linearThresholdDistance - || m_angularDistance>=m_angularThresholdDistance - || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){ - last_update_time_ = reading.getTime(); - - if (m_outputStream.is_open()){ - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << "FRAME " << m_readingCount; - m_outputStream << " " << m_linearDistance; - m_outputStream << " " << m_angularDistance << endl; - } - - if (m_infoStream) - m_infoStream << "update frame " << m_readingCount << endl - << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl; - - - cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y - << " " << reading.getPose().theta << endl; - - - //this is for converting the reading in a scan-matcher feedable form - assert(reading.size()==m_beams); - double * plainReading = new double[m_beams]; - for(unsigned int i=0; i(reading.getSensor()), - reading.getTime()); - - if (m_count>0){ - scanMatch(plainReading); - if (m_outputStream.is_open()){ - m_outputStream << "LASER_READING "<< reading.size() << " "; - m_outputStream << setiosflags(ios::fixed) << setprecision(2); - for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){ - m_outputStream << *b << " "; - } - OrientedPoint p=reading.getPose(); - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl; - m_outputStream << "SM_UPDATE "<< m_particles.size() << " "; - for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - const OrientedPoint& pose=it->pose; - m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; - m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; - } - m_outputStream << endl; - } - onScanmatchUpdate(); - - updateTreeWeights(false); - - if (m_infoStream){ - m_infoStream << "neff= " << m_neff << endl; - } - if (m_outputStream.is_open()){ - m_outputStream << setiosflags(ios::fixed) << setprecision(6); - m_outputStream << "NEFF " << m_neff << endl; - } - resample(plainReading, adaptParticles, reading_copy); - - } else { - m_infoStream << "Registering First Scan"<< endl; - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - m_matcher.invalidateActiveArea(); - m_matcher.computeActiveArea(it->map, it->pose, plainReading); - m_matcher.registerScan(it->map, it->pose, plainReading); - - // cyr: not needed anymore, particles refer to the root in the beginning! - TNode* node=new TNode(it->pose, 0., it->node, 0); - //node->reading=0; - node->reading = reading_copy; - it->node=node; - - } - } - // cerr << "Tree: normalizing, resetting and propagating weights at the end..." ; - updateTreeWeights(false); - // cerr << ".done!" <previousPose=it->pose; - } - - } - if (m_outputStream.is_open()) - m_outputStream << flush; - m_readingCount++; - return processed; - } - - - std::ofstream& GridSlamProcessor::outputStream(){ - return m_outputStream; - } - - std::ostream& GridSlamProcessor::infoStream(){ - return m_infoStream; - } - - - int GridSlamProcessor::getBestParticleIndex() const{ - unsigned int bi=0; - double bw=-std::numeric_limits::max(); - for (unsigned int i=0; i Date: Tue, 8 Nov 2016 12:51:30 +0100 Subject: [PATCH 05/14] added multi-threaded scan matching --- .../gridfastslam/gridslamprocessor.hxx~ | 217 ----------- .../gridfastslam/gridslamprocessor.h~ | 339 ------------------ 2 files changed, 556 deletions(-) delete mode 100644 include/gmapping/gridfastslam/gridslamprocessor.hxx~ delete mode 100644 include/gmapping/gridfastslam/gridslamprocessor.h~ diff --git a/include/gmapping/gridfastslam/gridslamprocessor.hxx~ b/include/gmapping/gridfastslam/gridslamprocessor.hxx~ deleted file mode 100644 index fa1cf75..0000000 --- a/include/gmapping/gridfastslam/gridslamprocessor.hxx~ +++ /dev/null @@ -1,217 +0,0 @@ - -#ifdef MACOSX -// This is to overcome a possible bug in Apple's GCC. -#define isnan(x) (x==FP_NAN) -#endif - -/**Just scan match every single particle. -If the scan matching fails, the particle gets a default likelihood.*/ -inline void GridSlamProcessor::scanMatch(const double* plainReading){ - // sample a new pose from each scan in the reference - double avg_duration = 0; - double sumScore=0; - double start = omp_get_wtime(); - -#pragma omp parallel for reduction(+:sumScore) - for (int i = 0; i < m_particles.size(); i++){ - OrientedPoint corrected; - double score, l, s; - score=m_matcher.optimize(corrected, m_particles[i].map, m_particles[i].pose, plainReading); - // it->pose=corrected; - // m_infoStream << i << std::endl; - if (score>m_minimumScore){ - m_particles[i].pose=corrected; - // m_infoStream << "Pose was corrected!" << std::endl; - } else { - if (m_infoStream){ - m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l < 0) avg_duration /= durations.size(); - - m_infoStream << "Scan matching took: " << avg_duration << std::endl; - /*for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++, i++){ - OrientedPoint corrected; - double score, l, s; - score=m_matcher.optimize(corrected, it->map, it->pose, plainReading); - // it->pose=corrected; - // m_infoStream << i << std::endl; - if (score>m_minimumScore){ - it->pose=corrected; - // m_infoStream << "Pose was corrected!" << std::endl; - } else { - if (m_infoStream){ - m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <map, it->pose, plainReading); - sumScore+=score; - it->weight+=l; - it->weightSum+=l; - - //set up the selective copy of the active area - //by detaching the areas that will be updated - m_matcher.invalidateActiveArea(); - m_matcher.computeActiveArea(it->map, it->pose, plainReading); - }*/ - if (m_infoStream) - m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl; -} - -inline void GridSlamProcessor::normalize(){ - //normalize the log m_weights - double gain=1./(m_obsSigmaGain*m_particles.size()); - double lmax= -std::numeric_limits::max(); - for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - lmax=it->weight>lmax?it->weight:lmax; - } - //cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl; - - m_weights.clear(); - double wcum=0; - m_neff=0; - for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ - m_weights.push_back(exp(gain*(it->weight-lmax))); - wcum+=m_weights.back(); - //cout << "l=" << it->weight<< endl; - } - - m_neff=0; - for (std::vector::iterator it=m_weights.begin(); it!=m_weights.end(); it++){ - *it=*it/wcum; - double w=*it; - m_neff+=w*w; - } - m_neff=1./m_neff; - -} - -inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){ - - bool hasResampled = false; - - TNodeVector oldGeneration; - for (unsigned int i=0; i resampler; - m_indexes=resampler.resampleIndexes(m_weights, adaptSize); - - if (m_outputStream.is_open()){ - m_outputStream << "RESAMPLE "<< m_indexes.size() << " "; - for (std::vector::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){ - m_outputStream << *it << " "; - } - m_outputStream << std::endl; - } - - onResampleUpdate(); - //BEGIN: BUILDING TREE - ParticleVector temp; - unsigned int j=0; - std::vector deletedParticles; //this is for deleteing the particles which have been resampled away. - - // cerr << "Existing Nodes:" ; - for (unsigned int i=0; i" << m_indexes[i] << "B("<childs <<") "; - node=new TNode(p.pose, 0, oldNode, 0); - //node->reading=0; - node->reading=reading; - // cerr << "A("<parent->childs <<") " <setWeight(0); - m_matcher.invalidateActiveArea(); - m_matcher.registerScan(it->map, it->pose, plainReading); - m_particles.push_back(*it); - } - std::cerr << " Done" <pose, 0.0, *node_it, 0); - - //node->reading=0; - node->reading=reading; - it->node=node; - - //END: BUILDING TREE - m_matcher.invalidateActiveArea(); - m_matcher.registerScan(it->map, it->pose, plainReading); - it->previousIndex=index; - index++; - node_it++; - - } - std::cerr << "Done" < -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "motionmodel.h" - -#include - - -namespace GMapping { - - /**This class defines the basic GridFastSLAM algorithm. It - implements a rao blackwellized particle filter. Each particle - has its own map and robot pose.
This implementation works - as follows: each time a new pair odometry/laser reading is - received, the particle's robot pose is updated according to the - motion model. This pose is subsequently used for initalizing a - scan matching algorithm. The scanmatcher performs a local - optimization for each particle. It is initialized with the - pose drawn from the motion model, and the pose is corrected - according to the each particle map.
- In order to avoid unnecessary computation the filter state is updated - only when the robot moves more than a given threshold. - */ - class GridSlamProcessor{ - public: - - - /**This class defines the the node of reversed tree in which the trajectories are stored. - Each node of a tree has a pointer to its parent and a counter indicating the number of childs of a node. - The tree is updated in a way consistent with the operation performed on the particles. - */ - struct TNode{ - /**Constructs a node of the trajectory tree. - @param pose: the pose of the robot in the trajectory - @param weight: the weight of the particle at that point in the trajectory - @param accWeight: the cumulative weight of the particle - @param parent: the parent node in the tree - @param childs: the number of childs - */ - TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0); - - /**Destroys a tree node, and consistently updates the tree. If a node whose parent has only one child is deleted, - also the parent node is deleted. This because the parent will not be reacheable anymore in the trajectory tree.*/ - ~TNode(); - - /**The pose of the robot*/ - OrientedPoint pose; - - /**The weight of the particle*/ - double weight; - - /**The sum of all the particle weights in the previous part of the trajectory*/ - double accWeight; - - double gweight; - - - /**The parent*/ - TNode* parent; - - /**The range reading to which this node is associated*/ - const RangeReading* reading; - - /**The number of childs*/ - unsigned int childs; - - /**counter in visiting the node (internally used)*/ - mutable unsigned int visitCounter; - - /**visit flag (internally used)*/ - mutable bool flag; - }; - - typedef std::vector TNodeVector; - typedef std::deque TNodeDeque; - - /**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/ - struct Particle{ - /**constructs a particle, given a map - @param map: the particle map - */ - Particle(const ScanMatcherMap& map); - - /** @returns the weight of a particle */ - inline operator double() const {return weight;} - /** @returns the pose of a particle */ - inline operator OrientedPoint() const {return pose;} - /** sets the weight of a particle - @param w the weight - */ - inline void setWeight(double w) {weight=w;} - /** The map */ - ScanMatcherMap map; - /** The pose of the robot */ - OrientedPoint pose; - - /** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */ - OrientedPoint previousPose; - - /** The weight of the particle */ - double weight; - - /** The cumulative weight of the particle */ - double weightSum; - - double gweight; - - /** The index of the previous particle in the trajectory tree */ - int previousIndex; - - /** Entry to the trajectory tree */ - TNode* node; - }; - - - typedef std::vector ParticleVector; - - /** Constructs a GridSlamProcessor, initialized with the default parameters */ - GridSlamProcessor(); - - /** Constructs a GridSlamProcessor, whose output is routed to a stream. - @param infoStr: the output stream - */ - GridSlamProcessor(std::ostream& infoStr); - - /** @returns a deep copy of the grid slam processor with all the internal structures. - */ - GridSlamProcessor* clone() const; - - /**Deleted the gridslamprocessor*/ - virtual ~GridSlamProcessor(); - - //methods for accessing the parameters - void setSensorMap(const SensorMap& smap); - void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, - OrientedPoint initialPose=OrientedPoint(0,0,0)); - void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, - int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0); - void setMotionModelParameters(double srr, double srt, double str, double stt); - void setUpdateDistances(double linear, double angular, double resampleThreshold); - void setUpdatePeriod(double p) {period_=p;} - - //the "core" algorithm - void processTruePos(const OdometryReading& odometry); - bool processScan(const RangeReading & reading, int adaptParticles=0); - - /**This method copies the state of the filter in a tree. - The tree is represented through reversed pointers (each node has a pointer to its parent). - The leafs are stored in a vector, whose size is the same as the number of particles. - @returns the leafs of the tree - */ - TNodeVector getTrajectories() const; - void integrateScanSequence(TNode* node); - - /**the scanmatcher algorithm*/ - ScanMatcher m_matcher; - /**the stream used for writing the output of the algorithm*/ - std::ofstream& outputStream(); - /**the stream used for writing the info/debug messages*/ - std::ostream& infoStream(); - /**@returns the particles*/ - inline const ParticleVector& getParticles() const {return m_particles; } - - inline const std::vector& getIndexes() const{return m_indexes; } - int getBestParticleIndex() const; - //callbacks - virtual void onOdometryUpdate(); - virtual void onResampleUpdate(); - virtual void onScanmatchUpdate(); - - //accessor methods - /**the maxrange of the laser to consider */ - MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public); - - /**the maximum usable range of the laser. A beam is cropped to this value. [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public); - - /**The sigma used by the greedy endpoint matching. [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public); - - /**The sigma of a beam used for likelihood computation [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public); - - /**The kernel in which to look for a correspondence[scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, int, kernelSize, protected, public, public); - - /**The optimization step in rotation [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public); - - /**The optimization step in translation [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public); - - /**The number of iterations of the scanmatcher [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public); - - /**the beams to skip for computing the likelihood (consider a beam every likelihoodSkip) [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public); - - /**translational sampling range for the likelihood [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public); - - /**angular sampling range for the likelihood [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public); - - /**translational sampling range for the likelihood [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public); - - /**angular sampling step for the likelihood [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public); - - /**generate an accupancy grid map [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public); - - /**enlarge the map when the robot goes out of the boundaries [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public); - - /**pose of the laser wrt the robot [scanmatcher]*/ - MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public); - - - /**odometry error in translation as a function of translation (rho/rho) [motionmodel]*/ - STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public); - - /**odometry error in translation as a function of rotation (rho/theta) [motionmodel]*/ - STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public); - - /**odometry error in rotation as a function of translation (theta/rho) [motionmodel]*/ - STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public); - - /**odometry error in rotation as a function of rotation (theta/theta) [motionmodel]*/ - STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public); - - /**minimum score for considering the outcome of the scanmatching good*/ - PARAM_SET_GET(double, minimumScore, protected, public, public); - - - protected: - /**Copy constructor*/ - GridSlamProcessor(const GridSlamProcessor& gsp); - - /**the laser beams*/ - unsigned int m_beams; - double last_update_time_; - double period_; - - /**the particles*/ - ParticleVector m_particles; - - /**the particle indexes after resampling (internally used)*/ - std::vector m_indexes; - - /**the particle weights (internally used)*/ - std::vector m_weights; - - /**the motion model*/ - MotionModel m_motionModel; - - /**this sets the neff based resampling threshold*/ - PARAM_SET_GET(double, resampleThreshold, protected, public, public); - - //state - int m_count, m_readingCount; - OrientedPoint m_lastPartPose; - OrientedPoint m_odoPose; - OrientedPoint m_pose; - double m_linearDistance, m_angularDistance; - PARAM_GET(double, neff, protected, public); - - //processing parameters (size of the map) - PARAM_GET(double, xmin, protected, public); - PARAM_GET(double, ymin, protected, public); - PARAM_GET(double, xmax, protected, public); - PARAM_GET(double, ymax, protected, public); - //processing parameters (resolution of the map) - PARAM_GET(double, delta, protected, public); - - //registration score (if a scan score is above this threshold it is registered in the map) - PARAM_SET_GET(double, regScore, protected, public, public); - //registration score (if a scan score is below this threshold a scan matching failure is reported) - PARAM_SET_GET(double, critScore, protected, public, public); - //registration score maximum move allowed between consecutive scans - PARAM_SET_GET(double, maxMove, protected, public, public); - - //process a scan each time the robot translates of linearThresholdDistance - PARAM_SET_GET(double, linearThresholdDistance, protected, public, public); - - //process a scan each time the robot rotates more than angularThresholdDistance - PARAM_SET_GET(double, angularThresholdDistance, protected, public, public); - - //smoothing factor for the likelihood - PARAM_SET_GET(double, obsSigmaGain, protected, public, public); - - //stream in which to write the gfs file - std::ofstream m_outputStream; - - // stream in which to write the messages - std::ostream& m_infoStream; - - // the functions below performs side effect on the internal structure, - //should be called only inside the processScan method - private: - - /**scanmatches all the particles*/ - inline void scanMatch(const double *plainReading); - /**normalizes the particle weights*/ - inline void normalize(); - - // return if a resampling occured or not - inline bool resample(const double* plainReading, int adaptParticles, - const RangeReading* rr=0); - - //tree utilities - - void updateTreeWeights(bool weightsAlreadyNormalized = false); - void resetTree(); - double propagateWeights(); - - }; - -typedef std::multimap TNodeMultimap; - - -#include "gridslamprocessor.hxx" - -}; - -#endif From fb10ec904313e5fb9d7cd63551177be85a5bb9cc Mon Sep 17 00:00:00 2001 From: Adrian Bauer Date: Tue, 8 Nov 2016 13:00:13 +0100 Subject: [PATCH 06/14] 0.1.3 --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 8a7562f..3e6a5e1 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ openslam_gmapping - 0.1.2 + 0.1.3 ROS-ified version of gmapping SLAM. Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ Vincent Rabaud CreativeCommons-by-nc-sa-2.0 From e0f3e15f70f0feb898d881fc3d914af484edab31 Mon Sep 17 00:00:00 2001 From: eybee Date: Sun, 20 Nov 2016 10:59:59 -0800 Subject: [PATCH 07/14] Update package.xml --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 3e6a5e1..8a7562f 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ openslam_gmapping - 0.1.3 + 0.1.2 ROS-ified version of gmapping SLAM. Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ Vincent Rabaud CreativeCommons-by-nc-sa-2.0 From d2495615a62aff8e1340a205fa74cdefad3a5e6f Mon Sep 17 00:00:00 2001 From: eybee Date: Sun, 20 Nov 2016 11:02:59 -0800 Subject: [PATCH 08/14] Update CMakeLists.txt --- CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index bd83aeb..17f2b55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,6 @@ find_package(catkin) catkin_package( INCLUDE_DIRS include LIBRARIES gridfastslam scanmatcher sensor_base sensor_range sensor_odometry utils - DEPENDS OpenMP ) include_directories(include) From 17d9948af055e142981c7fe1e36aaa7352b43c93 Mon Sep 17 00:00:00 2001 From: eybee Date: Sun, 20 Nov 2016 11:09:19 -0800 Subject: [PATCH 09/14] Update CMakeLists.txt --- gridfastslam/CMakeLists.txt | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/gridfastslam/CMakeLists.txt b/gridfastslam/CMakeLists.txt index 0be45c8..2dd2684 100644 --- a/gridfastslam/CMakeLists.txt +++ b/gridfastslam/CMakeLists.txt @@ -1,10 +1,5 @@ -find_package(OpenMP REQUIRED) if(OPENMP_FOUND) message(STATUS "OPENMP FOUND") - set(OpenMP_FLAGS ${OpenMP_CXX_FLAGS}) # or if you use C: ${OpenMP_C_FLAGS} - set(OpenMP_LIBS gomp) - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") - set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") endif() add_library(gridfastslam @@ -14,6 +9,6 @@ add_library(gridfastslam motionmodel.cpp ) -target_link_libraries(gridfastslam scanmatcher sensor_range ${OpenMP_LIBS}) +target_link_libraries(gridfastslam scanmatcher sensor_range) install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) From f05b787ce17673ce2fa73972115eec4103b4dd2c Mon Sep 17 00:00:00 2001 From: eybee Date: Sun, 20 Nov 2016 11:10:47 -0800 Subject: [PATCH 10/14] Update gridslamprocessor.h --- include/gmapping/gridfastslam/gridslamprocessor.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/gmapping/gridfastslam/gridslamprocessor.h b/include/gmapping/gridfastslam/gridslamprocessor.h index 45b52d6..cdd1eb1 100644 --- a/include/gmapping/gridfastslam/gridslamprocessor.h +++ b/include/gmapping/gridfastslam/gridslamprocessor.h @@ -15,7 +15,6 @@ #include #include "motionmodel.h" -#include namespace GMapping { From eb1f0f50fd2e3e9ff36cf882bce0c50765121ee2 Mon Sep 17 00:00:00 2001 From: eybee Date: Sun, 20 Nov 2016 11:11:50 -0800 Subject: [PATCH 11/14] Update gridslamprocessor.h --- include/gmapping/gridfastslam/gridslamprocessor.h | 1 - 1 file changed, 1 deletion(-) diff --git a/include/gmapping/gridfastslam/gridslamprocessor.h b/include/gmapping/gridfastslam/gridslamprocessor.h index cdd1eb1..af2220c 100644 --- a/include/gmapping/gridfastslam/gridslamprocessor.h +++ b/include/gmapping/gridfastslam/gridslamprocessor.h @@ -244,7 +244,6 @@ namespace GMapping { /**minimum score for considering the outcome of the scanmatching good*/ PARAM_SET_GET(double, minimumScore, protected, public, public); - protected: /**Copy constructor*/ GridSlamProcessor(const GridSlamProcessor& gsp); From ea1556cb5b6fd5637cb17a0642838b6a35983b43 Mon Sep 17 00:00:00 2001 From: eybee Date: Sun, 20 Nov 2016 11:27:33 -0800 Subject: [PATCH 12/14] Update gridslamprocessor.h --- include/gmapping/gridfastslam/gridslamprocessor.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/gmapping/gridfastslam/gridslamprocessor.h b/include/gmapping/gridfastslam/gridslamprocessor.h index af2220c..6b05bc6 100644 --- a/include/gmapping/gridfastslam/gridslamprocessor.h +++ b/include/gmapping/gridfastslam/gridslamprocessor.h @@ -15,6 +15,7 @@ #include #include "motionmodel.h" +#include namespace GMapping { From 7070ea9b8234aedc3fe52e58776caa0bf151c333 Mon Sep 17 00:00:00 2001 From: eybee Date: Tue, 22 Nov 2016 10:29:48 +0100 Subject: [PATCH 13/14] Update CMakeLists.txt --- gridfastslam/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gridfastslam/CMakeLists.txt b/gridfastslam/CMakeLists.txt index 2dd2684..3fdf03a 100644 --- a/gridfastslam/CMakeLists.txt +++ b/gridfastslam/CMakeLists.txt @@ -1,5 +1,6 @@ if(OPENMP_FOUND) message(STATUS "OPENMP FOUND") + set(OpenMP_LIBS gomp) endif() add_library(gridfastslam @@ -9,6 +10,6 @@ add_library(gridfastslam motionmodel.cpp ) -target_link_libraries(gridfastslam scanmatcher sensor_range) +target_link_libraries(gridfastslam scanmatcher sensor_range ${OpenMP_LIBS}) install(TARGETS gridfastslam DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) From 6e2221fc079dad86fd6429807ac2a8d3c1c66234 Mon Sep 17 00:00:00 2001 From: eybee Date: Wed, 23 Nov 2016 02:59:35 -0800 Subject: [PATCH 14/14] Update CMakeLists.txt --- gridfastslam/CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gridfastslam/CMakeLists.txt b/gridfastslam/CMakeLists.txt index 3fdf03a..6f64db8 100644 --- a/gridfastslam/CMakeLists.txt +++ b/gridfastslam/CMakeLists.txt @@ -1,6 +1,8 @@ +find_package(OpenMP REQUIRED) if(OPENMP_FOUND) - message(STATUS "OPENMP FOUND") set(OpenMP_LIBS gomp) + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif() add_library(gridfastslam