├── openslam_gmapping ├── log │ ├── configuration.cpp │ ├── Makefile │ ├── configuration.h │ ├── carmenconfiguration.h │ ├── sensorlog.h │ ├── log_test.cpp │ ├── sensorstream.h │ ├── scanstudio2carmen.cpp │ ├── rdk2carmen.cpp │ ├── log_plot.cpp │ ├── sensorlog.cpp │ └── sensorstream.cpp ├── sensor │ ├── Makefile │ ├── sensor_base │ │ ├── CMakeLists.txt │ │ ├── sensor.cpp │ │ ├── sensorreading.cpp │ │ └── sensoreading.h │ ├── CMakeLists.txt │ ├── sensor_odometry │ │ ├── CMakeLists.txt │ │ ├── odometrysensor.cpp │ │ └── odometryreading.cpp │ └── sensor_range │ │ ├── CMakeLists.txt │ │ ├── rangesensor.cpp │ │ └── rangereading.cpp ├── grid │ ├── Makefile │ ├── graphmap.cpp │ └── map_test.cpp ├── utils │ ├── CMakeLists.txt │ ├── printmemusage.h │ ├── printpgm.h │ ├── printmemusage.cpp │ ├── orientedboundingbox.h │ ├── autoptr_test.cpp │ ├── movement.h │ ├── stat_test.cpp │ ├── movement.cpp │ ├── orientedboundingbox.hxx │ ├── optimizer.h │ └── dmatrix.h ├── README ├── include │ └── gmapping │ │ ├── grid │ │ ├── accessstate.h │ │ └── array2d.h │ │ ├── log │ │ ├── configuration.h │ │ └── sensorlog.h │ │ ├── sensor │ │ ├── sensor_odometry │ │ │ ├── odometrysensor.h │ │ │ └── odometryreading.h │ │ ├── sensor_base │ │ │ ├── sensor.h │ │ │ └── sensorreading.h │ │ └── sensor_range │ │ │ ├── rangereading.h │ │ │ └── rangesensor.h │ │ ├── gridfastslam │ │ └── motionmodel.h │ │ ├── utils │ │ ├── gvalues.h │ │ ├── macro_params.h │ │ ├── autoptr.h │ │ ├── commandline.h │ │ ├── stat.h │ │ └── point.h │ │ └── scanmatcher │ │ ├── smmap.h │ │ └── icp.h ├── scanmatcher │ ├── CMakeLists.txt │ ├── smmap.cpp │ ├── eig3.h │ ├── lumiles.h │ ├── scanmatcherprocessor.h │ ├── icptest.cpp │ ├── gridlinetraversal.h │ └── scanmatch_test.cpp ├── build_tools │ ├── message │ ├── generate_shared_object │ ├── pretty_compiler │ ├── testlib │ ├── Makefile.subdirs │ ├── Makefile.app │ └── Makefile.generic-shared-object ├── gridfastslam │ ├── CMakeLists.txt │ ├── gfs2neff.cpp │ ├── gfs2log.cpp │ ├── gfs2stream.cpp │ ├── gfs2stat.cpp │ ├── gfsreader.h │ └── motionmodel.cpp ├── gfs-carmen │ └── Makefile ├── gui │ ├── qpixmapdumper.h │ ├── qpixmapdumper.cpp │ ├── qmappainter.cpp │ ├── qnavigatorwidget.h │ ├── qslamandnavwidget.h │ ├── CMakeLists.txt │ ├── gfs_logplayer.cpp │ ├── qmappainter.h │ ├── qgraphpainter.h │ ├── gfs_nogui.cpp │ ├── gfs_simplegui.cpp │ ├── qslamandnavwidget.cpp │ ├── qnavigatorwidget.cpp │ ├── qgraphpainter.cpp │ ├── qparticleviewer.h │ └── gsp_thread.h ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml ├── ini │ ├── gfs-LMS-20cm.ini │ ├── gfs-LMS-10cm.ini │ ├── gfs-LMS-5cm.ini │ ├── gfs-PLS-5cm.ini │ └── gfs-PLS-10cm.ini ├── particlefilter │ ├── particlefilter_test.cpp │ ├── range_bearing.cpp │ ├── particlefilter.cpp │ └── pf.h └── carmenwrapper │ └── carmenwrapper.h ├── .gitignore ├── slam_gmapping ├── launch │ └── slam_gmapping.launch.py ├── package.xml ├── CMakeLists.txt └── include │ └── slam_gmapping │ └── slam_gmapping.h └── README.md /openslam_gmapping/log/configuration.cpp: -------------------------------------------------------------------------------- 1 | #include "configuration.h" 2 | 3 | namespace GMapping { 4 | 5 | Configuration::~Configuration(){ 6 | } 7 | 8 | }; 9 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/Makefile: -------------------------------------------------------------------------------- 1 | -include ../global.mk 2 | 3 | SUBDIRS=sensor_base sensor_odometry sensor_range 4 | 5 | -include ../build_tools/Makefile.subdirs 6 | -------------------------------------------------------------------------------- /openslam_gmapping/grid/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= 2 | APPS= map_test 3 | 4 | LDFLAGS+= 5 | CPPFLAGS+= -DNDEBUG 6 | 7 | -include ../global.mk 8 | -include ../build_tools/Makefile.app 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(./) 2 | add_library(utils movement.cpp stat.cpp) 3 | install(TARGETS utils DESTINATION lib) 4 | 5 | ament_export_libraries(utils) 6 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(sensor_base sensor.cpp sensorreading.cpp) 2 | install(TARGETS sensor_base DESTINATION lib) 3 | 4 | ament_export_libraries(sensor_base) -------------------------------------------------------------------------------- /openslam_gmapping/README: -------------------------------------------------------------------------------- 1 | This is a fork from gmapping at https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ 2 | It includes a few patches that could be pushed upstream if needed 3 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/grid/accessstate.h: -------------------------------------------------------------------------------- 1 | #ifndef ACCESSTATE_H 2 | #define ACCESSTATE_H 3 | 4 | namespace GMapping { 5 | enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2}; 6 | }; 7 | 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/scanmatcher/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(scanmatcher STATIC eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp) 2 | target_link_libraries(scanmatcher sensor_range utils) 3 | 4 | install(TARGETS scanmatcher DESTINATION lib) 5 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/message: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | #echo "message: verbose = $VERBOSE" 4 | 5 | if ($VERBOSE) 6 | then 7 | exit 0; 8 | fi 9 | 10 | a=$MAKELEVEL 11 | 12 | while ((0<$a)); do echo -n " "; let "a = $a - 1";done 13 | 14 | echo $1 15 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(sensor_base) 2 | ament_export_libraries(sensor_base) 3 | 4 | add_subdirectory(sensor_odometry) 5 | ament_export_libraries(sensor_odometry) 6 | 7 | add_subdirectory(sensor_range) 8 | ament_export_libraries(sensor_range) -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_base/sensor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | Sensor::Sensor(const std::string& name){ 6 | m_name=name; 7 | } 8 | 9 | Sensor::~Sensor(){ 10 | } 11 | 12 | };// end namespace 13 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(sensor_odometry odometryreading.cpp odometrysensor.cpp) 2 | target_link_libraries(sensor_odometry sensor_base) 3 | 4 | install(TARGETS sensor_odometry DESTINATION lib) 5 | 6 | ament_export_libraries(sensor_odometry) 7 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | OdometrySensor::OdometrySensor(const std::string& name, bool ideal): Sensor(name){ m_ideal=ideal;} 6 | 7 | 8 | }; 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | OdometryReading::OdometryReading(const OdometrySensor* odo, double time): 6 | SensorReading(odo,time){} 7 | 8 | }; 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/printmemusage.h: -------------------------------------------------------------------------------- 1 | #ifndef PRINTMEMUSAGE_H 2 | #define PRINTMEMUSAGE_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace GMapping{ 10 | void printmemusage(); 11 | }; 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_range/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(sensor_range rangereading.cpp rangesensor.cpp) 2 | target_link_libraries(sensor_range sensor_base) 3 | #ament_target_dependencies(sensor_range sensor_base) 4 | 5 | install(TARGETS sensor_range DESTINATION lib) 6 | 7 | ament_export_libraries(sensor_range) -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/generate_shared_object: -------------------------------------------------------------------------------- 1 | #!/bin/tcsh 2 | 3 | echo decompressing file $1 4 | 5 | set FILELIST=`ar -t $1` 6 | echo "Object files:" 7 | foreach i ($FILELIST) 8 | echo $i 9 | end 10 | 11 | echo generating $1:r.so 12 | 13 | ar -x $1 14 | ld -shared -o $1:r.so $FILELIST 15 | 16 | rm $FILELIST 17 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_base/sensorreading.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | //SensorReading::SensorReading(const Sensor* s, double t){ 6 | // m_sensor=s; 7 | // m_time=t; 8 | //} 9 | // 10 | // 11 | //SensorReading::~SensorReading(){ 12 | //} 13 | 14 | }; 15 | 16 | -------------------------------------------------------------------------------- /openslam_gmapping/log/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= configuration.o carmenconfiguration.o sensorlog.o sensorstream.o 2 | APPS= log_test log_plot scanstudio2carmen rdk2carmen 3 | 4 | LDFLAGS+= -lsensor_range -lsensor_odometry -lsensor_base 5 | CPPFLAGS+= -I../sensor 6 | 7 | -include ../global.mk 8 | -include ../build_tools/Makefile.generic-shared-object 9 | 10 | -------------------------------------------------------------------------------- /openslam_gmapping/scanmatcher/smmap.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping { 4 | 5 | const PointAccumulator& PointAccumulator::Unknown(){ 6 | if (! unknown_ptr) 7 | unknown_ptr=new PointAccumulator; 8 | return *unknown_ptr; 9 | } 10 | 11 | PointAccumulator* PointAccumulator::unknown_ptr=0; 12 | 13 | }; 14 | 15 | 16 | -------------------------------------------------------------------------------- /openslam_gmapping/log/configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIGURATION_H 2 | #define CONFIGURATION_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping { 8 | 9 | class Configuration{ 10 | public: 11 | virtual ~Configuration(); 12 | virtual SensorMap computeSensorMap() const=0; 13 | }; 14 | 15 | }; 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /openslam_gmapping/gridfastslam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_library(gridfastslam 2 | gfsreader.cpp 3 | gridslamprocessor.cpp 4 | gridslamprocessor_tree.cpp 5 | motionmodel.cpp 6 | ) 7 | target_link_libraries(gridfastslam scanmatcher sensor_range) 8 | 9 | install(TARGETS gridfastslam DESTINATION lib) 10 | 11 | #ament_export_libraries(gridfastslam) 12 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/log/configuration.h: -------------------------------------------------------------------------------- 1 | #ifndef CONFIGURATION_H 2 | #define CONFIGURATION_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping { 8 | 9 | class Configuration{ 10 | public: 11 | virtual ~Configuration(); 12 | virtual SensorMap computeSensorMap() const=0; 13 | }; 14 | 15 | }; 16 | #endif 17 | 18 | -------------------------------------------------------------------------------- /openslam_gmapping/scanmatcher/eig3.h: -------------------------------------------------------------------------------- 1 | 2 | /* Eigen-decomposition for symmetric 3x3 real matrices. 3 | Public domain, copied from the public domain Java library JAMA. */ 4 | 5 | #ifndef _eig_h 6 | 7 | /* Symmetric matrix A => eigenvectors in columns of V, corresponding 8 | eigenvalues in d. */ 9 | void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /openslam_gmapping/gfs-carmen/Makefile: -------------------------------------------------------------------------------- 1 | OBJS= 2 | APPS= gfs-carmen 3 | 4 | LIBS+= -lcarmenwrapper -lgridfastslam -lconfigfile 5 | CPPFLAGS+= -I ../sensor -I$(CARMEN_HOME)/include 6 | 7 | -include ../global.mk 8 | ifeq ($(CARMENSUPPORT), 0) 9 | APPS= 10 | .PHONY: clean all 11 | 12 | all: 13 | 14 | clean: 15 | 16 | else 17 | -include ../build_tools/Makefile.app 18 | endif 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qpixmapdumper.h: -------------------------------------------------------------------------------- 1 | #ifndef _QPIXMAPDUMPER_H_ 2 | #define _QPIXMAPDUMPER_H_ 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | struct QPixmapDumper{ 9 | QPixmapDumper(std::string prefix, int cycles); 10 | void reset(); 11 | std::string prefix; 12 | std::string format; 13 | bool dump(const QPixmap& pixmap); 14 | int counter; 15 | int cycles; 16 | int frame; 17 | }; 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/pretty_compiler: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | 4 | #echo "pretty: verbose = $VERBOSE" 5 | 6 | if ($VERBOSE) 7 | then 8 | echo $1; 9 | if ! eval $1 10 | then 11 | echo "Failed command was:" 12 | echo $1 13 | echo "in directory " `pwd` 14 | exit 1 15 | fi 16 | else 17 | if ! eval $1 18 | then 19 | echo "Failed command was:" 20 | echo $1 21 | echo "in directory " `pwd` 22 | exit 1 23 | fi 24 | fi 25 | 26 | exit 0 27 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_base/sensoreading.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORREADING_H 2 | #define SENSORREADING_H 3 | 4 | #include "sensor.h" 5 | namespace GMapping{ 6 | 7 | class SensorReading{ 8 | public: 9 | SensorReading(const Sensor* s=0, double time=0); 10 | inline double getTime() const {return m_time;} 11 | inline const Sensor* getSensor() const {return m_sensor;} 12 | protected: 13 | double m_time; 14 | const Sensor* m_sensor; 15 | }; 16 | 17 | }; //end namespace 18 | #endif 19 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOMETRYSENSOR_H 2 | #define ODOMETRYSENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping{ 8 | 9 | class OdometrySensor: public Sensor{ 10 | public: 11 | OdometrySensor(const std::string& name, bool ideal=false); 12 | inline bool isIdeal() const { return m_ideal; } 13 | protected: 14 | bool m_ideal; 15 | }; 16 | 17 | }; 18 | 19 | #endif 20 | 21 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | */build/ 2 | */build_isolated/ 3 | */devel/ 4 | */devel_isolated/ 5 | */build_isolated/ 6 | */install_isolated/ 7 | */logs/ 8 | docs/ 9 | */.catkin_workspace 10 | */DISABLED 11 | *.bag 12 | *.pyc 13 | cmake-build-debug/ 14 | cmake-build-release/ 15 | *.idea 16 | *.swp 17 | *.kate-swp 18 | *.director 19 | *.DS_STORE 20 | *.catkin_tools 21 | *.bag 22 | *.bag.active 23 | *.stl 24 | *.directory 25 | build 26 | log 27 | !openslam_gmapping/include/gmapping/log/ 28 | !openslam_gmapping/log/ 29 | install 30 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/testlib: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | if [ -z "$1" ]; then 3 | echo "Syntax: rtestlib " 4 | exit 1 5 | fi 6 | 7 | exit 0 8 | 9 | FNAME=`mktemp rtestlibXXXXXX` 10 | echo "int main() { return 0; }" > $FNAME.cpp 11 | 12 | g++ $1 $FNAME.cpp -o $FNAME 13 | result=$? 14 | rm -f $FNAME.cpp $FNAME 15 | 16 | exit $result 17 | 18 | #if g++ $1 $FNAME.cpp -o $FNAME 19 | #then# 20 | # rm -f $FNAME.cpp $FNAME 21 | # exit 1 22 | #else 23 | # rm -f $FNAME.cpp $FNAME 24 | # exit 0 25 | #fi 26 | 27 | -------------------------------------------------------------------------------- /slam_gmapping/launch/slam_gmapping.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.substitutions import EnvironmentVariable 3 | import launch.actions 4 | import launch_ros.actions 5 | 6 | 7 | def generate_launch_description(): 8 | use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='true') 9 | return LaunchDescription([ 10 | launch_ros.actions.Node( 11 | package='slam_gmapping', node_executable='slam_gmapping', output='screen', parameters=[{'use_sim_time':use_sim_time}]), 12 | ]) 13 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSOR_H 2 | #define SENSOR_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping{ 8 | 9 | class Sensor{ 10 | public: 11 | Sensor(const std::string& name=""); 12 | virtual ~Sensor(); 13 | inline std::string getName() const {return m_name;} 14 | inline void setName(const std::string& name) {m_name=name;} 15 | protected: 16 | std::string m_name; 17 | }; 18 | 19 | typedef std::map SensorMap; 20 | 21 | }; //end namespace 22 | 23 | #endif 24 | 25 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/printpgm.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | using namespace std; 8 | ostream& printpgm(ostream& os, int xsize, int ysize, const double * const * matrix){ 9 | if (!os) 10 | return os; 11 | os<< "P5" << endl << xsize << endl << ysize << endl << 255 << endl; 12 | for (int y=ysize-1; y>=0; y--){ 13 | for (int x=0;x 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "configuration.h" 10 | 11 | namespace GMapping { 12 | 13 | class CarmenConfiguration: public Configuration, public std::map >{ 14 | public: 15 | virtual std::istream& load(std::istream& is); 16 | virtual SensorMap computeSensorMap() const; 17 | }; 18 | 19 | }; 20 | 21 | #endif 22 | 23 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/Makefile.subdirs: -------------------------------------------------------------------------------- 1 | export VERBOSE 2 | 3 | .PHONY: clean, all 4 | 5 | ifeq ($(VERBOSE), 0) 6 | QUIET=--no-print-directory 7 | endif 8 | 9 | all: 10 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; if ! $(MAKE) $(QUIET) -C $$subdir; then $(MESSAGE) "Compilation in $$subdir failed."; exit 1; fi; done 11 | 12 | clean: 13 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir clean; done 14 | 15 | dep: 16 | @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir dep; done 17 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/printmemusage.cpp: -------------------------------------------------------------------------------- 1 | #include "printmemusage.h" 2 | 3 | namespace GMapping{ 4 | 5 | using namespace std; 6 | void printmemusage(){ 7 | pid_t pid=getpid(); 8 | char procfilename[1000]; 9 | sprintf(procfilename, "/proc/%d/status", pid); 10 | ifstream is(procfilename); 11 | string line; 12 | while (is){ 13 | is >> line; 14 | if (line=="VmData:"){ 15 | is >> line; 16 | cerr << "#VmData:\t" << line << endl; 17 | } 18 | if (line=="VmSize:"){ 19 | is >> line; 20 | cerr << "#VmSize:\t" << line << endl; 21 | } 22 | 23 | } 24 | } 25 | 26 | }; 27 | 28 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/orientedboundingbox.h: -------------------------------------------------------------------------------- 1 | #ifndef ORIENTENDBOUNDINGBOX_H 2 | #define ORIENTENDBOUNDINGBOX_H 3 | 4 | 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace GMapping{ 11 | 12 | template 13 | class OrientedBoundingBox { 14 | 15 | public: 16 | OrientedBoundingBox(std::vector< point > p); 17 | double area(); 18 | 19 | protected: 20 | Point ul; 21 | Point ur; 22 | Point ll; 23 | Point lr; 24 | }; 25 | 26 | #include "orientedboundingbox.hxx" 27 | 28 | };// end namespace 29 | 30 | #endif 31 | 32 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORREADING_H 2 | #define SENSORREADING_H 3 | 4 | #include "sensor.h" 5 | namespace GMapping{ 6 | 7 | class SensorReading{ 8 | public: 9 | SensorReading(const Sensor* s, double time){ 10 | m_sensor=s; 11 | m_time=time; 12 | }; 13 | ~SensorReading(){}; 14 | inline double getTime() const {return m_time;} 15 | inline void setTime(double t) {m_time=t;} 16 | inline const Sensor* getSensor() const {return m_sensor;} 17 | protected: 18 | double m_time; 19 | const Sensor* m_sensor; 20 | 21 | }; 22 | 23 | }; //end namespace 24 | #endif 25 | 26 | 27 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/gridfastslam/motionmodel.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTIONMODEL_H 2 | #define MOTIONMODEL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace GMapping { 9 | 10 | struct MotionModel{ 11 | OrientedPoint drawFromMotion(const OrientedPoint& p, double linearMove, double angularMove) const; 12 | OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const; 13 | Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const; 14 | double srr, str, srt, stt; 15 | }; 16 | 17 | }; 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/gvalues.h: -------------------------------------------------------------------------------- 1 | #ifndef _GVALUES_H_ 2 | #define _GVALUES_H_ 3 | 4 | #define MAXDOUBLE 1e1000 5 | #ifdef LINUX 6 | #include 7 | #endif 8 | #ifdef MACOSX 9 | #include 10 | #include 11 | //#define isnan(x) (x==FP_NAN) 12 | #endif 13 | #ifdef _WIN32 14 | #include 15 | #ifndef __DRAND48_DEFINED__ 16 | #define __DRAND48_DEFINED__ 17 | inline double drand48() { return double(rand()) / RAND_MAX;} 18 | #endif 19 | #ifndef M_PI 20 | #define M_PI 3.1415926535897932384626433832795 21 | #endif 22 | #define round(d) (floor((d) + 0.5)) 23 | typedef unsigned int uint; 24 | #define isnan(x) (_isnan(x)) 25 | #endif 26 | 27 | #endif 28 | 29 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qpixmapdumper.cpp: -------------------------------------------------------------------------------- 1 | #include "qpixmapdumper.h" 2 | #include 3 | #include 4 | 5 | QPixmapDumper::QPixmapDumper(std::string p, int c){ 6 | format="PNG"; 7 | prefix=p; 8 | reset(); 9 | cycles=c; 10 | } 11 | 12 | void QPixmapDumper::reset(){ 13 | cycles=0; 14 | frame=0; 15 | counter=0; 16 | } 17 | 18 | #define filename_bufsize 1024 19 | 20 | bool QPixmapDumper::dump(const QPixmap& pixmap){ 21 | bool processed=false; 22 | if (!(counter%cycles)){ 23 | char buf[filename_bufsize]; 24 | sprintf(buf,"%s-%05d.%s",prefix.c_str(), frame, format.c_str()); 25 | QImage image=pixmap.convertToImage(); 26 | image.save(QString(buf), format.c_str(),0); 27 | frame ++; 28 | } 29 | counter++; 30 | return processed; 31 | } 32 | 33 | 34 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qmappainter.cpp: -------------------------------------------------------------------------------- 1 | #include "qmappainter.h" 2 | #include "moc_qmappainter.cpp" 3 | 4 | QMapPainter::QMapPainter( QWidget * parent, const char * name, WFlags f): 5 | QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){ 6 | m_pixmap=new QPixmap(size()); 7 | m_pixmap->fill(Qt::white); 8 | } 9 | 10 | void QMapPainter::resizeEvent(QResizeEvent * sizeev){ 11 | m_pixmap->resize(sizeev->size()); 12 | } 13 | 14 | QMapPainter::~QMapPainter(){ 15 | delete m_pixmap; 16 | } 17 | 18 | 19 | void QMapPainter::timerEvent(QTimerEvent * te) { 20 | if (te->timerId()==timer) 21 | update(); 22 | } 23 | 24 | void QMapPainter::start(int period){ 25 | timer=startTimer(period); 26 | } 27 | 28 | 29 | void QMapPainter::paintEvent ( QPaintEvent * ){ 30 | bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP); 31 | } 32 | 33 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # SLAM_GMAPPING 2 | 3 | SLAM(Simultaneous Localization and Mapping) is the computational problem of constructing or updating a map of an unknown environment while simultaneously keeping track of an agent's location within it. 4 | 5 | This contains package ```openslam_gmapping``` and ```slam_gmapping``` which is a ROS2 wrapper for OpenSlam's Gmapping. Using slam_gmapping, you can create a 2-D occupancy grid map (like a building floorplan) from laser and pose data collected by a mobile robot. 6 | 7 | ## Launch: 8 | 9 | ```bash 10 | ros2 launch slam_gmapping slam_gmapping.launch.py 11 | ``` 12 | 13 | The node slam_gmapping subscribes to sensor_msgs/LaserScan on ros2 topic ``scan``. It also expects appropriate TF to be available. 14 | 15 | It publishes the nav_msgs/OccupancyGrid on ``map``. 16 | 17 | Map Meta Data and Entropy is published on ``map_metadata`` and ``entropy`` respectively. 18 | -------------------------------------------------------------------------------- /openslam_gmapping/log/sensorlog.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORLOG_H 2 | #define SENSORLOG_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "configuration.h" 12 | 13 | namespace GMapping { 14 | 15 | class SensorLog : public std::list{ 16 | public: 17 | SensorLog(const SensorMap&); 18 | ~SensorLog(); 19 | std::istream& load(std::istream& is); 20 | OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const; 21 | protected: 22 | const SensorMap& m_sensorMap; 23 | OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const; 24 | RangeReading* parseRange(std::istream& is, const RangeSensor* ) const; 25 | }; 26 | 27 | }; 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /openslam_gmapping/gridfastslam/gfs2neff.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | 8 | int main(int argc, char**argv){ 9 | if (argc<3){ 10 | cout << "usage gfs2neff " << endl; 11 | return -1; 12 | } 13 | ifstream is(argv[1]); 14 | if (!is){ 15 | cout << "could read file "<< endl; 16 | return -1; 17 | } 18 | ofstream os(argv[2]); 19 | if (! os){ 20 | cout << "could write file "<< endl; 21 | return -1; 22 | } 23 | unsigned int frame=0; 24 | double neff=0; 25 | while(is){ 26 | char buf[8192]; 27 | is.getline(buf, 8192); 28 | istringstream lineStream(buf); 29 | string recordType; 30 | lineStream >> recordType; 31 | if (recordType=="FRAME"){ 32 | lineStream>> frame; 33 | } 34 | if (recordType=="NEFF"){ 35 | lineStream>> neff; 36 | os << frame << " " << neff << endl; 37 | } 38 | } 39 | os.close(); 40 | } 41 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGEREADING_H 2 | #define RANGEREADING_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace GMapping{ 9 | 10 | class RangeReading: public SensorReading, public std::vector{ 11 | public: 12 | RangeReading(const RangeSensor* rs, double time=0); 13 | RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0); 14 | virtual ~RangeReading(); 15 | inline const OrientedPoint& getPose() const {return m_pose;} 16 | inline void setPose(const OrientedPoint& pose) {m_pose=pose;} 17 | unsigned int rawView(double* v, double density=0.) const; 18 | std::vector cartesianForm(double maxRange=1e6) const; 19 | unsigned int activeBeams(double density=0.) const; 20 | protected: 21 | OrientedPoint m_pose; 22 | }; 23 | 24 | }; 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_range/rangesensor.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | namespace GMapping{ 4 | 5 | RangeSensor::RangeSensor(std::string name): Sensor(name){} 6 | 7 | RangeSensor::RangeSensor(std::string name, unsigned int beams_num, double res, const OrientedPoint& position, double span, double maxrange):Sensor(name), 8 | m_pose(position), m_beams(beams_num){ 9 | double angle=-.5*res*beams_num; 10 | for (unsigned int i=0; i 2 | #include "autoptr.h" 3 | 4 | using namespace std; 5 | using namespace GMapping; 6 | 7 | typedef autoptr DoubleAutoPtr; 8 | 9 | int main(int argc, const char * const * argv){ 10 | double* d1=new double(10.); 11 | double* d2=new double(20.); 12 | cout << "Construction test" << endl; 13 | DoubleAutoPtr pd1(d1); 14 | DoubleAutoPtr pd2(d2); 15 | cout << *pd1 << " " << *pd2 << endl; 16 | cout << "Copy Construction" << endl; 17 | DoubleAutoPtr pd3(pd1); 18 | cout << *pd3 << endl; 19 | cout << "assignment" << endl; 20 | pd3=pd2; 21 | pd1=pd2; 22 | cout << *pd1 << " " << *pd2 << " " << *pd3 << " " << endl; 23 | cout << "conversion operator" << endl; 24 | DoubleAutoPtr nullPtr; 25 | cout << "conversion operator " << !(nullPtr) << endl; 26 | cout << "neg conversion operator " << nullPtr << endl; 27 | cout << "conversion operator " << (int)pd1 << endl; 28 | cout << "neg conversion operator " << !(pd1) << endl; 29 | } 30 | -------------------------------------------------------------------------------- /openslam_gmapping/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package openslam_gmapping 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.1.2 (2016-04-23) 6 | ------------------ 7 | * better Windows compilation 8 | This is taken from `#9 `_ which can now be closed. 9 | * fix a few more graphics stuff for Qt5 10 | * get GUI back in shape for those interested 11 | * use srand instead of srand48 12 | srand48 is non-standard and we are using a seed that is an 13 | unsigned int so we might as well use srand 14 | * Contributors: Vincent Rabaud 15 | 16 | 0.1.1 (2015-06-25) 17 | ------------------ 18 | * fix cppcheck warnings 19 | * License from BSD to CC 20 | * Contributors: Isaac IY Saito, Vincent Rabaud 21 | 22 | 0.1.0 (2013-06-28 17:33:53 -0700) 23 | --------------------------------- 24 | - Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ 25 | - Catkinized and prepared for release into the ROS ecosystem 26 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/log/sensorlog.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORLOG_H 2 | #define SENSORLOG_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "configuration.h" 12 | 13 | namespace GMapping { 14 | 15 | class SensorLog : public std::list{ 16 | public: 17 | SensorLog(const SensorMap&); 18 | ~SensorLog(); 19 | std::istream& load(std::istream& is); 20 | OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const; 21 | protected: 22 | const SensorMap& m_sensorMap; 23 | OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const; 24 | RangeReading* parseRange(std::istream& is, const RangeSensor* ) const; 25 | }; 26 | 27 | }; 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qnavigatorwidget.h: -------------------------------------------------------------------------------- 1 | #ifndef _QNAVIGATOR_WIDGET_H 2 | #define _QNAVIGATOR_WIDGET_H 3 | 4 | #include "qmappainter.h" 5 | #include "qpixmapdumper.h" 6 | #include 7 | #include 8 | 9 | class QNavigatorWidget : public QMapPainter{ 10 | public: 11 | QNavigatorWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); 12 | virtual ~QNavigatorWidget(); 13 | std::list trajectoryPoints; 14 | bool repositionRobot; 15 | GMapping::IntPoint robotPose; 16 | double robotHeading; 17 | bool confirmLocalization; 18 | bool enableMotion; 19 | bool startWalker; 20 | bool startGlobalLocalization; 21 | bool trajectorySent; 22 | bool goHome; 23 | bool wantsQuit; 24 | bool writeImages; 25 | QPixmapDumper dumper; 26 | bool drawRobot; 27 | 28 | protected: 29 | virtual void paintEvent ( QPaintEvent *paintevent ); 30 | virtual void mousePressEvent ( QMouseEvent * e ); 31 | virtual void keyPressEvent ( QKeyEvent * e ); 32 | }; 33 | 34 | #endif 35 | 36 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qslamandnavwidget.h: -------------------------------------------------------------------------------- 1 | #ifndef _QSLAMANDNAV_WIDGET_H 2 | #define _QSLAMANDNAV_WIDGET_H 3 | 4 | #include "qmappainter.h" 5 | #include "qpixmapdumper.h" 6 | #include 7 | #include 8 | 9 | class QSLAMandNavWidget : public QMapPainter{ 10 | public: 11 | QSLAMandNavWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); 12 | virtual ~QSLAMandNavWidget(); 13 | std::list trajectoryPoints; 14 | GMapping::IntPoint robotPose; 15 | double robotHeading; 16 | 17 | bool slamRestart; 18 | bool slamFinished; 19 | bool enableMotion; 20 | bool startWalker; 21 | bool trajectorySent; 22 | bool goHome; 23 | bool wantsQuit; 24 | bool printHelp; 25 | bool saveGoalPoints; 26 | bool writeImages; 27 | bool drawRobot; 28 | QPixmapDumper dumper; 29 | 30 | 31 | protected: 32 | virtual void paintEvent ( QPaintEvent *paintevent ); 33 | virtual void mousePressEvent ( QMouseEvent * e ); 34 | virtual void keyPressEvent ( QKeyEvent * e ); 35 | }; 36 | 37 | #endif 38 | 39 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h: -------------------------------------------------------------------------------- 1 | #ifndef ODOMETRYREADING_H 2 | #define ODOMETRYREADING_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include "odometrysensor.h" 8 | 9 | namespace GMapping{ 10 | 11 | class OdometryReading: public SensorReading{ 12 | public: 13 | OdometryReading(const OdometrySensor* odo, double time=0); 14 | inline const OrientedPoint& getPose() const {return m_pose;} 15 | inline const OrientedPoint& getSpeed() const {return m_speed;} 16 | inline const OrientedPoint& getAcceleration() const {return m_acceleration;} 17 | inline void setPose(const OrientedPoint& pose) {m_pose=pose;} 18 | inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;} 19 | inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;} 20 | 21 | protected: 22 | OrientedPoint m_pose; 23 | OrientedPoint m_speed; 24 | OrientedPoint m_acceleration; 25 | }; 26 | 27 | }; 28 | #endif 29 | 30 | -------------------------------------------------------------------------------- /openslam_gmapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(openslam_gmapping) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-O3 -funroll-loops) 16 | endif() 17 | 18 | add_compile_options(-fPIC) 19 | find_package(ament_cmake REQUIRED) 20 | 21 | include_directories(include) 22 | 23 | add_subdirectory(gridfastslam) 24 | add_subdirectory(scanmatcher) 25 | add_subdirectory(sensor) 26 | add_subdirectory(utils) 27 | 28 | ament_export_libraries(gridfastslam) 29 | ament_export_libraries(scanmatcher) 30 | ament_export_libraries(sensor_base) 31 | ament_export_libraries(sensor_odometry) 32 | ament_export_libraries(sensor_range) 33 | ament_export_libraries(utils) 34 | ament_export_include_directories(include) 35 | 36 | install(DIRECTORY include/ 37 | DESTINATION include/) 38 | 39 | ament_package() 40 | -------------------------------------------------------------------------------- /openslam_gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | openslam_gmapping 5 | 0.1.2 6 | ROS-ified version of gmapping SLAM. Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ 7 | Vincent Rabaud 8 | CreativeCommons-by-nc-sa-2.0 9 | 10 | http://openslam.org/gmapping 11 | https://github.com/ros-perception/openslam_gmapping 12 | https://github.com/ros-perception/openslam_gmapping/issues 13 | 14 | Giorgio Grisetti 15 | Cyrill Stachniss 16 | Wolfram Burgard 17 | 18 | ament_lint_auto 19 | ament_lint_common 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(Qt5Widgets) 2 | if(Qt5Widgets_FOUND) 3 | else() 4 | find_package(Qt4) 5 | include(${QT_USE_FILE}) 6 | endif() 7 | 8 | include_directories(../include/gmapping ../ ../include/gmapping/log/) 9 | 10 | #add_executable(gfs_nogui gfs_nogui.cpp) 11 | 12 | add_executable(gfs_simplegui gfs_simplegui.cpp gsp_thread.cpp) 13 | if(Qt5Widgets_FOUND) 14 | target_link_libraries(gfs_simplegui gridfastslam Qt5::Widgets) 15 | else() 16 | target_link_libraries(gfs_simplegui gridfastslam ${QT_LIBRARIES}) 17 | endif() 18 | 19 | #add_executable(gfs2img gfs2img.cpp gridfastslam) 20 | 21 | #-include ../global.mk 22 | 23 | #OBJS= gsp_thread.o qparticleviewer.o qgraphpainter.o qmappainter.o 24 | 25 | #APPS= gfs_nogui gfs_simplegui gfs2img 26 | #LDFLAGS+= $(QT_LIB) $(KDE_LIB) -lgridfastslam -lscanmatcher -llog -lsensor_range -lsensor_odometry -lsensor_base -lconfigfile -lutils -lpthread 27 | 28 | #ifeq ($(CARMENSUPPORT),1) 29 | #LDFLAGS+= -lcarmenwrapper 30 | #endif 31 | 32 | #CPPFLAGS+= -I../sensor $(QT_INCLUDE) $(KDE_INCLUDE) -I$(CARMEN_HOME)/include 33 | 34 | 35 | #-include ../build_tools/Makefile.generic-shared-object 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h: -------------------------------------------------------------------------------- 1 | #ifndef RANGESENSOR_H 2 | #define RANGESENSOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace GMapping{ 9 | 10 | class RangeSensor: public Sensor{ 11 | friend class Configuration; 12 | friend class CarmenConfiguration; 13 | friend class CarmenWrapper; 14 | public: 15 | struct Beam{ 16 | OrientedPoint pose; //pose relative to the center of the sensor 17 | double span; //spam=0 indicates a line-like beam 18 | double maxRange; //maximum range of the sensor 19 | double s,c; //sinus and cosinus of the beam (optimization); 20 | }; 21 | RangeSensor(std::string name); 22 | RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0); 23 | inline const std::vector& beams() const {return m_beams;} 24 | inline std::vector& beams() {return m_beams;} 25 | inline OrientedPoint getPose() const {return m_pose;} 26 | void updateBeamsLookup(); 27 | bool newFormat; 28 | protected: 29 | OrientedPoint m_pose; 30 | std::vector m_beams; 31 | }; 32 | 33 | }; 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/movement.h: -------------------------------------------------------------------------------- 1 | #ifndef FSRMOVEMENT_H 2 | #define FSRMOVEMENT_H 3 | 4 | #include 5 | 6 | namespace GMapping { 7 | 8 | /** fsr-movement (forward, sideward, rotate) **/ 9 | class FSRMovement { 10 | public: 11 | FSRMovement(double f=0.0, double s=0.0, double r=0.0); 12 | FSRMovement(const FSRMovement& src); 13 | FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2); 14 | FSRMovement(const FSRMovement& move1, const FSRMovement& move2); 15 | 16 | 17 | void normalize(); 18 | void invert(); 19 | void compose(const FSRMovement& move2); 20 | OrientedPoint move(const OrientedPoint& pt) const; 21 | 22 | 23 | /* static members */ 24 | 25 | static OrientedPoint movePoint(const OrientedPoint& pt, const FSRMovement& move1); 26 | 27 | static FSRMovement composeMoves(const FSRMovement& move1, 28 | const FSRMovement& move2); 29 | 30 | static FSRMovement moveBetweenPoints(const OrientedPoint& pt1, 31 | const OrientedPoint& pt2); 32 | 33 | static FSRMovement invertMove(const FSRMovement& move1); 34 | 35 | static OrientedPoint frameTransformation(const OrientedPoint& reference_pt_frame1, 36 | const OrientedPoint& reference_pt_frame2, 37 | const OrientedPoint& pt_frame1); 38 | 39 | public: 40 | double f; 41 | double s; 42 | double r; 43 | }; 44 | } 45 | #endif 46 | -------------------------------------------------------------------------------- /openslam_gmapping/scanmatcher/lumiles.h: -------------------------------------------------------------------------------- 1 | #ifndef LUMILESPROCESSOR 2 | #define LUMILESPROCESSOR 3 | 4 | namespace GMapping{ 5 | 6 | class LuMilesProcessor{ 7 | typedef std:vector PointVector; 8 | static OrientedPoint step(const PointVector& src, const PointVector& dest); 9 | }; 10 | 11 | OrientedPoint LuMilesProcessors::step(const PointVector& src, const PointVector& dest){ 12 | assert(src.size()==dest.size()); 13 | unsigned int size=dest.size(); 14 | double smx=0, smy=0, dmx=0, dmy=0; 15 | for (PointVector::const_iterator it=src.begin(); it!=src.end(); it++){ 16 | smx+=it->x; 17 | smy+=it->y; 18 | } 19 | smx/=src.size(); 20 | smy/=src.size(); 21 | 22 | for (PointVector::const_iterator it=dest.begin(); it!=dest.end(); it++){ 23 | dmx+=it->x; 24 | dmy+=it->y; 25 | } 26 | dmx/=src.size(); 27 | dmy/=src.size(); 28 | 29 | double sxx=0, sxy=0; 30 | double syx=0, syy=0; 31 | for (unsigned int i=0; i 25 | #include "qparticleviewer.h" 26 | 27 | int main (int argc, char ** argv){ 28 | QApplication app(argc, argv); 29 | QParticleViewer * pviewer=new QParticleViewer(0); 30 | app.setMainWidget(pviewer); 31 | pviewer->show(); 32 | FILE* f=fopen(argv[1], "r"); 33 | if (!f) 34 | return -1; 35 | QTextIStream is(f); 36 | pviewer->tis=&is; 37 | pviewer->start(10); 38 | return app.exec(); 39 | std::cout << "DONE: " << argv[1] < 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | using namespace std; 9 | using namespace GMapping; 10 | 11 | int main(int argc, char ** argv){ 12 | if (argc<2){ 13 | cout << "usage log_test " << endl; 14 | exit (-1); 15 | } 16 | ifstream is(argv[1]); 17 | if (! is){ 18 | cout << "no file " << argv[1] << " found" << endl; 19 | exit (-1); 20 | } 21 | CarmenConfiguration conf; 22 | conf.load(is); 23 | 24 | SensorMap m=conf.computeSensorMap(); 25 | 26 | //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) 27 | // cout << it->first << " " << it->second->getName() << endl; 28 | 29 | SensorLog log(m); 30 | is.close(); 31 | 32 | ifstream ls(argv[1]); 33 | log.load(ls); 34 | ls.close(); 35 | cerr << "log size" << log.size() << endl; 36 | for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ 37 | RangeReading* rr=dynamic_cast(*it); 38 | if (rr){ 39 | //cerr << rr->getSensor()->getName() << " "; 40 | //cerr << rr->size()<< " "; 41 | //for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ 42 | // cerr << *it << " "; 43 | //} 44 | cout<< rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime() << endl; 45 | } 46 | } 47 | } 48 | -------------------------------------------------------------------------------- /slam_gmapping/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | slam_gmapping 5 | 0.0.0 6 | This package contains a ROS2 Crystal Clemmys wrapper for OpenSlam's Gmapping. 7 | The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), 8 | as a ROS node called slam_gmapping. Using slam_gmapping, you can create a 2-D occupancy 9 | grid map (like a building floorplan) from laser and pose data collected by a mobile robot. 10 | 11 | Brian Gerkey 12 | Vincent Rabaud 13 | CreativeCommons-by-nc-sa-2.0 14 | 15 | http://ros.org/wiki/gmapping 16 | 17 | ament_cmake 18 | 19 | std_msgs 20 | nav_msgs 21 | tf2 22 | tf2_ros 23 | tf2_geometry_msgs 24 | message_filters 25 | rclcpp 26 | sensor_msgs 27 | visualization_msgs 28 | openslam_gmapping 29 | 30 | ament_lint_auto 31 | ament_lint_common 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /openslam_gmapping/log/sensorstream.h: -------------------------------------------------------------------------------- 1 | #ifndef SENSORSTREAM_H 2 | #define SENSORSTREAM_H 3 | 4 | #include 5 | #include "sensorlog.h" 6 | 7 | namespace GMapping { 8 | class SensorStream{ 9 | public: 10 | SensorStream(const SensorMap& sensorMap); 11 | virtual ~SensorStream(); 12 | virtual operator bool() const=0; 13 | virtual bool rewind() = 0 ; 14 | virtual SensorStream& operator >>(const SensorReading*&) = 0; 15 | inline const SensorMap& getSensorMap() const {return m_sensorMap; } 16 | protected: 17 | const SensorMap& m_sensorMap; 18 | static SensorReading* parseReading(std::istream& is, const SensorMap& smap); 19 | static OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ); 20 | static RangeReading* parseRange(std::istream& is, const RangeSensor* ); 21 | }; 22 | 23 | class InputSensorStream: public SensorStream{ 24 | public: 25 | InputSensorStream(const SensorMap& sensorMap, std::istream& is); 26 | virtual operator bool() const; 27 | virtual bool rewind(); 28 | virtual SensorStream& operator >>(const SensorReading*&); 29 | 30 | //virtual SensorStream& operator >>(SensorLog*& log); 31 | protected: 32 | std::istream& m_inputStream; 33 | }; 34 | 35 | class LogSensorStream: public SensorStream{ 36 | public: 37 | LogSensorStream(const SensorMap& sensorMap, const SensorLog* log); 38 | virtual operator bool() const; 39 | virtual bool rewind(); 40 | virtual SensorStream& operator >>(const SensorReading*&); 41 | protected: 42 | const SensorLog* m_log; 43 | SensorLog::const_iterator m_cursor; 44 | }; 45 | 46 | }; 47 | #endif 48 | -------------------------------------------------------------------------------- /openslam_gmapping/gridfastslam/gfs2log.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "gfsreader.h" 9 | 10 | #define MAX_LINE_LENGHT (1000000) 11 | 12 | using namespace std; 13 | using namespace GMapping; 14 | using namespace GMapping::GFSReader; 15 | 16 | int main (int argc, const char * const * argv){ 17 | if (argc<3){ 18 | cout << "usage gfs2log [-err] [-neff] [-part] [-odom] " << endl; 19 | cout << " -odom : dump raw odometry in ODOM message instead of inpolated corrected one" << endl; 20 | return -1; 21 | } 22 | bool err=0; 23 | bool neff=0; 24 | bool part=0; 25 | bool odom=0; 26 | // int particle_num; 27 | unsigned int c=1; 28 | if (!strcmp(argv[c],"-err")){ 29 | err=true; 30 | c++; 31 | } 32 | if (!strcmp(argv[c],"-neff")){ 33 | neff=true; 34 | c++; 35 | } 36 | if (!strcmp(argv[c],"-part")){ 37 | part=true; 38 | c++; 39 | } 40 | if (!strcmp(argv[c],"-odom")){ 41 | odom=true; 42 | c++; 43 | } 44 | ifstream is(argv[c]); 45 | if (!is){ 46 | cout << "could read file "<< endl; 47 | return -1; 48 | } 49 | c++; 50 | RecordList rl; 51 | rl.read(is); 52 | unsigned int bestidx=rl.getBestIdx(); 53 | cout << endl << "best index = " << bestidx<< endl; 54 | ofstream os(argv[c]); 55 | if (! os){ 56 | cout << "could write file "<< endl; 57 | return -1; 58 | } 59 | rl.printPath(os,bestidx,err,odom); 60 | if(part) 61 | rl.printLastParticles(os); 62 | os.close(); 63 | return 0; 64 | } 65 | -------------------------------------------------------------------------------- /openslam_gmapping/log/scanstudio2carmen.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #define MAXLINELENGHT (10240) 9 | #define MAXREADINGS (10240) 10 | 11 | using namespace std; 12 | using namespace GMapping; 13 | 14 | int main (int argc, char** argv){ 15 | if (argc<3){ 16 | cout << "usage scanstudio2carmen scanfilename carmenfilename" << endl; 17 | exit(1); 18 | } 19 | ifstream is(argv[1]); 20 | if (!is){ 21 | cout << "cannopt open file" << argv[1] << endl; 22 | exit(1); 23 | } 24 | 25 | ofstream os(argv[2]); 26 | 27 | double readings[MAXREADINGS]; 28 | OrientedPoint pose; 29 | int nbeams; 30 | while (is){ 31 | char buf[MAXLINELENGHT]; 32 | is.getline(buf,MAXLINELENGHT); 33 | istringstream st(buf); 34 | string token; 35 | st>>token; 36 | if (token=="RobotPos:"){ 37 | st >> pose.x >> pose.y >> pose.theta; 38 | pose.x/=1000; 39 | pose.y/=1000; 40 | } else 41 | if (token=="NumPoints:"){ 42 | st >> nbeams; 43 | assert(nbeams> angle; 50 | is >> readings[c]; 51 | readings[c]/=1000; 52 | c++; 53 | } 54 | if (c==nbeams) 55 | os << "FLASER " << nbeams << " "; 56 | c=0; 57 | while (c 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | 8 | using namespace std; 9 | using namespace GMapping; 10 | 11 | int main(int argc, char ** argv){ 12 | if (argc<2){ 13 | cerr << "usage "< " << endl; 14 | cerr << "or "< for standard output" << endl; 15 | exit (-1); 16 | } 17 | ifstream is(argv[1]); 18 | if (! is){ 19 | cerr << "no file " << argv[1] << " found" << endl; 20 | exit (-1); 21 | } 22 | ostream *os; 23 | if (argc<3) 24 | os=&cout; 25 | else{ 26 | os=new ofstream(argv[2]); 27 | if (! os){ 28 | cerr << "no file " << argv[1] << " found" << endl; 29 | exit (-1); 30 | } 31 | } 32 | CarmenConfiguration conf; 33 | conf.load(is); 34 | 35 | SensorMap m=conf.computeSensorMap(); 36 | 37 | //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) 38 | // cout << it->first << " " << it->second->getName() << endl; 39 | 40 | SensorLog log(m); 41 | is.close(); 42 | 43 | ifstream ls(argv[1]); 44 | log.load(ls); 45 | ls.close(); 46 | cerr << "log size" << log.size() << endl; 47 | for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ 48 | RangeReading* rr=dynamic_cast(*it); 49 | if (rr){ 50 | *os << rr->getSensor()->getName() << " "; 51 | *os << rr->size()<< " "; 52 | for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ 53 | *os << (*it)*0.001 << " "; 54 | } 55 | *os<< rr->getPose().x*0.001 << " " << rr->getPose().y*0.001 << " " << rr->getPose().theta << endl; 56 | } 57 | } 58 | } 59 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/scanmatcher/smmap.h: -------------------------------------------------------------------------------- 1 | #ifndef SMMAP_H 2 | #define SMMAP_H 3 | #include 4 | #include 5 | #include 6 | #define SIGHT_INC 1 7 | 8 | namespace GMapping { 9 | 10 | struct PointAccumulator{ 11 | typedef point FloatPoint; 12 | /* before 13 | PointAccumulator(int i=-1): acc(0,0), n(0), visits(0){assert(i==-1);} 14 | */ 15 | /*after begin*/ 16 | PointAccumulator(): acc(0,0), n(0), visits(0){} 17 | PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);} 18 | /*after end*/ 19 | inline void update(bool value, const Point& p=Point(0,0)); 20 | inline Point mean() const {return 1./n*Point(acc.x, acc.y);} 21 | inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; } 22 | inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; } 23 | static const PointAccumulator& Unknown(); 24 | static PointAccumulator* unknown_ptr; 25 | FloatPoint acc; 26 | int n, visits; 27 | inline double entropy() const; 28 | }; 29 | 30 | void PointAccumulator::update(bool value, const Point& p){ 31 | if (value) { 32 | acc.x+= static_cast(p.x); 33 | acc.y+= static_cast(p.y); 34 | n++; 35 | visits+=SIGHT_INC; 36 | } else 37 | visits++; 38 | } 39 | 40 | double PointAccumulator::entropy() const{ 41 | if (!visits) 42 | return -log(.5); 43 | if (n==visits || n==0) 44 | return 0; 45 | double x=(double)n*SIGHT_INC/(double)visits; 46 | return -( x*log(x)+ (1-x)*log(1-x) ); 47 | } 48 | 49 | 50 | typedef Map > ScanMatcherMap; 51 | 52 | }; 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /openslam_gmapping/gridfastslam/gfs2stream.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include "gfsreader.h" 9 | #define MAX_LINE_LENGHT (1000000) 10 | 11 | using namespace std; 12 | using namespace GMapping; 13 | using namespace GMapping::GFSReader; 14 | 15 | computeBoundingBox() 16 | 17 | 18 | int main (unsigned int argc, const char * const * argv) 19 | double delta = 0.1; 20 | double skip = 2; 21 | double rotate = 0; 22 | double maxrange = 0; 23 | 24 | if (argc<3){ 25 | cout << "usage gfs2stream [-step Number] " << endl; 26 | return -1; 27 | } 28 | 29 | 30 | CMD_PARSE_BEGIN(1,argc-2); 31 | 32 | CMD_PARSE_END; 33 | 34 | if (argc<3){ 35 | cout << "usage gfs2stream [-step Number] " << endl; 36 | return -1; 37 | } 38 | bool err=0; 39 | bool neff=0; 40 | bool part=0; 41 | unsigned int c=1; 42 | if (!strcmp(argv[c],"-err")){ 43 | err=true; 44 | c++; 45 | } 46 | if (!strcmp(argv[c],"-neff")){ 47 | neff=true; 48 | c++; 49 | } 50 | if (!strcmp(argv[c],"-part")){ 51 | part=true; 52 | c++; 53 | } 54 | ifstream is(argv[c]); 55 | if (!is){ 56 | cout << "could read file "<< endl; 57 | return -1; 58 | } 59 | c++; 60 | RecordList rl; 61 | rl.read(is); 62 | unsigned int bestidx=rl.getBestIdx(); 63 | cout << endl << "best index = " << bestidx<< endl; 64 | ofstream os(argv[c]); 65 | if (! os){ 66 | cout << "could write file "<< endl; 67 | return -1; 68 | } 69 | rl.printPath(os,bestidx,err); 70 | if(part) 71 | rl.printLastParticles(os); 72 | os.close(); 73 | return 0; 74 | } 75 | -------------------------------------------------------------------------------- /openslam_gmapping/grid/graphmap.cpp: -------------------------------------------------------------------------------- 1 | #ifndef GRAPHMAP_H 2 | #define GRAPHMAP_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace GMapping { 9 | 10 | class RasterMap; 11 | 12 | struct GraphMapPatch{ 13 | typedef typename std::list PointList; 14 | /**Renders the map relatively to the center of the patch*/ 15 | //void render(RenderMap rmap); 16 | /**returns the lower left corner of the patch, relative to the center*/ 17 | //Point minBoundary() const; 18 | /**returns the upper right corner of the patch, relative to the center*/ 19 | //Point maxBoundary() const; // 20 | 21 | OrientedPoint center; 22 | PointList m_points; 23 | }; 24 | 25 | struct Covariance3{ 26 | double sxx, sxy, sxt, syy, syt ,stt; 27 | }; 28 | 29 | struct GraphMapEdge{ 30 | Covariance3 covariance; 31 | GraphMapPatch* first, *second; 32 | inline operator double() const{ 33 | return sqrt((first->center-second->center)*(first->center-second->center)); 34 | } 35 | }; 36 | 37 | 38 | struct GraphPatchGraph: public Graph{ 39 | void addEdge(Vertex* v1, Vertex* v2, const Covariance3& covariance); 40 | }; 41 | 42 | void GraphPatchGraph::addEdge(GraphPatchGraph::Vertex* v1, GraphPatchGraph::VertexVertex* v2, 43 | const Covariance3& cov){ 44 | GraphMapEdge gme; 45 | gme.covariance=cov; 46 | gme.first=v1; 47 | gme.second=v2; 48 | return Graph::addEdge(v1,v2,gme); 49 | } 50 | 51 | struct GraphPatchDirectoryCell: public std::set { 52 | GraphPatchDirectoryCell(double); 53 | }; 54 | 55 | typedef Map, Array2D::set > 56 | 57 | }; 58 | 59 | #endif -------------------------------------------------------------------------------- /openslam_gmapping/gridfastslam/gfs2stat.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "gfsreader.h" 6 | 7 | using namespace std; 8 | using namespace GMapping; 9 | using namespace GMapping::GFSReader; 10 | 11 | 12 | int main(int argc, char ** argv){ 13 | if (argc<2){ 14 | cout << "usage gfs2stat " << endl; 15 | return 0; 16 | } 17 | ifstream is(argv[1]); 18 | if (!is){ 19 | cout << "no file found: " << argv[1] << endl; 20 | return 0; 21 | } 22 | ofstream os(argv[2]); 23 | if (!os){ 24 | cout << "cannot open file: " << argv[1] << endl; 25 | return 0; 26 | } 27 | cout << "loading... "<< flush; 28 | RecordList rl; 29 | rl.read(is); 30 | cout << " done" << endl; 31 | int count=-1; 32 | for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){ 33 | 34 | count++; 35 | const ScanMatchRecord* rec=dynamic_cast(*it); 36 | if (!rec) 37 | continue; 38 | Gaussian3 gaussian; 39 | /* 40 | vector nweights; 41 | cout << "N"<< flush; 42 | back_insert_iterator< vector > out(nweights); 43 | toNormalForm(out,rec->weights.begin(), rec->weights.end()); 44 | cout << "G"<< flush; 45 | gaussian.computeFromSamples(rec->poses, nweights); 46 | */ 47 | gaussian.computeFromSamples(rec->poses); 48 | cout << "E"<< flush; 49 | os << count <<" "; 50 | os << gaussian.mean.x <<" "; 51 | os << gaussian.mean.y <<" "; 52 | os << gaussian.mean.theta <<" "; 53 | os << gaussian.covariance.eval[0] <<" "; 54 | os << gaussian.covariance.eval[1] <<" "; 55 | os << gaussian.covariance.eval[2] < 5 | #include 6 | #include 7 | //#include 8 | #include 9 | 10 | namespace GMapping { 11 | 12 | class ScanMatcherProcessor{ 13 | public: 14 | ScanMatcherProcessor(const ScanMatcherMap& m); 15 | ScanMatcherProcessor (double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta); 16 | virtual ~ScanMatcherProcessor (); 17 | virtual void processScan(const RangeReading & reading); 18 | void setSensorMap(const SensorMap& smap, std::string sensorName="FLASER"); 19 | void init(); 20 | void setMatchingParameters 21 | (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance=false); 22 | void setRegistrationParameters(double regScore, double critScore); 23 | OrientedPoint getPose() const; 24 | inline const ScanMatcherMap& getMap() const {return m_map;} 25 | inline ScanMatcher& matcher() {return m_matcher;} 26 | inline void setmaxMove(double mmove){m_maxMove=mmove;} 27 | bool useICP; 28 | protected: 29 | ScanMatcher m_matcher; 30 | bool m_computeCovariance; 31 | bool m_first; 32 | SensorMap m_sensorMap; 33 | double m_regScore, m_critScore; 34 | unsigned int m_beams; 35 | double m_maxMove; 36 | //state 37 | ScanMatcherMap m_map; 38 | OrientedPoint m_pose; 39 | OrientedPoint m_odoPose; 40 | int m_count; 41 | //gsl_eigen_symmv_workspace * m_eigenspace; 42 | }; 43 | 44 | }; 45 | 46 | #endif 47 | 48 | 49 | -------------------------------------------------------------------------------- /openslam_gmapping/grid/map_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "map.h" 3 | #include "harray2d.h" 4 | 5 | using namespace std; 6 | using namespace GMapping; 7 | 8 | struct SimpleCell{ 9 | int value; 10 | SimpleCell(int v=0){value=v;} 11 | static const SimpleCell& Unknown(); 12 | static SimpleCell* address; 13 | }; 14 | 15 | SimpleCell* SimpleCell::address=0; 16 | 17 | const SimpleCell& SimpleCell::Unknown(){ 18 | if (address) 19 | return *address; 20 | address=new SimpleCell(-1); 21 | return *address; 22 | } 23 | 24 | typedef Map< SimpleCell, HierarchicalArray2D > CGrid; 25 | 26 | int main (int argc, char ** argv){ 27 | CGrid g1(Point(0.,0.), 200, 200, 0.1); 28 | CGrid g2(Point(10.,10.), 200, 200, 0.1); 29 | { 30 | HierarchicalArray2D::PointSet ps; 31 | IntPoint pp=g1.world2map(Point(5.1,5.1)); 32 | cout << pp.x << " " << pp.y << endl; 33 | ps.insert(pp); 34 | g1.storage().setActiveArea(ps,false); 35 | g1.storage().allocActiveArea(); 36 | g1.cell(Point(5.1,5.1)).value=5; 37 | cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl; 38 | g1.resize(-150, -150, 150, 150); 39 | cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl; 40 | CGrid g3(g1); 41 | g1=g2; 42 | } 43 | cerr << "copy and modify test" << endl; 44 | CGrid *ap,* gp1=new CGrid(Point(0,0), 200, 200, 0.1); 45 | CGrid* gp0=new CGrid(*gp1); 46 | for (int i=1; i<10; i++){ 47 | ap=new CGrid(*gp1); 48 | delete gp1; 49 | gp1=gp0; 50 | gp0=ap; 51 | IntPoint pp=gp0->world2map(Point(5.1,5.1)); 52 | HierarchicalArray2D::PointSet ps; 53 | ps.insert(pp); 54 | gp1->storage().setActiveArea(ps,false); 55 | gp1->storage().allocActiveArea(); 56 | gp1->cell(Point(5.1,5.1)).value=i; 57 | cout << "cell value" << (int) gp1->cell(Point(5.1,5.1)).value << endl; 58 | } 59 | delete gp0; 60 | delete gp1; 61 | return 0; 62 | } 63 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/macro_params.h: -------------------------------------------------------------------------------- 1 | #ifndef MACRO_PARAMS_H 2 | #define MACRO_PARAMS_H 3 | 4 | #define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)\ 5 | qualifier: type m_##name;\ 6 | getqualifier: inline type get##name() const {return m_##name;}\ 7 | setqualifier: inline void set##name(type name) {m_##name=name;} 8 | 9 | #define PARAM_SET(type, name, qualifier, setqualifier)\ 10 | qualifier: type m_##name;\ 11 | setqualifier: inline void set##name(type name) {m_##name=name;} 12 | 13 | #define PARAM_GET(type, name, qualifier, getqualifier)\ 14 | qualifier: type m_##name;\ 15 | getqualifier: inline type get##name() const {return m_##name;} 16 | 17 | #define MEMBER_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\ 18 | getqualifier: inline type get##name() const {return member.get##name();}\ 19 | setqualifier: inline void set##name(type name) { member.set##name(name);} 20 | 21 | #define MEMBER_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\ 22 | setqualifier: inline void set##name(type name) { member.set##name(name);} 23 | 24 | #define MEMBER_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\ 25 | getqualifier: inline type get##name() const {return member.get##name();} 26 | 27 | #define STRUCT_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\ 28 | getqualifier: inline type get##name() const {return member.name;}\ 29 | setqualifier: inline void set##name(type name) {member.name=name;} 30 | 31 | #define STRUCT_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\ 32 | setqualifier: inline void set##name(type name) {member.name=name;} 33 | 34 | #define STRUCT_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\ 35 | getqualifier: inline type get##name() const {return member.name;}\ 36 | 37 | #define convertStringArgument(var,val,buf) if (!strcmp(buf,#val)) var=val 38 | #endif 39 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qmappainter.h: -------------------------------------------------------------------------------- 1 | #ifndef QMAPPAINTER_H 2 | #define QMAPPAINTER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | class QMapPainter : public QWidget{ 12 | public: 13 | QMapPainter( QWidget * parent = 0, const char * name = 0, WFlags f = 0); 14 | virtual ~QMapPainter(); 15 | public: 16 | template < typename Cell > 17 | void setPixmap(unsigned int xsize, unsigned int ysize, Cell** values); 18 | template < typename Iterator > 19 | void drawPoints(const Iterator& begin, const Iterator& end, unsigned char r, unsigned char g, unsigned char b); 20 | void start(int period); 21 | protected: 22 | virtual void timerEvent(QTimerEvent * te); 23 | virtual void resizeEvent(QResizeEvent *); 24 | int timer; 25 | virtual void paintEvent ( QPaintEvent *paintevent ); 26 | QPixmap * m_pixmap; 27 | }; 28 | 29 | template 30 | void QMapPainter::setPixmap(unsigned int xsize, unsigned int ysize, Cell** values){ 31 | QSize s(xsize, ysize); 32 | m_pixmap->resize(s); 33 | m_pixmap->fill(Qt::white); 34 | QPainter painter(m_pixmap); 35 | for (unsigned int x=0; x<(unsigned int)xsize; x++) 36 | for (unsigned int y=0; y<(unsigned int)ysize; y++){ 37 | double v=(double) values[x][y]; 38 | 39 | if (v>=0){ 40 | unsigned int grayVal=(unsigned char) (255-(unsigned char)(255*v)); 41 | painter.setPen(QColor(grayVal, grayVal, grayVal)); 42 | } else { 43 | painter.setPen(QColor(255, 100, 100)); 44 | } 45 | painter.drawPoint(x,ysize-y); 46 | } 47 | } 48 | 49 | template < typename Iterator > 50 | void QMapPainter::drawPoints(const Iterator& begin, const Iterator& end, unsigned char r, unsigned char g, unsigned char b){ 51 | QPainter painter(m_pixmap); 52 | painter.setPen(QColor(r,g,b)); 53 | for (Iterator it=begin; it!=end; it++){ 54 | GMapping::IntPoint p=(GMapping::IntPoint)*it; 55 | painter.drawPoint(p.x, height()-p.y); 56 | } 57 | } 58 | 59 | #endif 60 | 61 | -------------------------------------------------------------------------------- /slam_gmapping/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(slam_gmapping) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 15 | add_compile_options(-Wall -Wextra -Wpedantic) 16 | endif() 17 | set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -fuse-ld=lld -Wl,--disable-new-dtags") 18 | message(${CMAKE_SHARED_LINKER_FLAGS}) 19 | # find dependencies 20 | find_package(ament_cmake REQUIRED) 21 | find_package(std_msgs REQUIRED) 22 | find_package(nav_msgs REQUIRED) 23 | find_package(tf2 REQUIRED) 24 | find_package(tf2_ros REQUIRED) 25 | find_package(tf2_geometry_msgs REQUIRED) 26 | find_package(message_filters REQUIRED) 27 | find_package(rclcpp REQUIRED) 28 | find_package(sensor_msgs REQUIRED) 29 | find_package(visualization_msgs REQUIRED) 30 | find_package(openslam_gmapping REQUIRED) 31 | 32 | if(BUILD_TESTING) 33 | find_package(ament_lint_auto REQUIRED) 34 | # the following line skips the linter which checks for copyrights 35 | # remove the line when a copyright and license is present in all source files 36 | set(ament_cmake_copyright_FOUND TRUE) 37 | # the following line skips cpplint (only works in a git repo) 38 | # remove the line when this package is a git repo 39 | set(ament_cmake_cpplint_FOUND TRUE) 40 | ament_lint_auto_find_test_dependencies() 41 | endif() 42 | 43 | include_directories(include) 44 | 45 | add_executable(slam_gmapping src/slam_gmapping.cpp) 46 | 47 | ament_target_dependencies(slam_gmapping 48 | rclcpp 49 | tf2 50 | tf2_ros 51 | message_filters 52 | sensor_msgs 53 | nav_msgs 54 | tf2_geometry_msgs 55 | openslam_gmapping) 56 | 57 | install(TARGETS slam_gmapping 58 | DESTINATION lib/${PROJECT_NAME}) 59 | 60 | install(DIRECTORY launch 61 | DESTINATION share/${PROJECT_NAME}) 62 | 63 | ament_package() -------------------------------------------------------------------------------- /openslam_gmapping/log/log_plot.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | using namespace std; 10 | using namespace GMapping; 11 | 12 | int main(int argc, char ** argv){ 13 | double maxrange=2.; 14 | if (argc<2){ 15 | cout << "usage log_plot | gnuplot" << endl; 16 | exit (-1); 17 | } 18 | ifstream is(argv[1]); 19 | if (! is){ 20 | cout << "no file " << argv[1] << " found" << endl; 21 | exit (-1); 22 | } 23 | CarmenConfiguration conf; 24 | conf.load(is); 25 | 26 | SensorMap m=conf.computeSensorMap(); 27 | 28 | //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) 29 | // cout << it->first << " " << it->second->getName() << endl; 30 | 31 | SensorLog log(m); 32 | is.close(); 33 | 34 | ifstream ls(argv[1]); 35 | log.load(ls); 36 | ls.close(); 37 | int count=0; 38 | int frame=0; 39 | cerr << "log size" << log.size() << endl; 40 | for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ 41 | RangeReading* rr=dynamic_cast(*it); 42 | if (rr){ 43 | count++; 44 | if (count%3) 45 | continue; 46 | std::vector points(rr->size()); 47 | uint j=0; 48 | for (uint i=0; isize(); i++){ 49 | const RangeSensor * rs=dynamic_cast(rr->getSensor()); 50 | double c=rs->beams()[i].c, s=rs->beams()[i].s; 51 | double r=(*rr)[i]; 52 | if (r>maxrange) 53 | continue; 54 | points[j++]=Point(r*c,r*s); 55 | } 56 | if (j){ 57 | char buf[1024]; 58 | sprintf(buf,"frame-%05d.gif",frame); 59 | frame++; 60 | cout << "set terminal gif" << endl; 61 | cout << "set output \"" << buf << "\"" << endl; 62 | cout << "set size ratio -1" << endl; 63 | cout << "plot [-3:3][0:3] '-' w p ps 1" << endl; 64 | for (uint i=0; i 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | typedef std::deque DoubleDeque; 36 | 37 | class QGraphPainter : public QWidget{ 38 | Q_OBJECT 39 | public: 40 | QGraphPainter( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0); 41 | virtual ~QGraphPainter(); 42 | public slots: 43 | void clear(); 44 | void valueAdded(double); 45 | void valueAdded(double, double, double); 46 | void setYReference(double y); 47 | void disableYReference(); 48 | void setRange(double min, double max); 49 | void start(int period); 50 | void setTitle(const char* title); 51 | void setAutoscale(bool a); 52 | bool getAutoscale() const; 53 | protected: 54 | virtual void timerEvent(QTimerEvent * te); 55 | virtual void resizeEvent(QResizeEvent *); 56 | double min, max, reference; 57 | DoubleDeque values; 58 | bool autoscale; 59 | bool m_useYReference; 60 | int timer; 61 | virtual void paintEvent ( QPaintEvent *paintevent ); 62 | QPixmap * m_pixmap; 63 | QString title; 64 | }; 65 | 66 | #endif 67 | 68 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/gfs_nogui.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #include 25 | #include "gsp_thread.h" 26 | 27 | using namespace GMapping; 28 | 29 | int main (int argc, char ** argv){ 30 | cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ; 31 | cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl ; 32 | cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl; 33 | 34 | 35 | GridSlamProcessorThread* gsp= new GridSlamProcessorThread; 36 | if (gsp->init(argc, argv)){ 37 | cout << "GSP INIT ERROR" << endl; 38 | return -1; 39 | } 40 | cout <<"GSP INITIALIZED"<< endl; 41 | if (gsp->loadFiles()){ 42 | cout <<"GSP READFILE ERROR"<< endl; 43 | return -2; 44 | } 45 | cout <<"FILES LOADED"<< endl; 46 | gsp->setMapUpdateTime(1000000); 47 | gsp->start(); 48 | cout <<"THREAD STARTED"<< endl; 49 | bool done=false; 50 | while (!done){ 51 | GridSlamProcessorThread::EventDeque events=gsp->getEvents(); 52 | for (GridSlamProcessorThread::EventDeque::iterator it=events.begin(); it!=events.end(); it++){ 53 | cout << flush; 54 | GridSlamProcessorThread::DoneEvent* doneEvent=dynamic_cast(*it); 55 | if (doneEvent){ 56 | done=true; 57 | cout <<"DONE!"<< endl; 58 | gsp->stop(); 59 | } 60 | if (*it) 61 | delete(*it); 62 | } 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/autoptr.h: -------------------------------------------------------------------------------- 1 | #ifndef AUTOPTR_H 2 | #define AUTOPTR_H 3 | #include 4 | 5 | namespace GMapping{ 6 | 7 | template 8 | class autoptr{ 9 | protected: 10 | 11 | public: 12 | struct reference{ 13 | X* data; 14 | unsigned int shares; 15 | }; 16 | inline autoptr(X* p=(X*)(0)); 17 | inline autoptr(const autoptr& ap); 18 | inline autoptr& operator=(const autoptr& ap); 19 | inline ~autoptr(); 20 | inline operator int() const; 21 | inline X& operator*(); 22 | inline const X& operator*() const; 23 | //p 24 | reference * m_reference; 25 | protected: 26 | }; 27 | 28 | template 29 | autoptr::autoptr(X* p){ 30 | m_reference=0; 31 | if (p){ 32 | m_reference=new reference; 33 | m_reference->data=p; 34 | m_reference->shares=1; 35 | } 36 | } 37 | 38 | template 39 | autoptr::autoptr(const autoptr& ap){ 40 | m_reference=0; 41 | reference* ref=ap.m_reference; 42 | if (ap.m_reference){ 43 | m_reference=ref; 44 | m_reference->shares++; 45 | } 46 | } 47 | 48 | template 49 | autoptr& autoptr::operator=(const autoptr& ap){ 50 | reference* ref=ap.m_reference; 51 | if (m_reference==ref){ 52 | return *this; 53 | } 54 | if (m_reference && !(--m_reference->shares)){ 55 | delete m_reference->data; 56 | delete m_reference; 57 | m_reference=0; 58 | } 59 | if (ref){ 60 | m_reference=ref; 61 | m_reference->shares++; 62 | } 63 | //20050802 nasty changes begin 64 | else 65 | m_reference=0; 66 | //20050802 nasty changes end 67 | return *this; 68 | } 69 | 70 | template 71 | autoptr::~autoptr(){ 72 | if (m_reference && !(--m_reference->shares)){ 73 | delete m_reference->data; 74 | delete m_reference; 75 | m_reference=0; 76 | } 77 | } 78 | 79 | template 80 | autoptr::operator int() const{ 81 | return m_reference && m_reference->shares && m_reference->data; 82 | } 83 | 84 | template 85 | X& autoptr::operator*(){ 86 | assert(m_reference && m_reference->shares && m_reference->data); 87 | return *(m_reference->data); 88 | } 89 | 90 | template 91 | const X& autoptr::operator*() const{ 92 | assert(m_reference && m_reference->shares && m_reference->data); 93 | return *(m_reference->data); 94 | } 95 | 96 | }; 97 | #endif 98 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/Makefile.app: -------------------------------------------------------------------------------- 1 | # Makefile generico per applicazione 2 | # 3 | # Variabili: 4 | # APPS lista delle applicazioni 5 | # OBJS lista degli oggetti 6 | # QOBJS lista degli oggetti QT 7 | # LIBS librerie 8 | # 9 | # Ogni applicazione viene linkata con tutti gli oggetti 10 | 11 | export VERBOSE 12 | 13 | ifeq ($(LINUX),1) 14 | CPPFLAGS+=-DLINUX 15 | endif 16 | 17 | 18 | APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a)) 19 | all: $(APPLICATIONS) 20 | 21 | PACKAGE=$(notdir $(shell pwd)) 22 | 23 | .SECONDARY: $(OBJS) $(QOBJS) 24 | .PHONY: all clean copy doc 25 | 26 | $(QOBJS): %.o: %.cpp moc_%.cpp 27 | @$(MESSAGE) "Compiling (QT) $@" 28 | @$(PRETTY) "$(CXX) $(CPPFLAGS) $(QT_INCLUDE) $(CXXFLAGS) -c $< -o $@" 29 | 30 | moc_%.cpp: %.h 31 | @$(MESSAGE) "Generating MOC $@" 32 | @$(PRETTY) "$(MOC) -i $< -o $@" 33 | 34 | # Generazione degli oggetti 35 | %.o: %.cpp 36 | @$(MESSAGE) "Compiling $@" 37 | @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@" 38 | 39 | # Generazione delle applicazioni 40 | $(BINDIR)/%: %.cpp $(OBJS) $(QOBJS) 41 | @$(MESSAGE) "Linking application `basename $@`" 42 | @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(OBJS) $(QOBJS) $< -L$(LIBDIR) $(LIBS) -o $@" 43 | 44 | #Regole per la generazione di tabelle o altri file creati automaticamente 45 | table_%.cpp: gen_table_% 46 | @$(MESSAGE) "Generating $@" 47 | @$(PRETTY) "./$< > $@" 48 | 49 | gen_table_%: gen_table_%.cpp 50 | @$(MESSAGE) "Generating $@" 51 | @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $< -o $@" 52 | 53 | #Regole per la generazione delle dipendenze 54 | OBJDEPS=$(foreach module,$(basename $(OBJS) $(QOBJS)),$(module).d) 55 | 56 | $(OBJDEPS): %.d: %.cpp # ci va o no? %.h 57 | @$(MESSAGE) "Generating dependecies $@" 58 | @$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG -MF $@ $<" 59 | 60 | ifneq ($(MAKECMDGOALS),clean) 61 | ifneq ($(MAKECMDGOALS),copy) 62 | -include $(OBJDEPS) 63 | endif 64 | endif 65 | 66 | doc: 67 | rm -rf doc/$(PACKAGE) 68 | ifeq ($(strip $(DOCTITLE)),) 69 | kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS) 70 | else 71 | kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS) 72 | endif 73 | 74 | clean: 75 | @$(MESSAGE) "Cleaning $(PACKAGE)" 76 | @$(PRETTY) "rm -f *.d *.o moc_*.cpp *.d core *~ table_*.cpp gen_table*[^.][^c][^p][^p] $(APPLICATIONS)" 77 | @$(PRETTY) "rm -rf doc/$(PACKAGE)" 78 | 79 | copy: clean 80 | tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE) 81 | -------------------------------------------------------------------------------- /openslam_gmapping/gridfastslam/gfsreader.h: -------------------------------------------------------------------------------- 1 | #ifndef GFSREADER_H 2 | #define GFSREADER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #define MAX_LINE_LENGHT (1000000) 12 | 13 | namespace GMapping{ 14 | 15 | namespace GFSReader{ 16 | 17 | using namespace std; 18 | 19 | struct Record{ 20 | unsigned int dim; 21 | double time; 22 | virtual ~Record(); 23 | virtual void read(istream& is)=0; 24 | virtual void write(ostream& os); 25 | }; 26 | 27 | struct CommentRecord: public Record{ 28 | string text; 29 | virtual void read(istream& is); 30 | virtual void write(ostream& os); 31 | }; 32 | 33 | struct PoseRecord: public Record{ 34 | PoseRecord(bool ideal=false); 35 | void read(istream& is); 36 | virtual void write(ostream& os); 37 | bool truePos; 38 | OrientedPoint pose; 39 | }; 40 | 41 | struct NeffRecord: public Record{ 42 | void read(istream& is); 43 | virtual void write(ostream& os); 44 | double neff; 45 | }; 46 | 47 | struct EntropyRecord: public Record{ 48 | void read(istream& is); 49 | virtual void write(ostream& os); 50 | double poseEntropy; 51 | double trajectoryEntropy; 52 | double mapEntropy; 53 | }; 54 | 55 | 56 | struct OdometryRecord: public Record{ 57 | virtual void read(istream& is); 58 | vector poses; 59 | }; 60 | 61 | struct RawOdometryRecord: public Record{ 62 | virtual void read(istream& is); 63 | OrientedPoint pose; 64 | }; 65 | 66 | struct ScanMatchRecord: public Record{ 67 | virtual void read(istream& is); 68 | vector poses; 69 | vector weights; 70 | }; 71 | 72 | struct LaserRecord: public Record{ 73 | virtual void read(istream& is); 74 | virtual void write(ostream& os); 75 | vector readings; 76 | OrientedPoint pose; 77 | double weight; 78 | }; 79 | 80 | struct ResampleRecord: public Record{ 81 | virtual void read(istream& is); 82 | vector indexes; 83 | }; 84 | 85 | struct RecordList: public list{ 86 | mutable int sampleSize; 87 | istream& read(istream& is); 88 | double getLogWeight(unsigned int i) const; 89 | double getLogWeight(unsigned int i, RecordList::const_iterator frame) const; 90 | unsigned int getBestIdx() const ; 91 | void printLastParticles(ostream& os) const ; 92 | void printPath(ostream& os, unsigned int i, bool err=false, bool rawodom=false) const; 93 | RecordList computePath(unsigned int i, RecordList::const_iterator frame) const; 94 | void destroyReferences(); 95 | }; 96 | 97 | }; //end namespace GFSReader 98 | 99 | }; //end namespace GMapping 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /openslam_gmapping/ini/gfs-LMS-20cm.ini: -------------------------------------------------------------------------------- 1 | ### gfs dummy config file 2 | 3 | ## WARNING: Changing these parameters, can 4 | ## increase of decrese the performance of the 5 | ## mapper! 6 | 7 | 8 | [gfs] 9 | 10 | ################################################# 11 | ## 12 | ## These are probably the most improtant parameters 13 | ## 14 | 15 | ## gfs - number of particles 16 | particles 30 17 | 18 | ## gfs measurement integration 19 | angularUpdate 0.5 20 | linearUpdate 1 21 | 22 | ## map resolution 23 | delta 0.2 24 | ## scan matcher 25 | maxrange 80 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m 26 | maxUrange 80 # (use up to) 27 | sigma 0.05 # scan matcher cell sigma, for the greedy search 28 | regscore 10000 # minimum score for regsistering a scan 29 | iterations 5 # iterations 30 | critscore 0.0 # critical score (leave this) 31 | maxMove 1.0 # maximum move among two scans. This detects some corrupted logs 32 | autosize off # determine te map size by pre readoing the log 33 | 34 | 35 | lstep 0.2 # linear search step (choose delta) 36 | astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval 37 | lsigma 0.2 # sigma likelihood of 1 beam 38 | lskip 1. # beams to skip in the likelihood computation 39 | skipMatching off # do not perform scan matching before computing the statistics 40 | 41 | kernelSize 1 # the higher the value the slower the filter 42 | # the better it can deal with noise, but the less precise and slower 43 | ogain 3 # gain for smoothing the likelihood 44 | resampleThreshold 0.5 # when neff is below this value a resampling occurs 45 | randseed 0 # this is for the repeated experiments 46 | 47 | ## likelihood sampling 48 | llsamplerange 0.2 # linear range 49 | llsamplestep 0.2 # linear step 50 | lasamplerange 0.05 # angular range 51 | lasamplestep 0.05 # angular step 52 | 53 | ## motion model parameters 54 | srr 0.1 # translation as a function of translation 55 | srt 0.1 # translation as a function of rotation 56 | str 0.1 # rotation as a function of translation 57 | stt 0.1 # rotation as a function of rotation 58 | 59 | ## odometry integration in proposal 60 | linearOdometryReliability 0.0 61 | angularOdometryReliability 0.0 62 | considerOdometryCovariance off 63 | 64 | 65 | ## inital map params 66 | xmin -150.0 67 | ymin -100.0 68 | xmax 100.0 69 | ymax 100.0 70 | 71 | ## file parameters 72 | 73 | readFromStdin off 74 | onLine off 75 | generateMap off 76 | 77 | -------------------------------------------------------------------------------- /openslam_gmapping/ini/gfs-LMS-10cm.ini: -------------------------------------------------------------------------------- 1 | ### gfs dummy config file 2 | 3 | ## WARNING: Changing these parameters, can 4 | ## increase of decrese the performance of the 5 | ## mapper! 6 | 7 | 8 | [gfs] 9 | 10 | ################################################# 11 | ## 12 | ## These are probably the most improtant parameters 13 | ## 14 | 15 | ## gfs - number of particles 16 | particles 30 17 | 18 | ## gfs measurement integration 19 | angularUpdate 0.5 20 | linearUpdate 1 21 | 22 | ## map resolution 23 | delta 0.1 24 | 25 | ## scan matcher 26 | maxrange 81.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m 27 | maxUrange 80.0 # (use up to) 28 | sigma 0.05 # scan matcher cell sigma, for the greedy search 29 | regscore 0.0004 # minimum score for regsistering a scan 30 | iterations 5 # iterations 31 | critscore 0.0 # critical score (leave this) 32 | maxMove 1.0 # maximum move among two scans. This detects some corrupted logs 33 | autosize off # determine te map size by pre readoing the log 34 | 35 | 36 | ## default settings for a 0.1 m map cell 37 | lstep 0.1 # linear search step (choose delta) 38 | astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval 39 | lsigma 0.075 # sigma likelihood of 1 beam 40 | lskip 0 # beams to skip in the likelihood computation 41 | skipMatching off # do not perform scan matching before computing the statistics 42 | 43 | kernelSize 1 # the higher the value the slower the filter 44 | # the better it can deal with noise, but the less precise and slower 45 | ogain 3 # gain for smoothing the likelihood 46 | resampleThreshold 0.5 # when neff is below this value a resampling occurs 47 | randseed 0 # this is for the repeated experiments 48 | 49 | ## likelihood sampling 50 | llsamplerange 0.1 # linear range 51 | llsamplestep 0.1 # linear step 52 | lasamplerange 0.05 # angular range 53 | lasamplestep 0.05 # angular step 54 | 55 | ## motion model parameters 56 | srr 0.1 # translation as a function of translation 57 | srt 0.1 # translation as a function of rotation 58 | str 0.1 # rotation as a function of translation 59 | stt 0.1 # rotation as a function of rotation 60 | 61 | ## odometry integration in proposal 62 | linearOdometryReliability 0.0 63 | angularOdometryReliability 0.0 64 | considerOdometryCovariance off 65 | 66 | 67 | ## inital map params 68 | xmin -100.0 69 | ymin -100.0 70 | xmax 100.0 71 | ymax 100.0 72 | 73 | ## file parameters 74 | 75 | readFromStdin off 76 | onLine off 77 | generateMap off 78 | 79 | -------------------------------------------------------------------------------- /openslam_gmapping/ini/gfs-LMS-5cm.ini: -------------------------------------------------------------------------------- 1 | ### gfs dummy config file 2 | 3 | ## WARNING: Changing these parameters, can 4 | ## increase of decrese the performance of the 5 | ## mapper! 6 | 7 | 8 | [gfs] 9 | 10 | ################################################# 11 | ## 12 | ## These are probably the most improtant parameters 13 | ## 14 | 15 | ## gfs - number of particles 16 | particles 30 17 | 18 | ## gfs measurement integration 19 | angularUpdate 0.5 20 | linearUpdate 1 21 | 22 | ## map resolution 23 | delta 0.05 24 | 25 | ## scan matcher 26 | maxrange 81.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m 27 | maxUrange 80.0 # (use up to) 28 | sigma 0.05 # scan matcher cell sigma, for the greedy search 29 | regscore 0.0004 # minimum score for regsistering a scan 30 | iterations 5 # iterations 31 | critscore 0.0 # critical score (leave this) 32 | maxMove 1.0 # maximum move among two scans. This detects some corrupted logs 33 | autosize off # determine te map size by pre readoing the log 34 | 35 | 36 | ## default settings for a 0.1 m map cell 37 | lstep 0.05 # linear search step (choose delta) 38 | astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval 39 | lsigma 0.05 # sigma likelihood of 1 beam 40 | lskip 0 # beams to skip in the likelihood computation 41 | skipMatching off # do not perform scan matching before computing the statistics 42 | 43 | kernelSize 1 # the higher the value the slower the filter 44 | # the better it can deal with noise, but the less precise and slower 45 | ogain 3 # gain for smoothing the likelihood 46 | resampleThreshold 0.5 # when neff is below this value a resampling occurs 47 | randseed 0 # this is for the repeated experiments 48 | 49 | ## likelihood sampling 50 | llsamplerange 0.05 # linear range 51 | llsamplestep 0.05 # linear step 52 | lasamplerange 0.05 # angular range 53 | lasamplestep 0.05 # angular step 54 | 55 | ## motion model parameters 56 | srr 0.1 # translation as a function of translation 57 | srt 0.1 # translation as a function of rotation 58 | str 0.1 # rotation as a function of translation 59 | stt 0.1 # rotation as a function of rotation 60 | 61 | ## odometry integration in proposal 62 | linearOdometryReliability 0.0 63 | angularOdometryReliability 0.0 64 | considerOdometryCovariance off 65 | 66 | 67 | ## inital map params 68 | xmin -100.0 69 | ymin -100.0 70 | xmax 100.0 71 | ymax 100.0 72 | 73 | ## file parameters 74 | 75 | readFromStdin off 76 | onLine off 77 | generateMap off 78 | 79 | -------------------------------------------------------------------------------- /openslam_gmapping/ini/gfs-PLS-5cm.ini: -------------------------------------------------------------------------------- 1 | ### gfs dummy config file 2 | 3 | ## WARNING: Changing these parameters, can 4 | ## increase of decrese the performance of the 5 | ## mapper! 6 | 7 | 8 | [gfs] 9 | 10 | ################################################# 11 | ## 12 | ## These are probably the most improtant parameters 13 | ## 14 | 15 | ## gfs - number of particles 16 | particles 30 17 | 18 | ## gfs measurement integration 19 | angularUpdate 0.5 20 | linearUpdate 1 21 | 22 | ## map resolution 23 | delta 0.05 24 | 25 | ## scan matcher 26 | maxrange 50.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m 27 | maxUrange 50.0 # (use up to) 28 | sigma 0.07 # scan matcher cell sigma, for the greedy search 29 | regscore 0.0004 # minimum score for regsistering a scan 30 | iterations 5 # iterations 31 | critscore 0.0 # critical score (leave this) 32 | maxMove 1.0 # maximum move among two scans. This detects some corrupted logs 33 | autosize off # determine te map size by pre readoing the log 34 | 35 | 36 | ## default settings for a 0.1 m map cell 37 | lstep 0.05 # linear search step (choose delta) 38 | astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval 39 | lsigma 0.05 # sigma likelihood of 1 beam 40 | lskip 0 # beams to skip in the likelihood computation 41 | skipMatching off # do not perform scan matching before computing the statistics 42 | 43 | kernelSize 1 # the higher the value the slower the filter 44 | # the better it can deal with noise, but the less precise and slower 45 | ogain 3 # gain for smoothing the likelihood 46 | resampleThreshold 0.5 # when neff is below this value a resampling occurs 47 | randseed 0 # this is for the repeated experiments 48 | 49 | ## likelihood sampling 50 | llsamplerange 0.05 # linear range 51 | llsamplestep 0.05 # linear step 52 | lasamplerange 0.05 # angular range 53 | lasamplestep 0.05 # angular step 54 | 55 | ## motion model parameters 56 | srr 0.1 # translation as a function of translation 57 | srt 0.1 # translation as a function of rotation 58 | str 0.1 # rotation as a function of translation 59 | stt 0.1 # rotation as a function of rotation 60 | 61 | ## odometry integration in proposal 62 | linearOdometryReliability 0.0 63 | angularOdometryReliability 0.0 64 | considerOdometryCovariance off 65 | 66 | 67 | ## inital map params 68 | xmin -100.0 69 | ymin -100.0 70 | xmax 100.0 71 | ymax 100.0 72 | 73 | ## file parameters 74 | 75 | readFromStdin off 76 | onLine off 77 | generateMap off 78 | 79 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/stat_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "stat.h" 5 | 6 | using namespace std; 7 | using namespace GMapping; 8 | 9 | // struct Covariance3{ 10 | // double xx, yy, tt, xy, xt, yt; 11 | // }; 12 | 13 | #define SAMPLES_NUMBER 10000 14 | 15 | int main(int argc, char** argv){ 16 | Covariance3 cov={1.,0.01,0.01,0,0,0}; 17 | EigenCovariance3 ecov(cov); 18 | cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl; 19 | 20 | cout << "EigenVectors:" < points; 37 | for (unsigned int i=0; i::iterator b = points.begin(); 44 | std::vector::iterator e = points.end(); 45 | Gaussian3 gaussian=computeGaussianFromSamples(b, e); 46 | cov=gaussian.cov; 47 | ecov=gaussian.covariance; 48 | cout << "*************** Estimated with Templates ***************" << endl; 49 | cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl; 50 | cout << "EigenVectors:" < 2 | #include 3 | #include 4 | #include "particlefilter.h" 5 | 6 | using namespace std; 7 | 8 | #define test(s) {cout << s << " " << flush;} 9 | #define testOk() {cout << "OK" << endl;} 10 | 11 | struct Particle{ 12 | double p; 13 | double w; 14 | inline operator double() const {return w;} 15 | inline void setWeight(double _w) {w=_w;} 16 | }; 17 | 18 | ostream& printParticles(ostream& os, const vector& p) 19 | { 20 | for (vector::const_iterator it=p.begin(); it!=p.end(); ++it) { 21 | os << it->p<< " " << (double)*it << endl; 22 | } 23 | return os; 24 | } 25 | 26 | struct EvolutionModel{ 27 | Particle evolve(const Particle& p){ 28 | Particle pn(p); 29 | pn.p+=.5*(drand48()-.5); 30 | return pn; 31 | } 32 | }; 33 | 34 | struct QualificationModel{ 35 | Particle evolve(const Particle& p){ 36 | return p; 37 | } 38 | }; 39 | 40 | struct LikelyhoodModel{ 41 | double likelyhood(const Particle& p) const{ 42 | double v = 1./(0.1+10*(p.p-2)*(p.p-2))+0.5/(0.1+10*(p.p-8)*(p.p-8)); 43 | return v; 44 | } 45 | }; 46 | 47 | int main (unsigned int argc, const char * const * argv){ 48 | int nparticles=100; 49 | if (argc>1) 50 | nparticles=atoi(argv[1]); 51 | vector particles(nparticles); 52 | LikelyhoodModel likelyhoodModel; 53 | uniform_resampler resampler; 54 | auxiliary_evolver auxevolver; 55 | evolver evolver; 56 | 57 | for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ 58 | it->w=1; 59 | it->p=10*(drand48()); 60 | } 61 | 62 | vector sirparticles(particles); 63 | vector auxparticles(particles); 64 | 65 | /*sir step*/ 66 | while (1){ 67 | char buf[2]; 68 | cin.getline(buf,2); 69 | vector newgeneration; 70 | 71 | cout << "# SIR step" << endl; 72 | evolver.evolve(sirparticles); 73 | for (vector::iterator it=sirparticles.begin(); it!=sirparticles.end(); it++){ 74 | it->setWeight(likelyhoodModel.likelyhood(*it)); 75 | } 76 | ofstream os("sir.dat"); 77 | printParticles(os, sirparticles); 78 | os.close(); 79 | newgeneration=resampler.resample(sirparticles); 80 | sirparticles=newgeneration; 81 | 82 | cout << "# AUX step" << endl; 83 | auxevolver.evolve(auxparticles); 84 | for (vector::iterator it=auxparticles.begin(); it!=auxparticles.end(); it++){ 85 | it->setWeight(likelyhoodModel.likelyhood(*it)); 86 | } 87 | os.open("aux.dat"); 88 | printParticles(os, auxparticles); 89 | os.close(); 90 | newgeneration=resampler.resample(auxparticles); 91 | auxparticles=newgeneration; 92 | cout << "plot [0:10][0:10]\"sir.dat\" w impulses" << endl; 93 | cout << "replot 1./(0.1+10*(x-2)*(x-2))+0.5/(0.1+10*(x-8)*(x-8))" << endl; 94 | 95 | // cout << "replot \"aux.dat\" w p" << endl; 96 | } 97 | } 98 | 99 | -------------------------------------------------------------------------------- /openslam_gmapping/particlefilter/range_bearing.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "particlefilter.h" 6 | 7 | using namespace std; 8 | using namespace GMapping; 9 | 10 | #define test(s) {cout << s << " " << flush;} 11 | #define testOk() {cout << "OK" << endl;} 12 | 13 | struct Particle{ 14 | Particle(): p(0,0), w(0){} 15 | Point p; 16 | double w; 17 | operator double() const {return w; } 18 | void setWeight(double _w) {w=_w;} 19 | }; 20 | 21 | ostream& printParticles(ostream& os, const vector& p) 22 | { 23 | for (vector::const_iterator it=p.begin(); it!=p.end(); ++it) { 24 | os << it->p.x << " " << it->p.y << endl; 25 | } 26 | return os; 27 | } 28 | 29 | struct EvolutionModel{ 30 | Particle evolve(const Particle& p){ 31 | Particle pn(p); 32 | pn.p.x+=10*(drand48()-.5); 33 | pn.p.y+=10*(drand48()-.5); 34 | return pn; 35 | } 36 | }; 37 | 38 | 39 | struct LikelyhoodModel{ 40 | std::vector observerVector; 41 | std::vector observations; 42 | double sigma; 43 | double likelyhood(const Particle& p) const{ 44 | double v=1; 45 | std::vector::const_iterator oit=observations.begin(); 46 | for (std::vector::const_iterator it=observerVector.begin(); it!=observerVector.end();it++){ 47 | v*=exp(-pow(((p.p-*it)*(p.p-*it)-*oit*(*oit))/sigma, 2)); 48 | oit++; 49 | } 50 | cout << "#v=" << v << endl; 51 | return v; 52 | } 53 | }; 54 | 55 | int main (unsigned int argc, const char * const * argv){ 56 | vector particles(1000); 57 | LikelyhoodModel likelyhoodModel; 58 | uniform_resampler resampler; 59 | evolver evolver; 60 | 61 | for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ 62 | it->w=1; 63 | it->p.x=400*(drand48()-.5); 64 | it->p.y=400*(drand48()-.5); 65 | } 66 | 67 | vector sensors; 68 | 69 | sensors.push_back(Point(-50,0)); 70 | sensors.push_back(Point(50,0)); 71 | sensors.push_back(Point(0,100)); 72 | 73 | likelyhoodModel.sigma=1000; 74 | likelyhoodModel.observations.push_back(70); 75 | likelyhoodModel.observations.push_back(70); 76 | likelyhoodModel.observations.push_back(70); 77 | 78 | likelyhoodModel.observerVector=sensors; 79 | while (1){ 80 | char buf[2]; 81 | cin.getline(buf,2); 82 | vector newgeneration; 83 | 84 | cout << "# SIR step" << endl; 85 | evolver.evolve(particles); 86 | for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ 87 | it->w*=likelyhoodModel.likelyhood(*it); 88 | } 89 | 90 | ofstream os("sir.dat"); 91 | printParticles(os, particles); 92 | os.close(); 93 | vector newpart=resampler.resample(particles); 94 | particles=newpart; 95 | 96 | cout << "plot [-200:200][-200:200]\"sir.dat\" w p" << endl; 97 | } 98 | } 99 | 100 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/commandline.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #ifndef COMMANDLINE_H 25 | #define COMMANDLINE_H 26 | 27 | 28 | #define parseFlag(name,value)\ 29 | if (!strcmp(argv[c],name)){\ 30 | value=true;\ 31 | cout << name << " on"<< endl;\ 32 | recognized=true;\ 33 | }\ 34 | 35 | #define parseString(name,value)\ 36 | if (!strcmp(argv[c],name) && c 2 | #include 3 | #include 4 | 5 | #define MotionModelConditioningLinearCovariance 0.01 6 | #define MotionModelConditioningAngularCovariance 0.001 7 | 8 | namespace GMapping { 9 | 10 | 11 | 12 | OrientedPoint 13 | MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{ 14 | OrientedPoint n(p); 15 | double lm=linearMove + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str ); 16 | double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt ); 17 | n.x+=lm*cos(n.theta+.5*am); 18 | n.y+=lm*sin(n.theta+.5*am); 19 | n.theta+=am; 20 | n.theta=atan2(sin(n.theta), cos(n.theta)); 21 | return n; 22 | } 23 | 24 | OrientedPoint 25 | MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{ 26 | double sxy=0.3*srr; 27 | OrientedPoint delta=absoluteDifference(pnew, pold); 28 | OrientedPoint noisypoint(delta); 29 | noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y)); 30 | noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x)); 31 | noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y)); 32 | noisypoint.theta=fmod(noisypoint.theta, 2*M_PI); 33 | if (noisypoint.theta>M_PI) 34 | noisypoint.theta-=2*M_PI; 35 | return absoluteSum(p,noisypoint); 36 | } 37 | 38 | 39 | /* 40 | OrientedPoint 41 | MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{ 42 | 43 | //compute the three stps needed for perfectly matching the two poses if the noise is absent 44 | 45 | OrientedPoint delta=pnew-pold; 46 | double aoffset=atan2(delta.y, delta.x); 47 | double alpha1=aoffset-pold.theta; 48 | alpha1=atan2(sin(alpha1), cos(alpha1)); 49 | double rho=sqrt(delta*delta); 50 | double alpha2=pnew.theta-aoffset; 51 | alpha2=atan2(sin(alpha2), cos(alpha2)); 52 | 53 | OrientedPoint pret=drawFromMotion(p, 0, alpha1); 54 | pret=drawFromMotion(pret, rho, 0); 55 | pret=drawFromMotion(pret, 0, alpha2); 56 | return pret; 57 | } 58 | */ 59 | 60 | 61 | Covariance3 MotionModel::gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const{ 62 | OrientedPoint delta=absoluteDifference(pnew,pold); 63 | double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y); 64 | double angularMove=fabs(delta.x); 65 | double s11=srr*srr*linearMove*linearMove; 66 | double s22=stt*stt*angularMove*angularMove; 67 | double s12=str*angularMove*srt*linearMove; 68 | Covariance3 cov; 69 | double s=sin(pold.theta),c=cos(pold.theta); 70 | cov.xx=c*c*s11+MotionModelConditioningLinearCovariance; 71 | cov.yy=s*s*s11+MotionModelConditioningLinearCovariance; 72 | cov.tt=s22+MotionModelConditioningAngularCovariance; 73 | cov.xy=s*c*s11; 74 | cov.xt=c*s12; 75 | cov.yt=s*s12; 76 | return cov; 77 | } 78 | 79 | }; 80 | 81 | -------------------------------------------------------------------------------- /openslam_gmapping/scanmatcher/icptest.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | using namespace GMapping; 9 | using namespace std; 10 | 11 | typedef std::list PointPairList; 12 | 13 | PointPairList generateRandomPointPairs(int size, OrientedPoint t, double noise=0.){ 14 | PointPairList ppl; 15 | double s=sin(t.theta), c=cos(t.theta); 16 | for (int i=0; i> size >> t.x >> t.y >> t.theta; 38 | PointPairList ppl=generateRandomPointPairs(size, t, 3); 39 | OrientedPoint tc; 40 | OrientedPoint ttot(0.,0.,0.); 41 | bool method=true; 42 | while(1){ 43 | char buf[10]; 44 | cerr << "iterate?" << endl; 45 | cin.getline(buf,10); 46 | if (buf[0]=='n') 47 | method=false; 48 | else if (buf[0]=='l') 49 | method=true; 50 | else if (buf[0]!=char(0)) 51 | break; 52 | cout << "plot '-' w l, '-' w p, '-' w p" << endl; 53 | for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ 54 | cout << it->first.x << " " << it->first.y<< endl; 55 | cout << it->second.x << " " << it->second.y<< endl; 56 | cout << endl; 57 | } 58 | cout << "e" << endl; 59 | for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ 60 | cout << it->first.x << " " << it->first.y<< endl; 61 | } 62 | cout << "e" << endl; 63 | for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ 64 | cout << it->second.x << " " << it->second.y<< endl; 65 | } 66 | cout << "e" << endl; 67 | 68 | double error; 69 | if (!method){ 70 | cerr << "Nonlinear Optimization" << endl; 71 | error=icpNonlinearStep(tc,ppl); 72 | }else { 73 | cerr << "Linear Optimization" << endl; 74 | error=icpStep(tc,ppl); 75 | } 76 | cerr << "ICP err=" << error << " t.x=" << tc.x << " t.y=" << tc.y << " t.theta=" << tc.theta << endl; 77 | cerr << "\t" << error << " ttot.x=" << ttot.x << " ttot.y=" << ttot.y << " ttot.theta=" << ttot.theta << endl; 78 | double s=sin(tc.theta), c=cos(tc.theta); 79 | for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ 80 | Point p1(c*it->first.x-s*it->first.y+tc.x, 81 | s*it->first.x+c*it->first.y+tc.y); 82 | it->first=p1; 83 | } 84 | ttot.x+=tc.x; 85 | ttot.y+=tc.y; 86 | ttot.theta+=tc.theta; 87 | ttot.theta=atan2(sin(ttot.theta), cos(ttot.theta)); 88 | } 89 | } 90 | return 0; 91 | } 92 | -------------------------------------------------------------------------------- /openslam_gmapping/ini/gfs-PLS-10cm.ini: -------------------------------------------------------------------------------- 1 | ### gfs dummy config file 2 | 3 | ## WARNING: Changing these parameters, can 4 | ## increase of decrese the performance of the 5 | ## mapper! 6 | 7 | 8 | [gfs] 9 | 10 | ################################################# 11 | ## 12 | ## These are probably the most improtant parameters 13 | ## 14 | 15 | ## gfs - number of particles 16 | particles 30 17 | 18 | ## gfs measurement integration 19 | angularUpdate 0.5 20 | linearUpdate 1 21 | 22 | ## map resolution 23 | delta 0.1 24 | 25 | ## scan matcher 26 | maxrange 50.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m 27 | maxUrange 50.0 # (use up to) 28 | sigma 0.075 # scan matcher cell sigma, for the greedy search 29 | regscore 0.0004 # minimum score for regsistering a scan 30 | iterations 5 # iterations 31 | critscore 0.0 # critical score (leave this) 32 | maxMove 1.0 # maximum move among two scans. This detects some corrupted logs 33 | autosize off # determine te map size by pre readoing the log 34 | 35 | 36 | ## default settings for a 0.1 m map cell 37 | lstep 0.1 # linear search step (choose delta) 38 | astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval 39 | lsigma 0.1 # sigma likelihood of 1 beam 40 | lskip 0 # beams to skip in the likelihood computation 41 | skipMatching off # do not perform scan matching before computing the statistics 42 | 43 | kernelSize 1 # the higher the value the slower the filter 44 | # the better it can deal with noise, but the less precise and slower 45 | ogain 3 # gain for smoothing the likelihood 46 | resampleThreshold 0.5 # when neff is below this value a resampling occurs 47 | randseed 0 # this is for the repeated experiments 48 | 49 | ## likelihood sampling 50 | llsamplerange 0.1 # linear range 51 | llsamplestep 0.1 # linear step 52 | lasamplerange 0.05 # angular range 53 | lasamplestep 0.05 # angular step 54 | 55 | ## motion model parameters 56 | srr 0.1 # translation as a function of translation 57 | srt 0.1 # translation as a function of rotation 58 | str 0.1 # rotation as a function of translation 59 | stt 0.1 # rotation as a function of rotation 60 | 61 | ## odometry integration in proposal 62 | linearOdometryReliability 0.0 # condition the scan matcher with odometry while serching for max [translation component] 63 | angularOdometryReliability 0.0 # condition the scan matcher with odometry while serching for max [rotation component] 64 | considerOdometryCovariance off 65 | 66 | 67 | ## inital map params 68 | xmin -100.0 69 | ymin -100.0 70 | xmax 100.0 71 | ymax 100.0 72 | 73 | ## file parameters 74 | 75 | readFromStdin off 76 | onLine off 77 | generateMap off 78 | 79 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/scanmatcher/icp.h: -------------------------------------------------------------------------------- 1 | #ifndef _ICP_H_ 2 | #define _ICP_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace GMapping{ 10 | typedef std::pair PointPair; 11 | 12 | template 13 | double icpStep(OrientedPoint & retval, const PointPairContainer& container){ 14 | typedef typename PointPairContainer::const_iterator ContainerIterator; 15 | PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.)); 16 | int size=0; 17 | for (ContainerIterator it=container.begin(); it!=container.end(); it++){ 18 | mean.first=mean.first+it->first; 19 | mean.second=mean.second+it->second; 20 | size++; 21 | } 22 | mean.first=mean.first*(1./size); 23 | mean.second=mean.second*(1./size); 24 | double sxx=0, sxy=0, syx=0, syy=0; 25 | 26 | for (ContainerIterator it=container.begin(); it!=container.end(); it++){ 27 | PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second); 28 | sxx+=mf.first.x*mf.second.x; 29 | sxy+=mf.first.x*mf.second.y; 30 | syx+=mf.first.y*mf.second.x; 31 | syy+=mf.first.y*mf.second.y; 32 | } 33 | retval.theta=atan2(sxy-syx, sxx+sxy); 34 | double s=sin(retval.theta), c=cos(retval.theta); 35 | retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y); 36 | retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y); 37 | 38 | double error=0; 39 | for (ContainerIterator it=container.begin(); it!=container.end(); it++){ 40 | Point delta( 41 | c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y); 42 | error+=delta*delta; 43 | } 44 | return error; 45 | } 46 | 47 | template 48 | double icpNonlinearStep(OrientedPoint & retval, const PointPairContainer& container){ 49 | typedef typename PointPairContainer::const_iterator ContainerIterator; 50 | PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.)); 51 | int size=0; 52 | for (ContainerIterator it=container.begin(); it!=container.end(); it++){ 53 | mean.first=mean.first+it->first; 54 | mean.second=mean.second+it->second; 55 | size++; 56 | } 57 | 58 | mean.first=mean.first*(1./size); 59 | mean.second=mean.second*(1./size); 60 | 61 | double ms=0,mc=0; 62 | for (ContainerIterator it=container.begin(); it!=container.end(); it++){ 63 | PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second); 64 | double dalpha=atan2(mf.second.y, mf.second.x) - atan2(mf.first.y, mf.first.x); 65 | double gain=sqrt(mean.first*mean.first); 66 | ms+=gain*sin(dalpha); 67 | mc+=gain*cos(dalpha); 68 | } 69 | retval.theta=atan2(ms, mc); 70 | double s=sin(retval.theta), c=cos(retval.theta); 71 | retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y); 72 | retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y); 73 | 74 | double error=0; 75 | for (ContainerIterator it=container.begin(); it!=container.end(); it++){ 76 | Point delta( 77 | c*it->first.x-s*it->first.y+retval.x-it->second.x, s*it->first.x+c*it->first.y+retval.y-it->second.y); 78 | error+=delta*delta; 79 | } 80 | return error; 81 | } 82 | 83 | }//end namespace 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /openslam_gmapping/scanmatcher/gridlinetraversal.h: -------------------------------------------------------------------------------- 1 | #ifndef GRIDLINETRAVERSAL_H 2 | #define GRIDLINETRAVERSAL_H 3 | 4 | #include 5 | #include 6 | 7 | namespace GMapping { 8 | 9 | typedef struct { 10 | int num_points; 11 | IntPoint* points; 12 | } GridLineTraversalLine; 13 | 14 | struct GridLineTraversal { 15 | inline static void gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ; 16 | inline static void gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ; 17 | 18 | }; 19 | 20 | void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) 21 | { 22 | int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag; 23 | int cnt = 0; 24 | 25 | dx = abs(end.x-start.x); dy = abs(end.y-start.y); 26 | 27 | if (dy <= dx) { 28 | d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx); 29 | if (start.x > end.x) { 30 | x = end.x; y = end.y; 31 | ydirflag = (-1); 32 | xend = start.x; 33 | } else { 34 | x = start.x; y = start.y; 35 | ydirflag = 1; 36 | xend = end.x; 37 | } 38 | line->points[cnt].x=x; 39 | line->points[cnt].y=y; 40 | cnt++; 41 | if (((end.y - start.y) * ydirflag) > 0) { 42 | while (x < xend) { 43 | x++; 44 | if (d <0) { 45 | d+=incr1; 46 | } else { 47 | y++; d+=incr2; 48 | } 49 | line->points[cnt].x=x; 50 | line->points[cnt].y=y; 51 | cnt++; 52 | } 53 | } else { 54 | while (x < xend) { 55 | x++; 56 | if (d <0) { 57 | d+=incr1; 58 | } else { 59 | y--; d+=incr2; 60 | } 61 | line->points[cnt].x=x; 62 | line->points[cnt].y=y; 63 | cnt++; 64 | } 65 | } 66 | } else { 67 | d = 2*dx - dy; 68 | incr1 = 2*dx; incr2 = 2 * (dx - dy); 69 | if (start.y > end.y) { 70 | y = end.y; x = end.x; 71 | yend = start.y; 72 | xdirflag = (-1); 73 | } else { 74 | y = start.y; x = start.x; 75 | yend = end.y; 76 | xdirflag = 1; 77 | } 78 | line->points[cnt].x=x; 79 | line->points[cnt].y=y; 80 | cnt++; 81 | if (((end.x - start.x) * xdirflag) > 0) { 82 | while (y < yend) { 83 | y++; 84 | if (d <0) { 85 | d+=incr1; 86 | } else { 87 | x++; d+=incr2; 88 | } 89 | line->points[cnt].x=x; 90 | line->points[cnt].y=y; 91 | cnt++; 92 | } 93 | } else { 94 | while (y < yend) { 95 | y++; 96 | if (d <0) { 97 | d+=incr1; 98 | } else { 99 | x--; d+=incr2; 100 | } 101 | line->points[cnt].x=x; 102 | line->points[cnt].y=y; 103 | cnt++; 104 | } 105 | } 106 | } 107 | line->num_points = cnt; 108 | } 109 | 110 | void GridLineTraversal::gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) { 111 | int i,j; 112 | int half; 113 | IntPoint v; 114 | gridLineCore( start, end, line ); 115 | if ( start.x!=line->points[0].x || 116 | start.y!=line->points[0].y ) { 117 | half = line->num_points/2; 118 | for (i=0,j=line->num_points - 1;ipoints[i]; 120 | line->points[i] = line->points[j]; 121 | line->points[j] = v; 122 | } 123 | } 124 | } 125 | 126 | }; 127 | 128 | #endif 129 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/gfs_simplegui.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #include "qparticleviewer.h" 25 | #include "qgraphpainter.h" 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | class GFSMainWindow: public QMainWindow{ 34 | public: 35 | GFSMainWindow(GridSlamProcessorThread* t){ 36 | gsp_thread=t; 37 | QVBoxLayout* layout=new QVBoxLayout(this); 38 | pviewer=new QParticleViewer(this,0,0,gsp_thread); 39 | pviewer->setGeometry(0,0,500,500); 40 | pviewer->setFocusPolicy(Qt::ClickFocus); 41 | layout->addWidget(pviewer); 42 | 43 | gpainter=new QGraphPainter(this); 44 | gpainter->setFixedHeight(100); 45 | layout->addWidget(gpainter); 46 | gpainter->setRange(0,1); 47 | gpainter->setTitle("Neff"); 48 | 49 | help = new QLabel(QString("+/- - zoom | b - show/hide best path | p - show/hide all paths | c - center robot "),this); 50 | help->setMaximumHeight(30); 51 | layout->addWidget(help); 52 | 53 | QObject::connect( pviewer, SIGNAL(neffChanged(double) ), gpainter, SLOT(valueAdded(double)) ); 54 | setTabOrder(pviewer, pviewer); 55 | } 56 | 57 | void start(int c){ 58 | pviewer->start(c); 59 | gpainter->start(c); 60 | } 61 | 62 | protected: 63 | GridSlamProcessorThread* gsp_thread; 64 | QVBoxLayout* layout; 65 | QParticleViewer* pviewer; 66 | QGraphPainter* gpainter; 67 | QLabel* help; 68 | }; 69 | 70 | 71 | int main (int argc, char ** argv){ 72 | cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ; 73 | cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl; 74 | cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl; 75 | 76 | 77 | GridSlamProcessorThread* gsp= new GridSlamProcessorThread; 78 | if (gsp->init(argc, argv)){ 79 | cerr << "GridFastSlam: Initialization Error!" << endl; 80 | cerr << "(Did you specified an input file for reading?)" << endl; 81 | return -1; 82 | } 83 | if (gsp->loadFiles()){ 84 | cerr <<"Error reading file!"<< endl; 85 | return -2; 86 | } 87 | cerr <<"File successfully loaded!"<< endl; 88 | QApplication app(argc, argv); 89 | GFSMainWindow* mainWin=new GFSMainWindow(gsp); 90 | mainWin->show(); 91 | gsp->setEventBufferSize(10000); 92 | gsp->start(); 93 | mainWin->start(1000); 94 | return app.exec(); 95 | } 96 | 97 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/movement.cpp: -------------------------------------------------------------------------------- 1 | #include "movement.h" 2 | #include 3 | 4 | namespace GMapping { 5 | 6 | 7 | FSRMovement::FSRMovement(double f, double s, double r) { 8 | this->f = f; 9 | this->s = s; 10 | this->r = r; 11 | } 12 | 13 | FSRMovement::FSRMovement(const FSRMovement& src) { 14 | *this = src; 15 | } 16 | 17 | FSRMovement::FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2) { 18 | *this = moveBetweenPoints(pt1, pt2); 19 | } 20 | 21 | 22 | FSRMovement::FSRMovement(const FSRMovement& move1, const FSRMovement& move2) { 23 | *this = composeMoves(move1, move2); 24 | } 25 | 26 | void FSRMovement::normalize() 27 | { 28 | if (r >= -M_PI && r < M_PI) 29 | return; 30 | 31 | int multiplier = (int)(r / (2*M_PI)); 32 | r = r - multiplier*2*M_PI; 33 | if (r >= M_PI) 34 | r -= 2*M_PI; 35 | if (r < -M_PI) 36 | r += 2*M_PI; 37 | } 38 | 39 | OrientedPoint FSRMovement::move(const OrientedPoint& pt) const { 40 | return movePoint(pt, *this); 41 | } 42 | 43 | void FSRMovement::invert() { 44 | *this = invertMove(*this); 45 | } 46 | 47 | void FSRMovement::compose(const FSRMovement& move2) { 48 | *this = composeMoves(*this, move2); 49 | } 50 | 51 | 52 | FSRMovement FSRMovement::composeMoves(const FSRMovement& move1, 53 | const FSRMovement& move2) { 54 | FSRMovement comp; 55 | comp.f = cos(move1.r) * move2.f - sin(move1.r) * move2.s + move1.f; 56 | comp.s = sin(move1.r) * move2.f + cos(move1.r) * move2.s + move1.s; 57 | comp.r = (move1.r + move2.r); 58 | comp.normalize(); 59 | return comp; 60 | } 61 | 62 | OrientedPoint FSRMovement::movePoint(const OrientedPoint& pt, const FSRMovement& move1) { 63 | OrientedPoint pt2(pt); 64 | pt2.x += move1.f * cos(pt.theta) - move1.s * sin(pt.theta); 65 | pt2.y += move1.f * sin(pt.theta) + move1.s * cos(pt.theta); 66 | pt2.theta = (move1.r + pt.theta); 67 | pt2.normalize(); 68 | return pt2; 69 | } 70 | 71 | FSRMovement FSRMovement::moveBetweenPoints(const OrientedPoint& pt1, 72 | const OrientedPoint& pt2) { 73 | FSRMovement move; 74 | move.f = (pt2.y - pt1.y) * sin(pt1.theta) + (pt2.x - pt1.x) * cos(pt1.theta); 75 | move.s = + (pt2.y - pt1.y) * cos(pt1.theta) - (pt2.x - pt1.x) * sin(pt1.theta); 76 | move.r = (pt2.theta - pt1.theta); 77 | move.normalize(); 78 | return move; 79 | 80 | } 81 | 82 | FSRMovement FSRMovement::invertMove(const FSRMovement& move1) { 83 | FSRMovement p_inv; 84 | p_inv.f = - cos(move1.r) * move1.f - sin(move1.r) * move1.s; 85 | p_inv.s = sin(move1.r) * move1.f - cos(move1.r) * move1.s; 86 | p_inv.r = (-move1.r); 87 | p_inv.normalize(); 88 | return p_inv; 89 | } 90 | 91 | 92 | OrientedPoint FSRMovement::frameTransformation(const OrientedPoint& reference_pt_frame1, 93 | const OrientedPoint& reference_pt_frame2, 94 | const OrientedPoint& pt_frame1) { 95 | OrientedPoint zero; 96 | 97 | FSRMovement itrans_refp1(zero, reference_pt_frame1); 98 | itrans_refp1.invert(); 99 | 100 | FSRMovement trans_refp2(zero, reference_pt_frame2); 101 | FSRMovement trans_pt(zero, pt_frame1); 102 | 103 | FSRMovement tmp = composeMoves( composeMoves(trans_refp2, itrans_refp1), trans_pt); 104 | return tmp.move(zero); 105 | } 106 | 107 | 108 | } 109 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qslamandnavwidget.cpp: -------------------------------------------------------------------------------- 1 | #include "qslamandnavwidget.h" 2 | #include 3 | using namespace GMapping; 4 | 5 | 6 | QSLAMandNavWidget::QSLAMandNavWidget( QWidget * parent, const char * name, WFlags f) 7 | : QMapPainter(parent, name, f), dumper("slamandnav", 1){ 8 | robotPose=IntPoint(0,0); 9 | robotHeading=0; 10 | slamRestart=false; 11 | slamFinished=false; 12 | enableMotion=false; 13 | startWalker=false; 14 | trajectorySent=false; 15 | goHome=false; 16 | wantsQuit=false; 17 | printHelp=false; 18 | saveGoalPoints=false; 19 | writeImages=false; 20 | drawRobot=true; 21 | } 22 | 23 | QSLAMandNavWidget::~QSLAMandNavWidget(){} 24 | 25 | 26 | void QSLAMandNavWidget::mousePressEvent ( QMouseEvent * e ){ 27 | QPoint p=e->pos(); 28 | int mx=p.x(); 29 | int my=height()-p.y(); 30 | if ( e->state()&Qt::ShiftButton && e->button()==Qt::LeftButton) { 31 | if (trajectorySent) 32 | trajectoryPoints.clear(); 33 | e->accept(); 34 | IntPoint p=IntPoint(mx, my); 35 | trajectoryPoints.push_back(p); 36 | trajectorySent=false; 37 | } 38 | } 39 | 40 | void QSLAMandNavWidget::keyPressEvent ( QKeyEvent * e ){ 41 | if (e->key()==Qt::Key_Delete){ 42 | e->accept(); 43 | if (!trajectoryPoints.empty()) 44 | trajectoryPoints.pop_back(); 45 | } 46 | if (e->key()==Qt::Key_S){ 47 | e->accept(); 48 | enableMotion=!enableMotion; 49 | } 50 | if (e->key()==Qt::Key_W){ 51 | e->accept(); 52 | startWalker=!startWalker; 53 | } 54 | if (e->key()==Qt::Key_G){ 55 | e->accept(); 56 | slamRestart=true; 57 | } 58 | if (e->key()==Qt::Key_T){ 59 | e->accept(); 60 | trajectorySent=true; 61 | } 62 | if (e->key()==Qt::Key_R){ 63 | e->accept(); 64 | goHome=true; 65 | } 66 | if (e->key()==Qt::Key_C){ 67 | e->accept(); 68 | slamFinished=true; 69 | 70 | } 71 | if (e->key()==Qt::Key_Q){ 72 | e->accept(); 73 | wantsQuit=true; 74 | 75 | } 76 | if (e->key()==Qt::Key_H){ 77 | e->accept(); 78 | printHelp=true; 79 | //BABSI 80 | //insert the help here 81 | } 82 | if (e->key()==Qt::Key_Y){ 83 | e->accept(); 84 | saveGoalPoints=true; 85 | //BABSI 86 | //insert the help here 87 | } 88 | if (e->key()==Qt::Key_D){ 89 | e->accept(); 90 | drawRobot=!drawRobot; 91 | //BABSI 92 | //insert the help here 93 | } 94 | } 95 | 96 | void QSLAMandNavWidget::paintEvent ( QPaintEvent * ){ 97 | QPixmap pixmap(*m_pixmap); 98 | QPainter painter(&pixmap); 99 | if (trajectorySent) 100 | painter.setPen(Qt::red); 101 | bool first=true; 102 | int oldx=0, oldy=0; 103 | //paint the path 104 | for (std::list::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){ 105 | int x=it->x; 106 | int y=height()-it->y; 107 | if (! first) 108 | painter.drawLine(oldx, oldy, x,y); 109 | oldx=x; 110 | oldy=y; 111 | first=false; 112 | } 113 | 114 | //paint the robot 115 | if (drawRobot){ 116 | painter.setPen(Qt::black); 117 | int rx=robotPose.x; 118 | int ry=height()-robotPose.y; 119 | int robotSize=6; 120 | painter.drawLine(rx, ry, 121 | rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading))); 122 | painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize); 123 | } 124 | if (writeImages){ 125 | dumper.dump(pixmap); 126 | } 127 | bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP); 128 | } 129 | -------------------------------------------------------------------------------- /openslam_gmapping/build_tools/Makefile.generic-shared-object: -------------------------------------------------------------------------------- 1 | # Makefile generico per shared object 2 | export VERBOSE 3 | export CXX 4 | 5 | # Nome del package 6 | PACKAGE=$(notdir $(shell pwd)) 7 | 8 | # Libreria da generare: 9 | # Se non si setta la variabile LIBNAME la libreria si chiama 10 | # come la directory 11 | ifndef LIBNAME 12 | LIBNAME=$(PACKAGE) 13 | endif 14 | 15 | ifeq ($(MACOSX),1) 16 | SONAME=$(LIBDIR)/lib$(LIBNAME).dylib 17 | endif 18 | 19 | ifeq ($(LINUX),1) 20 | SONAME=$(LIBDIR)/lib$(LIBNAME).so 21 | endif 22 | 23 | APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a)) 24 | INSTALL_SCRIPTS=$(foreach a, $(SCRIPTS),$(BINDIR)/$(a)) 25 | 26 | all: $(SONAME) $(APPLICATIONS) $(INSTALL_SCRIPTS) 27 | 28 | .SECONDARY: $(OBJS) $(COBJS) 29 | .PHONY: all clean copy doc 30 | 31 | # Generazione della libreria 32 | $(SONAME): $(OBJS) $(COBJS) 33 | @$(MESSAGE) "Creating library lib$(LIBNAME).so" 34 | ifeq ($(MACOSX),1) 35 | @$(PRETTY) "$(CXX) $(LDFLAGS) -dynamiclib $(OBJS) $(COBJS) -L$(LIBDIR) $(LIBS) -install_name $@ -o $@" 36 | endif 37 | ifeq ($(LINUX),1) 38 | @$(PRETTY) "$(CXX) -fPIC -shared $(OBJS) $(COBJS) -L $(LIBDIR) $(LIBS) $(LDFLAGS) -o $@" 39 | @if ! $(PRETTY) "$(TESTLIB) $(SONAME)"; then $(MESSAGE) "Testing of $(SONAME) failed."; rm $(SONAME); exit 1; fi; 40 | endif 41 | 42 | # Generazione delle applicazioni 43 | $(BINDIR)/%: %.o $(SONAME) 44 | @$(MESSAGE) "Linking application `basename "$@"`" 45 | @$(PRETTY) "$(CXX) $< -l$(LIBNAME) $(LDFLAGS) -L$(LIBDIR) $(LIBS) -o $@" 46 | 47 | #Generazione dei moc files 48 | moc_%.cpp: %.h 49 | @$(MESSAGE) "Compiling MOC $@" 50 | @$(PRETTY) "$(MOC) -i $< -o $@" 51 | 52 | # Generazione degli oggetti 53 | %.o: %.cpp 54 | @$(MESSAGE) "Compiling $<" 55 | @$(PRETTY) "$(CXX) -fPIC $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@" 56 | 57 | %.o: %.c 58 | @$(MESSAGE) "Compiling $<" 59 | @$(PRETTY) "$(CC) -fPIC $(CPPFLAGS) $(CFLAGS) -c $< -o $@" 60 | 61 | #Regole per la generazione delle dipendenze 62 | OBJDEPS= $(foreach module,$(basename $(OBJS)),$(module).d) $(foreach a, $(APPS),$(a).d) 63 | COBJDEPS=$(foreach module,$(basename $(COBJS)),$(module).d) 64 | 65 | $(OBJDEPS): %.d: %.cpp 66 | @$(MESSAGE) "Generating dependencies for $<" 67 | @$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG $< -MF $@" 68 | 69 | $(COBJDEPS): %.d: %.c 70 | @$(MESSAGE) "Generating dependencies for $<" 71 | @$(PRETTY) "$(CC) $(CPPFLAGS) -MM -MG $< -MF $@" 72 | 73 | #HEADERS=`ls *.h` 74 | #PRECOMPILED_HEADERS=$(foreach file,$(basename $(HEADERS)), $(file).pch) 75 | 76 | ifneq ($(MAKECMDGOALS),clean) 77 | ifneq ($(MAKECMDGOALS),copy) 78 | ifneq ($(MAKECMDGOALS),dep) 79 | -include $(OBJDEPS) $(COBJDEPS) 80 | endif 81 | endif 82 | endif 83 | 84 | dep: $(OBJDEPS) $(COBJDEPS) 85 | 86 | 87 | # GLi script vengono semplicemente copiati 88 | $(BINDIR)/%.sh: %.sh 89 | @$(MESSAGE) "Installing script `basename "$@"`" 90 | @$(PRETTY) "cp $< $@" 91 | @$(PRETTY) "chmod +x $@" 92 | 93 | 94 | #doc: 95 | # rm -rf doc/$(PACKAGE) 96 | #ifeq ($(strip $(DOCTITLE)),) 97 | # kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS) 98 | #else 99 | # kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS) 100 | #endif 101 | 102 | 103 | clean: 104 | @$(MESSAGE) "Cleaning $(PACKAGE)" 105 | @$(PRETTY) "rm -f $(SONAME) $(APPLICATIONS)" 106 | @$(PRETTY) "rm -f *.o *.d core *~ moc_*.cpp" 107 | 108 | copy: clean 109 | tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE) 110 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qnavigatorwidget.cpp: -------------------------------------------------------------------------------- 1 | #include "qnavigatorwidget.h" 2 | #include 3 | using namespace GMapping; 4 | 5 | 6 | QNavigatorWidget::QNavigatorWidget( QWidget * parent, const char * name, WFlags f) 7 | : QMapPainter(parent, name, f), dumper("navigator", 1){ 8 | robotPose=IntPoint(0,0); 9 | robotHeading=0; 10 | confirmLocalization=false; 11 | repositionRobot=false; 12 | startWalker=false; 13 | enableMotion=false; 14 | goHome=false; 15 | trajectorySent=false; 16 | writeImages=false; 17 | drawRobot=true; 18 | wantsQuit=false; 19 | } 20 | 21 | QNavigatorWidget::~QNavigatorWidget(){} 22 | 23 | 24 | void QNavigatorWidget::mousePressEvent ( QMouseEvent * e ){ 25 | QPoint p=e->pos(); 26 | int mx=p.x(); 27 | int my=height()-p.y(); 28 | if (!(e->state()&Qt::ShiftButton) && e->button()==Qt::LeftButton) { 29 | if (trajectorySent) 30 | trajectoryPoints.clear(); 31 | e->accept(); 32 | IntPoint p=IntPoint(mx, my); 33 | trajectoryPoints.push_back(p); 34 | trajectorySent=false; 35 | } 36 | if (e->state()&Qt::ControlButton && e->button()==Qt::LeftButton){ 37 | e->accept(); 38 | robotPose=IntPoint(mx, my); 39 | repositionRobot=true; 40 | confirmLocalization=true; 41 | } 42 | if (e->state()&Qt::ControlButton && e->button()==Qt::RightButton){ 43 | e->accept(); 44 | IntPoint p(mx, my); 45 | p=p-robotPose; 46 | robotHeading=atan2(p.y, p.x); 47 | repositionRobot=true; 48 | confirmLocalization=true; 49 | } 50 | } 51 | 52 | void QNavigatorWidget::keyPressEvent ( QKeyEvent * e ){ 53 | if (e->key()==Qt::Key_Delete){ 54 | e->accept(); 55 | if (!trajectoryPoints.empty()) 56 | trajectoryPoints.pop_back(); 57 | } 58 | if (e->key()==Qt::Key_S){ 59 | e->accept(); 60 | enableMotion=!enableMotion; 61 | } 62 | if (e->key()==Qt::Key_W){ 63 | e->accept(); 64 | startWalker=!startWalker; 65 | } 66 | if (e->key()==Qt::Key_G){ 67 | e->accept(); 68 | startGlobalLocalization=true; 69 | } 70 | if (e->key()==Qt::Key_T){ 71 | e->accept(); 72 | trajectorySent=true; 73 | } 74 | if (e->key()==Qt::Key_R){ 75 | e->accept(); 76 | goHome=true; 77 | } 78 | if (e->key()==Qt::Key_C){ 79 | e->accept(); 80 | confirmLocalization=true; 81 | 82 | } 83 | if (e->key()==Qt::Key_Q){ 84 | e->accept(); 85 | wantsQuit=true; 86 | 87 | } 88 | if (e->key()==Qt::Key_D){ 89 | e->accept(); 90 | drawRobot=!drawRobot;; 91 | 92 | } 93 | } 94 | 95 | void QNavigatorWidget::paintEvent ( QPaintEvent * ){ 96 | QPixmap pixmap(*m_pixmap); 97 | QPainter painter(&pixmap); 98 | if (trajectorySent) 99 | painter.setPen(Qt::red); 100 | bool first=true; 101 | int oldx=0, oldy=0; 102 | //paint the path 103 | for (std::list::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){ 104 | int x=it->x; 105 | int y=height()-it->y; 106 | if (! first) 107 | painter.drawLine(oldx, oldy, x,y); 108 | oldx=x; 109 | oldy=y; 110 | first=false; 111 | } 112 | //paint the robot 113 | if (drawRobot){ 114 | painter.setPen(Qt::black); 115 | int rx=robotPose.x; 116 | int ry=height()-robotPose.y; 117 | int robotSize=6; 118 | painter.drawLine(rx, ry, 119 | rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading))); 120 | painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize); 121 | } 122 | if (writeImages){ 123 | dumper.dump(pixmap); 124 | } 125 | bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP); 126 | } 127 | -------------------------------------------------------------------------------- /openslam_gmapping/sensor/sensor_range/rangereading.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | namespace GMapping{ 9 | 10 | using namespace std; 11 | 12 | RangeReading::RangeReading(const RangeSensor* rs, double time): 13 | SensorReading(rs,time){} 14 | 15 | RangeReading::RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time): 16 | SensorReading(rs,time){ 17 | assert(n_beams==rs->beams().size()); 18 | resize(n_beams); 19 | for (unsigned int i=0; i(getSensor()); 36 | assert(rs); 37 | Point lp( 38 | cos(rs->beams()[i].pose.theta)*(*this)[i], 39 | sin(rs->beams()[i].pose.theta)*(*this)[i]); 40 | Point dp=lastPoint-lp; 41 | double distance=sqrt(dp*dp); 42 | if (distance::max(); 45 | suppressed++; 46 | } 47 | else{ 48 | lastPoint=lp; 49 | v[i]=(*this)[i]; 50 | } 51 | //std::cerr<< __PRETTY_FUNCTION__ << std::endl; 52 | //std::cerr<< "suppressed " << suppressed <<"/"<(size()); 57 | 58 | }; 59 | 60 | unsigned int RangeReading::activeBeams(double density) const{ 61 | if (density==0.) 62 | return size(); 63 | int ab=0; 64 | Point lastPoint(0,0); 65 | uint suppressed=0; 66 | for (unsigned int i=0; i(getSensor()); 68 | assert(rs); 69 | Point lp( 70 | cos(rs->beams()[i].pose.theta)*(*this)[i], 71 | sin(rs->beams()[i].pose.theta)*(*this)[i]); 72 | Point dp=lastPoint-lp; 73 | double distance=sqrt(dp*dp); 74 | if (distance RangeReading::cartesianForm(double maxRange) const{ 88 | const RangeSensor* rangeSensor=dynamic_cast(getSensor()); 89 | assert(rangeSensor && rangeSensor->beams().size()); 90 | // uint m_beams=rangeSensor->beams().size(); 91 | uint m_beams=static_cast(rangeSensor->beams().size()); 92 | std::vector cartesianPoints(m_beams); 93 | double px,py,ps,pc; 94 | px=rangeSensor->getPose().x; 95 | py=rangeSensor->getPose().y; 96 | ps=sin(rangeSensor->getPose().theta); 97 | pc=cos(rangeSensor->getPose().theta); 98 | for (unsigned int i=0; ibeams()[i].s; 101 | const double& c=rangeSensor->beams()[i].c; 102 | if (rho>=maxRange){ 103 | cartesianPoints[i]=Point(0,0); 104 | } else { 105 | Point p=Point(rangeSensor->beams()[i].pose.x+c*rho, rangeSensor->beams()[i].pose.y+s*rho); 106 | cartesianPoints[i].x=px+pc*p.x-ps*p.y; 107 | cartesianPoints[i].y=py+ps*p.x+pc*p.y; 108 | } 109 | } 110 | return cartesianPoints; 111 | } 112 | 113 | }; 114 | 115 | -------------------------------------------------------------------------------- /openslam_gmapping/carmenwrapper/carmenwrapper.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #ifndef CARMENWRAPPER_H 25 | #define CARMENWRAPPER_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | namespace GMapping{ 41 | 42 | class CarmenWrapper { 43 | public: 44 | static void initializeIPC(const char* name); 45 | static bool start(const char* name); 46 | static bool isRunning(); 47 | static void lock(); 48 | static void unlock(); 49 | static int registerLocalizationMessages(); 50 | 51 | static int queueLength(); 52 | static OrientedPoint getTruePos(); 53 | static bool getReading(RangeReading& reading); 54 | static void addReading(RangeReading& reading); 55 | static const SensorMap& sensorMap(); 56 | static bool sensorMapComputed(); 57 | static bool isStopped(); 58 | 59 | // conversion function 60 | static carmen_robot_laser_message reading2carmen(const RangeReading& reading); 61 | static RangeReading carmen2reading(const carmen_robot_laser_message& msg); 62 | static carmen_point_t point2carmen (const OrientedPoint& p); 63 | static OrientedPoint carmen2point (const carmen_point_t& p); 64 | 65 | 66 | // carmen interaction 67 | static void robot_frontlaser_handler(carmen_robot_laser_message* frontlaser); 68 | static void robot_rearlaser_handler(carmen_robot_laser_message* frontlaser); 69 | static void simulator_truepos_handler(carmen_simulator_truepos_message* truepos); 70 | //babsi: 71 | static void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ; 72 | static void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ; 73 | 74 | //babsi: 75 | static void publish_globalpos(carmen_localize_summary_p summary); 76 | static void publish_particles(carmen_localize_particle_filter_p filter, 77 | carmen_localize_summary_p summary); 78 | 79 | static void shutdown_module(int sig); 80 | 81 | private: 82 | static std::deque m_rangeDeque; 83 | static sem_t m_dequeSem; 84 | static pthread_mutex_t m_mutex, m_lock; 85 | static pthread_t m_readingThread; 86 | static void * m_reading_function(void*); 87 | static bool m_threadRunning; 88 | static SensorMap m_sensorMap; 89 | static RangeSensor* m_frontLaser, *m_rearLaser; 90 | static OrientedPoint m_truepos; 91 | static bool stopped; 92 | }; 93 | 94 | } //end namespace 95 | 96 | 97 | 98 | #endif 99 | /* 100 | int main (int argc, char** argv) { 101 | 102 | CarmenWrapper::init_carmen(argc, argv); 103 | while (true) { 104 | IPC_listenWait(100); 105 | } 106 | return 1; 107 | } 108 | */ 109 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/orientedboundingbox.hxx: -------------------------------------------------------------------------------- 1 | template 2 | double OrientedBoundingBox::area() { 3 | return sqrt((ul.x - ll.x)*(ul.x - ll.x) + (ul.y - ll.y)*(ul.y - ll.y)) * 4 | sqrt((ul.x - ur.x)*(ul.x - ur.x) + (ul.y - ur.y)*(ul.y - ur.y)) ; 5 | } 6 | 7 | template 8 | OrientedBoundingBox::OrientedBoundingBox(std::vector< point > p) { 9 | 10 | int nOfPoints = (int) p.size(); 11 | 12 | // calculate the center of all points (schwerpunkt) 13 | // ------------------------------------------------- 14 | double centerx = 0; 15 | double centery = 0; 16 | for (int i=0; i < nOfPoints; i++) { 17 | centerx += p[i].x; 18 | centery += p[i].y; 19 | } 20 | centerx /= (double) nOfPoints; 21 | centery /= (double) nOfPoints; 22 | 23 | 24 | 25 | // calcutae the covariance matrix 26 | // ------------------------------- 27 | // covariance matrix (x1 x2, x3 x4) 28 | double x1 = 0.0; 29 | double x2 = 0.0; 30 | double x3 = 0.0; 31 | double x4 = 0.0; 32 | 33 | for (int i=0; i < nOfPoints; i++) { 34 | double cix = p[i].x - centerx; 35 | double ciy = p[i].y - centery; 36 | 37 | x1 += cix*cix; 38 | x2 += cix*ciy; 39 | x4 += ciy*ciy; 40 | } 41 | x1 /= (double) nOfPoints; 42 | x2 /= (double) nOfPoints; 43 | x3 = x2; 44 | x4 /= (double) nOfPoints; 45 | // covariance & center done 46 | 47 | 48 | // calculate the eigenvectors 49 | // --------------------------- 50 | // catch 1/0 or sqrt(<0) 51 | if ((x3 == 0) || (x2 == 0)|| (x4*x4-2*x1*x4+x1*x1+4*x2*x3 < 0 )) { 52 | fprintf(stderr,"error computing the Eigenvectors (%s, line %d)\nx3=%lf, x2=%lf, term=%lf\n\n", 53 | __FILE__, __LINE__, x3,x2, (x4*x4-2*x1*x4+x1*x1+4*x2*x3) ); 54 | 55 | ul.x = 0; 56 | ul.y = 0; 57 | ur.x = 0; 58 | ur.y = 0; 59 | ll.x = 0; 60 | ll.y = 0; 61 | lr.x = 0; 62 | lr.y = 0; 63 | } 64 | 65 | // eigenvalues 66 | double lamda1 = 0.5* (x4 + x1 + sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3)); 67 | double lamda2 = 0.5* (x4 + x1 - sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3)); 68 | 69 | // eigenvector 1 with (x,y) 70 | double v1x = - (x4-lamda1) * (x4-lamda1) * (x1-lamda1) / (x2 * x3 * x3); 71 | double v1y = (x4-lamda1) * (x1-lamda1) / (x2 * x3); 72 | // eigenvector 2 with (x,y) 73 | double v2x = - (x4-lamda2) * (x4-lamda2) * (x1-lamda2) / (x2 * x3 * x3); 74 | double v2y = (x4-lamda2) * (x1-lamda2) / (x2 * x3); 75 | 76 | // norm the eigenvectors 77 | double lv1 = sqrt ( (v1x*v1x) + (v1y*v1y) ); 78 | double lv2 = sqrt ( (v2x*v2x) + (v2y*v2y) ); 79 | v1x /= lv1; 80 | v1y /= lv1; 81 | v2x /= lv2; 82 | v2y /= lv2; 83 | // eigenvectors done 84 | 85 | // get the points with maximal dot-product 86 | double x = 0.0; 87 | double y = 0.0; 88 | double xmin = 1e20; 89 | double xmax = -1e20; 90 | double ymin = 1e20; 91 | double ymax = -1e20; 92 | for(int i = 0; i< nOfPoints; i++) { 93 | // dot-product of relativ coordinates of every point 94 | x = (p[i].x - centerx) * v1x + (p[i].y - centery) * v1y; 95 | y = (p[i].x - centerx) * v2x + (p[i].y - centery) * v2y; 96 | 97 | if( x > xmax) xmax = x; 98 | if( x < xmin) xmin = x; 99 | if( y > ymax) ymax = y; 100 | if( y < ymin) ymin = y; 101 | } 102 | 103 | // now we can compute the corners of the bounding box 104 | ul.x = centerx + xmin * v1x + ymin * v2x; 105 | ul.y = centery + xmin * v1y + ymin * v2y; 106 | 107 | ur.x = centerx + xmax * v1x + ymin * v2x; 108 | ur.y = centery + xmax * v1y + ymin * v2y; 109 | 110 | ll.x = centerx + xmin * v1x + ymax * v2x; 111 | ll.y = centery + xmin * v1y + ymax * v2y; 112 | 113 | lr.x = centerx + xmax * v1x + ymax * v2x; 114 | lr.y = centery + xmax * v1y + ymax * v2y; 115 | 116 | } 117 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/stat.h: -------------------------------------------------------------------------------- 1 | #ifndef STAT_H 2 | #define STAT_H 3 | #include "point.h" 4 | #include 5 | #include "gvalues.h" 6 | 7 | namespace GMapping { 8 | 9 | /**stupid utility function for drawing particles form a zero mean, sigma variance normal distribution 10 | probably it should not go there*/ 11 | double sampleGaussian(double sigma,unsigned int S=0); 12 | 13 | double evalGaussian(double sigmaSquare, double delta); 14 | double evalLogGaussian(double sigmaSquare, double delta); 15 | int sampleUniformInt(int max); 16 | double sampleUniformDouble(double min, double max); 17 | 18 | struct Covariance3{ 19 | Covariance3 operator + (const Covariance3 & cov) const; 20 | static Covariance3 zero; 21 | double xx, yy, tt, xy, xt, yt; 22 | }; 23 | 24 | struct EigenCovariance3{ 25 | EigenCovariance3(); 26 | EigenCovariance3(const Covariance3& c); 27 | EigenCovariance3 rotate(double angle) const; 28 | OrientedPoint sample() const; 29 | double eval[3]; 30 | double evec[3][3]; 31 | }; 32 | 33 | struct Gaussian3{ 34 | OrientedPoint mean; 35 | EigenCovariance3 covariance; 36 | Covariance3 cov; 37 | double eval(const OrientedPoint& p) const; 38 | void computeFromSamples(const std::vector & poses); 39 | void computeFromSamples(const std::vector & poses, const std::vector& weights ); 40 | }; 41 | 42 | template 43 | Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){ 44 | Gaussian3 gaussian; 45 | OrientedPoint mean=OrientedPoint(0,0,0); 46 | double wcum=0; 47 | double s=0, c=0; 48 | WeightIterator wt=weightBegin; 49 | double *w=new double(); 50 | OrientedPoint *p=new OrientedPoint(); 51 | for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ 52 | *w=*wt; 53 | *p=*pt; 54 | s+=*w*sin(p->theta); 55 | c+=*w*cos(p->theta); 56 | mean.x+=*w*p->x; 57 | mean.y+=*w*p->y; 58 | wcum+=*w; 59 | wt++; 60 | } 61 | mean.x/=wcum; 62 | mean.y/=wcum; 63 | s/=wcum; 64 | c/=wcum; 65 | mean.theta=atan2(s,c); 66 | 67 | Covariance3 cov=Covariance3::zero; 68 | wt=weightBegin; 69 | for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ 70 | *w=*wt; 71 | *p=*pt; 72 | OrientedPoint delta=(*p)-mean; 73 | delta.theta=atan2(sin(delta.theta),cos(delta.theta)); 74 | cov.xx+=*w*delta.x*delta.x; 75 | cov.yy+=*w*delta.y*delta.y; 76 | cov.tt+=*w*delta.theta*delta.theta; 77 | cov.xy+=*w*delta.x*delta.y; 78 | cov.yt+=*w*delta.y*delta.theta; 79 | cov.xt+=*w*delta.x*delta.theta; 80 | wt++; 81 | } 82 | cov.xx/=wcum; 83 | cov.yy/=wcum; 84 | cov.tt/=wcum; 85 | cov.xy/=wcum; 86 | cov.yt/=wcum; 87 | cov.xt/=wcum; 88 | EigenCovariance3 ecov(cov); 89 | gaussian.mean=mean; 90 | gaussian.covariance=ecov; 91 | gaussian.cov=cov; 92 | delete w; 93 | delete p; 94 | return gaussian; 95 | } 96 | 97 | template 98 | Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){ 99 | Gaussian3 gaussian; 100 | OrientedPoint mean=OrientedPoint(0,0,0); 101 | double wcum=1; 102 | double s=0, c=0; 103 | OrientedPoint *p=new OrientedPoint(); 104 | for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ 105 | *p=*pt; 106 | s+=sin(p->theta); 107 | c+=cos(p->theta); 108 | mean.x+=p->x; 109 | mean.y+=p->y; 110 | wcum+=1.; 111 | } 112 | mean.x/=wcum; 113 | mean.y/=wcum; 114 | s/=wcum; 115 | c/=wcum; 116 | mean.theta=atan2(s,c); 117 | 118 | Covariance3 cov=Covariance3::zero; 119 | for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ 120 | *p=*pt; 121 | OrientedPoint delta=(*p)-mean; 122 | delta.theta=atan2(sin(delta.theta),cos(delta.theta)); 123 | cov.xx+=delta.x*delta.x; 124 | cov.yy+=delta.y*delta.y; 125 | cov.tt+=delta.theta*delta.theta; 126 | cov.xy+=delta.x*delta.y; 127 | cov.yt+=delta.y*delta.theta; 128 | cov.xt+=delta.x*delta.theta; 129 | } 130 | cov.xx/=wcum; 131 | cov.yy/=wcum; 132 | cov.tt/=wcum; 133 | cov.xy/=wcum; 134 | cov.yt/=wcum; 135 | cov.xt/=wcum; 136 | EigenCovariance3 ecov(cov); 137 | gaussian.mean=mean; 138 | gaussian.covariance=ecov; 139 | gaussian.cov=cov; 140 | delete p; 141 | return gaussian; 142 | } 143 | 144 | 145 | }; //end namespace 146 | #endif 147 | 148 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qgraphpainter.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #include 25 | #include "qgraphpainter.h" 26 | #include "moc_qgraphpainter.cpp" 27 | using namespace std; 28 | 29 | QGraphPainter::QGraphPainter( QWidget * parent, const char * name, WFlags f): 30 | QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){ 31 | m_pixmap=new QPixmap(size()); 32 | m_pixmap->fill(Qt::white); 33 | autoscale=false; 34 | m_useYReference = false; 35 | } 36 | 37 | void QGraphPainter::resizeEvent(QResizeEvent * sizeev){ 38 | m_pixmap->resize(sizeev->size()); 39 | } 40 | 41 | QGraphPainter::~QGraphPainter(){ 42 | delete m_pixmap; 43 | } 44 | 45 | void QGraphPainter::clear(){ 46 | values.clear(); 47 | } 48 | 49 | void QGraphPainter::valueAdded(double v){ 50 | values.push_back(v); 51 | } 52 | 53 | void QGraphPainter::valueAdded(double v, double _min, double _max){ 54 | setRange(_min, _max); 55 | values.push_back(v); 56 | } 57 | 58 | void QGraphPainter::setYReference(double y){ 59 | m_useYReference = true; 60 | reference=y; 61 | } 62 | 63 | void QGraphPainter::disableYReference(){ 64 | m_useYReference = false; 65 | } 66 | 67 | 68 | void QGraphPainter::setTitle(const char* t){ 69 | title=t; 70 | } 71 | 72 | void QGraphPainter::setRange(double _min, double _max){ 73 | min=_min; 74 | max=_max; 75 | } 76 | 77 | void QGraphPainter::setAutoscale(bool a) { 78 | autoscale=a; 79 | } 80 | 81 | bool QGraphPainter::getAutoscale() const { 82 | return autoscale; 83 | } 84 | 85 | void QGraphPainter::timerEvent(QTimerEvent * te) { 86 | if (te->timerId()==timer) 87 | update(); 88 | } 89 | 90 | void QGraphPainter::start(int period){ 91 | timer=startTimer(period); 92 | } 93 | 94 | 95 | 96 | void QGraphPainter::paintEvent ( QPaintEvent * ){ 97 | m_pixmap->fill(Qt::white); 98 | QPainter painter(m_pixmap); 99 | double _min=MAXDOUBLE, _max=-MAXDOUBLE; 100 | if (autoscale){ 101 | for (unsigned int i=0; i<(unsigned int)width() && ivalues[i]?_max:values[i]; 104 | } 105 | } else { 106 | _min=min; 107 | _max=max; 108 | } 109 | 110 | 111 | painter.setPen(Qt::black); 112 | painter.drawRect(0, 0, width(), height()); 113 | const int boundary=2; 114 | int xoffset=40; 115 | double scale=((double)height()-2*boundary-2)/(_max-_min); 116 | 117 | if (m_useYReference) { 118 | painter.setPen(Qt::green); 119 | painter.drawLine(xoffset+boundary/2, height()-(int)(scale*(reference-_min)), 120 | width()-boundary/2, height()-(int)(scale*(reference-_min))); 121 | } 122 | painter.setPen(Qt::blue); 123 | unsigned int start=0; 124 | if (values.size()>(unsigned int)width()-2*boundary-xoffset) 125 | start=values.size()-width()+2*boundary+xoffset; 126 | int oldv=0; 127 | if ((unsigned int)width()-2*boundary-xoffset>1 && values.size()>1) 128 | oldv = (int)(scale*(values[1+start]-_min)) + boundary; 129 | 130 | for (unsigned int i=1; i<(unsigned int)width()-2*boundary-xoffset && iwidth(),m_pixmap->height(),CopyROP); 141 | } 142 | 143 | -------------------------------------------------------------------------------- /openslam_gmapping/log/sensorlog.cpp: -------------------------------------------------------------------------------- 1 | #include "sensorlog.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #define LINEBUFFER_SIZE 100000 10 | 11 | namespace GMapping { 12 | 13 | using namespace std; 14 | 15 | SensorLog::SensorLog(const SensorMap& sm): m_sensorMap(sm){ 16 | } 17 | 18 | SensorLog::~SensorLog(){ 19 | for (iterator it=begin(); it!=end(); it++) 20 | if (*it) delete (*it); 21 | } 22 | 23 | istream& SensorLog::load(istream& is){ 24 | for (iterator it=begin(); it!=end(); it++) 25 | if (*it) delete (*it); 26 | clear(); 27 | 28 | char buf[LINEBUFFER_SIZE]; 29 | while (is){ 30 | is.getline(buf, LINEBUFFER_SIZE); 31 | istringstream lis(buf); 32 | 33 | string sensorname; 34 | 35 | if (lis) 36 | lis >>sensorname; 37 | else 38 | continue; 39 | 40 | 41 | 42 | SensorMap::const_iterator it=m_sensorMap.find(sensorname); 43 | if (it==m_sensorMap.end()){ 44 | continue; 45 | } 46 | 47 | Sensor* sensor=it->second; 48 | 49 | SensorReading* reading=0; 50 | OdometrySensor* odometry=dynamic_cast(sensor); 51 | if (odometry) 52 | reading=parseOdometry(lis, odometry); 53 | 54 | RangeSensor* range=dynamic_cast(sensor); 55 | if (range) 56 | reading=parseRange(lis, range); 57 | if (reading) 58 | push_back(reading); 59 | } 60 | return is; 61 | 62 | } 63 | 64 | OdometryReading* SensorLog::parseOdometry(istream& is, const OdometrySensor* osen) const{ 65 | OdometryReading* reading=new OdometryReading(osen); 66 | OrientedPoint pose; 67 | OrientedPoint speed; 68 | OrientedPoint accel; 69 | is >> pose.x >> pose.y >> pose.theta; 70 | is >> speed.x >>speed.theta; 71 | speed.y=0; 72 | is >> accel.x; 73 | accel.y=accel.theta=0; 74 | reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel); 75 | return reading; 76 | } 77 | 78 | RangeReading* SensorLog::parseRange(istream& is, const RangeSensor* rs) const{ 79 | if(rs->newFormat){ 80 | string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; 81 | is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy >> remission_mode; 82 | } 83 | 84 | unsigned int size; 85 | is >> size; 86 | assert(size==rs->beams().size()); 87 | 88 | RangeReading* reading=new RangeReading(rs); 89 | //cerr << "#R=" << size << endl; 90 | reading->resize(size); 91 | for (unsigned int i=0; i> (*reading)[i]; 93 | } 94 | if (rs->newFormat){ 95 | int reflectionBeams; 96 | is >> reflectionBeams; 97 | double reflection; 98 | for (int i=0; i> reflection; 100 | } 101 | //FIXME XXX 102 | OrientedPoint laserPose; 103 | is >> laserPose.x >> laserPose.y >> laserPose.theta; 104 | OrientedPoint pose; 105 | is >> pose.x >> pose.y >> pose.theta; 106 | reading->setPose(pose); 107 | double a,b,c; 108 | if (rs->newFormat){ 109 | string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis; 110 | is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis; 111 | } else { 112 | is >> a >> b >> c; 113 | } 114 | string s; 115 | is >> a >> s; 116 | is >> a; 117 | reading->setTime(a); 118 | return reading; 119 | } 120 | 121 | OrientedPoint SensorLog::boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const { 122 | xmin=ymin=1e6; 123 | xmax=ymax=-1e6; 124 | bool first=true; 125 | OrientedPoint start; 126 | for (const_iterator it=begin(); it!=end(); it++){ 127 | double lxmin=0., lxmax=0., lymin=0., lymax=0.; 128 | const SensorReading* reading=*it; 129 | const OdometryReading* odometry=dynamic_cast (reading); 130 | if (odometry){ 131 | lxmin=lxmax=odometry->getPose().x; 132 | lymin=lymax=odometry->getPose().y; 133 | } 134 | 135 | const RangeReading* rangeReading=dynamic_cast (reading); 136 | if (rangeReading){ 137 | lxmin=lxmax=rangeReading->getPose().x; 138 | lymin=lymax=rangeReading->getPose().y; 139 | if (first){ 140 | first=false; 141 | start=rangeReading->getPose(); 142 | } 143 | } 144 | xmin=xminlxmax?xmax:lxmax; 146 | ymin=yminlymax?ymax:lymax; 148 | } 149 | return start; 150 | } 151 | 152 | }; 153 | 154 | -------------------------------------------------------------------------------- /openslam_gmapping/log/sensorstream.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "sensorstream.h" 4 | //#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-)) 5 | // Can't declare a buffer that big on the stack. So we'll risk Cyrill's 6 | // unblessing, and make it smaller. 7 | #define LINEBUFFER_SIZE 8192 8 | 9 | namespace GMapping { 10 | 11 | using namespace std; 12 | 13 | //SensorStream 14 | SensorStream::SensorStream(const SensorMap& sensorMap) :m_sensorMap(sensorMap){} 15 | 16 | SensorStream::~SensorStream(){} 17 | 18 | SensorReading* SensorStream::parseReading(std::istream& is, const SensorMap& smap){ 19 | SensorReading* reading=0; 20 | if (is){ 21 | char buf[LINEBUFFER_SIZE]; 22 | is.getline(buf, LINEBUFFER_SIZE); 23 | istringstream lis(buf); 24 | 25 | string sensorname; 26 | 27 | if (lis){ 28 | lis >>sensorname; 29 | } else 30 | return 0; 31 | 32 | SensorMap::const_iterator it=smap.find(sensorname); 33 | if (it==smap.end()){ 34 | return 0; 35 | } 36 | 37 | Sensor* sensor=it->second; 38 | 39 | OdometrySensor* odometry=dynamic_cast(sensor); 40 | if (odometry) 41 | reading=parseOdometry(lis, odometry); 42 | 43 | RangeSensor* range=dynamic_cast(sensor); 44 | if (range) 45 | reading=parseRange(lis, range); 46 | } 47 | return reading; 48 | } 49 | 50 | OdometryReading* SensorStream::parseOdometry(std::istream& is, const OdometrySensor* osen ){ 51 | OdometryReading* reading=new OdometryReading(osen); 52 | OrientedPoint pose; 53 | OrientedPoint speed; 54 | OrientedPoint accel; 55 | is >> pose.x >> pose.y >> pose.theta; 56 | is >> speed.x >>speed.theta; 57 | speed.y=0; 58 | is >> accel.x; 59 | accel.y=accel.theta=0; 60 | reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel); 61 | double timestamp, reltimestamp; 62 | string s; 63 | is >> timestamp >>s >> reltimestamp; 64 | reading->setTime(timestamp); 65 | return reading; 66 | } 67 | 68 | RangeReading* SensorStream::parseRange(std::istream& is, const RangeSensor* rs){ 69 | //cerr << __PRETTY_FUNCTION__ << endl; 70 | if(rs->newFormat){ 71 | string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; 72 | is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode; 73 | //cerr << " New format laser msg" << endl; 74 | } 75 | unsigned int size; 76 | is >> size; 77 | assert(size==rs->beams().size()); 78 | RangeReading* reading=new RangeReading(rs); 79 | reading->resize(size); 80 | for (unsigned int i=0; i> (*reading)[i]; 82 | } 83 | if (rs->newFormat){ 84 | int reflectionBeams; 85 | is >> reflectionBeams; 86 | double reflection; 87 | for (int i=0; i> reflection; 89 | } 90 | OrientedPoint laserPose; 91 | is >> laserPose.x >> laserPose.y >> laserPose.theta; 92 | OrientedPoint pose; 93 | is >> pose.x >> pose.y >> pose.theta; 94 | reading->setPose(pose); 95 | 96 | if (rs->newFormat){ 97 | string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis; 98 | is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis; 99 | } 100 | // else { 101 | // double a,b,c; 102 | // is >> a >> b >> c; 103 | // } 104 | double timestamp, reltimestamp; 105 | string s; 106 | is >> timestamp >>s >> reltimestamp; 107 | reading->setTime(timestamp); 108 | return reading; 109 | 110 | } 111 | 112 | //LogSensorStream 113 | LogSensorStream::LogSensorStream(const SensorMap& sensorMap, const SensorLog* log): 114 | SensorStream(sensorMap){ 115 | m_log=log; 116 | assert(m_log); 117 | m_cursor=log->begin(); 118 | } 119 | 120 | LogSensorStream::operator bool() const{ 121 | return m_cursor==m_log->end(); 122 | } 123 | 124 | bool LogSensorStream::rewind(){ 125 | m_cursor=m_log->begin(); 126 | return true; 127 | } 128 | 129 | SensorStream& LogSensorStream::operator >>(const SensorReading*& rd){ 130 | rd=*m_cursor; 131 | m_cursor++; 132 | return *this; 133 | } 134 | 135 | //InputSensorStream 136 | InputSensorStream::InputSensorStream(const SensorMap& sensorMap, std::istream& is): 137 | SensorStream(sensorMap), m_inputStream(is){ 138 | } 139 | 140 | InputSensorStream::operator bool() const{ 141 | return (bool) m_inputStream; 142 | } 143 | 144 | bool InputSensorStream::rewind(){ 145 | //m_inputStream.rewind(); 146 | return false; 147 | } 148 | 149 | SensorStream& InputSensorStream::operator >>(const SensorReading*& reading){ 150 | reading=parseReading(m_inputStream, m_sensorMap); 151 | return *this; 152 | } 153 | 154 | }; 155 | 156 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/qparticleviewer.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #ifndef QPARTICLEVIEWER_H 25 | #define QPARTICLEVIEWER_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | #include "gsp_thread.h" 41 | 42 | namespace GMapping { 43 | 44 | class QParticleViewer : public QWidget{ 45 | Q_OBJECT 46 | public: 47 | struct StartParameters{ 48 | //motionmodel 49 | double srr, srt, str, stt; 50 | //map 51 | double xmin, ymin, xmax, ymax, delta; 52 | OrientedPoint initialPose; 53 | //likelihood 54 | double lsigma, lgain; 55 | unsigned int lskip; 56 | //update 57 | double linearUpdate, angularUpdate; 58 | //filter 59 | unsigned int particles; 60 | double resampleThreshold; 61 | //mode 62 | bool drawFromObservation; 63 | //output 64 | const char * outFileName; 65 | }; 66 | 67 | struct MatchingParameters{ 68 | //ranges 69 | double maxrange, urange; 70 | //score 71 | double ssigma, sreg, scrit; 72 | unsigned int ksize; 73 | //search 74 | double lstep, astep; 75 | unsigned int iterations; 76 | }; 77 | 78 | void refreshParameters(); //reads the parameters from the thread 79 | inline void setGSP( GridSlamProcessorThread* thread){gfs_thread=thread;} 80 | 81 | 82 | typedef std::vector OrientedPointVector; 83 | QParticleViewer( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0, GridSlamProcessorThread* thread=0 ); 84 | virtual ~QParticleViewer(); 85 | virtual void timerEvent(QTimerEvent * te); 86 | virtual void resizeEvent(QResizeEvent *); 87 | 88 | void drawFromFile(); 89 | void drawFromMemory(); 90 | void drawMap(const ScanMatcherMap& map); 91 | void start(int period); 92 | QTextStream* tis; 93 | 94 | MatchingParameters matchingParameters; 95 | StartParameters startParameters; 96 | 97 | int writeToFile; 98 | public slots: 99 | void setMatchingParameters(const MatchingParameters& mp); 100 | void setStartParameters(const StartParameters& mp); 101 | void start(); 102 | void stop(); 103 | void loadFile(const char *); 104 | signals: 105 | void neffChanged(double); 106 | void poseEntropyChanged(double, double, double); 107 | void trajectoryEntropyChanged(double, double, double); 108 | void mapsEntropyChanged(double); 109 | void mapsIGainChanged(double); 110 | 111 | protected: 112 | ifstream inputStream; 113 | ofstream outputStream; 114 | 115 | 116 | protected: 117 | inline Point pic2map(const IntPoint& p) 118 | {return viewCenter+Point(p.x/mapscale, -p.y/mapscale); } 119 | inline IntPoint map2pic(const Point& p) 120 | {return IntPoint((int)((p.x-viewCenter.x)*mapscale),(int)((viewCenter.y-p.y)*mapscale)); } 121 | 122 | int timer; 123 | virtual void paintEvent ( QPaintEvent *paintevent ); 124 | void drawParticleMove(const OrientedPointVector& start, const OrientedPointVector& end); 125 | QPixmap* m_pixmap; 126 | 127 | //thread interaction 128 | GridSlamProcessorThread* gfs_thread; 129 | GridSlamProcessorThread::EventDeque history; 130 | 131 | //mouse movement 132 | virtual void mousePressEvent(QMouseEvent*); 133 | virtual void mouseReleaseEvent(QMouseEvent*); 134 | virtual void mouseMoveEvent(QMouseEvent*); 135 | QPoint draggingPos; 136 | bool dragging; 137 | 138 | //particle plotting 139 | virtual void keyPressEvent ( QKeyEvent* e ); 140 | 141 | //map painting 142 | double mapscale; 143 | Point viewCenter; 144 | Point bestParticlePose; 145 | ScanMatcherMap * bestMap; 146 | 147 | // view mode 148 | bool showPaths; 149 | bool showBestPath; 150 | 151 | // file plotting 152 | QParticleViewer::OrientedPointVector m_oldPose, m_newPose; 153 | unsigned int m_particleSize; 154 | bool m_refresh; 155 | int count; 156 | }; 157 | 158 | }; 159 | 160 | #endif 161 | 162 | -------------------------------------------------------------------------------- /openslam_gmapping/particlefilter/particlefilter.cpp: -------------------------------------------------------------------------------- 1 | std::vector sistematicResampler::resample(const vector& particles) const{ 2 | Numeric cweight=0; 3 | 4 | //compute the cumulative weights 5 | unsigned int n=0; 6 | for (vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ 7 | cweight+=it->weight; 8 | n++; 9 | } 10 | 11 | //compute the interval 12 | Numeric interval=cweight/n; 13 | 14 | //compute the initial target weight 15 | Numeric target= 16 | //compute the resampled indexes 17 | 18 | cweight=0; 19 | std::vector indexes(n); 20 | n=0; 21 | unsigned int i=0; 22 | for (vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ 23 | cweight+=it->weight; 24 | while(cweight>target){ 25 | indexes[n++]=i; 26 | target+=interval; 27 | } 28 | } 29 | return indexes; 30 | } 31 | 32 | template 33 | std::vector indexResampler::resample(const vector >& weights) const{ 34 | Numeric cweight=0; 35 | 36 | //compute the cumulative weights 37 | unsigned int n=0; 38 | for (vector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ 39 | cweight+=*it; 40 | n++; 41 | } 42 | 43 | //compute the interval 44 | Numeric interval=cweight/n; 45 | 46 | //compute the initial target weight 47 | Numeric target= 48 | //compute the resampled indexes 49 | 50 | cweight=0; 51 | std::vector indexes(n); 52 | n=0; 53 | unsigned int i=0; 54 | for (vector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ 55 | cweight+=it->weight; 56 | while(cweight>target){ 57 | indexes[n++]=i; 58 | target+=interval; 59 | } 60 | } 61 | return indexes; 62 | } 63 | 64 | /* 65 | 66 | The following are patterns for the evolution and the observation classes 67 | The user should implement classes having the specified meaning 68 | 69 | template 70 | struct observer{ 71 | Observation& observation 72 | Numeric observe(const class State&) const; 73 | }; 74 | 75 | template 76 | struct evolver{ 77 | Input& input; 78 | State& evolve(const State& s); 79 | }; 80 | */ 81 | 82 | template 83 | void evolver::evolve(std::vector& particles) const{ 84 | for (std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it) 85 | *it=evolutionModel.evolve(*it); 86 | } 87 | 88 | void evolver::evolve(std::vector& dest, const std::vector& src) const{ 89 | dest.clear(); 90 | for (std::vector::const_iterator it=src.begin(); it!=src.end(); ++it) 91 | dest.push_back(evolutionModel.evolve(*it)); 92 | } 93 | 94 | template 95 | struct auxiliaryEvolver{ 96 | typedef particle Particle; 97 | 98 | EvolutionModel evolutionModel; 99 | QualificationModel qualificationModel; 100 | LikelyhoodModel likelyhoodModel; 101 | indexResampler resampler; 102 | 103 | void auxiliaryEvolver::evolve 104 | (std::vector&particles){ 105 | std::vector observationWeights(particles.size()); 106 | unsigned int i=0; 107 | for (std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){ 108 | observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); 109 | } 110 | std::vector indexes(indexResampler.resample(observationWeights)); 111 | for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ 112 | Particle & particle=particles[*it]; 113 | particle=evolutionModel.evolve(particle); 114 | particle.weight*=lykelyhoodModel.lykelyhood(particle)/observationWeights[*it]; 115 | } 116 | } 117 | 118 | void auxiliaryEvolver::evolve 119 | (std::vector& dest, const std::vector& src){ 120 | dest.clear(); 121 | std::vector observationWeights(particles.size()); 122 | unsigned int i=0; 123 | for (std::vector::const_iterator it=src.begin(); it!=src.end(); ++it, i++){ 124 | observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); 125 | } 126 | std::vector indexes(indexResampler.resample(observationWeights)); 127 | for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ 128 | Particle & particle=src[*it]; 129 | dest.push_back(evolutionModel.evolve(particle)); 130 | dest.back().weight*=likelyhoodModel.lykelyhood(particle)/observationWeights[*it]; 131 | } 132 | return dest(); 133 | } 134 | -------------------------------------------------------------------------------- /openslam_gmapping/gui/gsp_thread.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | * 3 | * This file is part of the GMAPPING project 4 | * 5 | * GMAPPING Copyright (c) 2004 Giorgio Grisetti, 6 | * Cyrill Stachniss, and Wolfram Burgard 7 | * 8 | * This software is licensed under the "Creative Commons 9 | * License (Attribution-NonCommercial-ShareAlike 2.0)" 10 | * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, 11 | * and Wolfram Burgard. 12 | * 13 | * Further information on this license can be found at: 14 | * http://creativecommons.org/licenses/by-nc-sa/2.0/ 15 | * 16 | * GMAPPING is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied 18 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 19 | * PURPOSE. 20 | * 21 | *****************************************************************/ 22 | 23 | 24 | #ifndef GSP_THREAD_H 25 | #define GSP_THREAD_H 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | using namespace std; 37 | using namespace GMapping; 38 | 39 | 40 | #define MAX_STRING_LENGTH 1024 41 | 42 | 43 | struct GridSlamProcessorThread : public GridSlamProcessor { 44 | struct Event{ 45 | virtual ~Event(); 46 | }; 47 | 48 | struct ParticleMoveEvent: public Event{ 49 | bool scanmatched; 50 | double neff; 51 | std::vector hypotheses; 52 | std::vector weightSums; 53 | }; 54 | 55 | struct TruePosEvent : public Event{ 56 | OrientedPoint pose; 57 | }; 58 | 59 | struct ResampleEvent: public Event{ 60 | std::vector indexes; 61 | }; 62 | 63 | struct MapEvent: public Event{ 64 | ScanMatcherMap* pmap; 65 | unsigned int index; 66 | OrientedPoint pose; 67 | virtual ~MapEvent(); 68 | }; 69 | 70 | struct DoneEvent: public Event{ 71 | }; 72 | 73 | typedef deque EventDeque; 74 | 75 | GridSlamProcessorThread(); 76 | ~GridSlamProcessorThread(); 77 | int init(int argc, const char * const * argv); 78 | int loadFiles(const char * fn=0); 79 | static void * fastslamthread(GridSlamProcessorThread* gpt); 80 | std::vector getHypotheses(); 81 | std::vector getIndexes(); 82 | 83 | EventDeque getEvents(); 84 | 85 | void start(); 86 | void stop(); 87 | 88 | virtual void onOdometryUpdate(); 89 | virtual void onResampleUpdate(); 90 | virtual void onScanmatchUpdate(); 91 | 92 | virtual void syncOdometryUpdate(); 93 | virtual void syncResampleUpdate(); 94 | virtual void syncScanmatchUpdate(); 95 | 96 | void setEventBufferSize(unsigned int length); 97 | inline void setMapUpdateTime(unsigned int ut) {mapUpdateTime=ut;} 98 | inline bool isRunning() const {return running;} 99 | OrientedPoint boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const; 100 | private: 101 | 102 | void addEvent(Event *); 103 | EventDeque eventBuffer; 104 | 105 | unsigned int eventBufferLength; 106 | unsigned int mapUpdateTime; 107 | unsigned int mapTimer; 108 | 109 | //thread interaction stuff 110 | std::vector hypotheses; 111 | std::vector indexes; 112 | std::vector weightSums; 113 | pthread_mutex_t hp_mutex, ind_mutex, hist_mutex; 114 | pthread_t gfs_thread; 115 | bool running; 116 | 117 | //This are the processor parameters 118 | std::string filename; 119 | std::string outfilename; 120 | 121 | double xmin; 122 | double ymin; 123 | double xmax; 124 | double ymax; 125 | bool autosize; 126 | double delta; 127 | double resampleThreshold; 128 | 129 | //scan matching parameters 130 | double sigma; 131 | double maxrange; 132 | double maxUrange; 133 | double regscore; 134 | double lstep; 135 | double astep; 136 | int kernelSize; 137 | int iterations; 138 | double critscore; 139 | double maxMove; 140 | unsigned int lskip; 141 | 142 | //likelihood 143 | double lsigma; 144 | double ogain; 145 | double llsamplerange, lasamplerange; 146 | double llsamplestep, lasamplestep; 147 | double linearOdometryReliability; 148 | double angularOdometryReliability; 149 | 150 | 151 | //motion model parameters 152 | double srr, srt, str, stt; 153 | //particle parameters 154 | int particles; 155 | bool skipMatching; 156 | 157 | //gfs parameters 158 | double angularUpdate; 159 | double linearUpdate; 160 | 161 | //robot config 162 | SensorMap sensorMap; 163 | //input stream 164 | InputSensorStream* input; 165 | std::ifstream plainStream; 166 | bool readFromStdin; 167 | bool onLine; 168 | bool generateMap; 169 | bool considerOdometryCovariance; 170 | unsigned int randseed; 171 | 172 | //dirty carmen interface 173 | const char* const * m_argv; 174 | unsigned int m_argc; 175 | 176 | }; 177 | #endif 178 | -------------------------------------------------------------------------------- /openslam_gmapping/particlefilter/pf.h: -------------------------------------------------------------------------------- 1 | #ifndef PARTICLEFILTER_H 2 | #define PARTICLEFILTER_H 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | 9 | 10 | /** 11 | the particle class has to be convertible into numeric data type; 12 | That means that a particle must define the Numeric conversion operator; 13 | operator Numeric() const. 14 | that returns the weight, and the method 15 | setWeight(Numeric) 16 | that sets the weight. 17 | 18 | */ 19 | 20 | typedef std::pair UIntPair; 21 | 22 | template 23 | double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){ 24 | //determine the maximum 25 | double lmax=-MAXDOUBLE; 26 | for (Iterator it=begin; it!=end; it++){ 27 | lmax=lmax>((double)(*it))? lmax: (double)(*it); 28 | } 29 | //convert to raw form 30 | for (Iterator it=begin; it!=end; it++){ 31 | *out=exp((double)(*it)-lmax); 32 | out++; 33 | } 34 | return lmax; 35 | } 36 | 37 | template 38 | void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){ 39 | //determine the maximum 40 | for (Iterator it=begin; it!=end; it++){ 41 | *out=log((Numeric)(*it))-lmax; 42 | out++; 43 | } 44 | return lmax; 45 | } 46 | 47 | template 48 | void resample(std::vector& indexes, const WeightVector& weights, unsigned int nparticles=0){ 49 | double cweight=0; 50 | 51 | //compute the cumulative weights 52 | unsigned int n=0; 53 | for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ 54 | cweight+=(double)*it; 55 | n++; 56 | } 57 | 58 | if (nparticles>0) 59 | n=nparticles; 60 | 61 | //compute the interval 62 | double interval=cweight/n; 63 | 64 | //compute the initial target weight 65 | double target=interval*::drand48(); 66 | //compute the resampled indexes 67 | 68 | cweight=0; 69 | indexes.resize(n); 70 | 71 | n=0; 72 | unsigned int i=0; 73 | for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ 74 | cweight+=(double)* it; 75 | while(cweight>target){ 76 | indexes[n++]=i; 77 | target+=interval; 78 | } 79 | } 80 | } 81 | 82 | template 83 | void normalizeWeights(WeightVector& weights, unsigned int size, double minWeight){ 84 | double wmin=MAXDOUBLE; 85 | double wmax=-MAXDOUBLE; 86 | for (uint i=0; iweights[i]?wmax:weights[i]; 89 | } 90 | double min_normalized_value=log(minWeight); 91 | double max_normalized_value=log(1.); 92 | double dn=max_normalized_value-min_normalized_value; 93 | double dw=wmax-wmin; 94 | if (dw==0) dw=1; 95 | double scale=dn/dw; 96 | double offset=-wmax*scale; 97 | for (uint i=0; i 105 | void repeatIndexes(Vector& dest, const std::vector& indexes, const Vector& particles){ 106 | /*<<<<<<< .mine 107 | assert(indexes.size()==particles.size()); 108 | if (dest.size()!=particles.size()) 109 | dest.resize(particles.size()); 110 | =======*/ 111 | //assert(indexes.size()==particles.size()); //DIEGO non ne vedo il senso, anzi è sbagliata 112 | //dest.resize(particles.size()); // è sbagliato anche questo 113 | dest.resize(indexes.size()); 114 | // >>>>>>> .r2534 115 | unsigned int i=0; 116 | for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ 117 | dest[i]=particles[*it]; 118 | i++; 119 | } 120 | } 121 | 122 | template 123 | void repeatIndexes(Vector& dest, const std::vector& indexes2, const Vector& particles, const std::vector& indexes){ 124 | // assert(indexes.size()==indexes2.size()); 125 | dest=particles; 126 | unsigned int i=0; 127 | for (std::vector::const_iterator it=indexes2.begin(); it!=indexes2.end(); ++it){ 128 | dest[indexes[i]]=particles[*it]; 129 | i++; 130 | } 131 | } 132 | 133 | 134 | template 135 | double neff(const Iterator& begin, const Iterator& end){ 136 | double sum=0; 137 | for (Iterator it=begin; it!=end; ++it){ 138 | sum+=*it; 139 | } 140 | double cum=0; 141 | for (Iterator it=begin; it!=end; ++it){ 142 | double w=*it/sum; 143 | cum+=w*w; 144 | } 145 | return 1./cum; 146 | } 147 | 148 | 149 | 150 | template 151 | void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){ 152 | unsigned int current=0; 153 | unsigned int count=0; 154 | for (Iterator it=begin; it!=end; it++){ 155 | if (it==begin){ 156 | current=*it; 157 | count=1; 158 | continue; 159 | } 160 | if (((uint)*it) ==current) 161 | count++; 162 | if (((uint)*it)!=current){ 163 | *out=std::make_pair(current,count); 164 | out++; 165 | current=*it; 166 | count=1; 167 | } 168 | } 169 | if (count>0) 170 | *out=std::make_pair(current,count); 171 | out++; 172 | } 173 | 174 | #endif 175 | 176 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/optimizer.h: -------------------------------------------------------------------------------- 1 | #ifndef _OPTIMIZER_H_ 2 | #define _OPTIMIZER_H_ 3 | 4 | #include "point.h" 5 | 6 | namespace GMapping { 7 | 8 | struct OptimizerParams{ 9 | double discretization; 10 | double angularStep, linearStep; 11 | int iterations; 12 | double maxRange; 13 | }; 14 | 15 | template 16 | struct Optimizer { 17 | Optimizer(const OptimizerParams& params); 18 | OptimizerParams params; 19 | Map lmap; 20 | Likelihood likelihood; 21 | OrientedPoint gradientDescent(const RangeReading& oldReading, const RangeReading& newReading); 22 | OrientedPoint gradientDescent(const RangeReading& oldReading, const OrientedPoint& pose, OLocalMap& Map); 23 | enum Move {Forward, Backward, Left, Right, TurnRight, TurnLeft}; 24 | }; 25 | 26 | template 27 | Optimizer::Optimizer(const OptimizerParams& p): 28 | params(p), 29 | lmap(p.discretization){} 30 | 31 | template 32 | OrientedPoint Optimizer::gradientDescent(const RangeReading& oldReading, const RangeReading& newReading){ 33 | lmap.clear(); 34 | lmap.update(oldReading, OrientedPoint(0,0,0), params.maxRange); 35 | OrientedPoint delta=absoluteDifference(newReading.getPose(), oldReading.getPose()); 36 | OrientedPoint bestPose=delta; 37 | double bestScore=likelihood(lmap, newReading, bestPose, params.maxRange); 38 | int it=0; 39 | double lstep=params.linearStep, astep=params.angularStep; 40 | bool increase; 41 | /* cerr << "bestScore=" << bestScore << endl;;*/ 42 | do { 43 | increase=false; 44 | OrientedPoint itBestPose=bestPose; 45 | double itBestScore=bestScore; 46 | bool itIncrease; 47 | do { 48 | itIncrease=false; 49 | OrientedPoint testBestPose=itBestPose; 50 | double testBestScore=itBestScore; 51 | for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){ 52 | OrientedPoint testPose=itBestPose; 53 | switch(move){ 54 | case Forward: testPose.x+=lstep; 55 | break; 56 | case Backward: testPose.x-=lstep; 57 | break; 58 | case Left: testPose.y+=lstep; 59 | break; 60 | case Right: testPose.y-=lstep; 61 | break; 62 | case TurnRight: testPose.theta-=astep; 63 | break; 64 | case TurnLeft: testPose.theta+=astep; 65 | break; 66 | } 67 | double score=likelihood(lmap, newReading, testPose, params.maxRange); 68 | if (score>testBestScore){ 69 | testBestScore=score; 70 | testBestPose=testPose; 71 | } 72 | } 73 | if (testBestScore > itBestScore){ 74 | itBestScore=testBestScore; 75 | itBestPose=testBestPose; 76 | /* cerr << "s=" << itBestScore << " ";*/ 77 | itIncrease=true; 78 | } 79 | } while(itIncrease); 80 | if (itBestScore > bestScore){ 81 | /* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/ 82 | bestScore=itBestScore; 83 | bestPose=itBestPose; 84 | increase=true; 85 | } else { 86 | it++; 87 | lstep*=0.5; 88 | astep*=0.5; 89 | } 90 | } while (it 97 | OrientedPoint Optimizer::gradientDescent(const RangeReading& reading, const OrientedPoint& pose, OLocalMap& lmap){ 98 | OrientedPoint bestPose=pose; 99 | double bestScore=likelihood(lmap, reading, bestPose, params.maxRange); 100 | int it=0; 101 | double lstep=params.linearStep, astep=params.angularStep; 102 | bool increase; 103 | /* cerr << "bestScore=" << bestScore << endl;;*/ 104 | do { 105 | increase=false; 106 | OrientedPoint itBestPose=bestPose; 107 | double itBestScore=bestScore; 108 | bool itIncrease; 109 | do { 110 | itIncrease=false; 111 | OrientedPoint testBestPose=itBestPose; 112 | double testBestScore=itBestScore; 113 | for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){ 114 | OrientedPoint testPose=itBestPose; 115 | switch(move){ 116 | case Forward: testPose.x+=lstep; 117 | break; 118 | case Backward: testPose.x-=lstep; 119 | break; 120 | case Left: testPose.y+=lstep; 121 | break; 122 | case Right: testPose.y-=lstep; 123 | break; 124 | case TurnRight: testPose.theta-=astep; 125 | break; 126 | case TurnLeft: testPose.theta+=astep; 127 | break; 128 | } 129 | double score=likelihood(lmap, reading, testPose, params.maxRange); 130 | if (score>testBestScore){ 131 | testBestScore=score; 132 | testBestPose=testPose; 133 | } 134 | } 135 | if (testBestScore > itBestScore){ 136 | itBestScore=testBestScore; 137 | itBestPose=testBestPose; 138 | /* cerr << "s=" << itBestScore << " ";*/ 139 | itIncrease=true; 140 | } 141 | } while(itIncrease); 142 | if (itBestScore > bestScore){ 143 | /* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/ 144 | bestScore=itBestScore; 145 | bestPose=itBestPose; 146 | increase=true; 147 | } else { 148 | it++; 149 | lstep*=0.5; 150 | astep*=0.5; 151 | } 152 | } while (it 27 | #include 28 | #include 29 | 30 | #include "rclcpp/rclcpp.hpp" 31 | #include "sensor_msgs/msg/laser_scan.hpp" 32 | #include "std_msgs/msg/float64.hpp" 33 | #include "nav_msgs/msg/occupancy_grid.hpp" 34 | #include "nav_msgs/msg/map_meta_data.hpp" 35 | #include "geometry_msgs/msg/pose.hpp" 36 | #include "geometry_msgs/msg/pose_stamped.hpp" 37 | #include "geometry_msgs/msg/transform_stamped.hpp" 38 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 39 | #include "tf2_ros/transform_listener.h" 40 | #include "tf2_ros/transform_broadcaster.h" 41 | #include "tf2/utils.h" 42 | #include "message_filters/subscriber.h" 43 | #include "tf2_ros/message_filter.h" 44 | 45 | #include "gmapping/gridfastslam/gridslamprocessor.h" 46 | #include "gmapping/sensor/sensor_base/sensor.h" 47 | #include "gmapping/sensor/sensor_range/rangesensor.h" 48 | #include "gmapping/sensor/sensor_odometry/odometrysensor.h" 49 | 50 | class SlamGmapping : public rclcpp::Node{ 51 | public: 52 | SlamGmapping(); 53 | ~SlamGmapping() override; 54 | 55 | void init(); 56 | void startLiveSlam(); 57 | void publishTransform(); 58 | void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); 59 | void publishLoop(double transform_publish_period); 60 | 61 | private: 62 | rclcpp::Node::SharedPtr node_; 63 | rclcpp::Publisher::SharedPtr entropy_publisher_; 64 | rclcpp::Publisher::SharedPtr sst_; 65 | rclcpp::Publisher::SharedPtr sstm_; 66 | 67 | std::shared_ptr buffer_; 68 | std::shared_ptr tfl_; 69 | 70 | std::shared_ptr> scan_filter_sub_; 71 | std::shared_ptr> scan_filter_; 72 | std::shared_ptr tfB_; 73 | 74 | GMapping::GridSlamProcessor* gsp_; 75 | GMapping::RangeSensor* gsp_laser_; 76 | // The angles in the laser, going from -x to x (adjustment is made to get the laser between 77 | // symmetrical bounds as that's what gmapping expects) 78 | std::vector laser_angles_; 79 | // The pose, in the original laser frame, of the corresponding centered laser with z facing up 80 | geometry_msgs::msg::PoseStamped centered_laser_pose_; 81 | // Depending on the order of the elements in the scan and the orientation of the scan frame, 82 | // We might need to change the order of the scan 83 | bool do_reverse_range_; 84 | unsigned int gsp_laser_beam_count_; 85 | GMapping::OdometrySensor* gsp_odom_; 86 | 87 | bool got_first_scan_; 88 | 89 | bool got_map_; 90 | nav_msgs::msg::OccupancyGrid map_; 91 | 92 | tf2::Duration map_update_interval_; 93 | tf2::Transform map_to_odom_; 94 | std::mutex map_to_odom_mutex_; 95 | std::mutex map_mutex_; 96 | 97 | int laser_count_; 98 | int throttle_scans_; 99 | 100 | std::shared_ptr transform_thread_; 101 | 102 | std::string base_frame_; 103 | std::string laser_frame_; 104 | std::string map_frame_; 105 | std::string odom_frame_; 106 | 107 | void updateMap(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); 108 | bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const rclcpp::Time& t); 109 | bool initMapper(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); 110 | bool addScan(sensor_msgs::msg::LaserScan::ConstSharedPtr scan, GMapping::OrientedPoint& gmap_pose); 111 | double computePoseEntropy(); 112 | 113 | // Parameters used by GMapping 114 | double maxRange_; 115 | double maxUrange_; 116 | double maxrange_; 117 | double minimum_score_; 118 | double sigma_; 119 | int kernelSize_; 120 | double lstep_; 121 | double astep_; 122 | int iterations_; 123 | double lsigma_; 124 | double ogain_; 125 | int lskip_; 126 | double srr_; 127 | double srt_; 128 | double str_; 129 | double stt_; 130 | double linearUpdate_; 131 | double angularUpdate_; 132 | double temporalUpdate_; 133 | double resampleThreshold_; 134 | int particles_; 135 | double xmin_; 136 | double ymin_; 137 | double xmax_; 138 | double ymax_; 139 | double delta_; 140 | double occ_thresh_; 141 | double llsamplerange_; 142 | double llsamplestep_; 143 | double lasamplerange_; 144 | double lasamplestep_; 145 | 146 | unsigned long int seed_; 147 | 148 | double transform_publish_period_; 149 | double tf_delay_; 150 | }; 151 | 152 | #endif //SLAM_GMAPPING_SLAM_GMAPPING_H_ 153 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/grid/array2d.h: -------------------------------------------------------------------------------- 1 | #ifndef ARRAY2D_H 2 | #define ARRAY2D_H 3 | 4 | #include 5 | #include 6 | #include "accessstate.h" 7 | 8 | #include 9 | 10 | #ifndef __PRETTY_FUNCTION__ 11 | #define __FUNCDNAME__ 12 | #endif 13 | 14 | namespace GMapping { 15 | 16 | template class Array2D{ 17 | public: 18 | Array2D(int xsize=0, int ysize=0); 19 | Array2D& operator=(const Array2D &); 20 | Array2D(const Array2D &); 21 | ~Array2D(); 22 | void clear(); 23 | void resize(int xmin, int ymin, int xmax, int ymax); 24 | 25 | 26 | inline bool isInside(int x, int y) const; 27 | inline const Cell& cell(int x, int y) const; 28 | inline Cell& cell(int x, int y); 29 | inline AccessibilityState cellState(int x, int y) const { return (AccessibilityState) (isInside(x,y)?(Inside|Allocated):Outside);} 30 | 31 | inline bool isInside(const IntPoint& p) const { return isInside(p.x, p.y);} 32 | inline const Cell& cell(const IntPoint& p) const {return cell(p.x,p.y);} 33 | inline Cell& cell(const IntPoint& p) {return cell(p.x,p.y);} 34 | inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x, p.y);} 35 | 36 | inline int getPatchSize() const{return 0;} 37 | inline int getPatchMagnitude() const{return 0;} 38 | inline int getXSize() const {return m_xsize;} 39 | inline int getYSize() const {return m_ysize;} 40 | inline Cell** cells() {return m_cells;} 41 | Cell ** m_cells; 42 | protected: 43 | int m_xsize, m_ysize; 44 | }; 45 | 46 | 47 | template 48 | Array2D::Array2D(int xsize, int ysize){ 49 | // assert(xsize>0); 50 | // assert(ysize>0); 51 | m_xsize=xsize; 52 | m_ysize=ysize; 53 | if (m_xsize>0 && m_ysize>0){ 54 | m_cells=new Cell*[m_xsize]; 55 | for (int i=0; i" << endl; 47 | cout << "\t -xmax " << endl; 48 | cout << "\t -ymin " << endl; 49 | cout << "\t -ymax " << endl; 50 | cout << "\t -maxrange : maxmimum preception range" << endl; 51 | cout << "\t -delta : patch size" << endl; 52 | cout << "\t -patchDelta : patch cell size" << endl; 53 | cout << "\t -lstep : linear serach step" << endl; 54 | cout << "\t -astep : ìangular search step" << endl; 55 | cout << "\t -regscore : registration scan score" << endl; 56 | cout << "\t -filename : log filename in carmen format" << endl; 57 | cout << "\t -sigma : convolution kernel size" << endl; 58 | cout << "Look the code for discovering another thousand of unuseful parameters" << endl; 59 | return -1; 60 | } 61 | 62 | CMD_PARSE_BEGIN(1,argc); 63 | parseString("-filename",filename); 64 | parseString("-outfilename",outfilename); 65 | parseDouble("-xmin",xmin); 66 | parseDouble("-xmax",xmax); 67 | parseDouble("-ymin",ymin); 68 | parseDouble("-ymax",ymax); 69 | parseDouble("-delta",delta); 70 | parseDouble("-patchDelta",patchDelta); 71 | parseDouble("-maxrange",maxrange); 72 | parseDouble("-maxUrange",maxUrange); 73 | parseDouble("-regscore",regscore); 74 | parseDouble("-critscore",critscore); 75 | parseInt("-kernelSize",kernelSize); 76 | parseDouble("-sigma",sigma); 77 | parseInt("-iterations",iterations); 78 | parseDouble("-lstep",lstep); 79 | parseDouble("-astep",astep); 80 | parseDouble("-maxMove",maxMove); 81 | parseFlag("-computeCovariance",computeCovariance); 82 | parseFlag("-stdin", readFromStdin); 83 | parseFlag("-useICP", useICP); 84 | parseDouble("-laserx",laserx); 85 | parseDouble("-lasery",lasery); 86 | parseDouble("-lasertheta",lasertheta); 87 | CMD_PARSE_END; 88 | 89 | if (!filename.size()){ 90 | cout << "no filename specified" << endl; 91 | return -1; 92 | } 93 | 94 | ifstream is; 95 | is.open(filename.c_str()); 96 | if (! is){ 97 | cout << "no file found" << endl; 98 | return -1; 99 | } 100 | 101 | 102 | DEBUG << "scanmatcher processor construction" << endl; 103 | ScanMatcherProcessor scanmatcher(xmin, ymin, xmax, ymax, delta, patchDelta); 104 | 105 | //double range, double sigma, int kernsize, double lopt, double aopt, int iterations 106 | scanmatcher.setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, computeCovariance); 107 | scanmatcher.setRegistrationParameters(regscore, critscore); 108 | scanmatcher.setmaxMove(maxMove); 109 | scanmatcher.useICP=useICP; 110 | scanmatcher.matcher().setlaserPose(OrientedPoint(laserx,lasery,lasertheta)); 111 | 112 | CarmenConfiguration conf; 113 | conf.load(is); 114 | is.close(); 115 | 116 | SensorMap sensorMap=conf.computeSensorMap(); 117 | scanmatcher.setSensorMap(sensorMap); 118 | 119 | InputSensorStream* input=0; 120 | 121 | ifstream plainStream; 122 | if (! readFromStdin){ 123 | plainStream.open(filename.c_str()); 124 | input=new InputSensorStream(sensorMap, plainStream); 125 | cout << "Plain Stream opened="<< (bool) plainStream << endl; 126 | } else { 127 | input=new InputSensorStream(sensorMap, cin); 128 | cout << "Plain Stream opened on stdin" << endl; 129 | } 130 | 131 | /* 132 | SensorLog log(sensorMap); 133 | ifstream logstream(filename); 134 | log.load(logstream); 135 | logstream.close(); 136 | cout << "Log loaded " << log.size() << " records" << endl; 137 | */ 138 | ostream* output; 139 | ofstream poseStream; 140 | if (! readFromStdin){ 141 | if (! outfilename.size()){ 142 | outfilename=string("scanmatched")+filename; 143 | } 144 | poseStream.open(outfilename.c_str()); 145 | output=&poseStream; 146 | } else { 147 | output=&cout; 148 | } 149 | scanmatcher.init(); 150 | ofstream odopathStream("odopath.dat"); 151 | while (*input){ 152 | const SensorReading* r; 153 | (*input) >> r; 154 | if (! r) 155 | continue; 156 | const RangeReading* rr=dynamic_cast(r); 157 | if (rr){ 158 | const RangeSensor* s=dynamic_cast(r->getSensor()); 159 | bool isFront= s->getPose().theta==0; 160 | 161 | if (! readFromStdin){ 162 | cout << "." << flush; 163 | } 164 | const RangeSensor* rs=dynamic_cast(rr->getSensor()); 165 | assert (rs && rs->beams().size()==rr->size()); 166 | odopathStream << rr->getPose().x << " " << rr->getPose().y << endl; 167 | scanmatcher.processScan(*rr); 168 | OrientedPoint p=scanmatcher.getPose(); 169 | if (isFront) 170 | *output << "FLASER "<< rr->size() << " "; 171 | else 172 | *output << "RLASER "<< rr->size() << " "; 173 | for (RangeReading::const_iterator b=rr->begin(); b!=rr->end(); b++){ 174 | *output << *b << " "; 175 | } 176 | *output << p.x << " " << p.y << " " << p.theta << " "; 177 | //p=rr->getPose(); 178 | double t=rr->getTime(); //FIXME 179 | *output << p.x << " " << p.y << " " << p.theta << " "; 180 | *output << t << " nohost " << t << endl; 181 | } 182 | } 183 | if (! readFromStdin){ 184 | poseStream.close(); 185 | } 186 | } 187 | -------------------------------------------------------------------------------- /openslam_gmapping/include/gmapping/utils/point.h: -------------------------------------------------------------------------------- 1 | #ifndef _POINT_H_ 2 | #define _POINT_H_ 3 | #include 4 | #include 5 | #include 6 | #include "gvalues.h" 7 | 8 | #define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME 9 | 10 | namespace GMapping { 11 | 12 | template 13 | struct point{ 14 | inline point():x(0),y(0) {} 15 | inline point(T _x, T _y):x(_x),y(_y){} 16 | T x, y; 17 | }; 18 | 19 | template 20 | inline point operator+(const point& p1, const point& p2){ 21 | return point(p1.x+p2.x, p1.y+p2.y); 22 | } 23 | 24 | template 25 | inline point operator - (const point & p1, const point & p2){ 26 | return point(p1.x-p2.x, p1.y-p2.y); 27 | } 28 | 29 | template 30 | inline point operator * (const point& p, const T& v){ 31 | return point(p.x*v, p.y*v); 32 | } 33 | 34 | template 35 | inline point operator * (const T& v, const point& p){ 36 | return point(p.x*v, p.y*v); 37 | } 38 | 39 | template 40 | inline T operator * (const point& p1, const point& p2){ 41 | return p1.x*p2.x+p1.y*p2.y; 42 | } 43 | 44 | 45 | template 46 | struct orientedpoint: public point{ 47 | inline orientedpoint() : point(0,0), theta(0) {}; 48 | inline orientedpoint(const point& p); 49 | inline orientedpoint(T x, T y, A _theta): point(x,y), theta(_theta){} 50 | inline void normalize(); 51 | inline orientedpoint rotate(A alpha){ 52 | T s=sin(alpha), c=cos(alpha); 53 | A a=alpha+theta; 54 | a=atan2(sin(a),cos(a)); 55 | return orientedpoint( 56 | c*this->x-s*this->y, 57 | s*this->x+c*this->y, 58 | a); 59 | } 60 | A theta; 61 | }; 62 | 63 | 64 | template 65 | void orientedpoint::normalize() { 66 | if (theta >= -M_PI && theta < M_PI) 67 | return; 68 | 69 | int multiplier = (int)(theta / (2*M_PI)); 70 | theta = theta - multiplier*2*M_PI; 71 | if (theta >= M_PI) 72 | theta -= 2*M_PI; 73 | if (theta < -M_PI) 74 | theta += 2*M_PI; 75 | } 76 | 77 | 78 | template 79 | orientedpoint::orientedpoint(const point& p){ 80 | this->x=p.x; 81 | this->y=p.y; 82 | this->theta=0.; 83 | } 84 | 85 | 86 | template 87 | orientedpoint operator+(const orientedpoint& p1, const orientedpoint& p2){ 88 | return orientedpoint(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta); 89 | } 90 | 91 | template 92 | orientedpoint operator - (const orientedpoint & p1, const orientedpoint & p2){ 93 | return orientedpoint(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta); 94 | } 95 | 96 | template 97 | orientedpoint operator * (const orientedpoint& p, const T& v){ 98 | return orientedpoint(p.x*v, p.y*v, p.theta*v); 99 | } 100 | 101 | template 102 | orientedpoint operator * (const T& v, const orientedpoint& p){ 103 | return orientedpoint(p.x*v, p.y*v, p.theta*v); 104 | } 105 | 106 | template 107 | orientedpoint absoluteDifference(const orientedpoint& p1,const orientedpoint& p2){ 108 | orientedpoint delta=p1-p2; 109 | delta.theta=atan2(sin(delta.theta), cos(delta.theta)); 110 | double s=sin(p2.theta), c=cos(p2.theta); 111 | return orientedpoint(c*delta.x+s*delta.y, 112 | -s*delta.x+c*delta.y, delta.theta); 113 | } 114 | 115 | template 116 | orientedpoint absoluteSum(const orientedpoint& p1,const orientedpoint& p2){ 117 | double s=sin(p1.theta), c=cos(p1.theta); 118 | return orientedpoint(c*p2.x-s*p2.y, 119 | s*p2.x+c*p2.y, p2.theta) + p1; 120 | } 121 | 122 | template 123 | point absoluteSum(const orientedpoint& p1,const point& p2){ 124 | double s=sin(p1.theta), c=cos(p1.theta); 125 | return point(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point) p1; 126 | } 127 | 128 | template 129 | struct pointcomparator{ 130 | bool operator ()(const point& a, const point& b) const { 131 | return a.x 136 | struct pointradialcomparator{ 137 | point origin; 138 | bool operator ()(const point& a, const point& b) const { 139 | point delta1=a-origin; 140 | point delta2=b-origin; 141 | return (atan2(delta1.y,delta1.x) 146 | inline point max(const point& p1, const point& p2){ 147 | point p=p1; 148 | p.x=p.x>p2.x?p.x:p2.x; 149 | p.y=p.y>p2.y?p.y:p2.y; 150 | return p; 151 | } 152 | 153 | template 154 | inline point min(const point& p1, const point& p2){ 155 | point p=p1; 156 | p.x=p.x 162 | inline point interpolate(const point& p1, const F& t1, const point& p2, const F& t2, const F& t3){ 163 | F gain=(t3-t1)/(t2-t1); 164 | point p=p1+(p2-p1)*gain; 165 | return p; 166 | } 167 | 168 | template 169 | inline orientedpoint 170 | interpolate(const orientedpoint& p1, const F& t1, const orientedpoint& p2, const F& t2, const F& t3){ 171 | F gain=(t3-t1)/(t2-t1); 172 | orientedpoint p; 173 | p.x=p1.x+(p2.x-p1.x)*gain; 174 | p.y=p1.y+(p2.y-p1.y)*gain; 175 | double s=sin(p1.theta)+sin(p2.theta)*gain, 176 | c=cos(p1.theta)+cos(p2.theta)*gain; 177 | p.theta=atan2(s,c); 178 | return p; 179 | } 180 | 181 | 182 | template 183 | inline double euclidianDist(const point& p1, const point& p2){ 184 | return hypot(p1.x-p2.x, p1.y-p2.y); 185 | } 186 | template 187 | inline double euclidianDist(const orientedpoint& p1, const orientedpoint& p2){ 188 | return hypot(p1.x-p2.x, p1.y-p2.y); 189 | } 190 | template 191 | inline double euclidianDist(const orientedpoint& p1, const point& p2){ 192 | return hypot(p1.x-p2.x, p1.y-p2.y); 193 | } 194 | template 195 | inline double euclidianDist(const point& p1, const orientedpoint& p2 ){ 196 | return hypot(p1.x-p2.x, p1.y-p2.y); 197 | } 198 | 199 | 200 | 201 | typedef point IntPoint; 202 | typedef point Point; 203 | typedef orientedpoint OrientedPoint; 204 | 205 | }; //end namespace 206 | 207 | #endif 208 | -------------------------------------------------------------------------------- /openslam_gmapping/utils/dmatrix.h: -------------------------------------------------------------------------------- 1 | #ifndef DMATRIX_HXX 2 | #define DMATRIX_HXX 3 | 4 | #include 5 | #include 6 | namespace GMapping { 7 | 8 | class DNotInvertibleMatrixException: public std::exception {}; 9 | class DIncompatibleMatrixException: public std::exception {}; 10 | class DNotSquareMatrixException: public std::exception {}; 11 | 12 | template class DMatrix { 13 | public: 14 | DMatrix(int n=0,int m=0); 15 | ~DMatrix(); 16 | 17 | DMatrix(const DMatrix&); 18 | DMatrix& operator=(const DMatrix&); 19 | 20 | X * operator[](int i) { 21 | if ((*shares)>1) detach(); 22 | return mrows[i]; 23 | } 24 | 25 | const X * operator[](int i) const { return mrows[i]; } 26 | 27 | const X det() const; 28 | DMatrix inv() const; 29 | DMatrix transpose() const; 30 | DMatrix operator*(const DMatrix&) const; 31 | DMatrix operator+(const DMatrix&) const; 32 | DMatrix operator-(const DMatrix&) const; 33 | DMatrix operator*(const X&) const; 34 | 35 | int rows() const { return nrows; } 36 | int columns() const { return ncols; } 37 | 38 | void detach(); 39 | 40 | static DMatrix I(int); 41 | 42 | protected: 43 | int nrows,ncols; 44 | X * elems; 45 | X ** mrows; 46 | 47 | int * shares; 48 | }; 49 | 50 | template DMatrix::DMatrix(int n,int m) { 51 | if (n<1) n=1; 52 | if (m<1) m=1; 53 | nrows=n; 54 | ncols=m; 55 | elems=new X[nrows*ncols]; 56 | mrows=new X* [nrows]; 57 | for (int i=0;i DMatrix::~DMatrix() { 64 | if (--(*shares)) return; 65 | delete [] elems; 66 | delete [] mrows; 67 | delete shares; 68 | } 69 | 70 | template DMatrix::DMatrix(const DMatrix& m) { 71 | shares=m.shares; 72 | elems=m.elems; 73 | nrows=m.nrows; 74 | ncols=m.ncols; 75 | mrows=m.mrows; 76 | (*shares)++; 77 | } 78 | 79 | template DMatrix& DMatrix::operator=(const DMatrix& m) { 80 | if (!--(*shares)) { 81 | delete [] elems; 82 | delete [] mrows; 83 | delete shares; 84 | } 85 | shares=m.shares; 86 | elems=m.elems; 87 | nrows=m.nrows; 88 | ncols=m.ncols; 89 | mrows=m.mrows; 90 | (*shares)++; 91 | return *this; 92 | } 93 | 94 | template DMatrix DMatrix::inv() const { 95 | if (nrows!=ncols) throw DNotInvertibleMatrixException(); 96 | DMatrix aux1(*this),aux2(I(nrows)); 97 | aux1.detach(); 98 | for (int i=0;i=nrows) throw DNotInvertibleMatrixException(); 102 | X val=aux1.mrows[k][i]; 103 | for (int j=0;j const X DMatrix::det() const { 130 | if (nrows!=ncols) throw DNotSquareMatrixException(); 131 | DMatrix aux(*this); 132 | X d=X(1); 133 | aux.detach(); 134 | for (int i=0;i=nrows) return X(0); 138 | X val=aux.mrows[k][i]; 139 | for (int j=0;j DMatrix DMatrix::transpose() const { 165 | DMatrix aux(ncols, nrows); 166 | for (int i=0; i DMatrix DMatrix::operator*(const DMatrix& m) const { 173 | if (ncols!=m.nrows) throw DIncompatibleMatrixException(); 174 | DMatrix aux(nrows,m.ncols); 175 | for (int i=0;i DMatrix DMatrix::operator+(const DMatrix& m) const { 186 | if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException(); 187 | DMatrix aux(nrows,ncols); 188 | for (int i=0;i DMatrix DMatrix::operator-(const DMatrix& m) const { 193 | if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException(); 194 | DMatrix aux(nrows,ncols); 195 | for (int i=0;i DMatrix DMatrix::operator*(const X& e) const { 200 | DMatrix aux(nrows,ncols); 201 | for (int i=0;i void DMatrix::detach() { 206 | DMatrix aux(nrows,ncols); 207 | for (int i=0;i DMatrix DMatrix::I(int n) { 212 | DMatrix aux(n,n); 213 | for (int i=0;i std::ostream& operator<<(std::ostream& os, const DMatrix &m) { 218 | os << "{"; 219 | for (int i=0;i0) os << ","; 221 | os << "{"; 222 | for (int j=0;j0) os << ","; 224 | os << m[i][j]; 225 | } 226 | os << "}"; 227 | } 228 | return os << "}"; 229 | } 230 | 231 | }; //namespace GMapping 232 | #endif 233 | --------------------------------------------------------------------------------