diff --git a/slam_gmapping/.gitignore b/slam_gmapping/.gitignore new file mode 100644 index 0000000..37e6331 --- /dev/null +++ b/slam_gmapping/.gitignore @@ -0,0 +1,29 @@ +*/build/ +*/build_isolated/ +*/devel/ +*/devel_isolated/ +*/build_isolated/ +*/install_isolated/ +*/logs/ +docs/ +*/.catkin_workspace +*/DISABLED +*.bag +*.pyc +cmake-build-debug/ +cmake-build-release/ +*.idea +*.swp +*.kate-swp +*.director +*.DS_STORE +*.catkin_tools +*.bag +*.bag.active +*.stl +*.directory +build +log +!openslam_gmapping/include/gmapping/log/ +!openslam_gmapping/log/ +install diff --git a/slam_gmapping/README.md b/slam_gmapping/README.md new file mode 100644 index 0000000..c3babdb --- /dev/null +++ b/slam_gmapping/README.md @@ -0,0 +1,17 @@ +# SLAM_GMAPPING + +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. + +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. + +## Launch: + +```bash +ros2 launch slam_gmapping slam_gmapping.launch.py +``` + +The node slam_gmapping subscribes to sensor_msgs/LaserScan on ros2 topic ``scan``. It also expects appropriate TF to be available. + +It publishes the nav_msgs/OccupancyGrid on ``map``. + +Map Meta Data and Entropy is published on ``map_metadata`` and ``entropy`` respectively. diff --git a/slam_gmapping/openslam_gmapping/CHANGELOG.rst b/slam_gmapping/openslam_gmapping/CHANGELOG.rst new file mode 100644 index 0000000..1649f0d --- /dev/null +++ b/slam_gmapping/openslam_gmapping/CHANGELOG.rst @@ -0,0 +1,25 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package openslam_gmapping +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.2 (2016-04-23) +------------------ +* better Windows compilation + This is taken from `#9 `_ which can now be closed. +* fix a few more graphics stuff for Qt5 +* get GUI back in shape for those interested +* use srand instead of srand48 + srand48 is non-standard and we are using a seed that is an + unsigned int so we might as well use srand +* Contributors: Vincent Rabaud + +0.1.1 (2015-06-25) +------------------ +* fix cppcheck warnings +* License from BSD to CC +* Contributors: Isaac IY Saito, Vincent Rabaud + +0.1.0 (2013-06-28 17:33:53 -0700) +--------------------------------- +- Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ +- Catkinized and prepared for release into the ROS ecosystem diff --git a/slam_gmapping/openslam_gmapping/CMakeLists.txt b/slam_gmapping/openslam_gmapping/CMakeLists.txt new file mode 100644 index 0000000..b63faba --- /dev/null +++ b/slam_gmapping/openslam_gmapping/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.5) +project(openslam_gmapping) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-O3 -funroll-loops) +endif() + +add_compile_options(-fPIC) +find_package(ament_cmake REQUIRED) + +include_directories(include) + +add_subdirectory(gridfastslam) +add_subdirectory(scanmatcher) +add_subdirectory(sensor) +add_subdirectory(utils) + +ament_export_libraries(gridfastslam) +ament_export_libraries(scanmatcher) +ament_export_libraries(sensor_base) +ament_export_libraries(sensor_odometry) +ament_export_libraries(sensor_range) +ament_export_libraries(utils) +ament_export_include_directories(include) + +install(DIRECTORY include/ + DESTINATION include/) + +ament_package() diff --git a/slam_gmapping/openslam_gmapping/README b/slam_gmapping/openslam_gmapping/README new file mode 100644 index 0000000..c5b275a --- /dev/null +++ b/slam_gmapping/openslam_gmapping/README @@ -0,0 +1,2 @@ +This is a fork from gmapping at https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ +It includes a few patches that could be pushed upstream if needed diff --git a/slam_gmapping/openslam_gmapping/build_tools/Makefile.app b/slam_gmapping/openslam_gmapping/build_tools/Makefile.app new file mode 100644 index 0000000..9668830 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/build_tools/Makefile.app @@ -0,0 +1,80 @@ +# Makefile generico per applicazione +# +# Variabili: +# APPS lista delle applicazioni +# OBJS lista degli oggetti +# QOBJS lista degli oggetti QT +# LIBS librerie +# +# Ogni applicazione viene linkata con tutti gli oggetti + +export VERBOSE + +ifeq ($(LINUX),1) +CPPFLAGS+=-DLINUX +endif + + +APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a)) +all: $(APPLICATIONS) + +PACKAGE=$(notdir $(shell pwd)) + +.SECONDARY: $(OBJS) $(QOBJS) +.PHONY: all clean copy doc + +$(QOBJS): %.o: %.cpp moc_%.cpp + @$(MESSAGE) "Compiling (QT) $@" + @$(PRETTY) "$(CXX) $(CPPFLAGS) $(QT_INCLUDE) $(CXXFLAGS) -c $< -o $@" + +moc_%.cpp: %.h + @$(MESSAGE) "Generating MOC $@" + @$(PRETTY) "$(MOC) -i $< -o $@" + +# Generazione degli oggetti +%.o: %.cpp + @$(MESSAGE) "Compiling $@" + @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@" + +# Generazione delle applicazioni +$(BINDIR)/%: %.cpp $(OBJS) $(QOBJS) + @$(MESSAGE) "Linking application `basename $@`" + @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $(OBJS) $(QOBJS) $< -L$(LIBDIR) $(LIBS) -o $@" + +#Regole per la generazione di tabelle o altri file creati automaticamente +table_%.cpp: gen_table_% + @$(MESSAGE) "Generating $@" + @$(PRETTY) "./$< > $@" + +gen_table_%: gen_table_%.cpp + @$(MESSAGE) "Generating $@" + @$(PRETTY) "$(CXX) $(CPPFLAGS) $(CXXFLAGS) $< -o $@" + +#Regole per la generazione delle dipendenze +OBJDEPS=$(foreach module,$(basename $(OBJS) $(QOBJS)),$(module).d) + +$(OBJDEPS): %.d: %.cpp # ci va o no? %.h + @$(MESSAGE) "Generating dependecies $@" + @$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG -MF $@ $<" + +ifneq ($(MAKECMDGOALS),clean) +ifneq ($(MAKECMDGOALS),copy) +-include $(OBJDEPS) +endif +endif + +doc: + rm -rf doc/$(PACKAGE) +ifeq ($(strip $(DOCTITLE)),) + kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS) +else + kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS) +endif + +clean: + @$(MESSAGE) "Cleaning $(PACKAGE)" + @$(PRETTY) "rm -f *.d *.o moc_*.cpp *.d core *~ table_*.cpp gen_table*[^.][^c][^p][^p] $(APPLICATIONS)" + @$(PRETTY) "rm -rf doc/$(PACKAGE)" + +copy: clean + tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE) diff --git a/slam_gmapping/openslam_gmapping/build_tools/Makefile.generic-shared-object b/slam_gmapping/openslam_gmapping/build_tools/Makefile.generic-shared-object new file mode 100644 index 0000000..8bb7ec6 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/build_tools/Makefile.generic-shared-object @@ -0,0 +1,109 @@ +# Makefile generico per shared object +export VERBOSE +export CXX + +# Nome del package +PACKAGE=$(notdir $(shell pwd)) + +# Libreria da generare: +# Se non si setta la variabile LIBNAME la libreria si chiama +# come la directory +ifndef LIBNAME +LIBNAME=$(PACKAGE) +endif + +ifeq ($(MACOSX),1) +SONAME=$(LIBDIR)/lib$(LIBNAME).dylib +endif + +ifeq ($(LINUX),1) +SONAME=$(LIBDIR)/lib$(LIBNAME).so +endif + +APPLICATIONS= $(foreach a, $(APPS),$(BINDIR)/$(a)) +INSTALL_SCRIPTS=$(foreach a, $(SCRIPTS),$(BINDIR)/$(a)) + +all: $(SONAME) $(APPLICATIONS) $(INSTALL_SCRIPTS) + +.SECONDARY: $(OBJS) $(COBJS) +.PHONY: all clean copy doc + +# Generazione della libreria +$(SONAME): $(OBJS) $(COBJS) + @$(MESSAGE) "Creating library lib$(LIBNAME).so" +ifeq ($(MACOSX),1) + @$(PRETTY) "$(CXX) $(LDFLAGS) -dynamiclib $(OBJS) $(COBJS) -L$(LIBDIR) $(LIBS) -install_name $@ -o $@" +endif +ifeq ($(LINUX),1) + @$(PRETTY) "$(CXX) -fPIC -shared $(OBJS) $(COBJS) -L $(LIBDIR) $(LIBS) $(LDFLAGS) -o $@" + @if ! $(PRETTY) "$(TESTLIB) $(SONAME)"; then $(MESSAGE) "Testing of $(SONAME) failed."; rm $(SONAME); exit 1; fi; +endif + +# Generazione delle applicazioni +$(BINDIR)/%: %.o $(SONAME) + @$(MESSAGE) "Linking application `basename "$@"`" + @$(PRETTY) "$(CXX) $< -l$(LIBNAME) $(LDFLAGS) -L$(LIBDIR) $(LIBS) -o $@" + +#Generazione dei moc files +moc_%.cpp: %.h + @$(MESSAGE) "Compiling MOC $@" + @$(PRETTY) "$(MOC) -i $< -o $@" + +# Generazione degli oggetti +%.o: %.cpp + @$(MESSAGE) "Compiling $<" + @$(PRETTY) "$(CXX) -fPIC $(CPPFLAGS) $(CXXFLAGS) -c $< -o $@" + +%.o: %.c + @$(MESSAGE) "Compiling $<" + @$(PRETTY) "$(CC) -fPIC $(CPPFLAGS) $(CFLAGS) -c $< -o $@" + +#Regole per la generazione delle dipendenze +OBJDEPS= $(foreach module,$(basename $(OBJS)),$(module).d) $(foreach a, $(APPS),$(a).d) +COBJDEPS=$(foreach module,$(basename $(COBJS)),$(module).d) + +$(OBJDEPS): %.d: %.cpp + @$(MESSAGE) "Generating dependencies for $<" + @$(PRETTY) "$(CXX) $(CPPFLAGS) -MM -MG $< -MF $@" + +$(COBJDEPS): %.d: %.c + @$(MESSAGE) "Generating dependencies for $<" + @$(PRETTY) "$(CC) $(CPPFLAGS) -MM -MG $< -MF $@" + +#HEADERS=`ls *.h` +#PRECOMPILED_HEADERS=$(foreach file,$(basename $(HEADERS)), $(file).pch) + +ifneq ($(MAKECMDGOALS),clean) +ifneq ($(MAKECMDGOALS),copy) +ifneq ($(MAKECMDGOALS),dep) +-include $(OBJDEPS) $(COBJDEPS) +endif +endif +endif + +dep: $(OBJDEPS) $(COBJDEPS) + + +# GLi script vengono semplicemente copiati +$(BINDIR)/%.sh: %.sh + @$(MESSAGE) "Installing script `basename "$@"`" + @$(PRETTY) "cp $< $@" + @$(PRETTY) "chmod +x $@" + + +#doc: +# rm -rf doc/$(PACKAGE) +#ifeq ($(strip $(DOCTITLE)),) +# kdoc -L doc -d doc/$(PACKAGE) -n "Package $(PACKAGE) (lib$(PACKAGE).so)" $(HEADERS) +#else +# kdoc -L doc -d doc/$(PACKAGE) -n "$(DOCTITLE) (lib$(PACKAGE).so)" $(HEADERS) +#endif + + +clean: + @$(MESSAGE) "Cleaning $(PACKAGE)" + @$(PRETTY) "rm -f $(SONAME) $(APPLICATIONS)" + @$(PRETTY) "rm -f *.o *.d core *~ moc_*.cpp" + +copy: clean + tar -C .. -cvzf `date +../$(PACKAGE)-%d%b%y.tgz` $(PACKAGE) diff --git a/slam_gmapping/openslam_gmapping/build_tools/Makefile.subdirs b/slam_gmapping/openslam_gmapping/build_tools/Makefile.subdirs new file mode 100644 index 0000000..879bb45 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/build_tools/Makefile.subdirs @@ -0,0 +1,16 @@ +export VERBOSE + +.PHONY: clean, all + +ifeq ($(VERBOSE), 0) +QUIET=--no-print-directory +endif + +all: + @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; if ! $(MAKE) $(QUIET) -C $$subdir; then $(MESSAGE) "Compilation in $$subdir failed."; exit 1; fi; done + +clean: + @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir clean; done + +dep: + @for subdir in $(SUBDIRS); do $(MESSAGE) "Entering $$subdir."; $(MAKE) $(QUIET) -C $$subdir dep; done diff --git a/slam_gmapping/openslam_gmapping/build_tools/generate_shared_object b/slam_gmapping/openslam_gmapping/build_tools/generate_shared_object new file mode 100755 index 0000000..422be75 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/build_tools/generate_shared_object @@ -0,0 +1,16 @@ +#!/bin/tcsh + +echo decompressing file $1 + +set FILELIST=`ar -t $1` +echo "Object files:" +foreach i ($FILELIST) + echo $i +end + +echo generating $1:r.so + +ar -x $1 +ld -shared -o $1:r.so $FILELIST + +rm $FILELIST diff --git a/slam_gmapping/openslam_gmapping/build_tools/message b/slam_gmapping/openslam_gmapping/build_tools/message new file mode 100755 index 0000000..219ca21 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/build_tools/message @@ -0,0 +1,14 @@ +#!/bin/sh + +#echo "message: verbose = $VERBOSE" + +if ($VERBOSE) +then + exit 0; +fi + +a=$MAKELEVEL + +while ((0<$a)); do echo -n " "; let "a = $a - 1";done + +echo $1 diff --git a/slam_gmapping/openslam_gmapping/build_tools/pretty_compiler b/slam_gmapping/openslam_gmapping/build_tools/pretty_compiler new file mode 100755 index 0000000..c57ce31 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/build_tools/pretty_compiler @@ -0,0 +1,26 @@ +#!/bin/sh + + +#echo "pretty: verbose = $VERBOSE" + +if ($VERBOSE) +then + echo $1; + if ! eval $1 + then + echo "Failed command was:" + echo $1 + echo "in directory " `pwd` + exit 1 + fi +else + if ! eval $1 + then + echo "Failed command was:" + echo $1 + echo "in directory " `pwd` + exit 1 + fi +fi + +exit 0 diff --git a/slam_gmapping/openslam_gmapping/build_tools/testlib b/slam_gmapping/openslam_gmapping/build_tools/testlib new file mode 100755 index 0000000..28702e4 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/build_tools/testlib @@ -0,0 +1,26 @@ +#!/bin/bash +if [ -z "$1" ]; then + echo "Syntax: rtestlib " + exit 1 +fi + +exit 0 + +FNAME=`mktemp rtestlibXXXXXX` +echo "int main() { return 0; }" > $FNAME.cpp + +g++ $1 $FNAME.cpp -o $FNAME +result=$? +rm -f $FNAME.cpp $FNAME + +exit $result + +#if g++ $1 $FNAME.cpp -o $FNAME +#then# +# rm -f $FNAME.cpp $FNAME +# exit 1 +#else +# rm -f $FNAME.cpp $FNAME +# exit 0 +#fi + diff --git a/slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.cpp b/slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.cpp new file mode 100644 index 0000000..9c193e7 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/carmenwrapper/carmenwrapper.cpp @@ -0,0 +1,490 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#include "carmenwrapper.h" + +using namespace GMapping; +using namespace std; + +//static vars for the carmenwrapper +SensorMap CarmenWrapper::m_sensorMap; +deque CarmenWrapper::m_rangeDeque; +pthread_mutex_t CarmenWrapper::m_mutex; +sem_t CarmenWrapper::m_dequeSem; +pthread_mutex_t CarmenWrapper::m_lock; +pthread_t CarmenWrapper::m_readingThread; +RangeSensor* CarmenWrapper::m_frontLaser=0; +RangeSensor* CarmenWrapper::m_rearLaser=0; +bool CarmenWrapper::m_threadRunning=false; +OrientedPoint CarmenWrapper::m_truepos; +bool CarmenWrapper::stopped=true; + + +void CarmenWrapper::initializeIPC(const char* name) { + carmen_ipc_initialize(1,(char **)&name); +} + + + + +int CarmenWrapper::registerLocalizationMessages(){ + lock(); + IPC_RETURN_TYPE err; + + /* register globalpos message */ + err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_NAME, IPC_VARIABLE_LENGTH, + CARMEN_LOCALIZE_GLOBALPOS_FMT); + carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_GLOBALPOS_NAME); + + /* register robot particle message */ + err = IPC_defineMsg(CARMEN_LOCALIZE_PARTICLE_NAME, IPC_VARIABLE_LENGTH, + CARMEN_LOCALIZE_PARTICLE_FMT); + carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_PARTICLE_NAME); + +/* + carmen_localize_subscribe_initialize_placename_message(NULL, + (carmen_handler_t) + carmen_localize_initialize_placename_handler, + CARMEN_SUBSCRIBE_LATEST); + + // register map request message + err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_QUERY_NAME, IPC_VARIABLE_LENGTH, + CARMEN_LOCALIZE_MAP_QUERY_FMT); + carmen_test_ipc_exit(err, "Could not define", + CARMEN_LOCALIZE_MAP_QUERY_NAME); + + err = IPC_defineMsg(CARMEN_LOCALIZE_MAP_NAME, IPC_VARIABLE_LENGTH, + CARMEN_LOCALIZE_MAP_FMT); + carmen_test_ipc_exit(err, "Could not define", CARMEN_LOCALIZE_MAP_NAME); + + // subscribe to map request message + err = IPC_subscribe(CARMEN_LOCALIZE_MAP_QUERY_NAME, map_query_handler, NULL); + carmen_test_ipc(err, "Could not subscribe", CARMEN_LOCALIZE_MAP_QUERY_NAME); + IPC_setMsgQueueLength(CARMEN_LOCALIZE_MAP_QUERY_NAME, 1); + + + // register globalpos request message + err = IPC_defineMsg(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, + IPC_VARIABLE_LENGTH, + CARMEN_DEFAULT_MESSAGE_FMT); + carmen_test_ipc_exit(err, "Could not define", + CARMEN_LOCALIZE_MAP_QUERY_NAME); + + // subscribe to globalpos request message + err = IPC_subscribe(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, + globalpos_query_handler, NULL); + carmen_test_ipc(err, "Could not subscribe", + CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME); + IPC_setMsgQueueLength(CARMEN_LOCALIZE_GLOBALPOS_QUERY_NAME, 1); +*/ + unlock(); + return 0; +} + +bool CarmenWrapper::start(const char* name){ + if (m_threadRunning) + return false; + carmen_robot_subscribe_frontlaser_message(NULL, (carmen_handler_t)robot_frontlaser_handler, CARMEN_SUBSCRIBE_LATEST); + carmen_robot_subscribe_rearlaser_message(NULL, (carmen_handler_t)robot_rearlaser_handler, CARMEN_SUBSCRIBE_LATEST); + carmen_simulator_subscribe_truepos_message(NULL,(carmen_handler_t) simulator_truepos_handler, CARMEN_SUBSCRIBE_LATEST); + + IPC_RETURN_TYPE err; + + err = IPC_subscribe(CARMEN_NAVIGATOR_GO_NAME, navigator_go_handler, NULL); + carmen_test_ipc_exit(err, "Could not subscribe", + CARMEN_NAVIGATOR_GO_NAME); + IPC_setMsgQueueLength(CARMEN_NAVIGATOR_GO_NAME, 1); + + err = IPC_subscribe(CARMEN_NAVIGATOR_STOP_NAME, navigator_stop_handler, NULL); + carmen_test_ipc_exit(err, "Could not subscribe", + CARMEN_NAVIGATOR_STOP_NAME); + IPC_setMsgQueueLength(CARMEN_NAVIGATOR_STOP_NAME, 1); + + + + signal(SIGINT, shutdown_module); + pthread_mutex_init(&m_mutex, 0); + pthread_mutex_init(&m_lock, 0); + sem_init(&m_dequeSem, 0, 0); + m_threadRunning=true; + pthread_create (&m_readingThread,0,m_reading_function,0); + return true; +} + +void CarmenWrapper::lock(){ + //cerr <<"LOCK" << endl; + pthread_mutex_lock(&m_lock); +} + +void CarmenWrapper::unlock(){ + //cerr <<"UNLOCK" << endl; + pthread_mutex_unlock(&m_lock); +} + + +bool CarmenWrapper::sensorMapComputed(){ + pthread_mutex_lock(&m_mutex); + bool smok=m_frontLaser; + pthread_mutex_unlock(&m_mutex); + return smok; +} + +const SensorMap& CarmenWrapper::sensorMap(){ + return m_sensorMap; +} + +bool CarmenWrapper::isRunning(){ + return m_threadRunning; +} + +bool CarmenWrapper::isStopped(){ + return stopped; +} + +int CarmenWrapper::queueLength(){ + int ql=0; + pthread_mutex_lock(&m_mutex); + ql=m_rangeDeque.size(); + pthread_mutex_unlock(&m_mutex); + return ql; +} + +OrientedPoint CarmenWrapper::getTruePos(){ + return m_truepos; +} + +bool CarmenWrapper::getReading(RangeReading& reading){ + bool present=false; + sem_wait(&m_dequeSem); + pthread_mutex_lock(&m_mutex); + if (!m_rangeDeque.empty()){ +// cerr << __PRETTY_FUNCTION__ << ": queue size=" <num_readings, res, OrientedPoint(0,0,0), 0, 89.9); + m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor)); + + cout << __PRETTY_FUNCTION__ + << ": FrontLaser configured." + << " Readings " << m_rangeSensor->beams().size() + << " Resolution " << res << endl; + } + + RangeReading reading(m_rangeSensor, frontlaser->timestamp); + reading.resize(m_rangeSensor->beams().size()); + for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){ + reading[i]=(double)frontlaser->range[i]; + } + reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta)); +*/ + RangeReading reading=carmen2reading(*frontlaser); + addReading(reading); +} + +void CarmenWrapper::robot_rearlaser_handler(carmen_robot_laser_message* rearlaser) { +/* if (! m_rangeSensor){ + double res=0; + if (frontlaser->num_readings==180 || frontlaser->num_readings==181) + res=M_PI/180; + if (frontlaser->num_readings==360 || frontlaser->num_readings==361) + res=M_PI/360; + assert(res>0); + m_rangeSensor=new RangeSensor("FLASER",frontlaser->num_readings, res, OrientedPoint(0,0,0), 0, 89.9); + m_sensorMap.insert(make_pair(string("FLASER"), m_rangeSensor)); + + cout << __PRETTY_FUNCTION__ + << ": FrontLaser configured." + << " Readings " << m_rangeSensor->beams().size() + << " Resolution " << res << endl; + } + + RangeReading reading(m_rangeSensor, frontlaser->timestamp); + reading.resize(m_rangeSensor->beams().size()); + for (unsigned int i=0; i< (unsigned int)frontlaser->num_readings; i++){ + reading[i]=(double)frontlaser->range[i]; + } + reading.setPose(OrientedPoint(frontlaser->x, frontlaser->y, frontlaser->theta)); +*/ + RangeReading reading=carmen2reading(*rearlaser); + addReading(reading); +} + + + + +void CarmenWrapper:: navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) { + carmen_navigator_go_message msg; + FORMATTER_PTR formatter; + IPC_RETURN_TYPE err; + + formatter = IPC_msgInstanceFormatter(msgRef); + err = IPC_unmarshallData(formatter, callData, &msg, + sizeof(carmen_navigator_go_message)); + IPC_freeByteArray(callData); + + carmen_test_ipc_return + (err, "Could not unmarshall", IPC_msgInstanceName(msgRef)); + cerr<<"go"<truepose.x; + m_truepos.y=truepos->truepose.y; + m_truepos.theta=truepos->truepose.theta; +} + +RangeReading CarmenWrapper::carmen2reading(const carmen_robot_laser_message& msg){ + //either front laser or rear laser + double dth=msg.laser_pose.theta-msg.robot_pose.theta; + dth=atan2(sin(dth), cos(dth)); + + if (msg.laser_pose.theta==msg.robot_pose.theta && !m_frontLaser){ + double res=0; + res = msg.config.angular_resolution; +// if (msg.num_readings==180 || msg.num_readings==181) +// res=M_PI/180; +// if (msg.num_readings==360 || msg.num_readings==361) +// res=M_PI/360; + assert(res>0); + string sensorName="FLASER"; + OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta); + OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta); + OrientedPoint dp=absoluteDifference(lpose, rpose); + m_frontLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, + msg.config.maximum_range); + m_frontLaser->updateBeamsLookup(); + m_sensorMap.insert(make_pair(sensorName, m_frontLaser)); + + cout << __PRETTY_FUNCTION__ + << ": " << sensorName <<" configured." + << " Readings " << m_frontLaser->beams().size() + << " Resolution " << res << endl; + } + if (msg.laser_pose.theta!=msg.robot_pose.theta && !m_rearLaser){ + double res=0; + res = msg.config.angular_resolution; +// if (msg.num_readings==180 || msg.num_readings==181) +// res=M_PI/180; +// if (msg.num_readings==360 || msg.num_readings==361) +// res=M_PI/360; + assert(res>0); + OrientedPoint rpose(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta); + OrientedPoint lpose(msg.laser_pose.x, msg.laser_pose.y, msg.laser_pose.theta); + OrientedPoint dp=absoluteDifference(lpose, rpose); + string sensorName="RLASER"; + m_rearLaser=new RangeSensor(sensorName,msg.num_readings, res, OrientedPoint(0,0,msg.laser_pose.theta-msg.robot_pose.theta), 0, + msg.config.maximum_range); + m_rearLaser->updateBeamsLookup(); + m_sensorMap.insert(make_pair(sensorName, m_rearLaser)); + + cout << __PRETTY_FUNCTION__ + << ": " << sensorName <<" configured." + << " Readings " << m_rearLaser->beams().size() + << " Resolution " << res << endl; + } + + const RangeSensor * rs=(msg.laser_pose.theta==msg.robot_pose.theta)?m_frontLaser:m_rearLaser; + RangeReading reading(rs, msg.timestamp); + reading.resize(rs->beams().size()); + for (unsigned int i=0; i< (unsigned int)msg.num_readings; i++){ + reading[i]=(double)msg.range[i]; + } + reading.setPose(OrientedPoint(msg.robot_pose.x, msg.robot_pose.y, msg.robot_pose.theta)); + return reading; +} + +void CarmenWrapper::publish_globalpos(carmen_localize_summary_p summary) +{ + lock(); + static carmen_localize_globalpos_message globalpos; + IPC_RETURN_TYPE err; + + globalpos.timestamp = carmen_get_time(); + globalpos.host = carmen_get_host(); + globalpos.globalpos = summary->mean; + globalpos.globalpos_std = summary->std; + globalpos.globalpos_xy_cov = summary->xy_cov; + globalpos.odometrypos = summary->odometry_pos; + globalpos.converged = summary->converged; + err = IPC_publishData(CARMEN_LOCALIZE_GLOBALPOS_NAME, &globalpos); + carmen_test_ipc_exit(err, "Could not publish", + CARMEN_LOCALIZE_GLOBALPOS_NAME); + unlock(); +} + +/* publish a particle message */ + +void CarmenWrapper::publish_particles(carmen_localize_particle_filter_p filter, + carmen_localize_summary_p summary) +{ + lock(); + static carmen_localize_particle_message pmsg; + IPC_RETURN_TYPE err; + + pmsg.timestamp = carmen_get_time(); + pmsg.host = carmen_get_host(); + pmsg.globalpos = summary->mean; + pmsg.globalpos_std = summary->mean; + pmsg.num_particles = filter->param->num_particles; + pmsg.particles = (carmen_localize_particle_ipc_p)filter->particles; + err = IPC_publishData(CARMEN_LOCALIZE_PARTICLE_NAME, &pmsg); + carmen_test_ipc_exit(err, "Could not publish", + CARMEN_LOCALIZE_PARTICLE_NAME); + fprintf(stderr, "P"); + unlock(); +} + + + + + +void * CarmenWrapper::m_reading_function(void*){ + while (true) { + lock(); + IPC_listen(100); + unlock(); + usleep(20000); + } + return 0; +} + +void CarmenWrapper::shutdown_module(int sig){ + if(sig == SIGINT) { + carmen_ipc_disconnect(); + + fprintf(stderr, "\nDisconnecting (shutdown_module(%d) called).\n",sig); + exit(0); + } +} +/* +typedef struct { + int num_readings; + float *range; + char *tooclose; + double x, y, theta;//position of the laser on the robot + double odom_x, odom_y, odom_theta; //position of the center of the robot + double tv, rv; + double forward_safety_dist, side_safety_dist; + double turn_axis; + double timestamp; + char host[10]; +} carmen_robot_laser_message; +*/ + +carmen_robot_laser_message CarmenWrapper::reading2carmen(const RangeReading& reading){ + carmen_robot_laser_message frontlaser; + frontlaser.num_readings=reading.size(); + frontlaser.range = new float[frontlaser.num_readings]; + frontlaser.tooclose=0; + frontlaser.laser_pose.x=frontlaser.robot_pose.x=reading.getPose().x; + frontlaser.laser_pose.y=frontlaser.robot_pose.y=reading.getPose().y; + frontlaser.laser_pose.theta=frontlaser.robot_pose.theta=reading.getPose().theta; + frontlaser.tv=frontlaser.rv=0; + frontlaser.forward_safety_dist=frontlaser.side_safety_dist=0; + frontlaser.turn_axis=0; + frontlaser.timestamp=reading.getTime(); + for (unsigned int i=0; i< reading.size(); i++){ + frontlaser.range[i]=(float)reading[i]; + } + return frontlaser; +} + +carmen_point_t CarmenWrapper::point2carmen (const OrientedPoint& p){ + return (carmen_point_t){p.x,p.y,p.theta}; +} + +OrientedPoint CarmenWrapper::carmen2point (const carmen_point_t& p){ + return OrientedPoint(p.x, p.y, p.theta); +} + + +/* +int main (int argc, char** argv) { + CarmenWrapper::start(argc, argv); + while(1){ + sleep(2); + RangeReading reading(0,0); + while(CarmenWrapper::getReading(reading)){ + cout << "FLASER " << reading.size(); + for (int i=0; i +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace GMapping{ + +class CarmenWrapper { +public: + static void initializeIPC(const char* name); + static bool start(const char* name); + static bool isRunning(); + static void lock(); + static void unlock(); + static int registerLocalizationMessages(); + + static int queueLength(); + static OrientedPoint getTruePos(); + static bool getReading(RangeReading& reading); + static void addReading(RangeReading& reading); + static const SensorMap& sensorMap(); + static bool sensorMapComputed(); + static bool isStopped(); + +// conversion function + static carmen_robot_laser_message reading2carmen(const RangeReading& reading); + static RangeReading carmen2reading(const carmen_robot_laser_message& msg); + static carmen_point_t point2carmen (const OrientedPoint& p); + static OrientedPoint carmen2point (const carmen_point_t& p); + + +// carmen interaction + static void robot_frontlaser_handler(carmen_robot_laser_message* frontlaser); + static void robot_rearlaser_handler(carmen_robot_laser_message* frontlaser); + static void simulator_truepos_handler(carmen_simulator_truepos_message* truepos); + //babsi: + static void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ; + static void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ; + + //babsi: + static void publish_globalpos(carmen_localize_summary_p summary); + static void publish_particles(carmen_localize_particle_filter_p filter, + carmen_localize_summary_p summary); + + static void shutdown_module(int sig); + + private: + static std::deque m_rangeDeque; + static sem_t m_dequeSem; + static pthread_mutex_t m_mutex, m_lock; + static pthread_t m_readingThread; + static void * m_reading_function(void*); + static bool m_threadRunning; + static SensorMap m_sensorMap; + static RangeSensor* m_frontLaser, *m_rearLaser; + static OrientedPoint m_truepos; + static bool stopped; +}; + +} //end namespace + + + +#endif +/* +int main (int argc, char** argv) { + + CarmenWrapper::init_carmen(argc, argv); + while (true) { + IPC_listenWait(100); + } + return 1; +} +*/ diff --git a/slam_gmapping/openslam_gmapping/gfs-carmen/Makefile b/slam_gmapping/openslam_gmapping/gfs-carmen/Makefile new file mode 100644 index 0000000..209b772 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gfs-carmen/Makefile @@ -0,0 +1,21 @@ +OBJS= +APPS= gfs-carmen + +LIBS+= -lcarmenwrapper -lgridfastslam -lconfigfile +CPPFLAGS+= -I ../sensor -I$(CARMEN_HOME)/include + +-include ../global.mk +ifeq ($(CARMENSUPPORT), 0) +APPS= +.PHONY: clean all + +all: + +clean: + +else + -include ../build_tools/Makefile.app +endif + + + diff --git a/slam_gmapping/openslam_gmapping/gfs-carmen/gfs-carmen.cpp b/slam_gmapping/openslam_gmapping/gfs-carmen/gfs-carmen.cpp new file mode 100644 index 0000000..14deee4 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gfs-carmen/gfs-carmen.cpp @@ -0,0 +1,243 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#include +#include +#include +#include +#include + +#define DEBUG cout << __PRETTY_FUNCTION__ + +/* +Example file for interfacing carmen, and gfs. + +if you want to look for a specific topic search for one of the following keywords in the file comments + +KEYWORDS: + CREATION + INITIALIZATION + SENSOR MAP + BEST PARTICLE INDEX + PARTICLE VECTOR + PARTICLE TRAJECTORIES + BEST MAP + BOUNDING BOX +*/ + +using namespace GMapping; +using namespace std; + +int main(int argc, const char * const * argv){ + + std::string outfilename=""; + double xmin=-100.; + double ymin=-100.; + double xmax=100.; + double ymax=100.; + double delta=0.05; + + //scan matching parameters + double sigma=0.05; + double maxrange=80.; + double maxUrange=80.; + double regscore=1e4; + double lstep=.05; + double astep=.05; + int kernelSize=1; + int iterations=5; + double critscore=0.; + double maxMove=1.; + double lsigma=.075; + double ogain=3; + int lskip=0; + + //motion model parameters + double srr=0.01, srt=0.01, str=0.01, stt=0.01; + //particle parameters + int particles=30; + + + //gfs parameters + double angularUpdate=0.5; + double linearUpdate=1; + double resampleThreshold=0.5; + bool generateMap=true; + + std::string configfilename = ""; + + CMD_PARSE_BEGIN_SILENT(1,argc); + parseStringSilent("-cfg",configfilename); + CMD_PARSE_END_SILENT; + + if (configfilename.length()>0){ + ConfigFile cfg(configfilename); + outfilename = (std::string) cfg.value("gfs","outfilename",outfilename); + xmin = cfg.value("gfs","xmin", xmin); + xmax = cfg.value("gfs","xmax",xmax); + ymin = cfg.value("gfs","ymin",ymin); + ymax = cfg.value("gfs","ymax",ymax); + delta = cfg.value("gfs","delta",delta); + maxrange = cfg.value("gfs","maxrange",maxrange); + maxUrange = cfg.value("gfs","maxUrange",maxUrange); + regscore = cfg.value("gfs","regscore",regscore); + critscore = cfg.value("gfs","critscore",critscore); + kernelSize = cfg.value("gfs","kernelSize",kernelSize); + sigma = cfg.value("gfs","sigma",sigma); + iterations = cfg.value("gfs","iterations",iterations); + lstep = cfg.value("gfs","lstep",lstep); + astep = cfg.value("gfs","astep",astep); + maxMove = cfg.value("gfs","maxMove",maxMove); + srr = cfg.value("gfs","srr", srr); + srt = cfg.value("gfs","srt", srt); + str = cfg.value("gfs","str", str); + stt = cfg.value("gfs","stt", stt); + particles = cfg.value("gfs","particles",particles); + angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate); + linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate); + lsigma = cfg.value("gfs","lsigma", lsigma); + ogain = cfg.value("gfs","lobsGain", ogain); + lskip = (int)cfg.value("gfs","lskip", lskip); + // randseed = cfg.value("gfs","randseed", randseed); + resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold); + generateMap = cfg.value("gfs","generateMap", generateMap); + } + + + CMD_PARSE_BEGIN(1,argc); + parseString("-cfg",configfilename); + parseString("-outfilename",outfilename); + parseDouble("-xmin",xmin); + parseDouble("-xmax",xmax); + parseDouble("-ymin",ymin); + parseDouble("-ymax",ymax); + parseDouble("-delta",delta); + parseDouble("-maxrange",maxrange); + parseDouble("-maxUrange",maxUrange); + parseDouble("-regscore",regscore); + parseDouble("-critscore",critscore); + parseInt("-kernelSize",kernelSize); + parseDouble("-sigma",sigma); + parseInt("-iterations",iterations); + parseDouble("-lstep",lstep); + parseDouble("-astep",astep); + parseDouble("-maxMove",maxMove); + parseDouble("-srr", srr); + parseDouble("-srt", srt); + parseDouble("-str", str); + parseDouble("-stt", stt); + parseInt("-particles",particles); + parseDouble("-angularUpdate", angularUpdate); + parseDouble("-linearUpdate", linearUpdate); + parseDouble("-lsigma", lsigma); + parseDouble("-lobsGain", ogain); + parseInt("-lskip", lskip); + parseDouble("-resampleThreshold", resampleThreshold); + parseFlag("-generateMap", generateMap); + CMD_PARSE_END; + + cerr << "Parameter parsed, connecting to Carmen!"; + + CarmenWrapper::initializeIPC(argv[0]); + CarmenWrapper::start(argv[0]); + + while (! CarmenWrapper::sensorMapComputed()){ + usleep(500000); + cerr << "." << flush; + } + + //CREATION + + GridSlamProcessor* processor=new GridSlamProcessor; + + //SENSOR MAP + //loads from the carmen wrapper the laser and robot settings + SensorMap sensorMap=CarmenWrapper::sensorMap(); + cerr << "Connected " << endl; + processor->setSensorMap(sensorMap); + + //set the command line parameters + processor->setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, lsigma, ogain, lskip); + processor->setMotionModelParameters(srr, srt, str, stt); + processor->setUpdateDistances(linearUpdate, angularUpdate, resampleThreshold); + processor->setgenerateMap(generateMap); + OrientedPoint initialPose(xmin+xmax/2, ymin+ymax/2, 0); + + + //INITIALIZATION + processor->init(particles, xmin, ymin, xmax, ymax, delta, initialPose); + if (outfilename.length()>0) + processor->outputStream().open(outfilename.c_str()); + + bool running=true; + + GridSlamProcessor* ap, *copy=processor->clone(); + ap=processor; processor=copy; copy=ap; + + //this is the CORE LOOP; + RangeReading rr(0,0); + while (running){ + while (CarmenWrapper::getReading(rr)){ + + + bool processed=processor->processScan(rr); + + //this returns true when the algorithm effectively processes (the traveled path since the last processing is over a given threshold) + if (processed){ + cerr << "PROCESSED" << endl; + //for searching for the BEST PARTICLE INDEX + // unsigned int best_idx=processor->getBestParticleIndex(); + + //if you want to access to the PARTICLE VECTOR + const GridSlamProcessor::ParticleVector& particles = processor->getParticles(); + //remember to use a const reference, otherwise it copys the whole particles and maps + + //this is for recovering the tree of PARTICLE TRAJECTORIES (obtaining the ancestor of each particle) + cerr << "Particle reproduction story begin" << endl; + for (unsigned int i=0; i" << i << " "; + } + cerr << "Particle reproduction story end" << endl; +/* + //then if you want to access the BEST MAP, + //of course by copying it in a plain structure + Map* mymap = processor->getParticles()[best_idx].map.toDoubleMap(); + //at this point mymap is yours. Can do what you want. + + double best_weight=particles[best_idx].weightSum; + cerr << "Best Particle is " << best_idx << " with weight " << best_weight << endl; + +*/ + cerr << __PRETTY_FUNCTION__ << "CLONING... " << endl; + GridSlamProcessor* newProcessor=processor->clone(); + cerr << "DONE" << endl; + cerr << __PRETTY_FUNCTION__ << "DELETING... " << endl; + delete processor; + cerr << "DONE" << endl; + processor=newProcessor; + } + } + } + return 0; +} + diff --git a/slam_gmapping/openslam_gmapping/grid/Makefile b/slam_gmapping/openslam_gmapping/grid/Makefile new file mode 100644 index 0000000..22de055 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/grid/Makefile @@ -0,0 +1,9 @@ +OBJS= +APPS= map_test + +LDFLAGS+= +CPPFLAGS+= -DNDEBUG + +-include ../global.mk +-include ../build_tools/Makefile.app + diff --git a/slam_gmapping/openslam_gmapping/grid/graphmap.cpp b/slam_gmapping/openslam_gmapping/grid/graphmap.cpp new file mode 100644 index 0000000..a262d4c --- /dev/null +++ b/slam_gmapping/openslam_gmapping/grid/graphmap.cpp @@ -0,0 +1,59 @@ +#ifndef GRAPHMAP_H +#define GRAPHMAP_H +#include +#include +#include +#include + +namespace GMapping { + +class RasterMap; + +struct GraphMapPatch{ + typedef typename std::list PointList; + /**Renders the map relatively to the center of the patch*/ + //void render(RenderMap rmap); + /**returns the lower left corner of the patch, relative to the center*/ + //Point minBoundary() const; + /**returns the upper right corner of the patch, relative to the center*/ + //Point maxBoundary() const; // + + OrientedPoint center; + PointList m_points; +}; + +struct Covariance3{ + double sxx, sxy, sxt, syy, syt ,stt; +}; + +struct GraphMapEdge{ + Covariance3 covariance; + GraphMapPatch* first, *second; + inline operator double() const{ + return sqrt((first->center-second->center)*(first->center-second->center)); + } +}; + + +struct GraphPatchGraph: public Graph{ + void addEdge(Vertex* v1, Vertex* v2, const Covariance3& covariance); +}; + +void GraphPatchGraph::addEdge(GraphPatchGraph::Vertex* v1, GraphPatchGraph::VertexVertex* v2, + const Covariance3& cov){ + GraphMapEdge gme; + gme.covariance=cov; + gme.first=v1; + gme.second=v2; + return Graph::addEdge(v1,v2,gme); +} + +struct GraphPatchDirectoryCell: public std::set { + GraphPatchDirectoryCell(double); +}; + +typedef Map, Array2D::set > + +}; + +#endif \ No newline at end of file diff --git a/slam_gmapping/openslam_gmapping/grid/map_test.cpp b/slam_gmapping/openslam_gmapping/grid/map_test.cpp new file mode 100644 index 0000000..41dbf5a --- /dev/null +++ b/slam_gmapping/openslam_gmapping/grid/map_test.cpp @@ -0,0 +1,62 @@ +#include +#include "map.h" +#include "harray2d.h" + +using namespace std; +using namespace GMapping; + +struct SimpleCell{ + int value; + SimpleCell(int v=0){value=v;} + static const SimpleCell& Unknown(); + static SimpleCell* address; +}; + +SimpleCell* SimpleCell::address=0; + +const SimpleCell& SimpleCell::Unknown(){ + if (address) + return *address; + address=new SimpleCell(-1); + return *address; +} + +typedef Map< SimpleCell, HierarchicalArray2D > CGrid; + +int main (int argc, char ** argv){ + CGrid g1(Point(0.,0.), 200, 200, 0.1); + CGrid g2(Point(10.,10.), 200, 200, 0.1); + { + HierarchicalArray2D::PointSet ps; + IntPoint pp=g1.world2map(Point(5.1,5.1)); + cout << pp.x << " " << pp.y << endl; + ps.insert(pp); + g1.storage().setActiveArea(ps,false); + g1.storage().allocActiveArea(); + g1.cell(Point(5.1,5.1)).value=5; + cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl; + g1.resize(-150, -150, 150, 150); + cout << "cell value" << (int) g1.cell(Point(5.1,5.1)).value << endl; + CGrid g3(g1); + g1=g2; + } + cerr << "copy and modify test" << endl; + CGrid *ap,* gp1=new CGrid(Point(0,0), 200, 200, 0.1); + CGrid* gp0=new CGrid(*gp1); + for (int i=1; i<10; i++){ + ap=new CGrid(*gp1); + delete gp1; + gp1=gp0; + gp0=ap; + IntPoint pp=gp0->world2map(Point(5.1,5.1)); + HierarchicalArray2D::PointSet ps; + ps.insert(pp); + gp1->storage().setActiveArea(ps,false); + gp1->storage().allocActiveArea(); + gp1->cell(Point(5.1,5.1)).value=i; + cout << "cell value" << (int) gp1->cell(Point(5.1,5.1)).value << endl; + } + delete gp0; + delete gp1; + return 0; +} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt b/slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt new file mode 100644 index 0000000..8df8800 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/CMakeLists.txt @@ -0,0 +1,11 @@ +add_library(gridfastslam + gfsreader.cpp + gridslamprocessor.cpp + gridslamprocessor_tree.cpp + motionmodel.cpp + ) +target_link_libraries(gridfastslam scanmatcher sensor_range) + +install(TARGETS gridfastslam DESTINATION lib) + +#ament_export_libraries(gridfastslam) diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp new file mode 100644 index 0000000..1324818 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2log.cpp @@ -0,0 +1,64 @@ +#include +#include +#include +#include +#include +#include +#include +#include "gfsreader.h" + +#define MAX_LINE_LENGHT (1000000) + +using namespace std; +using namespace GMapping; +using namespace GMapping::GFSReader; + +int main (int argc, const char * const * argv){ + if (argc<3){ + cout << "usage gfs2log [-err] [-neff] [-part] [-odom] " << endl; + cout << " -odom : dump raw odometry in ODOM message instead of inpolated corrected one" << endl; + return -1; + } + bool err=0; + bool neff=0; + bool part=0; + bool odom=0; + // int particle_num; + unsigned int c=1; + if (!strcmp(argv[c],"-err")){ + err=true; + c++; + } + if (!strcmp(argv[c],"-neff")){ + neff=true; + c++; + } + if (!strcmp(argv[c],"-part")){ + part=true; + c++; + } + if (!strcmp(argv[c],"-odom")){ + odom=true; + c++; + } + ifstream is(argv[c]); + if (!is){ + cout << "could read file "<< endl; + return -1; + } + c++; + RecordList rl; + rl.read(is); + unsigned int bestidx=rl.getBestIdx(); + cout << endl << "best index = " << bestidx<< endl; + ofstream os(argv[c]); + if (! os){ + cout << "could write file "<< endl; + return -1; + } + rl.printPath(os,bestidx,err,odom); + if(part) + rl.printLastParticles(os); + os.close(); + return 0; +} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp new file mode 100644 index 0000000..5faad99 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2neff.cpp @@ -0,0 +1,40 @@ +#include +#include +#include +#include + +using namespace std; + +int main(int argc, char**argv){ + if (argc<3){ + cout << "usage gfs2neff " << endl; + return -1; + } + ifstream is(argv[1]); + if (!is){ + cout << "could read file "<< endl; + return -1; + } + ofstream os(argv[2]); + if (! os){ + cout << "could write file "<< endl; + return -1; + } + unsigned int frame=0; + double neff=0; + while(is){ + char buf[8192]; + is.getline(buf, 8192); + istringstream lineStream(buf); + string recordType; + lineStream >> recordType; + if (recordType=="FRAME"){ + lineStream>> frame; + } + if (recordType=="NEFF"){ + lineStream>> neff; + os << frame << " " << neff << endl; + } + } + os.close(); +} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp new file mode 100644 index 0000000..5eb65fa --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2rec.cpp @@ -0,0 +1,406 @@ +#include +#include +#include +#include +#include +#include +#include + +#define MAX_LINE_LENGHT (1000000) + +using namespace GMapping; +using namespace std; + +struct Record{ + unsigned int dim; + double time; + virtual ~Record(){} + virtual void read(istream& is)=0; + virtual void write(ostream& os){}; +}; + +struct CommentRecord: public Record{ + string text; + virtual void read(istream& is){ + char buf[MAX_LINE_LENGHT]; + memset(buf,0, MAX_LINE_LENGHT*sizeof(char)); + is.getline(buf, MAX_LINE_LENGHT); + text=string(buf); + } + virtual void write(ostream& os){ + os << "#GFS_COMMENT: " << text << endl; + } +}; + +struct PoseRecord: public Record{ + PoseRecord(bool ideal=false){ + truePos=ideal; + } + bool truePos; + OrientedPoint pose; + void read(istream& is){ + is >> pose.x >> pose.y >> pose.theta; + time = 0; + if (is) + is >> time; + } + virtual void write(ostream& os){ + if (truePos) + os << "POS-CORR"; + else + os << "POS "; + // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": "; + os << "0 0: "; + os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta << endl; + } +}; + +struct NeffRecord: public Record{ + double neff; + void read(istream& is){ + is >> neff; + } + virtual void write(ostream& os){ + os << "NEFF " << neff << endl; + } +}; + + +struct OdometryRecord: public Record{ + vector poses; + virtual void read(istream& is){ + is >> dim; + for (unsigned int i=0; i< dim; i++){ + OrientedPoint p; + double w; + is >> p.x; + is >> p.y; + is >> p.theta; + is >> w; + poses.push_back(p); + } + time = 0; + if (is) + is >> time; + } +}; + + +struct ScanMatchRecord: public Record{ + vector poses; + vector weights; + virtual void read(istream& is){ + is >> dim; + for (unsigned int i=0; i< dim; i++){ + OrientedPoint p; + double w; + is >> p.x; + is >> p.y; + is >> p.theta; + is >> w; + poses.push_back(p); + weights.push_back(w); + } + } +}; + +struct LaserRecord: public Record{ + vector readings; + OrientedPoint pose; + virtual void read(istream& is){ + is >> dim; + for (unsigned int i=0; i< dim; i++){ + double r; + is >> r; + readings.push_back(r); + } + is >> pose.x; + is >> pose.y; + is >> pose.theta; + time = 0; + if (is) + is >> time; + } + + // dummy, &sec, &usec, &nLas, &nVal, &range ) == EOF) { + + virtual void write(ostream& os){ + os << "POS "; + // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": "; + os << "0 0: "; + os << pose.x*100 << " " << pose.y*100 << " " << 180/M_PI*pose.theta << endl; + + os << "LASER-RANGE "; + // FIXME os << floor(time) << " " << (int) (time-floor(time)*1e6) << ": "; + os << " 0 0 0 " << dim << " 180. : "; + for (unsigned int i=0; i< dim; i++){ + os <<" "<< readings[i]*100 ; + } + os << endl; + }; +}; + +struct ResampleRecord: public Record{ + vector indexes; + virtual void read(istream& is){ + is >> dim; + for (unsigned int i=0; i< dim; i++){ + unsigned int j; + is >> j; + indexes.push_back(j); + } + } +}; + +struct RecordList: public list{ + mutable int sampleSize; + + istream& read(istream& is){ + while(is){ + char buf[8192]; + is.getline(buf, 8192); + istringstream lineStream(buf); + string recordType; + lineStream >> recordType; + Record* rec=0; + if (recordType=="LASER_READING"){ + rec=new LaserRecord; + cout << "l" << flush; + } + if (recordType=="ODO_UPDATE"){ + rec=new OdometryRecord; + cout << "o" << flush; + } + if (recordType=="SM_UPDATE"){ + rec=new ScanMatchRecord; + cout << "m" << flush; + } + if (recordType=="SIMULATOR_POS"){ + rec=new PoseRecord(true); + cout << "t" << flush; + } + if (recordType=="RESAMPLE"){ + rec=new ResampleRecord; + cout << "r" << flush; + } + if (recordType=="NEFF"){ + rec=new NeffRecord; + cout << "n" << flush; + } + if (recordType=="COMMENT"){ + rec=new CommentRecord; + cout << "c" << flush; + } + if (rec){ + rec->read(lineStream); + push_back(rec); + } + } + return is; + } + + double getLogWeight(unsigned int i) const{ + double weight=0; + unsigned int currentIndex=i; + for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){ + ScanMatchRecord* scanmatch=dynamic_cast(*it); + if (scanmatch){ + weight+=scanmatch->weights[currentIndex]; + } + ResampleRecord* resample=dynamic_cast(*it); + if (resample){ + currentIndex=resample->indexes[currentIndex]; + } + } + return weight; + } + unsigned int getBestIdx() const { + if (empty()) + return 0; + const ScanMatchRecord* scanmatch=0; + const_reverse_iterator it=rbegin(); + while(!scanmatch){ + scanmatch=dynamic_cast(*it); + it++; + } + unsigned int dim=scanmatch->dim; + sampleSize=(int)dim; + double bestw=-1e200; + unsigned int best=scanmatch->dim+1; + for (unsigned i=0; ibestw){ + best=i; + bestw=w; + } + } + return best; + } + + void printPath(ostream& os, unsigned int i, bool err=false) const{ + unsigned int currentIndex=i; + OrientedPoint p(0,0,0); + + RecordList rl; + + //reconstruct a path + for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){ + const NeffRecord* neff=dynamic_cast(*it); + if (neff){ + NeffRecord* n=new NeffRecord(*neff); + rl.push_front(n); + } + const ScanMatchRecord* scanmatch=dynamic_cast(*it); + if (scanmatch){ + PoseRecord* pose=new PoseRecord; + pose->dim=0; + p=pose->pose=scanmatch->poses[currentIndex]; + rl.push_front(pose); + } + const OdometryRecord* odometry=dynamic_cast(*it); + if (odometry){ + PoseRecord* pose=new PoseRecord; + pose->dim=0; + p=pose->pose=odometry->poses[currentIndex]; + pose->time=odometry->time; + rl.push_front(pose); + } + const PoseRecord* tpose=dynamic_cast(*it); + if (tpose){ + PoseRecord* pose=new PoseRecord(*tpose); + rl.push_front(pose); + } + const LaserRecord* laser=dynamic_cast(*it); + if (laser){ + LaserRecord* claser=new LaserRecord(*laser); + claser->pose=p; + rl.push_front(claser); + } + const CommentRecord* comment=dynamic_cast(*it); + if (comment){ + CommentRecord* ccomment=new CommentRecord(*comment); + rl.push_front(ccomment); + } + const ResampleRecord* resample=dynamic_cast(*it); + if (resample){ + currentIndex=resample->indexes[currentIndex]; + ResampleRecord* r= new ResampleRecord(*resample); + rl.push_front(r); + } + } + bool started=false; + double ox=0, oy=0, rxx=0, rxy=0, ryx=0, ryy=0, rth=0; + bool computedTransformation=false; + bool truePosFound=false; + OrientedPoint truePose(0,0,0); + OrientedPoint currPose(0,0,0); + bool tpf=false; + double neff=0; + unsigned int count=0; + for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){ + NeffRecord* neffr=dynamic_cast(*it); + if (neffr) + neff=neffr->neff/(double)sampleSize; + started=started || dynamic_cast(*it)?true:false; + if (started && ! truePosFound){ + PoseRecord* tpose=dynamic_cast(*it); + if (tpose && tpose->truePos){ + truePosFound=true; + tpf=true; + truePose=tpose->pose; + os << "# "; + (*it)->write(os); + } + } + if (started && truePosFound && ! computedTransformation){ + PoseRecord* pos=dynamic_cast(*it); + if (pos && !pos->truePos){ + OrientedPoint pose=pos->pose; + rth=truePose.theta-pose.theta; + double s=sin(rth), c=cos(rth); + rxx=ryy=c; + rxy=-s; ryx=s; + ox=truePose.x-(rxx*pose.x+rxy*pose.y); + oy=truePose.y-(ryx*pose.x+ryy*pose.y); + computedTransformation=true; + os << "# "; + (*it)->write(os); + + } + } + ResampleRecord* resample=dynamic_cast(*it); + if(resample){ + os << "MARK-POS 0 0: " <(*it); + if (pos){ + if (pos->truePos){ + tpf=true; + truePose=pos->pose; + } else { + if (tpf){ + tpf=false; + OrientedPoint pose=pos->pose; + double ex, ey, eth=truePose.theta-pose.theta-rth; + ex=truePose.x-(ox+rxx*pose.x+rxy*pose.y); + ey=truePose.y-(oy+ryx*pose.x+ryy*pose.y); + eth=atan2(sin(eth), cos(eth)); + if (! err) + os << "# ERROR "; + os << neff << " " + << ex << " " << ey << " " << eth + << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl; + } + } + } + + } + PoseRecord* pos=dynamic_cast(*it); + if (pos) + currPose=pos->pose; + + if (! err) + (*it)->write(os); + delete *it; + } + } +}; + + + +int main (int argc, const char * const * argv){ + if (argc<3){ + cout << "usage gfs2rec [-err] " << endl; + return -1; + } + bool err=0; + bool neff=0; + unsigned int c=1; + if (!strcmp(argv[c],"-err")){ + err=true; + c++; + } + if (!strcmp(argv[c],"-neff")){ + neff=true; + c++; + } + ifstream is(argv[c]); + if (!is){ + cout << "could read file "<< endl; + return -1; + } + c++; + RecordList rl; + rl.read(is); + unsigned int bestidx=rl.getBestIdx(); + cout << endl << "best index = " << bestidx<< endl; + ofstream os(argv[c]); + if (! os){ + cout << "could write file "<< endl; + return -1; + } + rl.printPath(os,bestidx,err); + os.close(); + return 0; +} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp new file mode 100644 index 0000000..3346199 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/gfs2stat.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include "gfsreader.h" + +using namespace std; +using namespace GMapping; +using namespace GMapping::GFSReader; + + +int main(int argc, char ** argv){ + if (argc<2){ + cout << "usage gfs2stat " << endl; + return 0; + } + ifstream is(argv[1]); + if (!is){ + cout << "no file found: " << argv[1] << endl; + return 0; + } + ofstream os(argv[2]); + if (!os){ + cout << "cannot open file: " << argv[1] << endl; + return 0; + } + cout << "loading... "<< flush; + RecordList rl; + rl.read(is); + cout << " done" << endl; + int count=-1; + for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){ + + count++; + const ScanMatchRecord* rec=dynamic_cast(*it); + if (!rec) + continue; + Gaussian3 gaussian; + /* + vector nweights; + cout << "N"<< flush; + back_insert_iterator< vector > out(nweights); + toNormalForm(out,rec->weights.begin(), rec->weights.end()); + cout << "G"<< flush; + gaussian.computeFromSamples(rec->poses, nweights); + */ + gaussian.computeFromSamples(rec->poses); + cout << "E"<< flush; + os << count <<" "; + os << gaussian.mean.x <<" "; + os << gaussian.mean.y <<" "; + os << gaussian.mean.theta <<" "; + os << gaussian.covariance.eval[0] <<" "; + os << gaussian.covariance.eval[1] <<" "; + os << gaussian.covariance.eval[2] < +#include +#include +#include +#include +#include +#include +#include "gfsreader.h" +#define MAX_LINE_LENGHT (1000000) + +using namespace std; +using namespace GMapping; +using namespace GMapping::GFSReader; + +computeBoundingBox() + + +int main (unsigned int argc, const char * const * argv) + double delta = 0.1; + double skip = 2; + double rotate = 0; + double maxrange = 0; + + if (argc<3){ + cout << "usage gfs2stream [-step Number] " << endl; + return -1; + } + + + CMD_PARSE_BEGIN(1,argc-2); + + CMD_PARSE_END; + + if (argc<3){ + cout << "usage gfs2stream [-step Number] " << endl; + return -1; + } + bool err=0; + bool neff=0; + bool part=0; + unsigned int c=1; + if (!strcmp(argv[c],"-err")){ + err=true; + c++; + } + if (!strcmp(argv[c],"-neff")){ + neff=true; + c++; + } + if (!strcmp(argv[c],"-part")){ + part=true; + c++; + } + ifstream is(argv[c]); + if (!is){ + cout << "could read file "<< endl; + return -1; + } + c++; + RecordList rl; + rl.read(is); + unsigned int bestidx=rl.getBestIdx(); + cout << endl << "best index = " << bestidx<< endl; + ofstream os(argv[c]); + if (! os){ + cout << "could write file "<< endl; + return -1; + } + rl.printPath(os,bestidx,err); + if(part) + rl.printLastParticles(os); + os.close(); + return 0; +} diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp new file mode 100644 index 0000000..8b16427 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.cpp @@ -0,0 +1,506 @@ +#include +#include "gfsreader.h" +#include +#include + +namespace GMapping { + +namespace GFSReader{ + +Record::~Record(){} +void Record::write(ostream& os){}; + +void CommentRecord::read(istream& is){ + char buf[MAX_LINE_LENGHT]; + memset(buf,0, MAX_LINE_LENGHT*sizeof(char)); + is.getline(buf, MAX_LINE_LENGHT); + text=string(buf); +} + +void CommentRecord::write(ostream& os){ + os << "#GFS_COMMENT: " << text << endl; +} + +PoseRecord::PoseRecord(bool ideal){ + truePos=ideal; +} +void PoseRecord::read(istream& is){ + is >> pose.x >> pose.y >> pose.theta; + time = 0; + if (is) + is >> time; +} +void PoseRecord::write(ostream& os){ + if (truePos) + os << "TRUEPOS "; + else + os << "ODOM "; + os << setiosflags(ios::fixed) << setprecision(6); + os << pose.x << " " << pose.y << " " << pose.theta << " 0 0 0 "; + os << time << " pippo " << time << endl; +} + +void NeffRecord::read(istream& is){ + is >> neff; + time =0; + if (is) + is >> time; + +} + +void NeffRecord::write(ostream& os){ + os << "NEFF " << neff ; + os << setiosflags(ios::fixed) << setprecision(6); + os << " " << time << " pippo " << time << endl; +} + + +void OdometryRecord::read(istream& is){ + is >> dim; + for (unsigned int i=0; i< dim; i++){ + OrientedPoint p; + double w; + is >> p.x; + is >> p.y; + is >> p.theta; + is >> w; + poses.push_back(p); + } + time = 0; + if (is) + is >> time; +} + +void RawOdometryRecord::read(istream& is){ + is >> pose.x; + is >> pose.y; + is >> pose.theta; + time = 0; + assert(is); + is >> time; + +} + + +void EntropyRecord::read(istream& is){ + is >> poseEntropy >> trajectoryEntropy >> mapEntropy; + time =0; + if (is) + is >> time; +} + +void EntropyRecord::write(ostream& os){ + os << setiosflags(ios::fixed) << setprecision(6) << "ENTROPY " << poseEntropy << " " << trajectoryEntropy << " " << mapEntropy; + os << " " << time << " pippo " << time << endl; +} + +void ScanMatchRecord::read(istream& is){ + is >> dim; + for (unsigned int i=0; i< dim; i++){ + OrientedPoint p; + double w; + is >> p.x; + is >> p.y; + is >> p.theta; + is >> w; + poses.push_back(p); + weights.push_back(w); + } +} + +void LaserRecord::read(istream& is){ + is >> dim; + for (unsigned int i=0; i< dim; i++){ + double r; + is >> r; + readings.push_back(r); + } + is >> pose.x; + is >> pose.y; + is >> pose.theta; + time = 0; + if (is) + is >> time; +} + +void LaserRecord::write(ostream& os){ + os << "WEIGHT " << weight << endl; + os << "ROBOTLASER1 "; + + + if ((dim == 541)||(dim == 540)) { // S300 + os <<" 4"; // laser type + os <<" -2.351831"; // start_angle + os <<" 4.712389"; // fov + os <<" 0.008727"; // angular res + os <<" 30.0" ; // maxrange + } + else if ((dim == 180)||(dim == 181)) { // PLS + os <<" 0"; // laser type + os <<" -1.570796"; // start_angle + os <<" 3.141593"; // fov + os <<" 0.017453"; // angular res + os <<" 81.9" ; // maxrange + } + else if ((dim == 360)||(dim == 361)) { // LMS + os <<" 0"; // laser type + os <<" -1.570796"; // start_angle + os <<" 3.141593"; // fov + os <<" 0.008726"; // angular res + os <<" 81.9" ; // maxrange + } + else if ((dim == 682)||(dim == 683)) { // URG + os <<" 0"; // laser type + os <<" -2.094395"; // start_angle + os <<" 4.1887902"; // fov + os << " " << 360.0/1024.0/180.0*M_PI; // angular res + os <<" 5.5" ; // maxrange + } + else { // PLS + os <<" 0"; // laser type + os <<" -1.570796"; // start_angle + os <<" 3.141593"; // fov + os <<" 0.017453"; // angular res + os <<" 81.9" ; // maxrange + } + os <<" 0.01"; // accuracy + os <<" 0" ; // remission mode + os <<" "<< dim; // num readings + os << setiosflags(ios::fixed) << setprecision(2); + for (unsigned int i=0; i< dim; i++){ + os <<" "<< readings[i] ; + } + os << setiosflags(ios::fixed) << setprecision(6); + os <<" 0"; // num remession values + os <<" "<< pose.x; + os <<" "<< pose.y; + os <<" "<< pose.theta; + os <<" "<< pose.x; + os <<" "<< pose.y; + os <<" "<< pose.theta; + os <<" 0" ; // tv + os <<" 0" ; // rv + os <<" 0.55" ; // forward_safety_dist + os <<" 0.375" ; // sideward_safety_dist + os <<" 1000000.0" ; // turn_axis + os <<" "<< time << " localhost " << time << endl; +}; + +void ResampleRecord::read(istream& is){ + is >> dim; + for (unsigned int i=0; i< dim; i++){ + unsigned int j; + is >> j; + indexes.push_back(j); + } +} + +istream& RecordList::read(istream& is){ + while(is){ + char buf[MAX_LINE_LENGHT]; + is.getline(buf, MAX_LINE_LENGHT); + istringstream lineStream(buf); + string recordType; + lineStream >> recordType; + Record* rec=0; + if (recordType=="LASER_READING"){ + rec=new LaserRecord; +// cout << "l" << flush; + } + else if (recordType=="ODO_UPDATE"){ + rec=new OdometryRecord; +// cout << "o" << flush; + } + else if (recordType=="ODOM"){ + rec=new RawOdometryRecord; +// cout << "O" << flush; + } + else if (recordType=="SM_UPDATE"){ + rec=new ScanMatchRecord; +// cout << "m" << flush; + } + else if (recordType=="SIMULATOR_POS"){ + rec=new PoseRecord(true); +// cout << "t" << flush; + } + else if (recordType=="RESAMPLE"){ + rec=new ResampleRecord; +// cout << "r" << flush; + } + else if (recordType=="NEFF"){ + rec=new NeffRecord; +// cout << "n" << flush; + } + else if (recordType=="COMMENT" || recordType=="#COMMENT"){ + rec=new CommentRecord; +// cout << "c" << flush; + } + else if (recordType=="ENTROPY"){ + rec=new EntropyRecord; +// cout << "c" << flush; + } + + if (rec){ + rec->read(lineStream); + push_back(rec); + } + } + return is; +} + +double RecordList::getLogWeight(unsigned int i) const{ + double weight=0; + unsigned int currentIndex=i; + for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){ + ScanMatchRecord* scanmatch=dynamic_cast(*it); + if (scanmatch){ + weight+=scanmatch->weights[currentIndex]; + } + ResampleRecord* resample=dynamic_cast(*it); + if (resample){ + currentIndex=resample->indexes[currentIndex]; + } + } + return weight; +} + +double RecordList::getLogWeight(unsigned int i, RecordList::const_iterator frame) const{ + double weight=0; + unsigned int currentIndex=i; + for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){ + ScanMatchRecord* scanmatch=dynamic_cast(*it); + if (scanmatch){ + weight+=scanmatch->weights[currentIndex]; + } + ResampleRecord* resample=dynamic_cast(*it); + if (resample){ + currentIndex=resample->indexes[currentIndex]; + } + } + return weight; +} + +unsigned int RecordList::getBestIdx() const { + if (empty()) + return 0; + const ScanMatchRecord* scanmatch=0; + const_reverse_iterator it=rbegin(); + while(!scanmatch){ + scanmatch=dynamic_cast(*it); + it++; + } + unsigned int dim=scanmatch->dim; + sampleSize=(int)dim; + double bestw=-std::numeric_limits::max(); + unsigned int best=scanmatch->dim+1; + for (unsigned i=0; ibestw){ + best=i; + bestw=w; + } + } + return best; +} + +void RecordList::printLastParticles(ostream& os) const { + if (empty()) + return; + const ScanMatchRecord* scanmatch=0; + const_reverse_iterator it=rbegin(); + while(!scanmatch){ + scanmatch=dynamic_cast(*it); + it++; + } + if (! scanmatch) + return; + for (vector::const_iterator it=scanmatch->poses.begin(); it!=scanmatch->poses.end(); it++){ + os << "MARKER [color=black; circle=" << it->x*100 << "," << it->y*100 << ",10] 0 pippo 0" << endl; + } +} + +void RecordList::destroyReferences(){ + for(RecordList::iterator it=begin(); it!=end(); it++) + delete (*it); + +} + +RecordList RecordList::computePath(unsigned int i, RecordList::const_iterator frame) const{ + unsigned int currentIndex=i; + OrientedPoint p(0,0,0); + RecordList rl; + + //reconstruct a path + bool first=true; + for(RecordList::const_reverse_iterator it(frame); it!=rend(); it++){ + const ScanMatchRecord* scanmatch=dynamic_cast(*it); + if (scanmatch){ + p=scanmatch->poses[currentIndex]; + first=false; + } + const LaserRecord* laser=dynamic_cast(*it); + if (laser && !first){ + LaserRecord* claser=new LaserRecord(*laser); + claser->pose=p; + rl.push_front(claser); + } + const ResampleRecord* resample=dynamic_cast(*it); + if (resample){ + currentIndex=resample->indexes[currentIndex]; + } + } + return rl; +} + + +void RecordList::printPath(ostream& os, unsigned int i, bool err, bool rawodom) const{ + unsigned int currentIndex=i; + OrientedPoint p(0,0,0); + RecordList rl; + double oldWeight=0; + double w=0; + //reconstruct a path + for(RecordList::const_reverse_iterator it=rbegin(); it!=rend(); it++){ + const NeffRecord* neff=dynamic_cast(*it); + if (neff){ + NeffRecord* n=new NeffRecord(*neff); + rl.push_front(n); + } + const EntropyRecord* entropy=dynamic_cast(*it); + if (entropy){ + EntropyRecord* n=new EntropyRecord(*entropy); + rl.push_front(n); + } + const ScanMatchRecord* scanmatch=dynamic_cast(*it); + if (scanmatch){ + PoseRecord* pose=new PoseRecord; + pose->dim=0; + p=pose->pose=scanmatch->poses[currentIndex]; + w=scanmatch->weights[currentIndex]-oldWeight; + oldWeight=scanmatch->weights[currentIndex]; + + if (!rawodom) { + rl.push_front(pose); + } + } + const OdometryRecord* odometry=dynamic_cast(*it); + if (odometry){ + PoseRecord* pose=new PoseRecord; + pose->dim=0; + p=pose->pose=odometry->poses[currentIndex]; + pose->time=odometry->time; + if (!rawodom) { + rl.push_front(pose); + } + } + const RawOdometryRecord* rawodometry=dynamic_cast(*it); + if (rawodometry){ + PoseRecord* pose=new PoseRecord; + pose->dim=0; + pose->pose=rawodometry->pose; + pose->time=rawodometry->time; + if (rawodom) { + rl.push_front(pose); + } + } + const PoseRecord* tpose=dynamic_cast(*it); + if (tpose){ + PoseRecord* pose=new PoseRecord(*tpose); + rl.push_front(pose); + } + const LaserRecord* laser=dynamic_cast(*it); + if (laser){ + LaserRecord* claser=new LaserRecord(*laser); + claser->pose=p; + claser->weight=w; + rl.push_front(claser); + } + const CommentRecord* comment=dynamic_cast(*it); + if (comment){ + CommentRecord* ccomment=new CommentRecord(*comment); + rl.push_front(ccomment); + } + const ResampleRecord* resample=dynamic_cast(*it); + if (resample){ + rl.push_front(new ResampleRecord(*resample)); + currentIndex=resample->indexes[currentIndex]; + } + + } + bool started=false; + bool computedTransformation=false; + bool truePosFound=false; + OrientedPoint truePose; + OrientedPoint oldPose; + OrientedPoint trueStart, realStart; + bool tpf=false; + double neff=0; + double totalError=0; + int count=0; + for(RecordList::iterator it=rl.begin(); it!=rl.end(); it++){ + NeffRecord* neffr=dynamic_cast(*it); + if (neffr) + neff=neffr->neff/(double)sampleSize; + started=started || dynamic_cast(*it)?true:false; + if (started && ! truePosFound){ + PoseRecord* tpose=dynamic_cast(*it); + if (tpose && tpose->truePos){ + truePosFound=true; + tpf=true; + truePose=tpose->pose; + os << "# "; + (*it)->write(os); + } + } + if (started && truePosFound && ! computedTransformation){ + PoseRecord* pos=dynamic_cast(*it); + if (pos && !pos->truePos){ + trueStart=truePose; + realStart=pos->pose; + os << "# "; + (*it)->write(os); + computedTransformation=true; + } + } + if (computedTransformation){ + os << setiosflags(ios::fixed) << setprecision(6); + PoseRecord* pos=dynamic_cast(*it); + if (pos){ + if (pos->truePos){ + tpf=true; + truePose=pos->pose; + } else { + if (tpf){ + tpf=false; + OrientedPoint realDelta=absoluteDifference(pos->pose,realStart); + OrientedPoint trueDelta=absoluteDifference(truePose,trueStart); + double ex=realDelta.x-trueDelta.x; + double ey=realDelta.y-trueDelta.y; + double eth=realDelta.theta-trueDelta.theta; + eth=atan2(sin(eth), cos(eth)); + if (! err) + os << "# ERROR "; + os << neff << " " + << ex << " " << ey << " " << eth + << " " << sqrt(ex*ex+ey*ey) << " " << fabs(eth) << endl; + totalError+=sqrt(ex*ex+ey*ey); + count++; + } + } + } + + } + PoseRecord* pos=dynamic_cast(*it); + if (pos) + oldPose=pos->pose; + if (! err) + (*it)->write(os); + delete *it; + } + if (err) + cout << "average error" << totalError/count << endl; +} + +}; //gfsreader + +}; //GMapping; diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h b/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h new file mode 100644 index 0000000..16041f3 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/gfsreader.h @@ -0,0 +1,101 @@ +#ifndef GFSREADER_H +#define GFSREADER_H + +#include +#include +#include +#include +#include +#include + +#define MAX_LINE_LENGHT (1000000) + +namespace GMapping{ + +namespace GFSReader{ + +using namespace std; + +struct Record{ + unsigned int dim; + double time; + virtual ~Record(); + virtual void read(istream& is)=0; + virtual void write(ostream& os); +}; + +struct CommentRecord: public Record{ + string text; + virtual void read(istream& is); + virtual void write(ostream& os); +}; + +struct PoseRecord: public Record{ + PoseRecord(bool ideal=false); + void read(istream& is); + virtual void write(ostream& os); + bool truePos; + OrientedPoint pose; +}; + +struct NeffRecord: public Record{ + void read(istream& is); + virtual void write(ostream& os); + double neff; +}; + +struct EntropyRecord: public Record{ + void read(istream& is); + virtual void write(ostream& os); + double poseEntropy; + double trajectoryEntropy; + double mapEntropy; +}; + + +struct OdometryRecord: public Record{ + virtual void read(istream& is); + vector poses; +}; + +struct RawOdometryRecord: public Record{ + virtual void read(istream& is); + OrientedPoint pose; +}; + +struct ScanMatchRecord: public Record{ + virtual void read(istream& is); + vector poses; + vector weights; +}; + +struct LaserRecord: public Record{ + virtual void read(istream& is); + virtual void write(ostream& os); + vector readings; + OrientedPoint pose; + double weight; +}; + +struct ResampleRecord: public Record{ + virtual void read(istream& is); + vector indexes; +}; + +struct RecordList: public list{ + mutable int sampleSize; + istream& read(istream& is); + double getLogWeight(unsigned int i) const; + double getLogWeight(unsigned int i, RecordList::const_iterator frame) const; + unsigned int getBestIdx() const ; + void printLastParticles(ostream& os) const ; + void printPath(ostream& os, unsigned int i, bool err=false, bool rawodom=false) const; + RecordList computePath(unsigned int i, RecordList::const_iterator frame) const; + void destroyReferences(); +}; + +}; //end namespace GFSReader + +}; //end namespace GMapping + +#endif diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp new file mode 100644 index 0000000..a4a510e --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/gridslamprocessor.cpp @@ -0,0 +1,518 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//#define MAP_CONSISTENCY_CHECK +//#define GENERATE_TRAJECTORIES + +namespace GMapping { + +const double m_distanceThresholdCheck = 20; + +using namespace std; + + GridSlamProcessor::GridSlamProcessor(): m_infoStream(cout){ + + period_ = 5.0; + m_obsSigmaGain=1; + m_resampleThreshold=0.5; + m_minimumScore=0.; + } + + GridSlamProcessor::GridSlamProcessor(const GridSlamProcessor& gsp) + :last_update_time_(0.0), m_particles(gsp.m_particles), m_infoStream(cout){ + + period_ = 5.0; + + m_obsSigmaGain=gsp.m_obsSigmaGain; + m_resampleThreshold=gsp.m_resampleThreshold; + m_minimumScore=gsp.m_minimumScore; + + m_beams=gsp.m_beams; + m_indexes=gsp.m_indexes; + m_motionModel=gsp.m_motionModel; + m_resampleThreshold=gsp.m_resampleThreshold; + m_matcher=gsp.m_matcher; + + m_count=gsp.m_count; + m_readingCount=gsp.m_readingCount; + m_lastPartPose=gsp.m_lastPartPose; + m_pose=gsp.m_pose; + m_odoPose=gsp.m_odoPose; + m_linearDistance=gsp.m_linearDistance; + m_angularDistance=gsp.m_angularDistance; + m_neff=gsp.m_neff; + + cerr << "FILTER COPY CONSTRUCTOR" << endl; + cerr << "m_odoPose=" << m_odoPose.x << " " < >::reference* const, int> PointerMap; + PointerMap pmap; + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + const ScanMatcherMap& m1(it->map); + const HierarchicalArray2D& h1(m1.storage()); + for (int x=0; x >& a1(h1.m_cells[x][y]); + if (a1.m_reference){ + PointerMap::iterator f=pmap.find(a1.m_reference); + if (f==pmap.end()) + pmap.insert(make_pair(a1.m_reference, 1)); + else + f->second++; + } + } + } + } + cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; + for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) + assert(it->first->shares==(unsigned int)it->second); + + cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; +# endif + GridSlamProcessor* cloned=new GridSlamProcessor(*this); + +# ifdef MAP_CONSISTENCY_CHECK + cerr << __PRETTY_FUNCTION__ << ": trajectories end" << endl; + cerr << __PRETTY_FUNCTION__ << ": performing afterclone_fit_test" << endl; + ParticleVector::const_iterator jt=cloned->m_particles.begin(); + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + const ScanMatcherMap& m1(it->map); + const ScanMatcherMap& m2(jt->map); + const HierarchicalArray2D& h1(m1.storage()); + const HierarchicalArray2D& h2(m2.storage()); + jt++; + for (int x=0; x >& a1(h1.m_cells[x][y]); + const autoptr< Array2D >& a2(h2.m_cells[x][y]); + assert(a1.m_reference==a2.m_reference); + assert((!a1.m_reference) || !(a1.m_reference->shares%2)); + } + } + } + cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; +# endif + return cloned; +} + + GridSlamProcessor::~GridSlamProcessor(){ + cerr << __PRETTY_FUNCTION__ << ": Start" << endl; + cerr << __PRETTY_FUNCTION__ << ": Deleting tree" << endl; + for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ +#ifdef TREE_CONSISTENCY_CHECK + TNode* node=it->node; + while(node) + node=node->parent; + cerr << "@" << endl; +#endif + if (it->node) + delete it->node; + //cout << "l=" << it->weight<< endl; + } + +# ifdef MAP_CONSISTENCY_CHECK + cerr << __PRETTY_FUNCTION__ << ": performing predestruction_fit_test" << endl; + typedef std::map >::reference* const, int> PointerMap; + PointerMap pmap; + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + const ScanMatcherMap& m1(it->map); + const HierarchicalArray2D& h1(m1.storage()); + for (int x=0; x >& a1(h1.m_cells[x][y]); + if (a1.m_reference){ + PointerMap::iterator f=pmap.find(a1.m_reference); + if (f==pmap.end()) + pmap.insert(make_pair(a1.m_reference, 1)); + else + f->second++; + } + } + } + } + cerr << __PRETTY_FUNCTION__ << ": Number of allocated chunks" << pmap.size() << endl; + for(PointerMap::const_iterator it=pmap.begin(); it!=pmap.end(); it++) + assert(it->first->shares>=(unsigned int)it->second); + cerr << __PRETTY_FUNCTION__ << ": SUCCESS, the error is somewhere else" << endl; +# endif + } + + + + void GridSlamProcessor::setMatchingParameters (double urange, double range, double sigma, int kernsize, double lopt, double aopt, + int iterations, double likelihoodSigma, double likelihoodGain, unsigned int likelihoodSkip){ + m_obsSigmaGain=likelihoodGain; + m_matcher.setMatchingParameters(urange, range, sigma, kernsize, lopt, aopt, iterations, likelihoodSigma, likelihoodSkip); + if (m_infoStream) + m_infoStream << " -maxUrange "<< urange + << " -maxUrange "<< range + << " -sigma "<< sigma + << " -kernelSize "<< kernsize + << " -lstep " << lopt + << " -lobsGain " << m_obsSigmaGain + << " -astep " << aopt << endl; + + + } + +void GridSlamProcessor::setMotionModelParameters +(double srr, double srt, double str, double stt){ + m_motionModel.srr=srr; + m_motionModel.srt=srt; + m_motionModel.str=str; + m_motionModel.stt=stt; + + if (m_infoStream) + m_infoStream << " -srr "<< srr << " -srt "<< srt + << " -str "<< str << " -stt "<< stt << endl; + +} + + void GridSlamProcessor::setUpdateDistances(double linear, double angular, double resampleThreshold){ + m_linearThresholdDistance=linear; + m_angularThresholdDistance=angular; + m_resampleThreshold=resampleThreshold; + if (m_infoStream) + m_infoStream << " -linearUpdate " << linear + << " -angularUpdate "<< angular + << " -resampleThreshold " << m_resampleThreshold << endl; + } + + //HERE STARTS THE BEEF + + GridSlamProcessor::Particle::Particle(const ScanMatcherMap& m): + map(m), pose(0,0,0), weight(0), weightSum(0), gweight(0), previousIndex(0){ + node=0; + } + + + void GridSlamProcessor::setSensorMap(const SensorMap& smap){ + + /* + Construct the angle table for the sensor + + FIXME For now detect the readings of only the front laser, and assume its pose is in the center of the robot + */ + + SensorMap::const_iterator laser_it=smap.find(std::string("FLASER")); + if (laser_it==smap.end()){ + cerr << "Attempting to load the new carmen log format" << endl; + laser_it=smap.find(std::string("ROBOTLASER1")); + assert(laser_it!=smap.end()); + } + const RangeSensor* rangeSensor=dynamic_cast((laser_it->second)); + assert(rangeSensor && rangeSensor->beams().size()); + + m_beams=static_cast(rangeSensor->beams().size()); + double* angles=new double[rangeSensor->beams().size()]; + for (unsigned int i=0; ibeams()[i].pose.theta; + } + m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose()); + delete [] angles; + } + + void GridSlamProcessor::init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, OrientedPoint initialPose){ + m_xmin=xmin; + m_ymin=ymin; + m_xmax=xmax; + m_ymax=ymax; + m_delta=delta; + if (m_infoStream) + m_infoStream + << " -xmin "<< m_xmin + << " -xmax "<< m_xmax + << " -ymin "<< m_ymin + << " -ymax "<< m_ymax + << " -delta "<< m_delta + << " -particles "<< size << endl; + + + m_particles.clear(); + TNode* node=new TNode(initialPose, 0, 0, 0); + ScanMatcherMap lmap(Point(xmin+xmax, ymin+ymax)*.5, xmax-xmin, ymax-ymin, delta); + for (unsigned int i=0; i(o.getSensor()); + if (os && os->isIdeal() && m_outputStream){ + m_outputStream << setiosflags(ios::fixed) << setprecision(3); + m_outputStream << "SIMULATOR_POS " << o.getPose().x << " " << o.getPose().y << " " ; + m_outputStream << setiosflags(ios::fixed) << setprecision(6) << o.getPose().theta << " " << o.getTime() << endl; + } + } + + + bool GridSlamProcessor::processScan(const RangeReading & reading, int adaptParticles){ + + /**retireve the position from the reading, and compute the odometry*/ + OrientedPoint relPose=reading.getPose(); + if (!m_count){ + m_lastPartPose=m_odoPose=relPose; + } + + //write the state of the reading and update all the particles using the motion model + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + OrientedPoint& pose(it->pose); + pose=m_motionModel.drawFromMotion(it->pose, relPose, m_odoPose); + } + + // update the output file + if (m_outputStream.is_open()){ + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << "ODOM "; + m_outputStream << setiosflags(ios::fixed) << setprecision(3) << m_odoPose.x << " " << m_odoPose.y << " "; + m_outputStream << setiosflags(ios::fixed) << setprecision(6) << m_odoPose.theta << " "; + m_outputStream << reading.getTime(); + m_outputStream << endl; + } + if (m_outputStream.is_open()){ + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << "ODO_UPDATE "<< m_particles.size() << " "; + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + OrientedPoint& pose(it->pose); + m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; + m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; + } + m_outputStream << reading.getTime(); + m_outputStream << endl; + } + + //invoke the callback + onOdometryUpdate(); + + + // accumulate the robot translation and rotation + OrientedPoint move=relPose-m_odoPose; + move.theta=atan2(sin(move.theta), cos(move.theta)); + m_linearDistance+=sqrt(move*move); + m_angularDistance+=fabs(move.theta); + + // if the robot jumps throw a warning + if (m_linearDistance>m_distanceThresholdCheck){ + cerr << "***********************************************************************" << endl; + cerr << "********** Error: m_distanceThresholdCheck overridden!!!! *************" << endl; + cerr << "m_distanceThresholdCheck=" << m_distanceThresholdCheck << endl; + cerr << "Old Odometry Pose= " << m_odoPose.x << " " << m_odoPose.y + << " " <=m_linearThresholdDistance + || m_angularDistance>=m_angularThresholdDistance + || (period_ >= 0.0 && (reading.getTime() - last_update_time_) > period_)){ + last_update_time_ = reading.getTime(); + + if (m_outputStream.is_open()){ + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << "FRAME " << m_readingCount; + m_outputStream << " " << m_linearDistance; + m_outputStream << " " << m_angularDistance << endl; + } + + if (m_infoStream) + m_infoStream << "update frame " << m_readingCount << endl + << "update ld=" << m_linearDistance << " ad=" << m_angularDistance << endl; + + + cerr << "Laser Pose= " << reading.getPose().x << " " << reading.getPose().y + << " " << reading.getPose().theta << endl; + + + //this is for converting the reading in a scan-matcher feedable form + assert(reading.size()==m_beams); + double * plainReading = new double[m_beams]; + for(unsigned int i=0; i(reading.getSensor()), + reading.getTime()); + + if (m_count>0){ + scanMatch(plainReading); + if (m_outputStream.is_open()){ + m_outputStream << "LASER_READING "<< reading.size() << " "; + m_outputStream << setiosflags(ios::fixed) << setprecision(2); + for (RangeReading::const_iterator b=reading.begin(); b!=reading.end(); b++){ + m_outputStream << *b << " "; + } + OrientedPoint p=reading.getPose(); + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << p.x << " " << p.y << " " << p.theta << " " << reading.getTime()<< endl; + m_outputStream << "SM_UPDATE "<< m_particles.size() << " "; + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + const OrientedPoint& pose=it->pose; + m_outputStream << setiosflags(ios::fixed) << setprecision(3) << pose.x << " " << pose.y << " "; + m_outputStream << setiosflags(ios::fixed) << setprecision(6) << pose.theta << " " << it-> weight << " "; + } + m_outputStream << endl; + } + onScanmatchUpdate(); + + updateTreeWeights(false); + + if (m_infoStream){ + m_infoStream << "neff= " << m_neff << endl; + } + if (m_outputStream.is_open()){ + m_outputStream << setiosflags(ios::fixed) << setprecision(6); + m_outputStream << "NEFF " << m_neff << endl; + } + resample(plainReading, adaptParticles, reading_copy); + + } else { + m_infoStream << "Registering First Scan"<< endl; + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + m_matcher.invalidateActiveArea(); + m_matcher.computeActiveArea(it->map, it->pose, plainReading); + m_matcher.registerScan(it->map, it->pose, plainReading); + + // cyr: not needed anymore, particles refer to the root in the beginning! + TNode* node=new TNode(it->pose, 0., it->node, 0); + //node->reading=0; + node->reading = reading_copy; + it->node=node; + + } + } + // cerr << "Tree: normalizing, resetting and propagating weights at the end..." ; + updateTreeWeights(false); + // cerr << ".done!" <previousPose=it->pose; + } + + } + if (m_outputStream.is_open()) + m_outputStream << flush; + m_readingCount++; + return processed; + } + + + std::ofstream& GridSlamProcessor::outputStream(){ + return m_outputStream; + } + + std::ostream& GridSlamProcessor::infoStream(){ + return m_infoStream; + } + + + int GridSlamProcessor::getBestParticleIndex() const{ + unsigned int bi=0; + double bw=-std::numeric_limits::max(); + for (unsigned int i=0; i +#include +#include +#include +#include +#include +//#include + +#include +#include + +namespace GMapping { + +using namespace std; + +GridSlamProcessor::TNode::TNode(const OrientedPoint& p, double w, TNode* n, unsigned int c){ + pose=p; + weight=w; + childs=c; + parent=n; + reading=0; + gweight=0; + if (n){ + n->childs++; + } + flag=0; + accWeight=0; +} + + +GridSlamProcessor::TNode::~TNode(){ + if (parent && (--parent->childs)<=0) + delete parent; + assert(!childs); +} + + +//BEGIN State Save/Restore + +GridSlamProcessor::TNodeVector GridSlamProcessor::getTrajectories() const{ + TNodeVector v; + TNodeMultimap parentCache; + TNodeDeque border; + + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + TNode* node=it->node; + while(node){ + node->flag=false; + node=node->parent; + } + } + + for (ParticleVector::const_iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + TNode* newnode=new TNode(* (it->node) ); + + v.push_back(newnode); + assert(newnode->childs==0); + if (newnode->parent){ + parentCache.insert(make_pair(newnode->parent, newnode)); + //cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl; + if (! newnode->parent->flag){ + //cerr << __PRETTY_FUNCTION__ << ": node " << newnode->parent << " flag=" << newnode->parent->flag<< endl; + newnode->parent->flag=true; + border.push_back(newnode->parent); + } + } + } + + //cerr << __PRETTY_FUNCTION__ << ": border.size(INITIAL)=" << border.size() << endl; + //cerr << __PRETTY_FUNCTION__ << ": parentCache.size()=" << parentCache.size() << endl; + while (! border.empty()){ + //cerr << __PRETTY_FUNCTION__ << ": border.size(PREPROCESS)=" << border.size() << endl; + //cerr << __PRETTY_FUNCTION__ << ": parentCache.size(PREPROCESS)=" << parentCache.size() << endl; + const TNode* node=border.front(); + //cerr << __PRETTY_FUNCTION__ << ": node " << node << endl; + border.pop_front(); + if (! node) + continue; + + TNode* newnode=new TNode(*node); + node->flag=false; + + //update the parent of all of the referring childs + pair p=parentCache.equal_range(node); + double childs=0; + for (TNodeMultimap::iterator it=p.first; it!=p.second; it++){ + assert(it->second->parent==it->first); + (it->second)->parent=newnode; + //cerr << "PS(" << it->first << ", "<< it->second << ")"; + childs++; + } + ////cerr << endl; + parentCache.erase(p.first, p.second); + //cerr << __PRETTY_FUNCTION__ << ": parentCache.size(POSTERASE)=" << parentCache.size() << endl; + assert(childs==newnode->childs); + + //unmark the node + if ( node->parent ){ + parentCache.insert(make_pair(node->parent, newnode)); + if(! node->parent->flag){ + border.push_back(node->parent); + node->parent->flag=true; + } + } + //insert the parent in the cache + } + //cerr << __PRETTY_FUNCTION__ << " : checking cloned trajectories" << endl; + for (unsigned int i=0; iparent; + } + //cerr << endl; + } + + return v; + +} + +void GridSlamProcessor::integrateScanSequence(GridSlamProcessor::TNode* node){ + //reverse the list + TNode* aux=node; + TNode* reversed=0; + double count=0; + while(aux!=0){ + TNode * newnode=new TNode(*aux); + newnode->parent=reversed; + reversed=newnode; + aux=aux->parent; + count++; + } + + //attach the path to each particle and compute the map; + if (m_infoStream ) + m_infoStream << "Restoring State Nodes=" <pose; + first=false; + oldWeight=aux->weight; + } + + OrientedPoint dp=aux->pose-oldPose; + double dw=aux->weight-oldWeight; + oldPose=aux->pose; + + + double * plainReading = new double[m_beams]; + for(unsigned int i=0; ireading))[i]; + + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + //compute the position relative to the path; + double s=sin(oldPose.theta-it->pose.theta), + c=cos(oldPose.theta-it->pose.theta); + + it->pose.x+=c*dp.x-s*dp.y; + it->pose.y+=s*dp.x+c*dp.y; + it->pose.theta+=dp.theta; + it->pose.theta=atan2(sin(it->pose.theta), cos(it->pose.theta)); + + //register the scan + m_matcher.invalidateActiveArea(); + m_matcher.computeActiveArea(it->map, it->pose, plainReading); + it->weight+=dw; + it->weightSum+=dw; + + // this should not work, since it->weight is not the correct weight! + // it->node=new TNode(it->pose, it->weight, it->node); + it->node=new TNode(it->pose, 0.0, it->node); + //update the weight + } + + delete [] plainReading; + aux=aux->parent; + } + + //destroy the path + aux=reversed; + while (reversed){ + aux=reversed; + reversed=reversed->parent; + delete aux; + } +} + +//END State Save/Restore + +//BEGIN + +void GridSlamProcessor::updateTreeWeights(bool weightsAlreadyNormalized){ + + if (!weightsAlreadyNormalized) { + normalize(); + } + resetTree(); + propagateWeights(); +} + +void GridSlamProcessor::resetTree(){ + // don't calls this function directly, use updateTreeWeights(..) ! + + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + TNode* n=it->node; + while (n){ + n->accWeight=0; + n->visitCounter=0; + n=n->parent; + } + } +} + +double propagateWeight(GridSlamProcessor::TNode* n, double weight){ + if (!n) + return weight; + double w=0; + n->visitCounter++; + n->accWeight+=weight; + if (n->visitCounter==n->childs){ + w=propagateWeight(n->parent,n->accWeight); + } + assert(n->visitCounter<=n->childs); + return w; +} + +double GridSlamProcessor::propagateWeights(){ + // don't calls this function directly, use updateTreeWeights(..) ! + + // all nodes must be resetted to zero and weights normalized + + // the accumulated weight of the root + double lastNodeWeight=0; + // sum of the weights in the leafs + double aw=0; + + std::vector::iterator w=m_weights.begin(); + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + double weight=*w; + aw+=weight; + TNode * n=it->node; + n->accWeight=weight; + lastNodeWeight+=propagateWeight(n->parent,n->accWeight); + w++; + } + + if (fabs(aw-1.0) > 0.0001 || fabs(lastNodeWeight-1.0) > 0.0001) { + cerr << "ERROR: "; + cerr << "root->accWeight=" << lastNodeWeight << " sum_leaf_weights=" << aw << endl; + assert(0); + } + return lastNodeWeight; +} + +}; + +//END diff --git a/slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp b/slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp new file mode 100644 index 0000000..b625ab5 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gridfastslam/motionmodel.cpp @@ -0,0 +1,80 @@ +#include +#include +#include + +#define MotionModelConditioningLinearCovariance 0.01 +#define MotionModelConditioningAngularCovariance 0.001 + +namespace GMapping { + + + +OrientedPoint +MotionModel::drawFromMotion (const OrientedPoint& p, double linearMove, double angularMove) const{ + OrientedPoint n(p); + double lm=linearMove + fabs( linearMove ) * sampleGaussian( srr ) + fabs( angularMove ) * sampleGaussian( str ); + double am=angularMove + fabs( linearMove ) * sampleGaussian( srt ) + fabs( angularMove ) * sampleGaussian( stt ); + n.x+=lm*cos(n.theta+.5*am); + n.y+=lm*sin(n.theta+.5*am); + n.theta+=am; + n.theta=atan2(sin(n.theta), cos(n.theta)); + return n; +} + +OrientedPoint +MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{ + double sxy=0.3*srr; + OrientedPoint delta=absoluteDifference(pnew, pold); + OrientedPoint noisypoint(delta); + noisypoint.x+=sampleGaussian(srr*fabs(delta.x)+str*fabs(delta.theta)+sxy*fabs(delta.y)); + noisypoint.y+=sampleGaussian(srr*fabs(delta.y)+str*fabs(delta.theta)+sxy*fabs(delta.x)); + noisypoint.theta+=sampleGaussian(stt*fabs(delta.theta)+srt*sqrt(delta.x*delta.x+delta.y*delta.y)); + noisypoint.theta=fmod(noisypoint.theta, 2*M_PI); + if (noisypoint.theta>M_PI) + noisypoint.theta-=2*M_PI; + return absoluteSum(p,noisypoint); +} + + +/* +OrientedPoint +MotionModel::drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const{ + + //compute the three stps needed for perfectly matching the two poses if the noise is absent + + OrientedPoint delta=pnew-pold; + double aoffset=atan2(delta.y, delta.x); + double alpha1=aoffset-pold.theta; + alpha1=atan2(sin(alpha1), cos(alpha1)); + double rho=sqrt(delta*delta); + double alpha2=pnew.theta-aoffset; + alpha2=atan2(sin(alpha2), cos(alpha2)); + + OrientedPoint pret=drawFromMotion(p, 0, alpha1); + pret=drawFromMotion(pret, rho, 0); + pret=drawFromMotion(pret, 0, alpha2); + return pret; +} +*/ + + +Covariance3 MotionModel::gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const{ + OrientedPoint delta=absoluteDifference(pnew,pold); + double linearMove=sqrt(delta.x*delta.x+delta.y*delta.y); + double angularMove=fabs(delta.x); + double s11=srr*srr*linearMove*linearMove; + double s22=stt*stt*angularMove*angularMove; + double s12=str*angularMove*srt*linearMove; + Covariance3 cov; + double s=sin(pold.theta),c=cos(pold.theta); + cov.xx=c*c*s11+MotionModelConditioningLinearCovariance; + cov.yy=s*s*s11+MotionModelConditioningLinearCovariance; + cov.tt=s22+MotionModelConditioningAngularCovariance; + cov.xy=s*c*s11; + cov.xt=c*s12; + cov.yt=s*s12; + return cov; +} + +}; + diff --git a/slam_gmapping/openslam_gmapping/gui/CMakeLists.txt b/slam_gmapping/openslam_gmapping/gui/CMakeLists.txt new file mode 100644 index 0000000..c8fabda --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/CMakeLists.txt @@ -0,0 +1,38 @@ +find_package(Qt5Widgets) +if(Qt5Widgets_FOUND) +else() +find_package(Qt4) +include(${QT_USE_FILE}) +endif() + +include_directories(../include/gmapping ../ ../include/gmapping/log/) + +#add_executable(gfs_nogui gfs_nogui.cpp) + +add_executable(gfs_simplegui gfs_simplegui.cpp gsp_thread.cpp) +if(Qt5Widgets_FOUND) +target_link_libraries(gfs_simplegui gridfastslam Qt5::Widgets) +else() +target_link_libraries(gfs_simplegui gridfastslam ${QT_LIBRARIES}) +endif() + +#add_executable(gfs2img gfs2img.cpp gridfastslam) + +#-include ../global.mk + +#OBJS= gsp_thread.o qparticleviewer.o qgraphpainter.o qmappainter.o + +#APPS= gfs_nogui gfs_simplegui gfs2img +#LDFLAGS+= $(QT_LIB) $(KDE_LIB) -lgridfastslam -lscanmatcher -llog -lsensor_range -lsensor_odometry -lsensor_base -lconfigfile -lutils -lpthread + +#ifeq ($(CARMENSUPPORT),1) +#LDFLAGS+= -lcarmenwrapper +#endif + +#CPPFLAGS+= -I../sensor $(QT_INCLUDE) $(KDE_INCLUDE) -I$(CARMEN_HOME)/include + + +#-include ../build_tools/Makefile.generic-shared-object + + + diff --git a/slam_gmapping/openslam_gmapping/gui/gfs2img.cpp b/slam_gmapping/openslam_gmapping/gui/gfs2img.cpp new file mode 100644 index 0000000..cdd654e --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/gfs2img.cpp @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define MAX_LASER_BEAMS 1024 +#define MAX_FILENAME 1024 + +using namespace std; +using namespace GMapping; +using namespace GMapping::GFSReader; + +inline double min(double a, double b){ + return (ab)?a:b; +} + +void computeBoundingBox(double& xmin, double& ymin, double& xmax, double& ymax, const LaserRecord& laser, const OrientedPoint& pose, double maxrange){ + double theta=-M_PI/2+pose.theta; + double theta_step=(laser.readings.size()==180||laser.readings.size()==181)?M_PI/180:M_PI/360; + for (std::vector::const_iterator it=laser.readings.begin(); it!=laser.readings.end(); it++){ + if (*it(*it); + if (lr){ + lastLaser=lr; + continue; + } + const ScanMatchRecord* smr= dynamic_cast(*it); + if (smr && lastLaser){ + for (std::vector::const_iterator pit=smr->poses.begin(); pit!=smr->poses.end(); pit++){ + computeBoundingBox(xmin, ymin, xmax, ymax, *lastLaser, *pit, maxrange); + } + } + } +} +int main(int argc, char** argv){ + QApplication app(argc, argv); + double maxrange=50; + double delta=0.1; + int scanSkip=5; + const char* filename=0; + const char* format="PNG"; + CMD_PARSE_BEGIN(1, argc) + parseDouble("-maxrange", maxrange); + parseDouble("-delta", delta); + parseInt("-skip", scanSkip); + parseString("-filename",filename); + parseString("-format",format); + CMD_PARSE_END + + double maxUrange=maxrange; + if (! filename){ + cout << " supply a gfs file, please" << endl; + cout << " usage gfs2img [options] -filename " << endl; + cout << " [options]:" << endl; + cout << " -maxrange " << endl; + cout << " -delta " << endl; + cout << " -skip " << endl; + cout << " -format " << endl; + return -1; + } + ifstream is(filename); + if (!is){ + cout << " supply an EXISTING gfs file, please" << endl; + return -1; + } + RecordList rl; + rl.read(is); + + int particles=0; + int beams=0; + for (RecordList::const_iterator it=rl.begin(); it!=rl.end(); it++){ + const OdometryRecord* odometry=dynamic_cast(*it); + if (odometry){ + particles=odometry->dim; + } + const LaserRecord* s=dynamic_cast(*it); + if (s){ + beams=s->readings.size(); + } + if (particles && beams) + break; + } + cout << "Particles from gfs=" << particles << endl; + if (! particles){ + cout << "no particles found, terminating" << endl; + return -1; + } + cout << "Laser beams from gfs=" << beams << endl; + if (! beams){ + cout << "0 beams found, terminating" << endl; + return -1; + } + + + double laserBeamStep=0; + if (beams==180||beams==181){ + laserBeamStep=M_PI/180; + } else if (beams==360||beams==361){ + laserBeamStep=M_PI/360; + } + cout << "Laser beam step" << laserBeamStep << endl; + if (laserBeamStep==0){ + cout << "Invalid Beam Step, terminating" << endl; + return -1; + } + double laserAngles[MAX_LASER_BEAMS]; + double theta=-M_PI/2; + for (int i=0; i(*it); + if (!s) + continue; + scanCount++; + if (scanCount%scanSkip) + continue; + cout << "Frame " << frame << " "; + std::vector paths(particles); + int bestIdx=0; + double bestWeight=-MAXDOUBLE; + for (int p=0; pbestWeight){ + bestWeight=w; + bestIdx=p; + } + } + cout << "bestIdx=" << bestIdx << " bestWeight=" << bestWeight << endl; + + cout << "computing best map" << endl; + ScanMatcherMap smap(center, xmin, ymin, xmax, ymax, delta); + int count=0; + for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){ + const LaserRecord* s=dynamic_cast(*mt); + if (s){ + double rawreadings[MAX_LASER_BEAMS]; + for (uint i=0; ireadings.size(); i++) + rawreadings[i]=s->readings[i]; + matcher.invalidateActiveArea(); + matcher.computeActiveArea(smap, s->pose, rawreadings); +// matcher.allocActiveArea(smap, s->pose, rawreadings); + matcher.registerScan(smap, s->pose, rawreadings); + count++; + } + } + cout << "DONE " << count <=0){ + int grayValue=255-(int)(255.*v); + painter.setPen(QColor(grayValue, grayValue, grayValue)); + painter.drawPoint(x,smap.getMapSizeY()-y-1); + } + } + + /* + cout << "painting trajectories" << endl; + for (int p=0; p(*mt); + if (s){ + IntPoint ip=smap.world2map(s->pose); + ip.y=smap.getMapSizeY()-ip.y-1; + if (!first){ + painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y); + } + oldPoint=ip; + first=false; + } + } + paths[p].destroyReferences();; + } + painter.setPen(QColor(Qt::black)); + bool first=true; + IntPoint oldPoint(0,0); + for (RecordList::const_iterator mt=paths[bestIdx].begin(); mt!=paths[bestIdx].end(); mt++){ + const LaserRecord* s=dynamic_cast(*mt); + if (s){ + IntPoint ip=smap.world2map(s->pose); + ip.y=smap.getMapSizeY()-ip.y-1; + if (!first){ + painter.drawLine( oldPoint.x, oldPoint.y, ip.x, ip.y); + } + oldPoint=ip; + first=false; + } + } + paths[bestIdx].destroyReferences();; + */ + cout << " DONE" << endl; + cout << "writing image" << endl; + QImage img=pixmap.convertToImage(); + char ofilename[MAX_FILENAME]; + sprintf(ofilename,"%s-%.4d.%s",filename, frame, format); + cout << ofilename << endl; + img.save(QString(ofilename), format,0); + frame++; + + } + cout << "For Cyrill: \"The Evil is Outside\"" << endl; +} + diff --git a/slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp b/slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp new file mode 100644 index 0000000..674e59c --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/gfs_logplayer.cpp @@ -0,0 +1,41 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#include +#include "qparticleviewer.h" + +int main (int argc, char ** argv){ + QApplication app(argc, argv); + QParticleViewer * pviewer=new QParticleViewer(0); + app.setMainWidget(pviewer); + pviewer->show(); + FILE* f=fopen(argv[1], "r"); + if (!f) + return -1; + QTextIStream is(f); + pviewer->tis=&is; + pviewer->start(10); + return app.exec(); + std::cout << "DONE: " << argv[1] < +#include "gsp_thread.h" + +using namespace GMapping; + +int main (int argc, char ** argv){ + cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ; + cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl ; + cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl; + + + GridSlamProcessorThread* gsp= new GridSlamProcessorThread; + if (gsp->init(argc, argv)){ + cout << "GSP INIT ERROR" << endl; + return -1; + } + cout <<"GSP INITIALIZED"<< endl; + if (gsp->loadFiles()){ + cout <<"GSP READFILE ERROR"<< endl; + return -2; + } + cout <<"FILES LOADED"<< endl; + gsp->setMapUpdateTime(1000000); + gsp->start(); + cout <<"THREAD STARTED"<< endl; + bool done=false; + while (!done){ + GridSlamProcessorThread::EventDeque events=gsp->getEvents(); + for (GridSlamProcessorThread::EventDeque::iterator it=events.begin(); it!=events.end(); it++){ + cout << flush; + GridSlamProcessorThread::DoneEvent* doneEvent=dynamic_cast(*it); + if (doneEvent){ + done=true; + cout <<"DONE!"<< endl; + gsp->stop(); + } + if (*it) + delete(*it); + } + } +} diff --git a/slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp b/slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp new file mode 100644 index 0000000..ba0b1d7 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/gfs_simplegui.cpp @@ -0,0 +1,96 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#include "qparticleviewer.h" +#include "qgraphpainter.h" +#include +#include +#include +#include +#include +#include + +class GFSMainWindow: public QMainWindow{ +public: + GFSMainWindow(GridSlamProcessorThread* t){ + gsp_thread=t; + QVBoxLayout* layout=new QVBoxLayout(this); + pviewer=new QParticleViewer(this,0,0,gsp_thread); + pviewer->setGeometry(0,0,500,500); + pviewer->setFocusPolicy(Qt::ClickFocus); + layout->addWidget(pviewer); + + gpainter=new QGraphPainter(this); + gpainter->setFixedHeight(100); + layout->addWidget(gpainter); + gpainter->setRange(0,1); + gpainter->setTitle("Neff"); + + help = new QLabel(QString("+/- - zoom | b - show/hide best path | p - show/hide all paths | c - center robot "),this); + help->setMaximumHeight(30); + layout->addWidget(help); + + QObject::connect( pviewer, SIGNAL(neffChanged(double) ), gpainter, SLOT(valueAdded(double)) ); + setTabOrder(pviewer, pviewer); + } + + void start(int c){ + pviewer->start(c); + gpainter->start(c); + } + +protected: + GridSlamProcessorThread* gsp_thread; + QVBoxLayout* layout; + QParticleViewer* pviewer; + QGraphPainter* gpainter; + QLabel* help; +}; + + +int main (int argc, char ** argv){ + cerr << "GMAPPING copyright 2004 by Giorgio Grisetti, Cyrill Stachniss," << endl ; + cerr << "and Wolfram Burgard. To be published under the CreativeCommons license," << endl; + cerr << "see: http://creativecommons.org/licenses/by-nc-sa/2.0/" << endl << endl; + + + GridSlamProcessorThread* gsp= new GridSlamProcessorThread; + if (gsp->init(argc, argv)){ + cerr << "GridFastSlam: Initialization Error!" << endl; + cerr << "(Did you specified an input file for reading?)" << endl; + return -1; + } + if (gsp->loadFiles()){ + cerr <<"Error reading file!"<< endl; + return -2; + } + cerr <<"File successfully loaded!"<< endl; + QApplication app(argc, argv); + GFSMainWindow* mainWin=new GFSMainWindow(gsp); + mainWin->show(); + gsp->setEventBufferSize(10000); + gsp->start(); + mainWin->start(1000); + return app.exec(); +} + diff --git a/slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp b/slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp new file mode 100644 index 0000000..5d0022f --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/gsp_thread.cpp @@ -0,0 +1,643 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#include "gsp_thread.h" +#include +#include +#include + +#ifdef CARMEN_SUPPORT + #include +#endif + +#define DEBUG cout << __PRETTY_FUNCTION__ + +using namespace std; + +int GridSlamProcessorThread::init(int argc, const char * const * argv){ + m_argc=argc; + m_argv=argv; + std::string configfilename; + std::string ebuf="not_set"; + + CMD_PARSE_BEGIN_SILENT(1,argc); + parseStringSilent("-cfg",configfilename); + CMD_PARSE_END_SILENT; + + if (configfilename.length()>0){ + ConfigFile cfg(configfilename); + + filename = (std::string) cfg.value("gfs","filename",filename); + outfilename = (std::string) cfg.value("gfs","outfilename",outfilename); + xmin = cfg.value("gfs","xmin", xmin); + xmax = cfg.value("gfs","xmax",xmax); + ymin = cfg.value("gfs","ymin",ymin); + ymax = cfg.value("gfs","ymax",ymax); + delta = cfg.value("gfs","delta",delta); + maxrange = cfg.value("gfs","maxrange",maxrange); + maxUrange = cfg.value("gfs","maxUrange",maxUrange); + regscore = cfg.value("gfs","regscore",regscore); + critscore = cfg.value("gfs","critscore",critscore); + kernelSize = cfg.value("gfs","kernelSize",kernelSize); + sigma = cfg.value("gfs","sigma",sigma); + iterations = cfg.value("gfs","iterations",iterations); + lstep = cfg.value("gfs","lstep",lstep); + astep = cfg.value("gfs","astep",astep); + maxMove = cfg.value("gfs","maxMove",maxMove); + srr = cfg.value("gfs","srr", srr); + srt = cfg.value("gfs","srt", srt); + str = cfg.value("gfs","str", str); + stt = cfg.value("gfs","stt", stt); + particles = cfg.value("gfs","particles",particles); + angularUpdate = cfg.value("gfs","angularUpdate", angularUpdate); + linearUpdate = cfg.value("gfs","linearUpdate", linearUpdate); + lsigma = cfg.value("gfs","lsigma", lsigma); + ogain = cfg.value("gfs","lobsGain", ogain); + lskip = (int)cfg.value("gfs","lskip", lskip); + mapUpdateTime = cfg.value("gfs","mapUpdate", mapUpdateTime); + randseed = cfg.value("gfs","randseed", randseed); + autosize = cfg.value("gfs","autosize", autosize); + readFromStdin = cfg.value("gfs","stdin", readFromStdin); + resampleThreshold = cfg.value("gfs","resampleThreshold", resampleThreshold); + skipMatching = cfg.value("gfs","skipMatching", skipMatching); + onLine = cfg.value("gfs","onLine", onLine); + generateMap = cfg.value("gfs","generateMap", generateMap); + m_minimumScore = cfg.value("gfs","minimumScore", m_minimumScore); + llsamplerange = cfg.value("gfs","llsamplerange", llsamplerange); + lasamplerange = cfg.value("gfs","lasamplerange",lasamplerange ); + llsamplestep = cfg.value("gfs","llsamplestep", llsamplestep); + lasamplestep = cfg.value("gfs","lasamplestep", lasamplestep); + linearOdometryReliability = cfg.value("gfs","linearOdometryReliability",linearOdometryReliability); + angularOdometryReliability = cfg.value("gfs","angularOdometryReliability",angularOdometryReliability); + ebuf = (std::string) cfg.value("gfs","estrategy", ebuf); + considerOdometryCovariance = cfg.value("gfs","considerOdometryCovariance",considerOdometryCovariance); + + } + + + CMD_PARSE_BEGIN(1,argc); + parseString("-cfg",configfilename); /* to avoid the warning*/ + parseString("-filename",filename); + parseString("-outfilename",outfilename); + parseDouble("-xmin",xmin); + parseDouble("-xmax",xmax); + parseDouble("-ymin",ymin); + parseDouble("-ymax",ymax); + parseDouble("-delta",delta); + parseDouble("-maxrange",maxrange); + parseDouble("-maxUrange",maxUrange); + parseDouble("-regscore",regscore); + parseDouble("-critscore",critscore); + parseInt("-kernelSize",kernelSize); + parseDouble("-sigma",sigma); + parseInt("-iterations",iterations); + parseDouble("-lstep",lstep); + parseDouble("-astep",astep); + parseDouble("-maxMove",maxMove); + parseDouble("-srr", srr); + parseDouble("-srt", srt); + parseDouble("-str", str); + parseDouble("-stt", stt); + parseInt("-particles",particles); + parseDouble("-angularUpdate", angularUpdate); + parseDouble("-linearUpdate", linearUpdate); + parseDouble("-lsigma", lsigma); + parseDouble("-lobsGain", ogain); + parseInt("-lskip", lskip); + parseInt("-mapUpdate", mapUpdateTime); + parseInt("-randseed", randseed); + parseFlag("-autosize", autosize); + parseFlag("-stdin", readFromStdin); + parseDouble("-resampleThreshold", resampleThreshold); + parseFlag("-skipMatching", skipMatching); + parseFlag("-onLine", onLine); + parseFlag("-generateMap", generateMap); + parseDouble("-minimumScore", m_minimumScore); + parseDouble("-llsamplerange", llsamplerange); + parseDouble("-lasamplerange", lasamplerange); + parseDouble("-llsamplestep", llsamplestep); + parseDouble("-lasamplestep", lasamplestep); + parseDouble("-linearOdometryReliability",linearOdometryReliability); + parseDouble("-angularOdometryReliability",angularOdometryReliability); + parseString("-estrategy", ebuf); + + parseFlag("-considerOdometryCovariance",considerOdometryCovariance); + CMD_PARSE_END; + + if (filename.length() <=0){ + cout << "no filename specified" << endl; + return -1; + } + return 0; +} + +int GridSlamProcessorThread::loadFiles(const char * fn){ + if (onLine){ + cout << " onLineProcessing" << endl; + return 0; + } + ifstream is; + if (fn) + is.open(fn); + else + is.open(filename.c_str()); + if (! is){ + cout << "no file found" << endl; + return -1; + } + + CarmenConfiguration conf; + conf.load(is); + is.close(); + + sensorMap=conf.computeSensorMap(); + + if (input) + delete input; + + if (! readFromStdin){ + plainStream.open(filename.c_str()); + input=new InputSensorStream(sensorMap, plainStream); + cout << "Plain Stream opened="<< (bool) plainStream << endl; + } else { + input=new InputSensorStream(sensorMap, cin); + cout << "Plain Stream opened on stdin" << endl; + } + return 0; +} + +GridSlamProcessorThread::GridSlamProcessorThread(): GridSlamProcessor(cerr){ + //This are the processor parameters + filename=""; + outfilename=""; + xmin=-100.; + ymin=-100.; + xmax=100.; + ymax=100.; + delta=0.05; + + //scan matching parameters + sigma=0.05; + maxrange=80.; + maxUrange=80.; + regscore=1e4; + lstep=.05; + astep=.05; + kernelSize=1; + iterations=5; + critscore=0.; + maxMove=1.; + lsigma=.075; + ogain=3; + lskip=0; + autosize=false; + skipMatching=false; + + //motion model parameters + srr=0.1, srt=0.1, str=0.1, stt=0.1; + //particle parameters + particles=30; + randseed=0; + + //gfs parameters + angularUpdate=0.5; + linearUpdate=1; + resampleThreshold=0.5; + + input=0; + + pthread_mutex_init(&hp_mutex,0); + pthread_mutex_init(&ind_mutex,0); + pthread_mutex_init(&hist_mutex,0); + running=false; + eventBufferLength=0; + mapUpdateTime=5; + mapTimer=0; + readFromStdin=false; + onLine=false; + generateMap=false; + + // This are the dafault settings for a grid map of 5 cm + llsamplerange=0.01; + llsamplestep=0.01; + lasamplerange=0.005; + lasamplestep=0.005; + linearOdometryReliability=0.; + angularOdometryReliability=0.; + + considerOdometryCovariance=false; +/* + // This are the dafault settings for a grid map of 10 cm + m_llsamplerange=0.1; + m_llsamplestep=0.1; + m_lasamplerange=0.02; + m_lasamplestep=0.01; +*/ + // This are the dafault settings for a grid map of 20/25 cm +/* + m_llsamplerange=0.2; + m_llsamplestep=0.1; + m_lasamplerange=0.02; + m_lasamplestep=0.01; + m_generateMap=false; +*/ + + +} + +GridSlamProcessorThread::~GridSlamProcessorThread(){ + pthread_mutex_destroy(&hp_mutex); + pthread_mutex_destroy(&ind_mutex); + pthread_mutex_destroy(&hist_mutex); + + for (deque::const_iterator it=eventBuffer.begin(); it!=eventBuffer.end(); it++) + delete *it; +} + + +void * GridSlamProcessorThread::fastslamthread(GridSlamProcessorThread* gpt){ + if (! gpt->input && ! gpt->onLine) + return 0; + + + //if started online retrieve the settings from the connection +#ifdef CARMEN_SUPPORT + if (gpt->onLine){ + cout << "starting the process:" << endl; + CarmenWrapper::initializeIPC(gpt->m_argv[0]); + CarmenWrapper::start(gpt->m_argv[0]); + cout << "Waiting for retrieving the sensor map:" << endl; + while (! CarmenWrapper::sensorMapComputed()){ + usleep(500000); + cout << "." << flush; + } + gpt->sensorMap=CarmenWrapper::sensorMap(); + cout << "Connected " << endl; + } +#else + if (gpt->onLine){ + cout << "FATAL ERROR: cannot run online without the carmen support" << endl; + DoneEvent *done=new DoneEvent; + gpt->addEvent(done); + return 0; + } +#endif + + gpt->setSensorMap(gpt->sensorMap); + gpt->setMatchingParameters(gpt->maxUrange, gpt->maxrange, gpt->sigma, gpt->kernelSize, gpt->lstep, gpt->astep, gpt->iterations, gpt->lsigma, gpt->ogain, gpt->lskip); + + double xmin=gpt->xmin, + ymin=gpt->ymin, + xmax=gpt->xmax, + ymax=gpt->ymax; + + OrientedPoint initialPose(0,0,0); + + if (gpt->autosize){ + if (gpt->readFromStdin || gpt->onLine) + cout << "Error, cant autosize form stdin" << endl; + SensorLog * log=new SensorLog(gpt->sensorMap); + ifstream is(gpt->filename.c_str()); + log->load(is); + is.close(); + initialPose=gpt->boundingBox(log, xmin, ymin, xmax, ymax); + delete log; + } + + if( gpt->infoStream()){ + gpt->infoStream() << " initialPose=" << initialPose.x << " " << initialPose.y << " " << initialPose.theta + << cout << " xmin=" << xmin <<" ymin=" << ymin <<" xmax=" << xmax <<" ymax=" << ymax << endl; + } + gpt->setMotionModelParameters(gpt->srr, gpt->srt, gpt->str, gpt->stt); + gpt->setUpdateDistances(gpt->linearUpdate, gpt->angularUpdate, gpt->resampleThreshold); + gpt->setgenerateMap(gpt->generateMap); + gpt->GridSlamProcessor::init(gpt->particles, xmin, ymin, xmax, ymax, gpt->delta, initialPose); + gpt->setllsamplerange(gpt->llsamplerange); + gpt->setllsamplestep(gpt->llsamplestep); + gpt->setlasamplerange(gpt->llsamplerange); + gpt->setlasamplestep(gpt->llsamplestep); + +#define printParam(n)\ + gpt->outputStream() \ + << "PARAM " << \ + #n \ + << " " << gpt->n << endl + + if (gpt->outfilename.length()>0 ){ + gpt->outputStream().open(gpt->outfilename.c_str()); + printParam(filename); + printParam(outfilename); + printParam(xmin); + printParam(ymin); + printParam(xmax); + printParam(ymax); + printParam(delta); + + //scan matching parameters + printParam(sigma); + printParam(maxrange); + printParam(maxUrange); + printParam(regscore); + printParam(lstep); + printParam(astep); + printParam(kernelSize); + printParam(iterations); + printParam(critscore); + printParam(maxMove); + printParam(lsigma); + printParam(ogain); + printParam(lskip); + printParam(autosize); + printParam(skipMatching); + + //motion model parameters + printParam(srr); + printParam(srt); + printParam(str); + printParam(stt); + //particle parameters + printParam(particles); + printParam(randseed); + + //gfs parameters + printParam(angularUpdate); + printParam(linearUpdate); + printParam(resampleThreshold); + + printParam(llsamplerange); + printParam(lasamplerange); + printParam(llsamplestep); + printParam(lasamplestep); + } + #undef printParam + + if (gpt->randseed!=0) + sampleGaussian(1,gpt->randseed); + if (!gpt->infoStream()){ + cerr << "cant open info stream for writing by unuseful debug messages" << endl; + } else { + gpt->infoStream() << "setting randseed" << gpt->randseed<< endl; + } + + +#ifdef CARMEN_SUPPORT + list rrlist; + if (gpt->onLine){ + RangeReading rr(0,0); + while (1){ + while (CarmenWrapper::getReading(rr)){ + RangeReading* nr=new RangeReading(rr); + rrlist.push_back(nr); + gpt->processScan(*nr); + } + } + } +#endif + ofstream rawpath("rawpath.dat"); + if (!gpt->onLine){ + while(*(gpt->input) && gpt->running){ + const SensorReading* r; + (*(gpt->input)) >> r; + if (! r) + continue; + const RangeReading* rr=dynamic_cast(r); + if (rr && gpt->running){ + const RangeSensor* rs=dynamic_cast(rr->getSensor()); + assert (rs && rs->beams().size()==rr->size()); + + bool processed=gpt->processScan(*rr); + rawpath << rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << endl; + if (0 && processed){ + cerr << "Retrieving state .. "; + TNodeVector trajetories=gpt->getTrajectories(); + cerr << "Done" << endl; + cerr << "Deleting Tree state .. "; + for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++) + delete *it; + cerr << "Done" << endl; + } +// if (0 && processed){ +// cerr << "generating copy" << endl;; +// GridSlamProcessor* m_gsp=gpt->clone(); +// Map* pmap=m_gsp->getParticles()[0].map.toDoubleMap() ; +// cerr << "deleting" << endl; +// delete m_gsp; +// delete pmap; +// } + } + const OdometryReading* o=dynamic_cast(r); + if (o && gpt->running){ + gpt->processTruePos(*o); + TruePosEvent* truepos=new TruePosEvent; + truepos->pose=o->getPose(); + } + } + } + rawpath.close(); + + TNodeVector trajetories=gpt->getTrajectories(); + cerr << "WRITING WEIGHTS" << endl; + int pnumber=0; + for (TNodeVector::iterator it=trajetories.begin(); it!=trajetories.end(); it++){ + char buf[10]; + sprintf(buf, "w-%03d.dat",pnumber); + ofstream weightsStream(buf); + GridSlamProcessor::TNode* n=*it; + double oldWeight=0, oldgWeight=0; + while (n!=0){ + double w=n->weight-oldWeight; + double gw=n->gweight-oldgWeight; + oldWeight=n->weight; + oldgWeight=n->gweight; + weightsStream << w << " " << gw << endl; + n=n->parent; + } + weightsStream.close(); + pnumber++; + cerr << buf << endl; + } + + DoneEvent *done=new DoneEvent; + gpt->addEvent(done); + gpt->infoStream() << "Hallo, I am the gsp thread. I have finished. Do you think it is the case of checking for unlocked mutexes." << endl; + return 0; +} + +std::vector GridSlamProcessorThread::getHypotheses(){ + pthread_mutex_lock(&hp_mutex); + std::vector retval(hypotheses); + pthread_mutex_unlock(&hp_mutex); + return retval; +} + +std::vector GridSlamProcessorThread::getIndexes(){ + pthread_mutex_lock(&ind_mutex); + std::vector retval(indexes); + pthread_mutex_unlock(&ind_mutex); + return retval; +} + +void GridSlamProcessorThread::start(){ + if (running) + return; + running=true; + pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this); +} + +void GridSlamProcessorThread::stop(){ + if (! running){ + cout << "PORCO CAZZO" << endl; + return; + } + running=false; + void * retval; + pthread_join(gfs_thread, &retval); +} + +void GridSlamProcessorThread::onOdometryUpdate(){ + pthread_mutex_lock(&hp_mutex); + hypotheses.clear(); + weightSums.clear(); + for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){ + hypotheses.push_back(part->pose); + weightSums.push_back(part->weightSum); + } + + ParticleMoveEvent* event=new ParticleMoveEvent; + event->scanmatched=false; + event->hypotheses=hypotheses; + event->weightSums=weightSums; + event->neff=m_neff; + pthread_mutex_unlock(&hp_mutex); + + addEvent(event); + + syncOdometryUpdate(); +} + +void GridSlamProcessorThread::onResampleUpdate(){ + pthread_mutex_lock(&ind_mutex); + pthread_mutex_lock(&hp_mutex); + + indexes=GridSlamProcessor::getIndexes(); + + assert (indexes.size()==getParticles().size()); + ResampleEvent* event=new ResampleEvent; + event->indexes=indexes; + + pthread_mutex_unlock(&hp_mutex); + pthread_mutex_unlock(&ind_mutex); + + addEvent(event); + + syncResampleUpdate(); +} + +void GridSlamProcessorThread::onScanmatchUpdate(){ + pthread_mutex_lock(&hp_mutex); + hypotheses.clear(); + weightSums.clear(); + unsigned int bestIdx=0; + double bestWeight=-1e1000; + unsigned int idx=0; + for (GridSlamProcessor::ParticleVector::const_iterator part=getParticles().begin(); part!=getParticles().end(); part++ ){ + hypotheses.push_back(part->pose); + weightSums.push_back(part->weightSum); + if(part->weightSum>bestWeight){ + bestIdx=idx; + bestWeight=part->weightSum; + } + idx++; + } + + ParticleMoveEvent* event=new ParticleMoveEvent; + event->scanmatched=true; + event->hypotheses=hypotheses; + event->weightSums=weightSums; + event->neff=m_neff; + addEvent(event); + + if (! mapTimer){ + MapEvent* event=new MapEvent; + event->index=bestIdx; + event->pmap=new ScanMatcherMap(getParticles()[bestIdx].map); + event->pose=getParticles()[bestIdx].pose; + addEvent(event); + } + + mapTimer++; + mapTimer=mapTimer%mapUpdateTime; + + pthread_mutex_unlock(&hp_mutex); + + syncOdometryUpdate(); +} + +void GridSlamProcessorThread::syncOdometryUpdate(){ +} + +void GridSlamProcessorThread::syncResampleUpdate(){ +} + +void GridSlamProcessorThread::syncScanmatchUpdate(){ +} + +void GridSlamProcessorThread::addEvent(GridSlamProcessorThread::Event * e){ + pthread_mutex_lock(&hist_mutex); + while (eventBuffer.size()>eventBufferLength){ + Event* event=eventBuffer.front(); + delete event; + eventBuffer.pop_front(); + } + eventBuffer.push_back(e); + pthread_mutex_unlock(&hist_mutex); +} + +GridSlamProcessorThread::EventDeque GridSlamProcessorThread::getEvents(){ + pthread_mutex_lock(&hist_mutex); + EventDeque copy(eventBuffer); + eventBuffer.clear(); + pthread_mutex_unlock(&hist_mutex); + return copy; +} + +GridSlamProcessorThread::Event::~Event(){} + +GridSlamProcessorThread::MapEvent::~MapEvent(){ + if (pmap) + delete pmap; +} + +void GridSlamProcessorThread::setEventBufferSize(unsigned int length){ + eventBufferLength=length; +} + +OrientedPoint GridSlamProcessorThread::boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const{ + OrientedPoint initialPose(0,0,0); + initialPose=log->boundingBox(xmin, ymin, xmax, ymax); + xmin-=3*maxrange; + ymin-=3*maxrange; + xmax+=3*maxrange; + ymax+=3*maxrange; + return initialPose; +} diff --git a/slam_gmapping/openslam_gmapping/gui/gsp_thread.h b/slam_gmapping/openslam_gmapping/gui/gsp_thread.h new file mode 100644 index 0000000..51c0dc9 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/gsp_thread.h @@ -0,0 +1,177 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#ifndef GSP_THREAD_H +#define GSP_THREAD_H + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace GMapping; + + +#define MAX_STRING_LENGTH 1024 + + +struct GridSlamProcessorThread : public GridSlamProcessor { + struct Event{ + virtual ~Event(); + }; + + struct ParticleMoveEvent: public Event{ + bool scanmatched; + double neff; + std::vector hypotheses; + std::vector weightSums; + }; + + struct TruePosEvent : public Event{ + OrientedPoint pose; + }; + + struct ResampleEvent: public Event{ + std::vector indexes; + }; + + struct MapEvent: public Event{ + ScanMatcherMap* pmap; + unsigned int index; + OrientedPoint pose; + virtual ~MapEvent(); + }; + + struct DoneEvent: public Event{ + }; + + typedef deque EventDeque; + + GridSlamProcessorThread(); + ~GridSlamProcessorThread(); + int init(int argc, const char * const * argv); + int loadFiles(const char * fn=0); + static void * fastslamthread(GridSlamProcessorThread* gpt); + std::vector getHypotheses(); + std::vector getIndexes(); + + EventDeque getEvents(); + + void start(); + void stop(); + + virtual void onOdometryUpdate(); + virtual void onResampleUpdate(); + virtual void onScanmatchUpdate(); + + virtual void syncOdometryUpdate(); + virtual void syncResampleUpdate(); + virtual void syncScanmatchUpdate(); + + void setEventBufferSize(unsigned int length); + inline void setMapUpdateTime(unsigned int ut) {mapUpdateTime=ut;} + inline bool isRunning() const {return running;} + OrientedPoint boundingBox(SensorLog* log, double& xmin, double& ymin, double& xmax, double& ymax) const; + private: + + void addEvent(Event *); + EventDeque eventBuffer; + + unsigned int eventBufferLength; + unsigned int mapUpdateTime; + unsigned int mapTimer; + + //thread interaction stuff + std::vector hypotheses; + std::vector indexes; + std::vector weightSums; + pthread_mutex_t hp_mutex, ind_mutex, hist_mutex; + pthread_t gfs_thread; + bool running; + + //This are the processor parameters + std::string filename; + std::string outfilename; + + double xmin; + double ymin; + double xmax; + double ymax; + bool autosize; + double delta; + double resampleThreshold; + + //scan matching parameters + double sigma; + double maxrange; + double maxUrange; + double regscore; + double lstep; + double astep; + int kernelSize; + int iterations; + double critscore; + double maxMove; + unsigned int lskip; + + //likelihood + double lsigma; + double ogain; + double llsamplerange, lasamplerange; + double llsamplestep, lasamplestep; + double linearOdometryReliability; + double angularOdometryReliability; + + + //motion model parameters + double srr, srt, str, stt; + //particle parameters + int particles; + bool skipMatching; + + //gfs parameters + double angularUpdate; + double linearUpdate; + + //robot config + SensorMap sensorMap; + //input stream + InputSensorStream* input; + std::ifstream plainStream; + bool readFromStdin; + bool onLine; + bool generateMap; + bool considerOdometryCovariance; + unsigned int randseed; + + //dirty carmen interface + const char* const * m_argv; + unsigned int m_argc; + +}; +#endif diff --git a/slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp b/slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp new file mode 100644 index 0000000..744e23b --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qgraphpainter.cpp @@ -0,0 +1,142 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#include +#include "qgraphpainter.h" +#include "moc_qgraphpainter.cpp" +using namespace std; + +QGraphPainter::QGraphPainter( QWidget * parent, const char * name, WFlags f): + QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){ + m_pixmap=new QPixmap(size()); + m_pixmap->fill(Qt::white); + autoscale=false; + m_useYReference = false; +} + +void QGraphPainter::resizeEvent(QResizeEvent * sizeev){ + m_pixmap->resize(sizeev->size()); +} + +QGraphPainter::~QGraphPainter(){ + delete m_pixmap; +} + +void QGraphPainter::clear(){ + values.clear(); +} + +void QGraphPainter::valueAdded(double v){ + values.push_back(v); +} + +void QGraphPainter::valueAdded(double v, double _min, double _max){ + setRange(_min, _max); + values.push_back(v); +} + +void QGraphPainter::setYReference(double y){ + m_useYReference = true; + reference=y; +} + +void QGraphPainter::disableYReference(){ + m_useYReference = false; +} + + +void QGraphPainter::setTitle(const char* t){ + title=t; +} + +void QGraphPainter::setRange(double _min, double _max){ + min=_min; + max=_max; +} + +void QGraphPainter::setAutoscale(bool a) { + autoscale=a; +} + +bool QGraphPainter::getAutoscale() const { + return autoscale; +} + +void QGraphPainter::timerEvent(QTimerEvent * te) { + if (te->timerId()==timer) + update(); +} + +void QGraphPainter::start(int period){ + timer=startTimer(period); +} + + + +void QGraphPainter::paintEvent ( QPaintEvent * ){ + m_pixmap->fill(Qt::white); + QPainter painter(m_pixmap); + double _min=MAXDOUBLE, _max=-MAXDOUBLE; + if (autoscale){ + for (unsigned int i=0; i<(unsigned int)width() && ivalues[i]?_max:values[i]; + } + } else { + _min=min; + _max=max; + } + + + painter.setPen(Qt::black); + painter.drawRect(0, 0, width(), height()); + const int boundary=2; + int xoffset=40; + double scale=((double)height()-2*boundary-2)/(_max-_min); + + if (m_useYReference) { + painter.setPen(Qt::green); + painter.drawLine(xoffset+boundary/2, height()-(int)(scale*(reference-_min)), + width()-boundary/2, height()-(int)(scale*(reference-_min))); + } + painter.setPen(Qt::blue); + unsigned int start=0; + if (values.size()>(unsigned int)width()-2*boundary-xoffset) + start=values.size()-width()+2*boundary+xoffset; + int oldv=0; + if ((unsigned int)width()-2*boundary-xoffset>1 && values.size()>1) + oldv = (int)(scale*(values[1+start]-_min)) + boundary; + + for (unsigned int i=1; i<(unsigned int)width()-2*boundary-xoffset && iwidth(),m_pixmap->height(),CopyROP); +} + diff --git a/slam_gmapping/openslam_gmapping/gui/qgraphpainter.h b/slam_gmapping/openslam_gmapping/gui/qgraphpainter.h new file mode 100644 index 0000000..feb633b --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qgraphpainter.h @@ -0,0 +1,67 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#ifndef QGRAPHPAINTER_H +#define QGRAPHPAINTER_H + +#include +#include +#include +#include +#include +#include +#include + +typedef std::deque DoubleDeque; + +class QGraphPainter : public QWidget{ + Q_OBJECT + public: + QGraphPainter( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0); + virtual ~QGraphPainter(); + public slots: + void clear(); + void valueAdded(double); + void valueAdded(double, double, double); + void setYReference(double y); + void disableYReference(); + void setRange(double min, double max); + void start(int period); + void setTitle(const char* title); + void setAutoscale(bool a); + bool getAutoscale() const; + protected: + virtual void timerEvent(QTimerEvent * te); + virtual void resizeEvent(QResizeEvent *); + double min, max, reference; + DoubleDeque values; + bool autoscale; + bool m_useYReference; + int timer; + virtual void paintEvent ( QPaintEvent *paintevent ); + QPixmap * m_pixmap; + QString title; +}; + +#endif + diff --git a/slam_gmapping/openslam_gmapping/gui/qmappainter.cpp b/slam_gmapping/openslam_gmapping/gui/qmappainter.cpp new file mode 100644 index 0000000..3aa2dfb --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qmappainter.cpp @@ -0,0 +1,32 @@ +#include "qmappainter.h" +#include "moc_qmappainter.cpp" + +QMapPainter::QMapPainter( QWidget * parent, const char * name, WFlags f): + QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){ + m_pixmap=new QPixmap(size()); + m_pixmap->fill(Qt::white); +} + +void QMapPainter::resizeEvent(QResizeEvent * sizeev){ + m_pixmap->resize(sizeev->size()); +} + +QMapPainter::~QMapPainter(){ + delete m_pixmap; +} + + +void QMapPainter::timerEvent(QTimerEvent * te) { + if (te->timerId()==timer) + update(); +} + +void QMapPainter::start(int period){ + timer=startTimer(period); +} + + +void QMapPainter::paintEvent ( QPaintEvent * ){ + bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP); +} + diff --git a/slam_gmapping/openslam_gmapping/gui/qmappainter.h b/slam_gmapping/openslam_gmapping/gui/qmappainter.h new file mode 100644 index 0000000..d121d1b --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qmappainter.h @@ -0,0 +1,60 @@ +#ifndef QMAPPAINTER_H +#define QMAPPAINTER_H + +#include +#include +#include +#include +#include +#include + +class QMapPainter : public QWidget{ + public: + QMapPainter( QWidget * parent = 0, const char * name = 0, WFlags f = 0); + virtual ~QMapPainter(); + public: + template < typename Cell > + void setPixmap(unsigned int xsize, unsigned int ysize, Cell** values); + template < typename Iterator > + void drawPoints(const Iterator& begin, const Iterator& end, unsigned char r, unsigned char g, unsigned char b); + void start(int period); + protected: + virtual void timerEvent(QTimerEvent * te); + virtual void resizeEvent(QResizeEvent *); + int timer; + virtual void paintEvent ( QPaintEvent *paintevent ); + QPixmap * m_pixmap; +}; + +template +void QMapPainter::setPixmap(unsigned int xsize, unsigned int ysize, Cell** values){ + QSize s(xsize, ysize); + m_pixmap->resize(s); + m_pixmap->fill(Qt::white); + QPainter painter(m_pixmap); + for (unsigned int x=0; x<(unsigned int)xsize; x++) + for (unsigned int y=0; y<(unsigned int)ysize; y++){ + double v=(double) values[x][y]; + + if (v>=0){ + unsigned int grayVal=(unsigned char) (255-(unsigned char)(255*v)); + painter.setPen(QColor(grayVal, grayVal, grayVal)); + } else { + painter.setPen(QColor(255, 100, 100)); + } + painter.drawPoint(x,ysize-y); + } +} + +template < typename Iterator > +void QMapPainter::drawPoints(const Iterator& begin, const Iterator& end, unsigned char r, unsigned char g, unsigned char b){ + QPainter painter(m_pixmap); + painter.setPen(QColor(r,g,b)); + for (Iterator it=begin; it!=end; it++){ + GMapping::IntPoint p=(GMapping::IntPoint)*it; + painter.drawPoint(p.x, height()-p.y); + } +} + +#endif + diff --git a/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp b/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp new file mode 100644 index 0000000..d818554 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.cpp @@ -0,0 +1,126 @@ +#include "qnavigatorwidget.h" +#include +using namespace GMapping; + + +QNavigatorWidget::QNavigatorWidget( QWidget * parent, const char * name, WFlags f) +: QMapPainter(parent, name, f), dumper("navigator", 1){ + robotPose=IntPoint(0,0); + robotHeading=0; + confirmLocalization=false; + repositionRobot=false; + startWalker=false; + enableMotion=false; + goHome=false; + trajectorySent=false; + writeImages=false; + drawRobot=true; + wantsQuit=false; +} + +QNavigatorWidget::~QNavigatorWidget(){} + + +void QNavigatorWidget::mousePressEvent ( QMouseEvent * e ){ + QPoint p=e->pos(); + int mx=p.x(); + int my=height()-p.y(); + if (!(e->state()&Qt::ShiftButton) && e->button()==Qt::LeftButton) { + if (trajectorySent) + trajectoryPoints.clear(); + e->accept(); + IntPoint p=IntPoint(mx, my); + trajectoryPoints.push_back(p); + trajectorySent=false; + } + if (e->state()&Qt::ControlButton && e->button()==Qt::LeftButton){ + e->accept(); + robotPose=IntPoint(mx, my); + repositionRobot=true; + confirmLocalization=true; + } + if (e->state()&Qt::ControlButton && e->button()==Qt::RightButton){ + e->accept(); + IntPoint p(mx, my); + p=p-robotPose; + robotHeading=atan2(p.y, p.x); + repositionRobot=true; + confirmLocalization=true; + } +} + +void QNavigatorWidget::keyPressEvent ( QKeyEvent * e ){ + if (e->key()==Qt::Key_Delete){ + e->accept(); + if (!trajectoryPoints.empty()) + trajectoryPoints.pop_back(); + } + if (e->key()==Qt::Key_S){ + e->accept(); + enableMotion=!enableMotion; + } + if (e->key()==Qt::Key_W){ + e->accept(); + startWalker=!startWalker; + } + if (e->key()==Qt::Key_G){ + e->accept(); + startGlobalLocalization=true; + } + if (e->key()==Qt::Key_T){ + e->accept(); + trajectorySent=true; + } + if (e->key()==Qt::Key_R){ + e->accept(); + goHome=true; + } + if (e->key()==Qt::Key_C){ + e->accept(); + confirmLocalization=true; + + } + if (e->key()==Qt::Key_Q){ + e->accept(); + wantsQuit=true; + + } + if (e->key()==Qt::Key_D){ + e->accept(); + drawRobot=!drawRobot;; + + } +} + +void QNavigatorWidget::paintEvent ( QPaintEvent * ){ + QPixmap pixmap(*m_pixmap); + QPainter painter(&pixmap); + if (trajectorySent) + painter.setPen(Qt::red); + bool first=true; + int oldx=0, oldy=0; + //paint the path + for (std::list::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){ + int x=it->x; + int y=height()-it->y; + if (! first) + painter.drawLine(oldx, oldy, x,y); + oldx=x; + oldy=y; + first=false; + } + //paint the robot + if (drawRobot){ + painter.setPen(Qt::black); + int rx=robotPose.x; + int ry=height()-robotPose.y; + int robotSize=6; + painter.drawLine(rx, ry, + rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading))); + painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize); + } + if (writeImages){ + dumper.dump(pixmap); + } + bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP); +} diff --git a/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h b/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h new file mode 100644 index 0000000..9766b9d --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qnavigatorwidget.h @@ -0,0 +1,35 @@ +#ifndef _QNAVIGATOR_WIDGET_H +#define _QNAVIGATOR_WIDGET_H + +#include "qmappainter.h" +#include "qpixmapdumper.h" +#include +#include + +class QNavigatorWidget : public QMapPainter{ + public: + QNavigatorWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); + virtual ~QNavigatorWidget(); + std::list trajectoryPoints; + bool repositionRobot; + GMapping::IntPoint robotPose; + double robotHeading; + bool confirmLocalization; + bool enableMotion; + bool startWalker; + bool startGlobalLocalization; + bool trajectorySent; + bool goHome; + bool wantsQuit; + bool writeImages; + QPixmapDumper dumper; + bool drawRobot; + + protected: + virtual void paintEvent ( QPaintEvent *paintevent ); + virtual void mousePressEvent ( QMouseEvent * e ); + virtual void keyPressEvent ( QKeyEvent * e ); +}; + +#endif + diff --git a/slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp b/slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp new file mode 100644 index 0000000..633aee5 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qparticleviewer.cpp @@ -0,0 +1,453 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#include "qparticleviewer.h" +#include "moc_qparticleviewer.cpp" +#include + + +using namespace GMapping; + +QParticleViewer::QParticleViewer( QWidget * parent, const char * name , WFlags f, GridSlamProcessorThread* thread): QWidget(parent, name, f|WRepaintNoErase|WResizeNoErase){ + viewCenter=Point(0.,0.); + setMinimumSize(500,500); + mapscale=10.; + m_pixmap=new QPixmap(500,500); + m_pixmap->fill(Qt::white); + gfs_thread=thread; + tis=0; + m_particleSize=0; + m_refresh=false; + bestMap=0; + dragging=false; + showPaths=0; + showBestPath=1; + count=0; + writeToFile=0; +} + +QParticleViewer::~QParticleViewer(){ + if (m_pixmap) + delete m_pixmap; +} + +void QParticleViewer::paintEvent ( QPaintEvent *paintevent ){ + if (! m_pixmap) + return; + bitBlt(this,0,0,m_pixmap,0,0,m_pixmap->width(),m_pixmap->height(),CopyROP); +} + +void QParticleViewer::mousePressEvent ( QMouseEvent *event ){ + if (event->button()==LeftButton){ + dragging=true; + draggingPos=event->pos(); + } +} +void QParticleViewer::mouseMoveEvent ( QMouseEvent *event ){ + if (dragging){ + QPoint delta=event->pos()-draggingPos; + draggingPos=event->pos(); + viewCenter.x-=delta.x()/mapscale; + viewCenter.y+=delta.y()/mapscale; + update(); + } +} + +void QParticleViewer::mouseReleaseEvent ( QMouseEvent *event ){ + if (event->button()==LeftButton){ + dragging=false; + } +} + +void QParticleViewer::keyPressEvent ( QKeyEvent* e ){ + switch (e->key()){ + case Qt::Key_B: showBestPath=!showBestPath; break; + case Qt::Key_P: showPaths=!showPaths; break; + case Qt::Key_Plus: mapscale *=1.25; break; + case Qt::Key_Minus: mapscale/=1.25; break; + case Qt::Key_C: viewCenter=bestParticlePose; break; + default:; + } +} + + +void QParticleViewer::resizeEvent(QResizeEvent * sizeev){ + if (!m_pixmap) + return; + cerr << "QParticleViewer::resizeEvent" << sizeev->size().width()<< " " << sizeev->size().height() << endl; + m_pixmap->resize(sizeev->size()); +} + +void QParticleViewer::drawParticleMove(const QParticleViewer::OrientedPointVector& oldPose, const QParticleViewer::OrientedPointVector& newPose){ + assert(oldPose.size()==newPose.size()); + QPainter painter(m_pixmap); + painter.setPen(Qt::red); + OrientedPointVector::const_iterator nit=newPose.begin(); + for(OrientedPointVector::const_iterator it=oldPose.begin(); it!=oldPose.end(); it++, nit++){ + IntPoint p0=map2pic(*it); + IntPoint p1=map2pic(*nit); + painter.drawLine( + (int)(p0.x), (int)(p0.y), (int)(p1.x), (int)(p1.y) + ); + } +} + +void QParticleViewer::drawFromFile(){ + if(! tis) + return; + if (tis->atEnd()) + return; + QTextIStream& is=*tis; + + string line=is.readLine(); + istringstream lineStream(line); + string recordType; + lineStream >> recordType; + if (recordType=="LASER_READING"){ + //do nothing with the laser + cout << "l" << flush; + } + if (recordType=="ODO_UPDATE"){ + //just move the particles + if (m_particleSize) + m_refresh=true; + m_oldPose=m_newPose; + m_newPose.clear(); + unsigned int size; + lineStream >> size; + if (!m_particleSize) + m_particleSize=size; + assert(m_particleSize==size); + for (unsigned int i=0; i< size; i++){ + OrientedPoint p; + double w; + lineStream >> p.x; + lineStream >> p.y; + lineStream >> p.theta; + lineStream >> w; + m_newPose.push_back(p); + } + cout << "o" << flush; + } + if (recordType=="SM_UPDATE"){ + if (m_particleSize) + m_refresh=true; + m_oldPose=m_newPose; + m_newPose.clear(); + unsigned int size; + lineStream >> size; + if (!m_particleSize) + m_particleSize=size; + assert(m_particleSize==size); + for (unsigned int i=0; i< size; i++){ + OrientedPoint p; + double w; + lineStream >> p.x; + lineStream >> p.y; + lineStream >> p.theta; + lineStream >> w; + m_newPose.push_back(p); + } + cout << "u" << flush; + } + if (recordType=="RESAMPLE"){ + unsigned int size; + lineStream >> size; + if (!m_particleSize) + m_particleSize=size; + assert(m_particleSize==size); + OrientedPointVector temp(size); + for (unsigned int i=0; i< size; i++){ + unsigned int ind; + lineStream >> ind; + temp[i]=m_newPose[ind]; + } + m_newPose=temp; + cout << "r" << flush; + } + if (m_refresh){ + drawParticleMove(m_oldPose, m_newPose); + m_refresh=false; + } +} + +void QParticleViewer::drawMap(const ScanMatcherMap& map){ + //cout << "Map received" << map.getMapSizeX() << " " << map.getMapSizeY() << endl; + QPainter painter(m_pixmap); + painter.setPen(Qt::black); + m_pixmap->fill(QColor(200,200,255)); + unsigned int count=0; + + Point wmin=Point(pic2map(IntPoint(-m_pixmap->width()/2,m_pixmap->height()/2))); + Point wmax=Point(pic2map(IntPoint(m_pixmap->width()/2,-m_pixmap->height()/2))); + IntPoint imin=map.world2map(wmin); + IntPoint imax=map.world2map(wmax); + /* cout << __PRETTY_FUNCTION__ << endl; + cout << " viewCenter=" << viewCenter.x << "," << viewCenter.y << endl; + cout << " wmin=" << wmin.x << "," << wmin.y << " wmax=" << wmax.x << "," << wmax.y << endl; + cout << " imin=" << imin.x << "," << imin.y << " imax=" << imax.x << "," << imax.y << endl; + cout << " mapSize=" << map.getMapSizeX() << "," << map.getMapSizeY() << endl;*/ + for(int x=0; xwidth(); x++) + for(int y=0; yheight(); y++){ + //IntPoint ip=IntPoint(x,y)+imin; + //Point p=map.map2world(ip); + Point p=pic2map(IntPoint(x-m_pixmap->width()/2, + y-m_pixmap->height()/2)); + + //if (map.storage().isInside(map.world2map(p))){ + double v=map.cell(p); + if (v>=0){ + int grayValue=255-(int)(255.*v); + painter.setPen(QColor(grayValue, grayValue, grayValue)); + painter.drawPoint(x,y); + count++; + } + } +} + + +void QParticleViewer::drawFromMemory(){ + if (! gfs_thread) + return; + m_pixmap->fill(Qt::white); + GridSlamProcessorThread::EventDeque events=gfs_thread->getEvents(); + for (GridSlamProcessorThread::EventDeque::const_iterator it=events.begin(); it!=events.end();it++){ + GridSlamProcessorThread::MapEvent* mapEvent= dynamic_cast(*it); + if (mapEvent){ + //cout << "Map: bestIdx=" << mapEvent->index <pmap; + mapEvent->pmap=0; + bestParticlePose=mapEvent->pose; + delete mapEvent; + }else{ + GridSlamProcessorThread::DoneEvent* doneEvent= dynamic_cast(*it); + if (doneEvent){ + gfs_thread->stop(); + delete doneEvent; + } else + history.push_back(*it); + } + + } + if (bestMap) + drawMap(*bestMap); + + unsigned int particleSize=0; + std::vector oldPose, newPose; + vector indexes; + + GridSlamProcessorThread::EventDeque::reverse_iterator it=history.rbegin(); + while (!particleSize && it!=history.rend()){ + GridSlamProcessorThread::ParticleMoveEvent* move= dynamic_cast(*it); + GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast(*it); + if (move) + particleSize=move->hypotheses.size(); + if (resample) + particleSize=resample->indexes.size(); + it++; + } + + //check for the best index + double wmax=-1e2000; + unsigned int bestIdx=0; + bool emitted=false; + for (unsigned int i=0; i(*it); + if (move && move->scanmatched){ + double cw=move->weightSums[currentIndex]; + if (cw>wmax){ + wmax=cw; + bestIdx=currentIndex; + } + done=true; + if (! emitted){ + emit neffChanged(move->neff/particleSize); + emitted=true; + } + } + GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast(*it); + if (resample){ + currentIndex=resample->indexes[currentIndex]; + } + } + } + //cout << "bestIdx=" << bestIdx << endl; + QPainter painter(m_pixmap); + + for (unsigned int i=0; i(*it); + if (move){ + OrientedPoint pold=move->hypotheses[currentIndex]; + IntPoint p0=map2pic(pold)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2); + IntPoint p1=map2pic(pnew)+IntPoint(m_pixmap->width()/2,m_pixmap->height()/2);; + if (first){ + painter.drawPoint(p0.x, p0.y); + } else { + painter.drawLine(p0.x, p0.y, p1.x, p1.y); + } + first=false; + if (!(showPaths || showBestPath&&i==particleSize)) + break; + pnew=pold; + } + GridSlamProcessorThread::ResampleEvent* resample= dynamic_cast(*it); + if (resample && ! first){ + currentIndex=resample->indexes[currentIndex]; + } + } + } + if (writeToFile && bestMap){ + if (! (count%writeToFile) ){ + char name[100]; + sprintf(name,"dump-%05d.png", count/writeToFile); + cout << " Writing " << name <<" ..." << flush; + QImage image=m_pixmap->convertToImage(); + bool rv=image.save(name,"PNG"); + if (rv) + cout << " Done"; + else + cout << " ERROR"; + cout << endl; + } + count++; + } +} + +void QParticleViewer::timerEvent(QTimerEvent * te) { + if (te->timerId()==timer) { + if ( tis) + drawFromFile(); + else{ + drawFromMemory(); + update(); + } + } +} + + +void QParticleViewer::start(int period){ + timer=startTimer(period); +} + +void QParticleViewer::refreshParameters(){ + //scanmatcher + matchingParameters.maxrange=gfs_thread->getlaserMaxRange(); + matchingParameters.urange=gfs_thread->getusableRange(); + matchingParameters.ssigma=gfs_thread->getgaussianSigma(); + matchingParameters.sreg=gfs_thread->getregScore(); + matchingParameters.scrit=gfs_thread->getcritScore(); + matchingParameters.ksize=gfs_thread->getkernelSize(); + matchingParameters.lstep=gfs_thread->getoptLinearDelta(); + matchingParameters.astep=gfs_thread->getoptAngularDelta(); + matchingParameters.iterations=gfs_thread->getoptRecursiveIterations(); + + //start + startParameters.srr=gfs_thread->getsrr(); + startParameters.stt=gfs_thread->getstt(); + startParameters.str=gfs_thread->getstr(); + startParameters.srt=gfs_thread->getsrt(); + + startParameters.xmin=gfs_thread->getxmin(); + startParameters.ymin=gfs_thread->getymin(); + startParameters.xmax=gfs_thread->getxmax(); + startParameters.ymax=gfs_thread->getymax(); + startParameters.delta=gfs_thread->getdelta(); + + startParameters.particles=gfs_thread->getParticles().size(); + startParameters.resampleThreshold=gfs_thread->getresampleThreshold(); + startParameters.outFileName=0; +} + +void QParticleViewer::start(){ + gfs_thread->setMatchingParameters( + matchingParameters.urange, + matchingParameters.maxrange, + matchingParameters.ssigma, + matchingParameters.ksize, + matchingParameters.lstep, + matchingParameters.astep, + matchingParameters.iterations, + startParameters.lsigma, + startParameters.lgain, + startParameters.lskip); + gfs_thread->setMotionModelParameters( + startParameters.srr, + startParameters.srt, + startParameters.srt, + startParameters.stt); + gfs_thread->setUpdateDistances( + startParameters.linearUpdate, + startParameters.angularUpdate, + startParameters.resampleThreshold + ); + ((GridSlamProcessor*)(gfs_thread))->init( + startParameters.particles, + startParameters.xmin, + startParameters.ymin, + startParameters.xmax, + startParameters.ymax, + startParameters.delta, + startParameters.initialPose); + gfs_thread->start(); +} + +void QParticleViewer::setMatchingParameters(const QParticleViewer::MatchingParameters& mp){ + matchingParameters=mp; +} + +void QParticleViewer::setStartParameters(const QParticleViewer::StartParameters& sp){ + startParameters=sp; +} + +void QParticleViewer::stop(){ + gfs_thread->stop(); +} + +void QParticleViewer::loadFile(const char * fn){ + gfs_thread->loadFiles(fn); + /* + startParameters.initialPose= + gfs_thread->boundingBox( + startParameters.xmin, + startParameters.ymin, + startParameters.xmax, + startParameters.ymax); + */ +} diff --git a/slam_gmapping/openslam_gmapping/gui/qparticleviewer.h b/slam_gmapping/openslam_gmapping/gui/qparticleviewer.h new file mode 100644 index 0000000..63ff4fd --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qparticleviewer.h @@ -0,0 +1,161 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#ifndef QPARTICLEVIEWER_H +#define QPARTICLEVIEWER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "gsp_thread.h" + +namespace GMapping { + +class QParticleViewer : public QWidget{ + Q_OBJECT + public: + struct StartParameters{ + //motionmodel + double srr, srt, str, stt; + //map + double xmin, ymin, xmax, ymax, delta; + OrientedPoint initialPose; + //likelihood + double lsigma, lgain; + unsigned int lskip; + //update + double linearUpdate, angularUpdate; + //filter + unsigned int particles; + double resampleThreshold; + //mode + bool drawFromObservation; + //output + const char * outFileName; + }; + + struct MatchingParameters{ + //ranges + double maxrange, urange; + //score + double ssigma, sreg, scrit; + unsigned int ksize; + //search + double lstep, astep; + unsigned int iterations; + }; + + void refreshParameters(); //reads the parameters from the thread + inline void setGSP( GridSlamProcessorThread* thread){gfs_thread=thread;} + + + typedef std::vector OrientedPointVector; + QParticleViewer( QWidget * parent = 0, const char * name = 0, Qt::WindowFlags f = 0, GridSlamProcessorThread* thread=0 ); + virtual ~QParticleViewer(); + virtual void timerEvent(QTimerEvent * te); + virtual void resizeEvent(QResizeEvent *); + + void drawFromFile(); + void drawFromMemory(); + void drawMap(const ScanMatcherMap& map); + void start(int period); + QTextStream* tis; + + MatchingParameters matchingParameters; + StartParameters startParameters; + + int writeToFile; + public slots: + void setMatchingParameters(const MatchingParameters& mp); + void setStartParameters(const StartParameters& mp); + void start(); + void stop(); + void loadFile(const char *); + signals: + void neffChanged(double); + void poseEntropyChanged(double, double, double); + void trajectoryEntropyChanged(double, double, double); + void mapsEntropyChanged(double); + void mapsIGainChanged(double); + + protected: + ifstream inputStream; + ofstream outputStream; + + + protected: + inline Point pic2map(const IntPoint& p) + {return viewCenter+Point(p.x/mapscale, -p.y/mapscale); } + inline IntPoint map2pic(const Point& p) + {return IntPoint((int)((p.x-viewCenter.x)*mapscale),(int)((viewCenter.y-p.y)*mapscale)); } + + int timer; + virtual void paintEvent ( QPaintEvent *paintevent ); + void drawParticleMove(const OrientedPointVector& start, const OrientedPointVector& end); + QPixmap* m_pixmap; + + //thread interaction + GridSlamProcessorThread* gfs_thread; + GridSlamProcessorThread::EventDeque history; + + //mouse movement + virtual void mousePressEvent(QMouseEvent*); + virtual void mouseReleaseEvent(QMouseEvent*); + virtual void mouseMoveEvent(QMouseEvent*); + QPoint draggingPos; + bool dragging; + + //particle plotting + virtual void keyPressEvent ( QKeyEvent* e ); + + //map painting + double mapscale; + Point viewCenter; + Point bestParticlePose; + ScanMatcherMap * bestMap; + + // view mode + bool showPaths; + bool showBestPath; + + // file plotting + QParticleViewer::OrientedPointVector m_oldPose, m_newPose; + unsigned int m_particleSize; + bool m_refresh; + int count; +}; + +}; + +#endif + diff --git a/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp b/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp new file mode 100644 index 0000000..239b209 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.cpp @@ -0,0 +1,33 @@ +#include "qpixmapdumper.h" +#include +#include + +QPixmapDumper::QPixmapDumper(std::string p, int c){ + format="PNG"; + prefix=p; + reset(); + cycles=c; +} + +void QPixmapDumper::reset(){ + cycles=0; + frame=0; + counter=0; +} + +#define filename_bufsize 1024 + +bool QPixmapDumper::dump(const QPixmap& pixmap){ + bool processed=false; + if (!(counter%cycles)){ + char buf[filename_bufsize]; + sprintf(buf,"%s-%05d.%s",prefix.c_str(), frame, format.c_str()); + QImage image=pixmap.convertToImage(); + image.save(QString(buf), format.c_str(),0); + frame ++; + } + counter++; + return processed; +} + + diff --git a/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h b/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h new file mode 100644 index 0000000..7193d12 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qpixmapdumper.h @@ -0,0 +1,19 @@ +#ifndef _QPIXMAPDUMPER_H_ +#define _QPIXMAPDUMPER_H_ +#include +#include +#include + + +struct QPixmapDumper{ + QPixmapDumper(std::string prefix, int cycles); + void reset(); + std::string prefix; + std::string format; + bool dump(const QPixmap& pixmap); + int counter; + int cycles; + int frame; +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp b/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp new file mode 100644 index 0000000..9c81264 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.cpp @@ -0,0 +1,128 @@ +#include "qslamandnavwidget.h" +#include +using namespace GMapping; + + +QSLAMandNavWidget::QSLAMandNavWidget( QWidget * parent, const char * name, WFlags f) +: QMapPainter(parent, name, f), dumper("slamandnav", 1){ + robotPose=IntPoint(0,0); + robotHeading=0; + slamRestart=false; + slamFinished=false; + enableMotion=false; + startWalker=false; + trajectorySent=false; + goHome=false; + wantsQuit=false; + printHelp=false; + saveGoalPoints=false; + writeImages=false; + drawRobot=true; +} + +QSLAMandNavWidget::~QSLAMandNavWidget(){} + + +void QSLAMandNavWidget::mousePressEvent ( QMouseEvent * e ){ + QPoint p=e->pos(); + int mx=p.x(); + int my=height()-p.y(); + if ( e->state()&Qt::ShiftButton && e->button()==Qt::LeftButton) { + if (trajectorySent) + trajectoryPoints.clear(); + e->accept(); + IntPoint p=IntPoint(mx, my); + trajectoryPoints.push_back(p); + trajectorySent=false; + } +} + +void QSLAMandNavWidget::keyPressEvent ( QKeyEvent * e ){ + if (e->key()==Qt::Key_Delete){ + e->accept(); + if (!trajectoryPoints.empty()) + trajectoryPoints.pop_back(); + } + if (e->key()==Qt::Key_S){ + e->accept(); + enableMotion=!enableMotion; + } + if (e->key()==Qt::Key_W){ + e->accept(); + startWalker=!startWalker; + } + if (e->key()==Qt::Key_G){ + e->accept(); + slamRestart=true; + } + if (e->key()==Qt::Key_T){ + e->accept(); + trajectorySent=true; + } + if (e->key()==Qt::Key_R){ + e->accept(); + goHome=true; + } + if (e->key()==Qt::Key_C){ + e->accept(); + slamFinished=true; + + } + if (e->key()==Qt::Key_Q){ + e->accept(); + wantsQuit=true; + + } + if (e->key()==Qt::Key_H){ + e->accept(); + printHelp=true; + //BABSI + //insert the help here + } + if (e->key()==Qt::Key_Y){ + e->accept(); + saveGoalPoints=true; + //BABSI + //insert the help here + } + if (e->key()==Qt::Key_D){ + e->accept(); + drawRobot=!drawRobot; + //BABSI + //insert the help here + } +} + +void QSLAMandNavWidget::paintEvent ( QPaintEvent * ){ + QPixmap pixmap(*m_pixmap); + QPainter painter(&pixmap); + if (trajectorySent) + painter.setPen(Qt::red); + bool first=true; + int oldx=0, oldy=0; + //paint the path + for (std::list::const_iterator it=trajectoryPoints.begin(); it!=trajectoryPoints.end(); it++){ + int x=it->x; + int y=height()-it->y; + if (! first) + painter.drawLine(oldx, oldy, x,y); + oldx=x; + oldy=y; + first=false; + } + + //paint the robot + if (drawRobot){ + painter.setPen(Qt::black); + int rx=robotPose.x; + int ry=height()-robotPose.y; + int robotSize=6; + painter.drawLine(rx, ry, + rx+(int)(robotSize*cos(robotHeading)), ry-(int)(robotSize*sin(robotHeading))); + painter.drawEllipse(rx-robotSize, ry-robotSize, 2*robotSize, 2*robotSize); + } + if (writeImages){ + dumper.dump(pixmap); + } + bitBlt(this,0,0,&pixmap,0,0,pixmap.width(),pixmap.height(),CopyROP); +} diff --git a/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h b/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h new file mode 100644 index 0000000..e33ca3f --- /dev/null +++ b/slam_gmapping/openslam_gmapping/gui/qslamandnavwidget.h @@ -0,0 +1,38 @@ +#ifndef _QSLAMANDNAV_WIDGET_H +#define _QSLAMANDNAV_WIDGET_H + +#include "qmappainter.h" +#include "qpixmapdumper.h" +#include +#include + +class QSLAMandNavWidget : public QMapPainter{ + public: + QSLAMandNavWidget( QWidget * parent = 0, const char * name = 0, WFlags f = 0); + virtual ~QSLAMandNavWidget(); + std::list trajectoryPoints; + GMapping::IntPoint robotPose; + double robotHeading; + + bool slamRestart; + bool slamFinished; + bool enableMotion; + bool startWalker; + bool trajectorySent; + bool goHome; + bool wantsQuit; + bool printHelp; + bool saveGoalPoints; + bool writeImages; + bool drawRobot; + QPixmapDumper dumper; + + + protected: + virtual void paintEvent ( QPaintEvent *paintevent ); + virtual void mousePressEvent ( QMouseEvent * e ); + virtual void keyPressEvent ( QKeyEvent * e ); +}; + +#endif + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h b/slam_gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h new file mode 100644 index 0000000..43bd86a --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/grid/accessstate.h @@ -0,0 +1,9 @@ +#ifndef ACCESSTATE_H +#define ACCESSTATE_H + +namespace GMapping { +enum AccessibilityState{Outside=0x0, Inside=0x1, Allocated=0x2}; +}; + +#endif + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/grid/array2d.h b/slam_gmapping/openslam_gmapping/include/gmapping/grid/array2d.h new file mode 100644 index 0000000..67c9388 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/grid/array2d.h @@ -0,0 +1,188 @@ +#ifndef ARRAY2D_H +#define ARRAY2D_H + +#include +#include +#include "accessstate.h" + +#include + +#ifndef __PRETTY_FUNCTION__ +#define __FUNCDNAME__ +#endif + +namespace GMapping { + +template class Array2D{ + public: + Array2D(int xsize=0, int ysize=0); + Array2D& operator=(const Array2D &); + Array2D(const Array2D &); + ~Array2D(); + void clear(); + void resize(int xmin, int ymin, int xmax, int ymax); + + + inline bool isInside(int x, int y) const; + inline const Cell& cell(int x, int y) const; + inline Cell& cell(int x, int y); + inline AccessibilityState cellState(int x, int y) const { return (AccessibilityState) (isInside(x,y)?(Inside|Allocated):Outside);} + + inline bool isInside(const IntPoint& p) const { return isInside(p.x, p.y);} + inline const Cell& cell(const IntPoint& p) const {return cell(p.x,p.y);} + inline Cell& cell(const IntPoint& p) {return cell(p.x,p.y);} + inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x, p.y);} + + inline int getPatchSize() const{return 0;} + inline int getPatchMagnitude() const{return 0;} + inline int getXSize() const {return m_xsize;} + inline int getYSize() const {return m_ysize;} + inline Cell** cells() {return m_cells;} + Cell ** m_cells; + protected: + int m_xsize, m_ysize; +}; + + +template +Array2D::Array2D(int xsize, int ysize){ +// assert(xsize>0); +// assert(ysize>0); + m_xsize=xsize; + m_ysize=ysize; + if (m_xsize>0 && m_ysize>0){ + m_cells=new Cell*[m_xsize]; + for (int i=0; i +class HierarchicalArray2D: public Array2D > >{ + public: + typedef std::set< point, pointcomparator > PointSet; + HierarchicalArray2D(int xsize, int ysize, int patchMagnitude=5); + HierarchicalArray2D(const HierarchicalArray2D& hg); + HierarchicalArray2D& operator=(const HierarchicalArray2D& hg); + virtual ~HierarchicalArray2D(){} + void resize(int ixmin, int iymin, int ixmax, int iymax); + inline int getPatchSize() const {return m_patchMagnitude;} + inline int getPatchMagnitude() const {return m_patchMagnitude;} + + inline const Cell& cell(int x, int y) const; + inline Cell& cell(int x, int y); + inline bool isAllocated(int x, int y) const; + inline AccessibilityState cellState(int x, int y) const ; + inline IntPoint patchIndexes(int x, int y) const; + + inline const Cell& cell(const IntPoint& p) const { return cell(p.x,p.y); } + inline Cell& cell(const IntPoint& p) { return cell(p.x,p.y); } + inline bool isAllocated(const IntPoint& p) const { return isAllocated(p.x,p.y);} + inline AccessibilityState cellState(const IntPoint& p) const { return cellState(p.x,p.y); } + inline IntPoint patchIndexes(const IntPoint& p) const { return patchIndexes(p.x,p.y);} + + inline void setActiveArea(const PointSet&, bool patchCoords=false); + const PointSet& getActiveArea() const {return m_activeArea; } + inline void allocActiveArea(); + protected: + virtual Array2D * createPatch(const IntPoint& p) const; + PointSet m_activeArea; + int m_patchMagnitude; + int m_patchSize; +}; + +template +HierarchicalArray2D::HierarchicalArray2D(int xsize, int ysize, int patchMagnitude) + :Array2D > >::Array2D((xsize>>patchMagnitude), (ysize>>patchMagnitude)){ + m_patchMagnitude=patchMagnitude; + m_patchSize=1< +HierarchicalArray2D::HierarchicalArray2D(const HierarchicalArray2D& hg) + :Array2D > >::Array2D((hg.m_xsize>>hg.m_patchMagnitude), (hg.m_ysize>>hg.m_patchMagnitude)) // added by cyrill: if you have a resize error, check this again +{ + this->m_xsize=hg.m_xsize; + this->m_ysize=hg.m_ysize; + this->m_cells=new autoptr< Array2D >*[this->m_xsize]; + for (int x=0; xm_xsize; x++){ + this->m_cells[x]=new autoptr< Array2D >[this->m_ysize]; + for (int y=0; ym_ysize; y++) + this->m_cells[x][y]=hg.m_cells[x][y]; + } + this->m_patchMagnitude=hg.m_patchMagnitude; + this->m_patchSize=hg.m_patchSize; +} + +template +void HierarchicalArray2D::resize(int xmin, int ymin, int xmax, int ymax){ + int xsize=xmax-xmin; + int ysize=ymax-ymin; + autoptr< Array2D > ** newcells=new autoptr< Array2D > *[xsize]; + for (int x=0; x >[ysize]; + for (int y=0; y >(0); + } + } + int dx= xmin < 0 ? 0 : xmin; + int dy= ymin < 0 ? 0 : ymin; + int Dx=xmaxm_xsize?xmax:this->m_xsize; + int Dy=ymaxm_ysize?ymax:this->m_ysize; + for (int x=dx; xm_cells[x][y]; + } + delete [] this->m_cells[x]; + } + delete [] this->m_cells; + this->m_cells=newcells; + this->m_xsize=xsize; + this->m_ysize=ysize; +} + +template +HierarchicalArray2D& HierarchicalArray2D::operator=(const HierarchicalArray2D& hg){ +// Array2D > >::operator=(hg); + if (this->m_xsize!=hg.m_xsize || this->m_ysize!=hg.m_ysize){ + for (int i=0; im_xsize; i++) + delete [] this->m_cells[i]; + delete [] this->m_cells; + this->m_xsize=hg.m_xsize; + this->m_ysize=hg.m_ysize; + this->m_cells=new autoptr< Array2D >*[this->m_xsize]; + for (int i=0; im_xsize; i++) + this->m_cells[i]=new autoptr< Array2D > [this->m_ysize]; + } + for (int x=0; xm_xsize; x++) + for (int y=0; ym_ysize; y++) + this->m_cells[x][y]=hg.m_cells[x][y]; + + m_activeArea.clear(); + m_patchMagnitude=hg.m_patchMagnitude; + m_patchSize=hg.m_patchSize; + return *this; +} + + +template +void HierarchicalArray2D::setActiveArea(const typename HierarchicalArray2D::PointSet& aa, bool patchCoords){ + m_activeArea.clear(); + for (PointSet::const_iterator it= aa.begin(); it!=aa.end(); ++it) { + IntPoint p; + if (patchCoords) + p=*it; + else + p=patchIndexes(*it); + m_activeArea.insert(p); + } +} + +template +Array2D* HierarchicalArray2D::createPatch(const IntPoint& ) const{ + return new Array2D(1< +AccessibilityState HierarchicalArray2D::cellState(int x, int y) const { + if (this->isInside(patchIndexes(x,y))) { + if(isAllocated(x,y)) + return (AccessibilityState)((int)Inside|(int)Allocated); + else + return Inside; + } + return Outside; +} + +template +void HierarchicalArray2D::allocActiveArea(){ + for (PointSet::const_iterator it= m_activeArea.begin(); it!=m_activeArea.end(); ++it){ + const autoptr< Array2D >& ptr=this->m_cells[it->x][it->y]; + Array2D* patch=0; + if (!ptr){ + patch=createPatch(*it); + } else{ + patch=new Array2D(*ptr); + } + this->m_cells[it->x][it->y]=autoptr< Array2D >(patch); + } +} + +template +bool HierarchicalArray2D::isAllocated(int x, int y) const{ + IntPoint c=patchIndexes(x,y); + autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; + return (ptr != 0); +} + +template +IntPoint HierarchicalArray2D::patchIndexes(int x, int y) const{ + if (x>=0 && y>=0) + return IntPoint(x>>m_patchMagnitude, y>>m_patchMagnitude); + return IntPoint(-1, -1); +} + +template +Cell& HierarchicalArray2D::cell(int x, int y){ + IntPoint c=patchIndexes(x,y); + assert(this->isInside(c.x, c.y)); + if (!this->m_cells[c.x][c.y]){ + Array2D* patch=createPatch(IntPoint(x,y)); + this->m_cells[c.x][c.y]=autoptr< Array2D >(patch); + //cerr << "!!! FATAL: your dick is going to fall down" << endl; + } + autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; + return (*ptr).cell(IntPoint(x-(c.x< +const Cell& HierarchicalArray2D::cell(int x, int y) const{ + assert(isAllocated(x,y)); + IntPoint c=patchIndexes(x,y); + const autoptr< Array2D >& ptr=this->m_cells[c.x][c.y]; + return (*ptr).cell(IntPoint(x-(c.x< +#include +#include "accessstate.h" +#include "array2d.h" + +namespace GMapping { +/** +The cells have to define the special value Cell::Unknown to handle with the unallocated areas. +The cells have to define (int) constructor; +*/ +typedef Array2D DoubleArray2D; + +template +class Map{ + public: + Map(int mapSizeX, int mapSizeY, double delta); + Map(const Point& center, double worldSizeX, double worldSizeY, double delta); + Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta); + /* the standard implementation works filen in this case*/ + //Map(const Map& g); + //Map& operator =(const Map& g); + void resize(double xmin, double ymin, double xmax, double ymax); + void grow(double xmin, double ymin, double xmax, double ymax); + inline IntPoint world2map(const Point& p) const; + inline Point map2world(const IntPoint& p) const; + inline IntPoint world2map(double x, double y) const + { return world2map(Point(x,y)); } + inline Point map2world(int x, int y) const + { return map2world(IntPoint(x,y)); } + + inline Point getCenter() const {return m_center;} + inline double getWorldSizeX() const {return m_worldSizeX;} + inline double getWorldSizeY() const {return m_worldSizeY;} + inline int getMapSizeX() const {return m_mapSizeX;} + inline int getMapSizeY() const {return m_mapSizeY;} + inline double getDelta() const { return m_delta;} + inline double getMapResolution() const { return m_delta;} + inline double getResolution() const { return m_delta;} + inline void getSize(double & xmin, double& ymin, double& xmax, double& ymax) const { + Point min=map2world(0,0), max=map2world(IntPoint(m_mapSizeX-1, m_mapSizeY-1)); + xmin=min.x, ymin=min.y, xmax=max.x, ymax=max.y; + } + + inline Cell& cell(int x, int y) { + return cell(IntPoint(x, y)); + } + inline Cell& cell(const IntPoint& p); + + inline const Cell& cell(int x, int y) const { + return cell(IntPoint(x, y)); + } + inline const Cell& cell(const IntPoint& p) const; + + inline Cell& cell(double x, double y) { + return cell(Point(x, y)); + } + inline Cell& cell(const Point& p); + + inline const Cell& cell(double x, double y) const { + return cell(Point(x, y)); + } + + inline bool isInside(int x, int y) const { + return m_storage.cellState(IntPoint(x,y))&Inside; + } + inline bool isInside(const IntPoint& p) const { + return m_storage.cellState(p)&Inside; + } + + inline bool isInside(double x, double y) const { + return m_storage.cellState(world2map(x,y))&Inside; + } + inline bool isInside(const Point& p) const { + return m_storage.cellState(world2map(p))&Inside; + } + + inline const Cell& cell(const Point& p) const; + + inline Storage& storage() { return m_storage; } + inline const Storage& storage() const { return m_storage; } + DoubleArray2D* toDoubleArray() const; + Map* toDoubleMap() const; + + protected: + Point m_center; + double m_worldSizeX, m_worldSizeY, m_delta; + Storage m_storage; + int m_mapSizeX, m_mapSizeY; + int m_sizeX2, m_sizeY2; + static const Cell m_unknown; +}; + +typedef Map DoubleMap; + +template + const Cell Map::m_unknown = Cell(-1); + +template +Map::Map(int mapSizeX, int mapSizeY, double delta): + m_storage(mapSizeX, mapSizeY){ + m_worldSizeX=mapSizeX * delta; + m_worldSizeY=mapSizeY * delta; + m_delta=delta; + m_center=Point(0.5*m_worldSizeX, 0.5*m_worldSizeY); + m_sizeX2=m_mapSizeX>>1; + m_sizeY2=m_mapSizeY>>1; +} + +template +Map::Map(const Point& center, double worldSizeX, double worldSizeY, double delta): + m_storage((int)ceil(worldSizeX/delta), (int)ceil(worldSizeY/delta)){ + m_center=center; + m_worldSizeX=worldSizeX; + m_worldSizeY=worldSizeY; + m_delta=delta; + m_mapSizeX=m_storage.getXSize()<>1; + m_sizeY2=m_mapSizeY>>1; +} + +template +Map::Map(const Point& center, double xmin, double ymin, double xmax, double ymax, double delta): + m_storage((int)ceil((xmax-xmin)/delta), (int)ceil((ymax-ymin)/delta)){ + m_center=center; + m_worldSizeX=xmax-xmin; + m_worldSizeY=ymax-ymin; + m_delta=delta; + m_mapSizeX=m_storage.getXSize()< +void Map::resize(double xmin, double ymin, double xmax, double ymax){ + IntPoint imin=world2map(xmin, ymin); + IntPoint imax=world2map(xmax, ymax); + int pxmin, pymin, pxmax, pymax; + pxmin=(int)floor((float)imin.x/(1< +void Map::grow(double xmin, double ymin, double xmax, double ymax){ + IntPoint imin=world2map(xmin, ymin); + IntPoint imax=world2map(xmax, ymax); + if (isInside(imin) && isInside(imax)) + return; + imin=min(imin, IntPoint(0,0)); + imax=max(imax, IntPoint(m_mapSizeX-1,m_mapSizeY-1)); + int pxmin, pymin, pxmax, pymax; + pxmin=(int)floor((float)imin.x/(1< +IntPoint Map::world2map(const Point& p) const{ + return IntPoint( (int)round((p.x-m_center.x)/m_delta)+m_sizeX2, (int)round((p.y-m_center.y)/m_delta)+m_sizeY2); +} + +template +Point Map::map2world(const IntPoint& p) const{ + return Point( (p.x-m_sizeX2)*m_delta, + (p.y-m_sizeY2)*m_delta)+m_center; +} + + +template +Cell& Map::cell(const IntPoint& p) { + AccessibilityState s=m_storage.cellState(p); + if (! (s&Inside)) + assert(0); + //if (s&Allocated) return m_storage.cell(p); assert(0); + + // this will never happend. Just to satify the compiler.. + return m_storage.cell(p); + +} + +template +Cell& Map::cell(const Point& p) { + IntPoint ip=world2map(p); + AccessibilityState s=m_storage.cellState(ip); + if (! (s&Inside)) + assert(0); + //if (s&Allocated) return m_storage.cell(ip); assert(0); + + // this will never happend. Just to satify the compiler.. + return m_storage.cell(ip); +} + +template + const Cell& Map::cell(const IntPoint& p) const { + AccessibilityState s=m_storage.cellState(p); + //if (! s&Inside) assert(0); + if (s&Allocated) + return m_storage.cell(p); + return m_unknown; +} + +template +const Cell& Map::cell(const Point& p) const { + IntPoint ip=world2map(p); + AccessibilityState s=m_storage.cellState(ip); + //if (! s&Inside) assert(0); + if (s&Allocated) + return m_storage.cell(ip); + return m_unknown; +} + + +//FIXME check why the last line of the map is corrupted. +template +DoubleArray2D* Map::toDoubleArray() const{ + DoubleArray2D* darr=new DoubleArray2D(getMapSizeX()-1, getMapSizeY()-1); + for(int x=0; xcell(p)=cell(p); + } + return darr; +} + + +template +Map* Map::toDoubleMap() const{ +//FIXME size the map so that m_center will be setted accordingly + Point pmin=map2world(IntPoint(0,0)); + Point pmax=map2world(getMapSizeX()-1,getMapSizeY()-1); + Point center=(pmax+pmin)*0.5; + Map* plainMap=new Map(center, (pmax-pmin).x, (pmax-pmin).y, getDelta()); + for(int x=0; xcell(p)=cell(p); + } + return plainMap; +} + +}; + +#endif + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h b/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h new file mode 100644 index 0000000..9a859fb --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.h @@ -0,0 +1,337 @@ +#ifndef GRIDSLAMPROCESSOR_H +#define GRIDSLAMPROCESSOR_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "motionmodel.h" + + +namespace GMapping { + + /**This class defines the basic GridFastSLAM algorithm. It + implements a rao blackwellized particle filter. Each particle + has its own map and robot pose.
This implementation works + as follows: each time a new pair odometry/laser reading is + received, the particle's robot pose is updated according to the + motion model. This pose is subsequently used for initalizing a + scan matching algorithm. The scanmatcher performs a local + optimization for each particle. It is initialized with the + pose drawn from the motion model, and the pose is corrected + according to the each particle map.
+ In order to avoid unnecessary computation the filter state is updated + only when the robot moves more than a given threshold. + */ + class GridSlamProcessor{ + public: + + + /**This class defines the the node of reversed tree in which the trajectories are stored. + Each node of a tree has a pointer to its parent and a counter indicating the number of childs of a node. + The tree is updated in a way consistent with the operation performed on the particles. + */ + struct TNode{ + /**Constructs a node of the trajectory tree. + @param pose: the pose of the robot in the trajectory + @param weight: the weight of the particle at that point in the trajectory + @param accWeight: the cumulative weight of the particle + @param parent: the parent node in the tree + @param childs: the number of childs + */ + TNode(const OrientedPoint& pose, double weight, TNode* parent=0, unsigned int childs=0); + + /**Destroys a tree node, and consistently updates the tree. If a node whose parent has only one child is deleted, + also the parent node is deleted. This because the parent will not be reacheable anymore in the trajectory tree.*/ + ~TNode(); + + /**The pose of the robot*/ + OrientedPoint pose; + + /**The weight of the particle*/ + double weight; + + /**The sum of all the particle weights in the previous part of the trajectory*/ + double accWeight; + + double gweight; + + + /**The parent*/ + TNode* parent; + + /**The range reading to which this node is associated*/ + const RangeReading* reading; + + /**The number of childs*/ + unsigned int childs; + + /**counter in visiting the node (internally used)*/ + mutable unsigned int visitCounter; + + /**visit flag (internally used)*/ + mutable bool flag; + }; + + typedef std::vector TNodeVector; + typedef std::deque TNodeDeque; + + /**This class defines a particle of the filter. Each particle has a map, a pose, a weight and retains the current node in the trajectory tree*/ + struct Particle{ + /**constructs a particle, given a map + @param map: the particle map + */ + Particle(const ScanMatcherMap& map); + + /** @returns the weight of a particle */ + inline operator double() const {return weight;} + /** @returns the pose of a particle */ + inline operator OrientedPoint() const {return pose;} + /** sets the weight of a particle + @param w the weight + */ + inline void setWeight(double w) {weight=w;} + /** The map */ + ScanMatcherMap map; + /** The pose of the robot */ + OrientedPoint pose; + + /** The pose of the robot at the previous time frame (used for computing thr odometry displacements) */ + OrientedPoint previousPose; + + /** The weight of the particle */ + double weight; + + /** The cumulative weight of the particle */ + double weightSum; + + double gweight; + + /** The index of the previous particle in the trajectory tree */ + int previousIndex; + + /** Entry to the trajectory tree */ + TNode* node; + }; + + + typedef std::vector ParticleVector; + + /** Constructs a GridSlamProcessor, initialized with the default parameters */ + GridSlamProcessor(); + + /** Constructs a GridSlamProcessor, whose output is routed to a stream. + @param infoStr: the output stream + */ + GridSlamProcessor(std::ostream& infoStr); + + /** @returns a deep copy of the grid slam processor with all the internal structures. + */ + GridSlamProcessor* clone() const; + + /**Deleted the gridslamprocessor*/ + virtual ~GridSlamProcessor(); + + //methods for accessing the parameters + void setSensorMap(const SensorMap& smap); + void init(unsigned int size, double xmin, double ymin, double xmax, double ymax, double delta, + OrientedPoint initialPose=OrientedPoint(0,0,0)); + void setMatchingParameters(double urange, double range, double sigma, int kernsize, double lopt, double aopt, + int iterations, double likelihoodSigma=1, double likelihoodGain=1, unsigned int likelihoodSkip=0); + void setMotionModelParameters(double srr, double srt, double str, double stt); + void setUpdateDistances(double linear, double angular, double resampleThreshold); + void setUpdatePeriod(double p) {period_=p;} + + //the "core" algorithm + void processTruePos(const OdometryReading& odometry); + bool processScan(const RangeReading & reading, int adaptParticles=0); + + /**This method copies the state of the filter in a tree. + The tree is represented through reversed pointers (each node has a pointer to its parent). + The leafs are stored in a vector, whose size is the same as the number of particles. + @returns the leafs of the tree + */ + TNodeVector getTrajectories() const; + void integrateScanSequence(TNode* node); + + /**the scanmatcher algorithm*/ + ScanMatcher m_matcher; + /**the stream used for writing the output of the algorithm*/ + std::ofstream& outputStream(); + /**the stream used for writing the info/debug messages*/ + std::ostream& infoStream(); + /**@returns the particles*/ + inline const ParticleVector& getParticles() const {return m_particles; } + + inline const std::vector& getIndexes() const{return m_indexes; } + int getBestParticleIndex() const; + //callbacks + virtual void onOdometryUpdate(); + virtual void onResampleUpdate(); + virtual void onScanmatchUpdate(); + + //accessor methods + /**the maxrange of the laser to consider */ + MEMBER_PARAM_SET_GET(m_matcher, double, laserMaxRange, protected, public, public); + + /**the maximum usable range of the laser. A beam is cropped to this value. [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, usableRange, protected, public, public); + + /**The sigma used by the greedy endpoint matching. [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher,double, gaussianSigma, protected, public, public); + + /**The sigma of a beam used for likelihood computation [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher,double, likelihoodSigma, protected, public, public); + + /**The kernel in which to look for a correspondence[scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, int, kernelSize, protected, public, public); + + /**The optimization step in rotation [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, optAngularDelta, protected, public, public); + + /**The optimization step in translation [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, optLinearDelta, protected, public, public); + + /**The number of iterations of the scanmatcher [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, unsigned int, optRecursiveIterations, protected, public, public); + + /**the beams to skip for computing the likelihood (consider a beam every likelihoodSkip) [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, unsigned int, likelihoodSkip, protected, public, public); + + /**translational sampling range for the likelihood [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, llsamplerange, protected, public, public); + + /**angular sampling range for the likelihood [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, lasamplerange, protected, public, public); + + /**translational sampling range for the likelihood [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, llsamplestep, protected, public, public); + + /**angular sampling step for the likelihood [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, double, lasamplestep, protected, public, public); + + /**generate an accupancy grid map [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, bool, generateMap, protected, public, public); + + /**enlarge the map when the robot goes out of the boundaries [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, bool, enlargeStep, protected, public, public); + + /**pose of the laser wrt the robot [scanmatcher]*/ + MEMBER_PARAM_SET_GET(m_matcher, OrientedPoint, laserPose, protected, public, public); + + + /**odometry error in translation as a function of translation (rho/rho) [motionmodel]*/ + STRUCT_PARAM_SET_GET(m_motionModel, double, srr, protected, public, public); + + /**odometry error in translation as a function of rotation (rho/theta) [motionmodel]*/ + STRUCT_PARAM_SET_GET(m_motionModel, double, srt, protected, public, public); + + /**odometry error in rotation as a function of translation (theta/rho) [motionmodel]*/ + STRUCT_PARAM_SET_GET(m_motionModel, double, str, protected, public, public); + + /**odometry error in rotation as a function of rotation (theta/theta) [motionmodel]*/ + STRUCT_PARAM_SET_GET(m_motionModel, double, stt, protected, public, public); + + /**minimum score for considering the outcome of the scanmatching good*/ + PARAM_SET_GET(double, minimumScore, protected, public, public); + + protected: + /**Copy constructor*/ + GridSlamProcessor(const GridSlamProcessor& gsp); + + /**the laser beams*/ + unsigned int m_beams; + double last_update_time_; + double period_; + + /**the particles*/ + ParticleVector m_particles; + + /**the particle indexes after resampling (internally used)*/ + std::vector m_indexes; + + /**the particle weights (internally used)*/ + std::vector m_weights; + + /**the motion model*/ + MotionModel m_motionModel; + + /**this sets the neff based resampling threshold*/ + PARAM_SET_GET(double, resampleThreshold, protected, public, public); + + //state + int m_count, m_readingCount; + OrientedPoint m_lastPartPose; + OrientedPoint m_odoPose; + OrientedPoint m_pose; + double m_linearDistance, m_angularDistance; + PARAM_GET(double, neff, protected, public); + + //processing parameters (size of the map) + PARAM_GET(double, xmin, protected, public); + PARAM_GET(double, ymin, protected, public); + PARAM_GET(double, xmax, protected, public); + PARAM_GET(double, ymax, protected, public); + //processing parameters (resolution of the map) + PARAM_GET(double, delta, protected, public); + + //registration score (if a scan score is above this threshold it is registered in the map) + PARAM_SET_GET(double, regScore, protected, public, public); + //registration score (if a scan score is below this threshold a scan matching failure is reported) + PARAM_SET_GET(double, critScore, protected, public, public); + //registration score maximum move allowed between consecutive scans + PARAM_SET_GET(double, maxMove, protected, public, public); + + //process a scan each time the robot translates of linearThresholdDistance + PARAM_SET_GET(double, linearThresholdDistance, protected, public, public); + + //process a scan each time the robot rotates more than angularThresholdDistance + PARAM_SET_GET(double, angularThresholdDistance, protected, public, public); + + //smoothing factor for the likelihood + PARAM_SET_GET(double, obsSigmaGain, protected, public, public); + + //stream in which to write the gfs file + std::ofstream m_outputStream; + + // stream in which to write the messages + std::ostream& m_infoStream; + + + // the functions below performs side effect on the internal structure, + //should be called only inside the processScan method + private: + + /**scanmatches all the particles*/ + inline void scanMatch(const double *plainReading); + /**normalizes the particle weights*/ + inline void normalize(); + + // return if a resampling occured or not + inline bool resample(const double* plainReading, int adaptParticles, + const RangeReading* rr=0); + + //tree utilities + + void updateTreeWeights(bool weightsAlreadyNormalized = false); + void resetTree(); + double propagateWeights(); + + }; + +typedef std::multimap TNodeMultimap; + + +#include "gridslamprocessor.hxx" + +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx b/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx new file mode 100644 index 0000000..8a8b7a4 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/gridfastslam/gridslamprocessor.hxx @@ -0,0 +1,177 @@ + +#ifdef MACOSX +// This is to overcome a possible bug in Apple's GCC. +#define isnan(x) (x==FP_NAN) +#endif + +/**Just scan match every single particle. +If the scan matching fails, the particle gets a default likelihood.*/ +inline void GridSlamProcessor::scanMatch(const double* plainReading){ + // sample a new pose from each scan in the reference + + double sumScore=0; + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + OrientedPoint corrected; + double score, l, s; + score=m_matcher.optimize(corrected, it->map, it->pose, plainReading); + // it->pose=corrected; + if (score>m_minimumScore){ + it->pose=corrected; + } else { + if (m_infoStream){ + m_infoStream << "Scan Matching Failed, using odometry. Likelihood=" << l <map, it->pose, plainReading); + sumScore+=score; + it->weight+=l; + it->weightSum+=l; + + //set up the selective copy of the active area + //by detaching the areas that will be updated + m_matcher.invalidateActiveArea(); + m_matcher.computeActiveArea(it->map, it->pose, plainReading); + } + if (m_infoStream) + m_infoStream << "Average Scan Matching Score=" << sumScore/m_particles.size() << std::endl; +} + +inline void GridSlamProcessor::normalize(){ + //normalize the log m_weights + double gain=1./(m_obsSigmaGain*m_particles.size()); + double lmax= -std::numeric_limits::max(); + for (ParticleVector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + lmax=it->weight>lmax?it->weight:lmax; + } + //cout << "!!!!!!!!!!! maxwaight= "<< lmax << endl; + + m_weights.clear(); + double wcum=0; + m_neff=0; + for (std::vector::iterator it=m_particles.begin(); it!=m_particles.end(); it++){ + m_weights.push_back(exp(gain*(it->weight-lmax))); + wcum+=m_weights.back(); + //cout << "l=" << it->weight<< endl; + } + + m_neff=0; + for (std::vector::iterator it=m_weights.begin(); it!=m_weights.end(); it++){ + *it=*it/wcum; + double w=*it; + m_neff+=w*w; + } + m_neff=1./m_neff; + +} + +inline bool GridSlamProcessor::resample(const double* plainReading, int adaptSize, const RangeReading* reading){ + + bool hasResampled = false; + + TNodeVector oldGeneration; + for (unsigned int i=0; i resampler; + m_indexes=resampler.resampleIndexes(m_weights, adaptSize); + + if (m_outputStream.is_open()){ + m_outputStream << "RESAMPLE "<< m_indexes.size() << " "; + for (std::vector::const_iterator it=m_indexes.begin(); it!=m_indexes.end(); it++){ + m_outputStream << *it << " "; + } + m_outputStream << std::endl; + } + + onResampleUpdate(); + //BEGIN: BUILDING TREE + ParticleVector temp; + unsigned int j=0; + std::vector deletedParticles; //this is for deleteing the particles which have been resampled away. + + // cerr << "Existing Nodes:" ; + for (unsigned int i=0; i" << m_indexes[i] << "B("<childs <<") "; + node=new TNode(p.pose, 0, oldNode, 0); + //node->reading=0; + node->reading=reading; + // cerr << "A("<parent->childs <<") " <setWeight(0); + m_matcher.invalidateActiveArea(); + m_matcher.registerScan(it->map, it->pose, plainReading); + m_particles.push_back(*it); + } + std::cerr << " Done" <pose, 0.0, *node_it, 0); + + //node->reading=0; + node->reading=reading; + it->node=node; + + //END: BUILDING TREE + m_matcher.invalidateActiveArea(); + m_matcher.registerScan(it->map, it->pose, plainReading); + it->previousIndex=index; + index++; + node_it++; + + } + std::cerr << "Done" < +#include +#include + +namespace GMapping { + +struct MotionModel{ + OrientedPoint drawFromMotion(const OrientedPoint& p, double linearMove, double angularMove) const; + OrientedPoint drawFromMotion(const OrientedPoint& p, const OrientedPoint& pnew, const OrientedPoint& pold) const; + Covariance3 gaussianApproximation(const OrientedPoint& pnew, const OrientedPoint& pold) const; + double srr, str, srt, stt; +}; + +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/log/configuration.h b/slam_gmapping/openslam_gmapping/include/gmapping/log/configuration.h new file mode 100644 index 0000000..ce1c81c --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/log/configuration.h @@ -0,0 +1,17 @@ +#ifndef CONFIGURATION_H +#define CONFIGURATION_H + +#include +#include + +namespace GMapping { + +class Configuration{ + public: + virtual ~Configuration(); + virtual SensorMap computeSensorMap() const=0; +}; + +}; +#endif + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h b/slam_gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h new file mode 100644 index 0000000..f45ddda --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/log/sensorlog.h @@ -0,0 +1,29 @@ +#ifndef SENSORLOG_H +#define SENSORLOG_H + +#include +#include +#include +#include +#include +#include +#include +#include "configuration.h" + +namespace GMapping { + +class SensorLog : public std::list{ + public: + SensorLog(const SensorMap&); + ~SensorLog(); + std::istream& load(std::istream& is); + OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const; + protected: + const SensorMap& m_sensorMap; + OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const; + RangeReading* parseRange(std::istream& is, const RangeSensor* ) const; +}; + +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/particlefilter/particlefilter.h b/slam_gmapping/openslam_gmapping/include/gmapping/particlefilter/particlefilter.h new file mode 100644 index 0000000..2a8b8a9 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/particlefilter/particlefilter.h @@ -0,0 +1,328 @@ +#ifndef PARTICLEFILTER_H +#define PARTICLEFILTER_H +#include +#include +#include +#include +#include +#include +#include + + +/** +the particle class has to be convertible into numeric data type; +That means that a particle must define the Numeric conversion operator; + operator Numeric() const. +that returns the weight, and the method + setWeight(Numeric) +that sets the weight. + +*/ + +typedef std::pair UIntPair; + +template +double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){ + //determine the maximum + double lmax = -std::numeric_limits::max(); + for (Iterator it=begin; it!=end; it++){ + lmax=lmax>((double)(*it))? lmax: (double)(*it); + } + //convert to raw form + for (Iterator it=begin; it!=end; it++){ + *out=exp((double)(*it)-lmax); + out++; + } + return lmax; +} + +template +void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){ + //determine the maximum + for (Iterator it=begin; it!=end; it++){ + *out=log((Numeric)(*it))-lmax; + out++; + } + return lmax; +} + +template +void resample(std::vector& indexes, const WeightVector& weights, unsigned int nparticles=0){ + double cweight=0; + + //compute the cumulative weights + unsigned int n=0; + for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ + cweight+=(double)*it; + n++; + } + + if (nparticles>0) + n=nparticles; + + //compute the interval + double interval=cweight/n; + + //compute the initial target weight + double target=interval*::drand48(); + //compute the resampled indexes + + cweight=0; + indexes.resize(n); + + n=0; + unsigned int i=0; + for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ + cweight+=(double)* it; + while(cweight>target){ + indexes[n++]=i; + target+=interval; + } + } +} + +template +void repeatIndexes(Vector& dest, const std::vector& indexes, const Vector& particles){ + assert(indexes.size()==particles.size()); + dest.resize(particles.size()); + unsigned int i=0; + for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ + dest[i]=particles[*it]; + i++; + } +} + + +template +double neff(const Iterator& begin, const Iterator& end){ + double sum=0; + for (Iterator it=begin; it!=end; ++it){ + sum+=*it; + } + double cum=0; + for (Iterator it=begin; it!=end; ++it){ + double w=*it/sum; + cum+=w*w; + } + return 1./cum; +} + +template +void normalize(const Iterator& begin, const Iterator& end){ + double sum=0; + for (Iterator it=begin; it!=end; ++it){ + sum+=*it; + } + for (Iterator it=begin; it!=end; ++it){ + *it=*it/sum; + } +} + +template +void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){ + unsigned int current=0; + unsigned int count=0; + for (Iterator it=begin; it!=end; it++){ + if (it==begin){ + current=*it; + count=1; + continue; + } + if (((uint)*it) ==current) + count++; + if (((uint)*it)!=current){ + *out=std::make_pair(current,count); + out++; + current=*it; + count=1; + } + } + if (count>0) + *out=std::make_pair(current,count); + out++; +} + +//BEGIN legacy +template +struct uniform_resampler{ + std::vector resampleIndexes(const std::vector & particles, int nparticles=0) const; + std::vector resample(const std::vector & particles, int nparticles=0) const; + Numeric neff(const std::vector & particles) const; +}; + +/*Implementation of the above stuff*/ +template +std::vector uniform_resampler:: resampleIndexes(const std::vector& particles, int nparticles) const{ + Numeric cweight=0; + + //compute the cumulative weights + unsigned int n=0; + for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ + cweight+=(Numeric)*it; + n++; + } + + if (nparticles>0) + n=nparticles; + + //compute the interval + Numeric interval=cweight/n; + + //compute the initial target weight + Numeric target=interval*::drand48(); + //compute the resampled indexes + + cweight=0; + std::vector indexes(n); + n=0; + unsigned int i=0; + for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ + cweight+=(Numeric)* it; + while(cweight>target){ + indexes[n++]=i; + target+=interval; + } + } + return indexes; +} + +template +std::vector uniform_resampler::resample + (const typename std::vector& particles, int nparticles) const{ + Numeric cweight=0; + + //compute the cumulative weights + unsigned int n=0; + for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ + cweight+=(Numeric)*it; + n++; + } + + if (nparticles>0) + n=nparticles; + + //weight of the particles after resampling + double uw=1./n; + + //compute the interval + Numeric interval=cweight/n; + + //compute the initial target weight + Numeric target=interval*::drand48(); + //compute the resampled indexes + + cweight=0; + std::vector resampled; + n=0; + unsigned int i=0; + for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ + cweight+=(Numeric)*it; + while(cweight>target){ + resampled.push_back(*it); + resampled.back().setWeight(uw); + target+=interval; + } + } + return resampled; +} + +template +Numeric uniform_resampler::neff(const std::vector & particles) const{ + double cum=0; + double sum=0; + for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ + Numeric w=(Numeric)*it; + cum+=w*w; + sum+=w; + } + return sum*sum/cum; +} + + +/* + +The following are patterns for the evolution and the observation classes +The user should implement classes having the specified meaning + +template +struct observer{ + Observation& observation + Numeric observe(const class State&) const; +}; + +template +struct evolver{ + Input& input; + State& evolve(const State& s); +}; +*/ + + +template +struct evolver{ + EvolutionModel evolutionModel; + void evolve(std::vector& particles); + void evolve(std::vector& dest, const std::vector& src); +}; + +template +void evolver::evolve(std::vector& particles){ + for (typename std::vector::iterator it=particles.begin(); it!=particles.end(); ++it){ + *it=evolutionModel.evolve(*it); + } +} + +template +void evolver::evolve(std::vector& dest, const std::vector& src){ + dest.clear(); + for (typename std::vector::const_iterator it=src.begin(); it!=src.end(); ++it) + dest.push_back(evolutionModel.evolve(*it)); +} + + +template +struct auxiliary_evolver{ + EvolutionModel evolutionModel; + QualificationModel qualificationModel; + LikelyhoodModel likelyhoodModel; + void evolve(std::vector& particles); + void evolve(std::vector& dest, const std::vector& src); +}; + +template +void auxiliary_evolver::evolve + (std::vector&particles){ + std::vector observationWeights(particles.size()); + unsigned int i=0; + for (typename std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){ + observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); + } + uniform_resampler resampler; + std::vector indexes(resampler.resampleIndexes(observationWeights)); + for (typename std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ + Particle & particle=particles[*it]; + particle=evolutionModel.evolve(particle); + particle.setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]); + } +} + +template +void auxiliary_evolver::evolve + (std::vector& dest, const std::vector& src){ + dest.clear(); + std::vector observationWeights(src.size()); + unsigned int i=0; + for (typename std::vector::const_iterator it=src.begin(); it!=src.end(); ++it, i++){ + observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); + } + uniform_resampler resampler; + std::vector indexes(resampler.resampleIndexes(observationWeights)); + for (typename std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ + Particle & particle=src[*it]; + dest.push_back(evolutionModel.evolve(particle)); + dest.back().weight*=likelyhoodModel.likelyhood(particle)/observationWeights[*it]; + } +} +//END legacy + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/icp.h b/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/icp.h new file mode 100644 index 0000000..64c257c --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/icp.h @@ -0,0 +1,85 @@ +#ifndef _ICP_H_ +#define _ICP_H_ + +#include +#include +#include +#include + +namespace GMapping{ +typedef std::pair PointPair; + +template +double icpStep(OrientedPoint & retval, const PointPairContainer& container){ + typedef typename PointPairContainer::const_iterator ContainerIterator; + PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.)); + int size=0; + for (ContainerIterator it=container.begin(); it!=container.end(); it++){ + mean.first=mean.first+it->first; + mean.second=mean.second+it->second; + size++; + } + mean.first=mean.first*(1./size); + mean.second=mean.second*(1./size); + double sxx=0, sxy=0, syx=0, syy=0; + + for (ContainerIterator it=container.begin(); it!=container.end(); it++){ + PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second); + sxx+=mf.first.x*mf.second.x; + sxy+=mf.first.x*mf.second.y; + syx+=mf.first.y*mf.second.x; + syy+=mf.first.y*mf.second.y; + } + retval.theta=atan2(sxy-syx, sxx+sxy); + double s=sin(retval.theta), c=cos(retval.theta); + retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y); + retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y); + + double error=0; + for (ContainerIterator it=container.begin(); it!=container.end(); it++){ + Point delta( + 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); + error+=delta*delta; + } + return error; +} + +template +double icpNonlinearStep(OrientedPoint & retval, const PointPairContainer& container){ + typedef typename PointPairContainer::const_iterator ContainerIterator; + PointPair mean=std::make_pair(Point(0.,0.), Point(0.,0.)); + int size=0; + for (ContainerIterator it=container.begin(); it!=container.end(); it++){ + mean.first=mean.first+it->first; + mean.second=mean.second+it->second; + size++; + } + + mean.first=mean.first*(1./size); + mean.second=mean.second*(1./size); + + double ms=0,mc=0; + for (ContainerIterator it=container.begin(); it!=container.end(); it++){ + PointPair mf=std::make_pair(it->first-mean.first, it->second-mean.second); + double dalpha=atan2(mf.second.y, mf.second.x) - atan2(mf.first.y, mf.first.x); + double gain=sqrt(mean.first*mean.first); + ms+=gain*sin(dalpha); + mc+=gain*cos(dalpha); + } + retval.theta=atan2(ms, mc); + double s=sin(retval.theta), c=cos(retval.theta); + retval.x=mean.second.x-(c*mean.first.x-s*mean.first.y); + retval.y=mean.second.y-(s*mean.first.x+c*mean.first.y); + + double error=0; + for (ContainerIterator it=container.begin(); it!=container.end(); it++){ + Point delta( + 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); + error+=delta*delta; + } + return error; +} + +}//end namespace + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h b/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h new file mode 100644 index 0000000..1f0a4d4 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/scanmatcher/scanmatcher.h @@ -0,0 +1,252 @@ +#ifndef SCANMATCHER_H +#define SCANMATCHER_H + +#include "icp.h" +#include "smmap.h" +#include +#include +#include +#include +#define LASER_MAXBEAMS 2048 + +namespace GMapping { + +class ScanMatcher{ + public: + typedef Covariance3 CovarianceMatrix; + + ScanMatcher(); + ~ScanMatcher(); + double icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; + double optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; + double optimize(OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; + + double registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings); + void setLaserParameters + (unsigned int beams, double* angles, const OrientedPoint& lpose); + void setMatchingParameters + (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, double likelihoodSigma=1, unsigned int likelihoodSkip=0 ); + void invalidateActiveArea(); + void computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings); + + inline double icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; + inline double score(const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; + inline unsigned int likelihoodAndScore(double& s, double& l, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const; + double likelihood(double& lmax, OrientedPoint& mean, CovarianceMatrix& cov, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings); + double likelihood(double& _lmax, OrientedPoint& _mean, CovarianceMatrix& _cov, const ScanMatcherMap& map, const OrientedPoint& p, Gaussian3& odometry, const double* readings, double gain=180.); + inline const double* laserAngles() const { return m_laserAngles; } + inline unsigned int laserBeams() const { return m_laserBeams; } + + static const double nullLikelihood; + protected: + //state of the matcher + bool m_activeAreaComputed; + + /**laser parameters*/ + unsigned int m_laserBeams; + double m_laserAngles[LASER_MAXBEAMS]; + //OrientedPoint m_laserPose; + PARAM_SET_GET(OrientedPoint, laserPose, protected, public, public) + PARAM_SET_GET(double, laserMaxRange, protected, public, public) + /**scan_matcher parameters*/ + PARAM_SET_GET(double, usableRange, protected, public, public) + PARAM_SET_GET(double, gaussianSigma, protected, public, public) + PARAM_SET_GET(double, likelihoodSigma, protected, public, public) + PARAM_SET_GET(int, kernelSize, protected, public, public) + PARAM_SET_GET(double, optAngularDelta, protected, public, public) + PARAM_SET_GET(double, optLinearDelta, protected, public, public) + PARAM_SET_GET(unsigned int, optRecursiveIterations, protected, public, public) + PARAM_SET_GET(unsigned int, likelihoodSkip, protected, public, public) + PARAM_SET_GET(double, llsamplerange, protected, public, public) + PARAM_SET_GET(double, llsamplestep, protected, public, public) + PARAM_SET_GET(double, lasamplerange, protected, public, public) + PARAM_SET_GET(double, lasamplestep, protected, public, public) + PARAM_SET_GET(bool, generateMap, protected, public, public) + PARAM_SET_GET(double, enlargeStep, protected, public, public) + PARAM_SET_GET(double, fullnessThreshold, protected, public, public) + PARAM_SET_GET(double, angularOdometryReliability, protected, public, public) + PARAM_SET_GET(double, linearOdometryReliability, protected, public, public) + PARAM_SET_GET(double, freeCellRatio, protected, public, public) + PARAM_SET_GET(unsigned int, initialBeamsSkip, protected, public, public) + + // allocate this large array only once + IntPoint* m_linePoints; +}; + +inline double ScanMatcher::icpStep(OrientedPoint & pret, const ScanMatcherMap& map, const OrientedPoint& p, const double* readings) const{ + const double * angle=m_laserAngles+m_initialBeamsSkip; + OrientedPoint lp=p; + lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; + lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; + lp.theta+=m_laserPose.theta; + unsigned int skip=0; + double freeDelta=map.getDelta()*m_freeCellRatio; + std::list pairs; + + for (const double* r=readings+m_initialBeamsSkip; rm_likelihoodSkip?0:skip; + if (*r>m_usableRange||*r==0.0) continue; + if (skip) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + IntPoint iphit=map.world2map(phit); + Point pfree=lp; + pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle); + pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle); + pfree=pfree-phit; + IntPoint ipfree=map.world2map(pfree); + bool found=false; + Point bestMu(0.,0.); + Point bestCell(0.,0.); + for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++) + for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){ + IntPoint pr=iphit+IntPoint(xx,yy); + IntPoint pf=pr+ipfree; + //AccessibilityState s=map.storage().cellState(pr); + //if (s&Inside && s&Allocated){ + const PointAccumulator& cell=map.cell(pr); + const PointAccumulator& fcell=map.cell(pf); + if (((double)cell )> m_fullnessThreshold && ((double)fcell )m_likelihoodSkip?0:skip; + if (skip||*r>m_usableRange||*r==0.0) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + IntPoint iphit=map.world2map(phit); + Point pfree=lp; + pfree.x+=(*r-map.getDelta()*freeDelta)*cos(lp.theta+*angle); + pfree.y+=(*r-map.getDelta()*freeDelta)*sin(lp.theta+*angle); + pfree=pfree-phit; + IntPoint ipfree=map.world2map(pfree); + bool found=false; + Point bestMu(0.,0.); + for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++) + for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){ + IntPoint pr=iphit+IntPoint(xx,yy); + IntPoint pf=pr+ipfree; + //AccessibilityState s=map.storage().cellState(pr); + //if (s&Inside && s&Allocated){ + const PointAccumulator& cell=map.cell(pr); + const PointAccumulator& fcell=map.cell(pf); + if (((double)cell )> m_fullnessThreshold && ((double)fcell )m_likelihoodSkip?0:skip; + if (*r>m_usableRange) continue; + if (skip) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + IntPoint iphit=map.world2map(phit); + Point pfree=lp; + pfree.x+=(*r-freeDelta)*cos(lp.theta+*angle); + pfree.y+=(*r-freeDelta)*sin(lp.theta+*angle); + pfree=pfree-phit; + IntPoint ipfree=map.world2map(pfree); + bool found=false; + Point bestMu(0.,0.); + for (int xx=-m_kernelSize; xx<=m_kernelSize; xx++) + for (int yy=-m_kernelSize; yy<=m_kernelSize; yy++){ + IntPoint pr=iphit+IntPoint(xx,yy); + IntPoint pf=pr+ipfree; + //AccessibilityState s=map.storage().cellState(pr); + //if (s&Inside && s&Allocated){ + const PointAccumulator& cell=map.cell(pr); + const PointAccumulator& fcell=map.cell(pf); + if (((double)cell )>m_fullnessThreshold && ((double)fcell ) +#include +#include +#define SIGHT_INC 1 + +namespace GMapping { + +struct PointAccumulator{ + typedef point FloatPoint; + /* before + PointAccumulator(int i=-1): acc(0,0), n(0), visits(0){assert(i==-1);} + */ + /*after begin*/ + PointAccumulator(): acc(0,0), n(0), visits(0){} + PointAccumulator(int i): acc(0,0), n(0), visits(0){assert(i==-1);} + /*after end*/ + inline void update(bool value, const Point& p=Point(0,0)); + inline Point mean() const {return 1./n*Point(acc.x, acc.y);} + inline operator double() const { return visits?(double)n*SIGHT_INC/(double)visits:-1; } + inline void add(const PointAccumulator& p) {acc=acc+p.acc; n+=p.n; visits+=p.visits; } + static const PointAccumulator& Unknown(); + static PointAccumulator* unknown_ptr; + FloatPoint acc; + int n, visits; + inline double entropy() const; +}; + +void PointAccumulator::update(bool value, const Point& p){ + if (value) { + acc.x+= static_cast(p.x); + acc.y+= static_cast(p.y); + n++; + visits+=SIGHT_INC; + } else + visits++; +} + +double PointAccumulator::entropy() const{ + if (!visits) + return -log(.5); + if (n==visits || n==0) + return 0; + double x=(double)n*SIGHT_INC/(double)visits; + return -( x*log(x)+ (1-x)*log(1-x) ); +} + + +typedef Map > ScanMatcherMap; + +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h new file mode 100644 index 0000000..4368809 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensor.h @@ -0,0 +1,24 @@ +#ifndef SENSOR_H +#define SENSOR_H + +#include +#include + +namespace GMapping{ + +class Sensor{ + public: + Sensor(const std::string& name=""); + virtual ~Sensor(); + inline std::string getName() const {return m_name;} + inline void setName(const std::string& name) {m_name=name;} + protected: + std::string m_name; +}; + +typedef std::map SensorMap; + +}; //end namespace + +#endif + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h new file mode 100644 index 0000000..66fa537 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_base/sensorreading.h @@ -0,0 +1,26 @@ +#ifndef SENSORREADING_H +#define SENSORREADING_H + +#include "sensor.h" +namespace GMapping{ + +class SensorReading{ + public: + SensorReading(const Sensor* s, double time){ + m_sensor=s; + m_time=time; + }; + ~SensorReading(){}; + inline double getTime() const {return m_time;} + inline void setTime(double t) {m_time=t;} + inline const Sensor* getSensor() const {return m_sensor;} + protected: + double m_time; + const Sensor* m_sensor; + +}; + +}; //end namespace +#endif + + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h new file mode 100644 index 0000000..1a6456c --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometryreading.h @@ -0,0 +1,29 @@ +#ifndef ODOMETRYREADING_H +#define ODOMETRYREADING_H + +#include +#include +#include +#include "odometrysensor.h" + +namespace GMapping{ + +class OdometryReading: public SensorReading{ + public: + OdometryReading(const OdometrySensor* odo, double time=0); + inline const OrientedPoint& getPose() const {return m_pose;} + inline const OrientedPoint& getSpeed() const {return m_speed;} + inline const OrientedPoint& getAcceleration() const {return m_acceleration;} + inline void setPose(const OrientedPoint& pose) {m_pose=pose;} + inline void setSpeed(const OrientedPoint& speed) {m_speed=speed;} + inline void setAcceleration(const OrientedPoint& acceleration) {m_acceleration=acceleration;} + + protected: + OrientedPoint m_pose; + OrientedPoint m_speed; + OrientedPoint m_acceleration; +}; + +}; +#endif + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h new file mode 100644 index 0000000..1d18bd3 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_odometry/odometrysensor.h @@ -0,0 +1,20 @@ +#ifndef ODOMETRYSENSOR_H +#define ODOMETRYSENSOR_H + +#include +#include + +namespace GMapping{ + +class OdometrySensor: public Sensor{ + public: + OdometrySensor(const std::string& name, bool ideal=false); + inline bool isIdeal() const { return m_ideal; } + protected: + bool m_ideal; +}; + +}; + +#endif + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h new file mode 100644 index 0000000..1582c67 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangereading.h @@ -0,0 +1,26 @@ +#ifndef RANGEREADING_H +#define RANGEREADING_H + +#include +#include +#include + +namespace GMapping{ + +class RangeReading: public SensorReading, public std::vector{ + public: + RangeReading(const RangeSensor* rs, double time=0); + RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time=0); + virtual ~RangeReading(); + inline const OrientedPoint& getPose() const {return m_pose;} + inline void setPose(const OrientedPoint& pose) {m_pose=pose;} + unsigned int rawView(double* v, double density=0.) const; + std::vector cartesianForm(double maxRange=1e6) const; + unsigned int activeBeams(double density=0.) const; + protected: + OrientedPoint m_pose; +}; + +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h new file mode 100644 index 0000000..65feffb --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/sensor/sensor_range/rangesensor.h @@ -0,0 +1,35 @@ +#ifndef RANGESENSOR_H +#define RANGESENSOR_H + +#include +#include +#include + +namespace GMapping{ + +class RangeSensor: public Sensor{ + friend class Configuration; + friend class CarmenConfiguration; + friend class CarmenWrapper; + public: + struct Beam{ + OrientedPoint pose; //pose relative to the center of the sensor + double span; //spam=0 indicates a line-like beam + double maxRange; //maximum range of the sensor + double s,c; //sinus and cosinus of the beam (optimization); + }; + RangeSensor(std::string name); + RangeSensor(std::string name, unsigned int beams, double res, const OrientedPoint& position=OrientedPoint(0,0,0), double span=0, double maxrange=89.0); + inline const std::vector& beams() const {return m_beams;} + inline std::vector& beams() {return m_beams;} + inline OrientedPoint getPose() const {return m_pose;} + void updateBeamsLookup(); + bool newFormat; + protected: + OrientedPoint m_pose; + std::vector m_beams; +}; + +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/autoptr.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/autoptr.h new file mode 100644 index 0000000..abc647c --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/utils/autoptr.h @@ -0,0 +1,97 @@ +#ifndef AUTOPTR_H +#define AUTOPTR_H +#include + +namespace GMapping{ + +template +class autoptr{ + protected: + + public: + struct reference{ + X* data; + unsigned int shares; + }; + inline autoptr(X* p=(X*)(0)); + inline autoptr(const autoptr& ap); + inline autoptr& operator=(const autoptr& ap); + inline ~autoptr(); + inline operator int() const; + inline X& operator*(); + inline const X& operator*() const; + //p + reference * m_reference; + protected: +}; + +template +autoptr::autoptr(X* p){ + m_reference=0; + if (p){ + m_reference=new reference; + m_reference->data=p; + m_reference->shares=1; + } +} + +template +autoptr::autoptr(const autoptr& ap){ + m_reference=0; + reference* ref=ap.m_reference; + if (ap.m_reference){ + m_reference=ref; + m_reference->shares++; + } +} + +template +autoptr& autoptr::operator=(const autoptr& ap){ + reference* ref=ap.m_reference; + if (m_reference==ref){ + return *this; + } + if (m_reference && !(--m_reference->shares)){ + delete m_reference->data; + delete m_reference; + m_reference=0; + } + if (ref){ + m_reference=ref; + m_reference->shares++; + } +//20050802 nasty changes begin + else + m_reference=0; +//20050802 nasty changes end + return *this; +} + +template +autoptr::~autoptr(){ + if (m_reference && !(--m_reference->shares)){ + delete m_reference->data; + delete m_reference; + m_reference=0; + } +} + +template +autoptr::operator int() const{ + return m_reference && m_reference->shares && m_reference->data; +} + +template +X& autoptr::operator*(){ + assert(m_reference && m_reference->shares && m_reference->data); + return *(m_reference->data); +} + +template +const X& autoptr::operator*() const{ + assert(m_reference && m_reference->shares && m_reference->data); + return *(m_reference->data); +} + +}; +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/commandline.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/commandline.h new file mode 100644 index 0000000..bbd1ea4 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/utils/commandline.h @@ -0,0 +1,115 @@ +/***************************************************************** + * + * This file is part of the GMAPPING project + * + * GMAPPING Copyright (c) 2004 Giorgio Grisetti, + * Cyrill Stachniss, and Wolfram Burgard + * + * This software is licensed under the "Creative Commons + * License (Attribution-NonCommercial-ShareAlike 2.0)" + * and is copyrighted by Giorgio Grisetti, Cyrill Stachniss, + * and Wolfram Burgard. + * + * Further information on this license can be found at: + * http://creativecommons.org/licenses/by-nc-sa/2.0/ + * + * GMAPPING is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR + * PURPOSE. + * + *****************************************************************/ + + +#ifndef COMMANDLINE_H +#define COMMANDLINE_H + + +#define parseFlag(name,value)\ +if (!strcmp(argv[c],name)){\ + value=true;\ + cout << name << " on"<< endl;\ + recognized=true;\ +}\ + +#define parseString(name,value)\ +if (!strcmp(argv[c],name) && c +#endif +#ifdef MACOSX + #include + #include + //#define isnan(x) (x==FP_NAN) +#endif +#ifdef _WIN32 + #include + #ifndef __DRAND48_DEFINED__ + #define __DRAND48_DEFINED__ + inline double drand48() { return double(rand()) / RAND_MAX;} + #endif + #ifndef M_PI + #define M_PI 3.1415926535897932384626433832795 + #endif + #define round(d) (floor((d) + 0.5)) + typedef unsigned int uint; + #define isnan(x) (_isnan(x)) +#endif + +#endif + diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h new file mode 100644 index 0000000..5051408 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/utils/macro_params.h @@ -0,0 +1,38 @@ +#ifndef MACRO_PARAMS_H +#define MACRO_PARAMS_H + +#define PARAM_SET_GET(type, name, qualifier, setqualifier, getqualifier)\ +qualifier: type m_##name;\ +getqualifier: inline type get##name() const {return m_##name;}\ +setqualifier: inline void set##name(type name) {m_##name=name;} + +#define PARAM_SET(type, name, qualifier, setqualifier)\ +qualifier: type m_##name;\ +setqualifier: inline void set##name(type name) {m_##name=name;} + +#define PARAM_GET(type, name, qualifier, getqualifier)\ +qualifier: type m_##name;\ +getqualifier: inline type get##name() const {return m_##name;} + +#define MEMBER_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\ +getqualifier: inline type get##name() const {return member.get##name();}\ +setqualifier: inline void set##name(type name) { member.set##name(name);} + +#define MEMBER_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\ +setqualifier: inline void set##name(type name) { member.set##name(name);} + +#define MEMBER_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\ +getqualifier: inline type get##name() const {return member.get##name();} + +#define STRUCT_PARAM_SET_GET(member, type, name, qualifier, setqualifier, getqualifier)\ +getqualifier: inline type get##name() const {return member.name;}\ +setqualifier: inline void set##name(type name) {member.name=name;} + +#define STRUCT_PARAM_SET(member, type, name, qualifier, setqualifier, getqualifier)\ +setqualifier: inline void set##name(type name) {member.name=name;} + +#define STRUCT_PARAM_GET(member, type, name, qualifier, setqualifier, getqualifier)\ +getqualifier: inline type get##name() const {return member.name;}\ + +#define convertStringArgument(var,val,buf) if (!strcmp(buf,#val)) var=val +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/point.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/point.h new file mode 100644 index 0000000..92bab48 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/utils/point.h @@ -0,0 +1,207 @@ +#ifndef _POINT_H_ +#define _POINT_H_ +#include +#include +#include +#include "gvalues.h" + +#define DEBUG_STREAM cerr << __PRETTY_FUNCTION__ << ":" //FIXME + +namespace GMapping { + +template +struct point{ + inline point():x(0),y(0) {} + inline point(T _x, T _y):x(_x),y(_y){} + T x, y; +}; + +template +inline point operator+(const point& p1, const point& p2){ + return point(p1.x+p2.x, p1.y+p2.y); +} + +template +inline point operator - (const point & p1, const point & p2){ + return point(p1.x-p2.x, p1.y-p2.y); +} + +template +inline point operator * (const point& p, const T& v){ + return point(p.x*v, p.y*v); +} + +template +inline point operator * (const T& v, const point& p){ + return point(p.x*v, p.y*v); +} + +template +inline T operator * (const point& p1, const point& p2){ + return p1.x*p2.x+p1.y*p2.y; +} + + +template +struct orientedpoint: public point{ + inline orientedpoint() : point(0,0), theta(0) {}; + inline orientedpoint(const point& p); + inline orientedpoint(T x, T y, A _theta): point(x,y), theta(_theta){} + inline void normalize(); + inline orientedpoint rotate(A alpha){ + T s=sin(alpha), c=cos(alpha); + A a=alpha+theta; + a=atan2(sin(a),cos(a)); + return orientedpoint( + c*this->x-s*this->y, + s*this->x+c*this->y, + a); + } + A theta; +}; + + +template +void orientedpoint::normalize() { + if (theta >= -M_PI && theta < M_PI) + return; + + int multiplier = (int)(theta / (2*M_PI)); + theta = theta - multiplier*2*M_PI; + if (theta >= M_PI) + theta -= 2*M_PI; + if (theta < -M_PI) + theta += 2*M_PI; +} + + +template +orientedpoint::orientedpoint(const point& p){ + this->x=p.x; + this->y=p.y; + this->theta=0.; +} + + +template +orientedpoint operator+(const orientedpoint& p1, const orientedpoint& p2){ + return orientedpoint(p1.x+p2.x, p1.y+p2.y, p1.theta+p2.theta); +} + +template +orientedpoint operator - (const orientedpoint & p1, const orientedpoint & p2){ + return orientedpoint(p1.x-p2.x, p1.y-p2.y, p1.theta-p2.theta); +} + +template +orientedpoint operator * (const orientedpoint& p, const T& v){ + return orientedpoint(p.x*v, p.y*v, p.theta*v); +} + +template +orientedpoint operator * (const T& v, const orientedpoint& p){ + return orientedpoint(p.x*v, p.y*v, p.theta*v); +} + +template +orientedpoint absoluteDifference(const orientedpoint& p1,const orientedpoint& p2){ + orientedpoint delta=p1-p2; + delta.theta=atan2(sin(delta.theta), cos(delta.theta)); + double s=sin(p2.theta), c=cos(p2.theta); + return orientedpoint(c*delta.x+s*delta.y, + -s*delta.x+c*delta.y, delta.theta); +} + +template +orientedpoint absoluteSum(const orientedpoint& p1,const orientedpoint& p2){ + double s=sin(p1.theta), c=cos(p1.theta); + return orientedpoint(c*p2.x-s*p2.y, + s*p2.x+c*p2.y, p2.theta) + p1; +} + +template +point absoluteSum(const orientedpoint& p1,const point& p2){ + double s=sin(p1.theta), c=cos(p1.theta); + return point(c*p2.x-s*p2.y, s*p2.x+c*p2.y) + (point) p1; +} + +template +struct pointcomparator{ + bool operator ()(const point& a, const point& b) const { + return a.x +struct pointradialcomparator{ + point origin; + bool operator ()(const point& a, const point& b) const { + point delta1=a-origin; + point delta2=b-origin; + return (atan2(delta1.y,delta1.x) +inline point max(const point& p1, const point& p2){ + point p=p1; + p.x=p.x>p2.x?p.x:p2.x; + p.y=p.y>p2.y?p.y:p2.y; + return p; +} + +template +inline point min(const point& p1, const point& p2){ + point p=p1; + p.x=p.x +inline point interpolate(const point& p1, const F& t1, const point& p2, const F& t2, const F& t3){ + F gain=(t3-t1)/(t2-t1); + point p=p1+(p2-p1)*gain; + return p; +} + +template +inline orientedpoint +interpolate(const orientedpoint& p1, const F& t1, const orientedpoint& p2, const F& t2, const F& t3){ + F gain=(t3-t1)/(t2-t1); + orientedpoint p; + p.x=p1.x+(p2.x-p1.x)*gain; + p.y=p1.y+(p2.y-p1.y)*gain; + double s=sin(p1.theta)+sin(p2.theta)*gain, + c=cos(p1.theta)+cos(p2.theta)*gain; + p.theta=atan2(s,c); + return p; +} + + +template +inline double euclidianDist(const point& p1, const point& p2){ + return hypot(p1.x-p2.x, p1.y-p2.y); +} +template +inline double euclidianDist(const orientedpoint& p1, const orientedpoint& p2){ + return hypot(p1.x-p2.x, p1.y-p2.y); +} +template +inline double euclidianDist(const orientedpoint& p1, const point& p2){ + return hypot(p1.x-p2.x, p1.y-p2.y); +} +template +inline double euclidianDist(const point& p1, const orientedpoint& p2 ){ + return hypot(p1.x-p2.x, p1.y-p2.y); +} + + + +typedef point IntPoint; +typedef point Point; +typedef orientedpoint OrientedPoint; + +}; //end namespace + +#endif diff --git a/slam_gmapping/openslam_gmapping/include/gmapping/utils/stat.h b/slam_gmapping/openslam_gmapping/include/gmapping/utils/stat.h new file mode 100644 index 0000000..b5e6115 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/include/gmapping/utils/stat.h @@ -0,0 +1,147 @@ +#ifndef STAT_H +#define STAT_H +#include "point.h" +#include +#include "gvalues.h" + +namespace GMapping { + +/**stupid utility function for drawing particles form a zero mean, sigma variance normal distribution +probably it should not go there*/ +double sampleGaussian(double sigma,unsigned int S=0); + +double evalGaussian(double sigmaSquare, double delta); +double evalLogGaussian(double sigmaSquare, double delta); +int sampleUniformInt(int max); +double sampleUniformDouble(double min, double max); + +struct Covariance3{ + Covariance3 operator + (const Covariance3 & cov) const; + static Covariance3 zero; + double xx, yy, tt, xy, xt, yt; +}; + +struct EigenCovariance3{ + EigenCovariance3(); + EigenCovariance3(const Covariance3& c); + EigenCovariance3 rotate(double angle) const; + OrientedPoint sample() const; + double eval[3]; + double evec[3][3]; +}; + +struct Gaussian3{ + OrientedPoint mean; + EigenCovariance3 covariance; + Covariance3 cov; + double eval(const OrientedPoint& p) const; + void computeFromSamples(const std::vector & poses); + void computeFromSamples(const std::vector & poses, const std::vector& weights ); +}; + +template +Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){ + Gaussian3 gaussian; + OrientedPoint mean=OrientedPoint(0,0,0); + double wcum=0; + double s=0, c=0; + WeightIterator wt=weightBegin; + double *w=new double(); + OrientedPoint *p=new OrientedPoint(); + for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ + *w=*wt; + *p=*pt; + s+=*w*sin(p->theta); + c+=*w*cos(p->theta); + mean.x+=*w*p->x; + mean.y+=*w*p->y; + wcum+=*w; + wt++; + } + mean.x/=wcum; + mean.y/=wcum; + s/=wcum; + c/=wcum; + mean.theta=atan2(s,c); + + Covariance3 cov=Covariance3::zero; + wt=weightBegin; + for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ + *w=*wt; + *p=*pt; + OrientedPoint delta=(*p)-mean; + delta.theta=atan2(sin(delta.theta),cos(delta.theta)); + cov.xx+=*w*delta.x*delta.x; + cov.yy+=*w*delta.y*delta.y; + cov.tt+=*w*delta.theta*delta.theta; + cov.xy+=*w*delta.x*delta.y; + cov.yt+=*w*delta.y*delta.theta; + cov.xt+=*w*delta.x*delta.theta; + wt++; + } + cov.xx/=wcum; + cov.yy/=wcum; + cov.tt/=wcum; + cov.xy/=wcum; + cov.yt/=wcum; + cov.xt/=wcum; + EigenCovariance3 ecov(cov); + gaussian.mean=mean; + gaussian.covariance=ecov; + gaussian.cov=cov; + delete w; + delete p; + return gaussian; +} + +template +Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){ + Gaussian3 gaussian; + OrientedPoint mean=OrientedPoint(0,0,0); + double wcum=1; + double s=0, c=0; + OrientedPoint *p=new OrientedPoint(); + for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ + *p=*pt; + s+=sin(p->theta); + c+=cos(p->theta); + mean.x+=p->x; + mean.y+=p->y; + wcum+=1.; + } + mean.x/=wcum; + mean.y/=wcum; + s/=wcum; + c/=wcum; + mean.theta=atan2(s,c); + + Covariance3 cov=Covariance3::zero; + for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){ + *p=*pt; + OrientedPoint delta=(*p)-mean; + delta.theta=atan2(sin(delta.theta),cos(delta.theta)); + cov.xx+=delta.x*delta.x; + cov.yy+=delta.y*delta.y; + cov.tt+=delta.theta*delta.theta; + cov.xy+=delta.x*delta.y; + cov.yt+=delta.y*delta.theta; + cov.xt+=delta.x*delta.theta; + } + cov.xx/=wcum; + cov.yy/=wcum; + cov.tt/=wcum; + cov.xy/=wcum; + cov.yt/=wcum; + cov.xt/=wcum; + EigenCovariance3 ecov(cov); + gaussian.mean=mean; + gaussian.covariance=ecov; + gaussian.cov=cov; + delete p; + return gaussian; +} + + +}; //end namespace +#endif + diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-10cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-10cm.ini new file mode 100644 index 0000000..544bf78 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-10cm.ini @@ -0,0 +1,78 @@ +### gfs dummy config file + +## WARNING: Changing these parameters, can +## increase of decrese the performance of the +## mapper! + + +[gfs] + +################################################# +## +## These are probably the most improtant parameters +## + +## gfs - number of particles +particles 30 + +## gfs measurement integration +angularUpdate 0.5 +linearUpdate 1 + +## map resolution +delta 0.1 + +## scan matcher +maxrange 81.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m +maxUrange 80.0 # (use up to) +sigma 0.05 # scan matcher cell sigma, for the greedy search +regscore 0.0004 # minimum score for regsistering a scan +iterations 5 # iterations +critscore 0.0 # critical score (leave this) +maxMove 1.0 # maximum move among two scans. This detects some corrupted logs +autosize off # determine te map size by pre readoing the log + + +## default settings for a 0.1 m map cell +lstep 0.1 # linear search step (choose delta) +astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval +lsigma 0.075 # sigma likelihood of 1 beam +lskip 0 # beams to skip in the likelihood computation +skipMatching off # do not perform scan matching before computing the statistics + +kernelSize 1 # the higher the value the slower the filter + # the better it can deal with noise, but the less precise and slower +ogain 3 # gain for smoothing the likelihood +resampleThreshold 0.5 # when neff is below this value a resampling occurs +randseed 0 # this is for the repeated experiments + +## likelihood sampling +llsamplerange 0.1 # linear range +llsamplestep 0.1 # linear step +lasamplerange 0.05 # angular range +lasamplestep 0.05 # angular step + +## motion model parameters +srr 0.1 # translation as a function of translation +srt 0.1 # translation as a function of rotation +str 0.1 # rotation as a function of translation +stt 0.1 # rotation as a function of rotation + +## odometry integration in proposal +linearOdometryReliability 0.0 +angularOdometryReliability 0.0 +considerOdometryCovariance off + + +## inital map params +xmin -100.0 +ymin -100.0 +xmax 100.0 +ymax 100.0 + +## file parameters + +readFromStdin off +onLine off +generateMap off + diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-20cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-20cm.ini new file mode 100644 index 0000000..e0962ce --- /dev/null +++ b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-20cm.ini @@ -0,0 +1,76 @@ +### gfs dummy config file + +## WARNING: Changing these parameters, can +## increase of decrese the performance of the +## mapper! + + +[gfs] + +################################################# +## +## These are probably the most improtant parameters +## + +## gfs - number of particles +particles 30 + +## gfs measurement integration +angularUpdate 0.5 +linearUpdate 1 + +## map resolution +delta 0.2 +## scan matcher +maxrange 80 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m +maxUrange 80 # (use up to) +sigma 0.05 # scan matcher cell sigma, for the greedy search +regscore 10000 # minimum score for regsistering a scan +iterations 5 # iterations +critscore 0.0 # critical score (leave this) +maxMove 1.0 # maximum move among two scans. This detects some corrupted logs +autosize off # determine te map size by pre readoing the log + + +lstep 0.2 # linear search step (choose delta) +astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval +lsigma 0.2 # sigma likelihood of 1 beam +lskip 1. # beams to skip in the likelihood computation +skipMatching off # do not perform scan matching before computing the statistics + +kernelSize 1 # the higher the value the slower the filter + # the better it can deal with noise, but the less precise and slower +ogain 3 # gain for smoothing the likelihood +resampleThreshold 0.5 # when neff is below this value a resampling occurs +randseed 0 # this is for the repeated experiments + +## likelihood sampling +llsamplerange 0.2 # linear range +llsamplestep 0.2 # linear step +lasamplerange 0.05 # angular range +lasamplestep 0.05 # angular step + +## motion model parameters +srr 0.1 # translation as a function of translation +srt 0.1 # translation as a function of rotation +str 0.1 # rotation as a function of translation +stt 0.1 # rotation as a function of rotation + +## odometry integration in proposal +linearOdometryReliability 0.0 +angularOdometryReliability 0.0 +considerOdometryCovariance off + + +## inital map params +xmin -150.0 +ymin -100.0 +xmax 100.0 +ymax 100.0 + +## file parameters + +readFromStdin off +onLine off +generateMap off + diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-LMS-5cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-5cm.ini new file mode 100644 index 0000000..c54fa02 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/ini/gfs-LMS-5cm.ini @@ -0,0 +1,78 @@ +### gfs dummy config file + +## WARNING: Changing these parameters, can +## increase of decrese the performance of the +## mapper! + + +[gfs] + +################################################# +## +## These are probably the most improtant parameters +## + +## gfs - number of particles +particles 30 + +## gfs measurement integration +angularUpdate 0.5 +linearUpdate 1 + +## map resolution +delta 0.05 + +## scan matcher +maxrange 81.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m +maxUrange 80.0 # (use up to) +sigma 0.05 # scan matcher cell sigma, for the greedy search +regscore 0.0004 # minimum score for regsistering a scan +iterations 5 # iterations +critscore 0.0 # critical score (leave this) +maxMove 1.0 # maximum move among two scans. This detects some corrupted logs +autosize off # determine te map size by pre readoing the log + + +## default settings for a 0.1 m map cell +lstep 0.05 # linear search step (choose delta) +astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval +lsigma 0.05 # sigma likelihood of 1 beam +lskip 0 # beams to skip in the likelihood computation +skipMatching off # do not perform scan matching before computing the statistics + +kernelSize 1 # the higher the value the slower the filter + # the better it can deal with noise, but the less precise and slower +ogain 3 # gain for smoothing the likelihood +resampleThreshold 0.5 # when neff is below this value a resampling occurs +randseed 0 # this is for the repeated experiments + +## likelihood sampling +llsamplerange 0.05 # linear range +llsamplestep 0.05 # linear step +lasamplerange 0.05 # angular range +lasamplestep 0.05 # angular step + +## motion model parameters +srr 0.1 # translation as a function of translation +srt 0.1 # translation as a function of rotation +str 0.1 # rotation as a function of translation +stt 0.1 # rotation as a function of rotation + +## odometry integration in proposal +linearOdometryReliability 0.0 +angularOdometryReliability 0.0 +considerOdometryCovariance off + + +## inital map params +xmin -100.0 +ymin -100.0 +xmax 100.0 +ymax 100.0 + +## file parameters + +readFromStdin off +onLine off +generateMap off + diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-PLS-10cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-PLS-10cm.ini new file mode 100644 index 0000000..1e6fb34 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/ini/gfs-PLS-10cm.ini @@ -0,0 +1,78 @@ +### gfs dummy config file + +## WARNING: Changing these parameters, can +## increase of decrese the performance of the +## mapper! + + +[gfs] + +################################################# +## +## These are probably the most improtant parameters +## + +## gfs - number of particles +particles 30 + +## gfs measurement integration +angularUpdate 0.5 +linearUpdate 1 + +## map resolution +delta 0.1 + +## scan matcher +maxrange 50.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m +maxUrange 50.0 # (use up to) +sigma 0.075 # scan matcher cell sigma, for the greedy search +regscore 0.0004 # minimum score for regsistering a scan +iterations 5 # iterations +critscore 0.0 # critical score (leave this) +maxMove 1.0 # maximum move among two scans. This detects some corrupted logs +autosize off # determine te map size by pre readoing the log + + +## default settings for a 0.1 m map cell +lstep 0.1 # linear search step (choose delta) +astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval +lsigma 0.1 # sigma likelihood of 1 beam +lskip 0 # beams to skip in the likelihood computation +skipMatching off # do not perform scan matching before computing the statistics + +kernelSize 1 # the higher the value the slower the filter + # the better it can deal with noise, but the less precise and slower +ogain 3 # gain for smoothing the likelihood +resampleThreshold 0.5 # when neff is below this value a resampling occurs +randseed 0 # this is for the repeated experiments + +## likelihood sampling +llsamplerange 0.1 # linear range +llsamplestep 0.1 # linear step +lasamplerange 0.05 # angular range +lasamplestep 0.05 # angular step + +## motion model parameters +srr 0.1 # translation as a function of translation +srt 0.1 # translation as a function of rotation +str 0.1 # rotation as a function of translation +stt 0.1 # rotation as a function of rotation + +## odometry integration in proposal +linearOdometryReliability 0.0 # condition the scan matcher with odometry while serching for max [translation component] +angularOdometryReliability 0.0 # condition the scan matcher with odometry while serching for max [rotation component] +considerOdometryCovariance off + + +## inital map params +xmin -100.0 +ymin -100.0 +xmax 100.0 +ymax 100.0 + +## file parameters + +readFromStdin off +onLine off +generateMap off + diff --git a/slam_gmapping/openslam_gmapping/ini/gfs-PLS-5cm.ini b/slam_gmapping/openslam_gmapping/ini/gfs-PLS-5cm.ini new file mode 100644 index 0000000..871dcc5 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/ini/gfs-PLS-5cm.ini @@ -0,0 +1,78 @@ +### gfs dummy config file + +## WARNING: Changing these parameters, can +## increase of decrese the performance of the +## mapper! + + +[gfs] + +################################################# +## +## These are probably the most improtant parameters +## + +## gfs - number of particles +particles 30 + +## gfs measurement integration +angularUpdate 0.5 +linearUpdate 1 + +## map resolution +delta 0.05 + +## scan matcher +maxrange 50.0 # (maximum valid) for SICK LMS, 81m max, SICK PLS 50m +maxUrange 50.0 # (use up to) +sigma 0.07 # scan matcher cell sigma, for the greedy search +regscore 0.0004 # minimum score for regsistering a scan +iterations 5 # iterations +critscore 0.0 # critical score (leave this) +maxMove 1.0 # maximum move among two scans. This detects some corrupted logs +autosize off # determine te map size by pre readoing the log + + +## default settings for a 0.1 m map cell +lstep 0.05 # linear search step (choose delta) +astep 0.05 # angular search step, this is fine, depending on the odometry error and the update interval +lsigma 0.05 # sigma likelihood of 1 beam +lskip 0 # beams to skip in the likelihood computation +skipMatching off # do not perform scan matching before computing the statistics + +kernelSize 1 # the higher the value the slower the filter + # the better it can deal with noise, but the less precise and slower +ogain 3 # gain for smoothing the likelihood +resampleThreshold 0.5 # when neff is below this value a resampling occurs +randseed 0 # this is for the repeated experiments + +## likelihood sampling +llsamplerange 0.05 # linear range +llsamplestep 0.05 # linear step +lasamplerange 0.05 # angular range +lasamplestep 0.05 # angular step + +## motion model parameters +srr 0.1 # translation as a function of translation +srt 0.1 # translation as a function of rotation +str 0.1 # rotation as a function of translation +stt 0.1 # rotation as a function of rotation + +## odometry integration in proposal +linearOdometryReliability 0.0 +angularOdometryReliability 0.0 +considerOdometryCovariance off + + +## inital map params +xmin -100.0 +ymin -100.0 +xmax 100.0 +ymax 100.0 + +## file parameters + +readFromStdin off +onLine off +generateMap off + diff --git a/slam_gmapping/openslam_gmapping/log/Makefile b/slam_gmapping/openslam_gmapping/log/Makefile new file mode 100644 index 0000000..02a0515 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/Makefile @@ -0,0 +1,9 @@ +OBJS= configuration.o carmenconfiguration.o sensorlog.o sensorstream.o +APPS= log_test log_plot scanstudio2carmen rdk2carmen + +LDFLAGS+= -lsensor_range -lsensor_odometry -lsensor_base +CPPFLAGS+= -I../sensor + +-include ../global.mk +-include ../build_tools/Makefile.generic-shared-object + diff --git a/slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp b/slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp new file mode 100644 index 0000000..13784e3 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/carmenconfiguration.cpp @@ -0,0 +1,463 @@ +#include +#include "carmenconfiguration.h" +#include +#include +#include +#include +#include +#include + + +#define LINEBUFFER_SIZE 10000 + +namespace GMapping { + +using namespace std; + +istream& CarmenConfiguration::load(istream& is){ + clear(); + char buf[LINEBUFFER_SIZE]; + bool laseron=false; + bool rlaseron=false; + bool rlaser1=false; + bool rlaser2=false; + + string beams; + string rbeams; + + while (is){ + is.getline(buf, LINEBUFFER_SIZE); + istringstream lis(buf); + + string qualifier; + string name; + + if (lis) + lis >> qualifier; + else + continue; + //this is a workaround for carmen log files + //the number lf laser beams should be specofoed in the config + //part of the log + if (qualifier=="FLASER"){ + laseron=true; + lis >> beams; + } + if (qualifier=="RLASER"){ + rlaseron=true; + lis >> rbeams; + } + if (qualifier=="ROBOTLASER1"){ + string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; + lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> beams; + rlaser1=true; + } + if (qualifier=="ROBOTLASER2"){ + string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; + lis >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode>> rbeams; + rlaser2=true; + } + if (qualifier!="PARAM") + continue; + if (lis) + lis >> name; + else continue; + + + vector v; + while (lis){ + string cparm; + lis >> cparm; + if (lis) + v.push_back(cparm); + } + insert(make_pair(name, v)); + } + if (laseron || rlaser1){ + vector v; + v.push_back(beams); + insert(make_pair("laser_beams", v)); + cerr << "FRONT LASER BEAMS FROM LOG: " << beams << endl; + v.clear(); + v.push_back("on"); + insert(make_pair("robot_use_laser", v)); + } + if (rlaseron || rlaser2){ + vector v; + v.push_back(rbeams); + insert(make_pair("rear_laser_beams", v)); + cerr << "REAR LASER BEAMS FROM LOG: " << beams << endl; + v.clear(); + v.push_back("on"); + insert(make_pair("robot_use_rear_laser", v)); + } + return is; +} + +SensorMap CarmenConfiguration::computeSensorMap() const{ + //this boring stuff is for retrieving the parameters from the loaded tokens + + SensorMap smap; + //odometry + OdometrySensor* odometry=new OdometrySensor("ODOM"); + OdometrySensor* truepos=new OdometrySensor("TRUEPOS", true); + + smap.insert(make_pair(odometry->getName(), odometry)); + smap.insert(make_pair(truepos->getName(), truepos)); + //sonars + const_iterator key=find("robot_use_sonar"); + if (key!=end() && key->second.front()=="on"){ + RangeSensor* sonar=new RangeSensor("SONAR"); + + //the center of the sonar is the center of the base + sonar->m_pose.x=sonar->m_pose.y=sonar->m_pose.theta=0; + + double maxrange=10.; + key=find("robot_max_sonar"); + if (key!=end()){ + maxrange=atof(key->second.front().c_str()); + cerr << "max sonar:" << maxrange << endl; + } + + unsigned int sonar_num=0; + key=find("robot_num_sonars"); + if (key!=end()){ + sonar_num=atoi(key->second.front().c_str()); + cerr << "robot_num_sonars" << sonar_num << endl; + } + + key=find("robot_sonar_offsets"); + if (key!=end()){ + const vector & soff=key->second; + + if( (soff.size()/3m_beams.push_back(beam); + cerr << "beam_x" << beam.pose.x; + cerr << " beam_y" << beam.pose.y; + cerr << " beam_theta" << beam.pose.theta << endl;; + } + } + sonar->updateBeamsLookup(); + smap.insert(make_pair(sonar->getName(), sonar)); + } + + //laser + key=find("robot_use_laser"); + + if (key!=end() && key->second.front()=="on"){ + RangeSensor* laser=new RangeSensor("FLASER"); + laser->newFormat=false; + //by default the center of the robot is the center of the laser + laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0; + key=find("robot_frontlaser_offset"); + if (key!=end()){ + laser->m_pose.x=atof(key->second.front().c_str()); + cerr << "FRONT OFFSET= " << laser->m_pose.x << endl; + } + + + + RangeSensor::Beam beam; + + //double angle=-.5*M_PI; + unsigned int beam_no=180; + + key=find("laser_beams"); + if (key!=end()){ + beam_no=atoi(key->second.front().c_str()); + cerr << "FRONT BEAMS="<< beam_no << endl; + } + + double maxrange=50; + double resolution=1.; + + + if (beam_no==180 || beam_no==181) + resolution =1.; + else if (beam_no==360 || beam_no==361) + resolution =.5; + else if (beam_no==540 || beam_no==541) + resolution =.5; + else if (beam_no==769) { + resolution =360./1024.; + maxrange = 4.1; + } + else if (beam_no==682) { + resolution =360./1024.; + maxrange = 4.1; + } + else if (beam_no==683) { + resolution =360./1024.; + maxrange = 5.5; + } + else { + key=find("laser_front_laser_resolution"); + if (key!=end()){ + resolution=atof(key->second.front().c_str()); + cerr << "FRONT RES " << resolution << endl; + } + } + + laser->m_beams.resize(beam_no); + double center_beam=(double)beam_no/2.; + uint low_index=(uint)floor(center_beam); + uint up_index=(uint)ceil(center_beam); + double step=resolution*M_PI/180.; + double angle=beam_no%2?0:step; + unsigned int i=beam_no%2?0:1; + for (; im_beams[low_index-i]=beam; + beam.pose.theta=angle; + laser->m_beams[up_index+i-1]=beam; + } + laser->updateBeamsLookup(); + smap.insert(make_pair(laser->getName(), laser)); + cerr << "front beams " << beam_no << endl; + cerr << "maxrange " << maxrange << endl; + } + + + key=find("robot_use_laser"); + if (key!=end() && key->second.front()=="on"){ + RangeSensor* laser=new RangeSensor("ROBOTLASER1"); + laser->newFormat=true; + cerr << "ROBOTLASER1 inserted" << endl; + //by default the center of the robot is the center of the laser + laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0; + key=find("robot_frontlaser_offset"); + if (key!=end()){ + laser->m_pose.x=atof(key->second.front().c_str()); + cerr << "FRONT OFFSET=" << laser->m_pose.x << endl; + } + + RangeSensor::Beam beam; + + //double angle=-.5*M_PI; + unsigned int beam_no=180; + + key=find("laser_beams"); + if (key!=end()){ + beam_no=atoi(key->second.front().c_str()); + cerr << "FRONT BEAMS="<< beam_no << endl; + } + + double maxrange=50; + double resolution=1.; + + + if (beam_no==180 || beam_no==181) + resolution =1.; + else if (beam_no==360 || beam_no==361) + resolution =.5; + else if (beam_no==540 || beam_no==541) + resolution =.5; + else if (beam_no==769) + resolution =360./1024.; + else if (beam_no==683) { + resolution =360./1024.; + maxrange=5.50; + } + else { + key=find("laser_front_laser_resolution"); + if (key!=end()){ + resolution=atof(key->second.front().c_str()); + cerr << "FRONT RES" << resolution << endl; + } + } + + laser->m_beams.resize(beam_no); + double center_beam=(double)beam_no/2.; + uint low_index=(uint)floor(center_beam); + uint up_index=(uint)ceil(center_beam); + double step=resolution*M_PI/180.; + double angle=beam_no%2?0:step; + unsigned int i=beam_no%2?0:1; + for (; im_beams[low_index-i]=beam; + beam.pose.theta=angle; + laser->m_beams[up_index+i-1]=beam; + } + laser->updateBeamsLookup(); + smap.insert(make_pair(laser->getName(), laser)); + cerr << "front beams" << beam_no << endl; + } + + + //vertical laser + key=find("robot_use_rear_laser"); + + if (key!=end() && key->second.front()=="on"){ + RangeSensor* laser=new RangeSensor("RLASER"); + + //by default the center of the robot is the center of the laser + laser->m_pose.x=laser->m_pose.y=laser->m_pose.theta=0; + laser->m_pose.theta=M_PI; + key=find("robot_rearlaser_offset"); + if (key!=end()){ + laser->m_pose.x=atof(key->second.front().c_str()); + cerr << "REAR OFFSET = " << laser->m_pose.x << endl; + } + + + + RangeSensor::Beam beam; + + //double angle=-.5*M_PI; + unsigned int beam_no=180; + + key=find("rear_laser_beams"); + if (key!=end()){ + beam_no=atoi(key->second.front().c_str()); + cerr << "REAR BEAMS="<< beam_no << endl; + } + + double maxrange=89; + double resolution=1.; + + + if (beam_no==180 || beam_no==181) + resolution =1.; + else if (beam_no==360 || beam_no==361) + resolution =.5; + else if (beam_no==540 || beam_no==541) + resolution =.5; + else if (beam_no==769) + resolution =360./1024.; + else { + key=find("laser_rear_laser_resolution"); + if (key!=end()){ + resolution=atof(key->second.front().c_str()); + cerr << "REAR RES" << resolution << endl; + } + } + + laser->m_beams.resize(beam_no); + double center_beam=(double)beam_no/2.; + uint low_index=(uint)floor(center_beam); + uint up_index=(uint)ceil(center_beam); + double step=resolution*M_PI/180.; + double angle=beam_no%2?0:step; + unsigned int i=beam_no%2?0:1; + for (; im_beams[low_index-i]=beam; + beam.pose.theta=angle; + laser->m_beams[up_index+i-1]=beam; + } + laser->updateBeamsLookup(); + smap.insert(make_pair(laser->getName(), laser)); + cerr<< "rear beams" << beam_no << endl; + } + + key=find("robot_use_rear_laser"); + if (key!=end() && key->second.front()=="on"){ + RangeSensor* laser=new RangeSensor("ROBOTLASER2"); + laser->newFormat=true; + cerr << "ROBOTLASER2 inserted" << endl; + //by default the center of the robot is the center of the laser + laser->m_pose.x=laser->m_pose.y=0; + laser->m_pose.theta=M_PI; + key=find("robot_rearlaser_offset"); + if (key!=end()){ + // laser->m_pose.x==atof(key->second.front().c_str()); + cerr << "REAR OFFSET not used" << laser->m_pose.x << endl; + } + + RangeSensor::Beam beam; + + //double angle=-.5*M_PI; + unsigned int beam_no=180; + + key=find("rear_laser_beams"); + if (key!=end()){ + beam_no=atoi(key->second.front().c_str()); + cerr << "REAR BEAMS="<< beam_no << endl; + } + + double maxrange=50; + double resolution=1.; + + + if (beam_no==180 || beam_no==181) + resolution =1.; + else if (beam_no==360 || beam_no==361) + resolution =.5; + else if (beam_no==540 || beam_no==541) + resolution =.5; + else if (beam_no==769) + resolution =360./1024.; + else { + key=find("laser_rear_laser_resolution"); + if (key!=end()){ + resolution=atof(key->second.front().c_str()); + cerr << "REAR RES" << resolution << endl; + } + } + + laser->m_beams.resize(beam_no); + double center_beam=(double)beam_no/2.; + uint low_index=(uint)floor(center_beam); + uint up_index=(uint)ceil(center_beam); + double step=resolution*M_PI/180.; + double angle=beam_no%2?0:step; + unsigned int i=beam_no%2?0:1; + for (; im_beams[low_index-i]=beam; + beam.pose.theta=angle; + laser->m_beams[up_index+i-1]=beam; + } + laser->updateBeamsLookup(); + smap.insert(make_pair(laser->getName(), laser)); + cerr << "rear beams" << beam_no << endl; + } + + + return smap; +} + +}; + diff --git a/slam_gmapping/openslam_gmapping/log/carmenconfiguration.h b/slam_gmapping/openslam_gmapping/log/carmenconfiguration.h new file mode 100644 index 0000000..4cf9bfb --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/carmenconfiguration.h @@ -0,0 +1,22 @@ +#ifndef CARMENCONFIGURATION_H +#define CARMENCONFIGURATION_H + +#include +#include +#include +#include +#include +#include "configuration.h" + +namespace GMapping { + +class CarmenConfiguration: public Configuration, public std::map >{ + public: + virtual std::istream& load(std::istream& is); + virtual SensorMap computeSensorMap() const; +}; + +}; + +#endif + diff --git a/slam_gmapping/openslam_gmapping/log/configuration.cpp b/slam_gmapping/openslam_gmapping/log/configuration.cpp new file mode 100644 index 0000000..9c81c5e --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/configuration.cpp @@ -0,0 +1,8 @@ +#include "configuration.h" + +namespace GMapping { + +Configuration::~Configuration(){ +} + +}; diff --git a/slam_gmapping/openslam_gmapping/log/configuration.h b/slam_gmapping/openslam_gmapping/log/configuration.h new file mode 100644 index 0000000..f02d17f --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/configuration.h @@ -0,0 +1,17 @@ +#ifndef CONFIGURATION_H +#define CONFIGURATION_H + +#include +#include + +namespace GMapping { + +class Configuration{ + public: + virtual ~Configuration(); + virtual SensorMap computeSensorMap() const=0; +}; + +}; +#endif + diff --git a/slam_gmapping/openslam_gmapping/log/log_plot.cpp b/slam_gmapping/openslam_gmapping/log/log_plot.cpp new file mode 100644 index 0000000..ad42861 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/log_plot.cpp @@ -0,0 +1,71 @@ +#include +#include +#include +#include +#include +#include + + +using namespace std; +using namespace GMapping; + +int main(int argc, char ** argv){ + double maxrange=2.; + if (argc<2){ + cout << "usage log_plot | gnuplot" << endl; + exit (-1); + } + ifstream is(argv[1]); + if (! is){ + cout << "no file " << argv[1] << " found" << endl; + exit (-1); + } + CarmenConfiguration conf; + conf.load(is); + + SensorMap m=conf.computeSensorMap(); + + //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) + // cout << it->first << " " << it->second->getName() << endl; + + SensorLog log(m); + is.close(); + + ifstream ls(argv[1]); + log.load(ls); + ls.close(); + int count=0; + int frame=0; + cerr << "log size" << log.size() << endl; + for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ + RangeReading* rr=dynamic_cast(*it); + if (rr){ + count++; + if (count%3) + continue; + std::vector points(rr->size()); + uint j=0; + for (uint i=0; isize(); i++){ + const RangeSensor * rs=dynamic_cast(rr->getSensor()); + double c=rs->beams()[i].c, s=rs->beams()[i].s; + double r=(*rr)[i]; + if (r>maxrange) + continue; + points[j++]=Point(r*c,r*s); + } + if (j){ + char buf[1024]; + sprintf(buf,"frame-%05d.gif",frame); + frame++; + cout << "set terminal gif" << endl; + cout << "set output \"" << buf << "\"" << endl; + cout << "set size ratio -1" << endl; + cout << "plot [-3:3][0:3] '-' w p ps 1" << endl; + for (uint i=0; i +#include +#include +#include +#include + + +using namespace std; +using namespace GMapping; + +int main(int argc, char ** argv){ + if (argc<2){ + cout << "usage log_test " << endl; + exit (-1); + } + ifstream is(argv[1]); + if (! is){ + cout << "no file " << argv[1] << " found" << endl; + exit (-1); + } + CarmenConfiguration conf; + conf.load(is); + + SensorMap m=conf.computeSensorMap(); + + //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) + // cout << it->first << " " << it->second->getName() << endl; + + SensorLog log(m); + is.close(); + + ifstream ls(argv[1]); + log.load(ls); + ls.close(); + cerr << "log size" << log.size() << endl; + for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ + RangeReading* rr=dynamic_cast(*it); + if (rr){ + //cerr << rr->getSensor()->getName() << " "; + //cerr << rr->size()<< " "; + //for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ + // cerr << *it << " "; + //} + cout<< rr->getPose().x << " " << rr->getPose().y << " " << rr->getPose().theta << " " << rr->getTime() << endl; + } + } +} diff --git a/slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp b/slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp new file mode 100644 index 0000000..199c6f7 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/rdk2carmen.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include + + +using namespace std; +using namespace GMapping; + +int main(int argc, char ** argv){ + if (argc<2){ + cerr << "usage "< " << endl; + cerr << "or "< for standard output" << endl; + exit (-1); + } + ifstream is(argv[1]); + if (! is){ + cerr << "no file " << argv[1] << " found" << endl; + exit (-1); + } + ostream *os; + if (argc<3) + os=&cout; + else{ + os=new ofstream(argv[2]); + if (! os){ + cerr << "no file " << argv[1] << " found" << endl; + exit (-1); + } + } + CarmenConfiguration conf; + conf.load(is); + + SensorMap m=conf.computeSensorMap(); + + //for (SensorMap::const_iterator it=m.begin(); it!=m.end(); it++) + // cout << it->first << " " << it->second->getName() << endl; + + SensorLog log(m); + is.close(); + + ifstream ls(argv[1]); + log.load(ls); + ls.close(); + cerr << "log size" << log.size() << endl; + for (SensorLog::iterator it=log.begin(); it!=log.end(); it++){ + RangeReading* rr=dynamic_cast(*it); + if (rr){ + *os << rr->getSensor()->getName() << " "; + *os << rr->size()<< " "; + for (RangeReading::const_iterator it=rr->begin(); it!=rr->end(); it++){ + *os << (*it)*0.001 << " "; + } + *os<< rr->getPose().x*0.001 << " " << rr->getPose().y*0.001 << " " << rr->getPose().theta << endl; + } + } +} diff --git a/slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp b/slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp new file mode 100644 index 0000000..1fdde8a --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/scanstudio2carmen.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include +#include +#include + +#define MAXLINELENGHT (10240) +#define MAXREADINGS (10240) + +using namespace std; +using namespace GMapping; + +int main (int argc, char** argv){ + if (argc<3){ + cout << "usage scanstudio2carmen scanfilename carmenfilename" << endl; + exit(1); + } + ifstream is(argv[1]); + if (!is){ + cout << "cannopt open file" << argv[1] << endl; + exit(1); + } + + ofstream os(argv[2]); + + double readings[MAXREADINGS]; + OrientedPoint pose; + int nbeams; + while (is){ + char buf[MAXLINELENGHT]; + is.getline(buf,MAXLINELENGHT); + istringstream st(buf); + string token; + st>>token; + if (token=="RobotPos:"){ + st >> pose.x >> pose.y >> pose.theta; + pose.x/=1000; + pose.y/=1000; + } else + if (token=="NumPoints:"){ + st >> nbeams; + assert(nbeams> angle; + is >> readings[c]; + readings[c]/=1000; + c++; + } + if (c==nbeams) + os << "FLASER " << nbeams << " "; + c=0; + while (c +#include +#include +#include +#include + +#define LINEBUFFER_SIZE 100000 + +namespace GMapping { + +using namespace std; + +SensorLog::SensorLog(const SensorMap& sm): m_sensorMap(sm){ +} + +SensorLog::~SensorLog(){ + for (iterator it=begin(); it!=end(); it++) + if (*it) delete (*it); +} + +istream& SensorLog::load(istream& is){ + for (iterator it=begin(); it!=end(); it++) + if (*it) delete (*it); + clear(); + + char buf[LINEBUFFER_SIZE]; + while (is){ + is.getline(buf, LINEBUFFER_SIZE); + istringstream lis(buf); + + string sensorname; + + if (lis) + lis >>sensorname; + else + continue; + + + + SensorMap::const_iterator it=m_sensorMap.find(sensorname); + if (it==m_sensorMap.end()){ + continue; + } + + Sensor* sensor=it->second; + + SensorReading* reading=0; + OdometrySensor* odometry=dynamic_cast(sensor); + if (odometry) + reading=parseOdometry(lis, odometry); + + RangeSensor* range=dynamic_cast(sensor); + if (range) + reading=parseRange(lis, range); + if (reading) + push_back(reading); + } + return is; + +} + +OdometryReading* SensorLog::parseOdometry(istream& is, const OdometrySensor* osen) const{ + OdometryReading* reading=new OdometryReading(osen); + OrientedPoint pose; + OrientedPoint speed; + OrientedPoint accel; + is >> pose.x >> pose.y >> pose.theta; + is >> speed.x >>speed.theta; + speed.y=0; + is >> accel.x; + accel.y=accel.theta=0; + reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel); + return reading; +} + +RangeReading* SensorLog::parseRange(istream& is, const RangeSensor* rs) const{ + if(rs->newFormat){ + string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; + is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy >> remission_mode; + } + + unsigned int size; + is >> size; + assert(size==rs->beams().size()); + + RangeReading* reading=new RangeReading(rs); + //cerr << "#R=" << size << endl; + reading->resize(size); + for (unsigned int i=0; i> (*reading)[i]; + } + if (rs->newFormat){ + int reflectionBeams; + is >> reflectionBeams; + double reflection; + for (int i=0; i> reflection; + } + //FIXME XXX + OrientedPoint laserPose; + is >> laserPose.x >> laserPose.y >> laserPose.theta; + OrientedPoint pose; + is >> pose.x >> pose.y >> pose.theta; + reading->setPose(pose); + double a,b,c; + if (rs->newFormat){ + string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis; + is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis; + } else { + is >> a >> b >> c; + } + string s; + is >> a >> s; + is >> a; + reading->setTime(a); + return reading; +} + +OrientedPoint SensorLog::boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const { + xmin=ymin=1e6; + xmax=ymax=-1e6; + bool first=true; + OrientedPoint start; + for (const_iterator it=begin(); it!=end(); it++){ + double lxmin=0., lxmax=0., lymin=0., lymax=0.; + const SensorReading* reading=*it; + const OdometryReading* odometry=dynamic_cast (reading); + if (odometry){ + lxmin=lxmax=odometry->getPose().x; + lymin=lymax=odometry->getPose().y; + } + + const RangeReading* rangeReading=dynamic_cast (reading); + if (rangeReading){ + lxmin=lxmax=rangeReading->getPose().x; + lymin=lymax=rangeReading->getPose().y; + if (first){ + first=false; + start=rangeReading->getPose(); + } + } + xmin=xminlxmax?xmax:lxmax; + ymin=yminlymax?ymax:lymax; + } + return start; +} + +}; + diff --git a/slam_gmapping/openslam_gmapping/log/sensorlog.h b/slam_gmapping/openslam_gmapping/log/sensorlog.h new file mode 100644 index 0000000..c6b380f --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/sensorlog.h @@ -0,0 +1,29 @@ +#ifndef SENSORLOG_H +#define SENSORLOG_H + +#include +#include +#include +#include +#include +#include +#include +#include "configuration.h" + +namespace GMapping { + +class SensorLog : public std::list{ + public: + SensorLog(const SensorMap&); + ~SensorLog(); + std::istream& load(std::istream& is); + OrientedPoint boundingBox(double& xmin, double& ymin, double& xmax, double& ymax) const; + protected: + const SensorMap& m_sensorMap; + OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ) const; + RangeReading* parseRange(std::istream& is, const RangeSensor* ) const; +}; + +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/log/sensorstream.cpp b/slam_gmapping/openslam_gmapping/log/sensorstream.cpp new file mode 100644 index 0000000..3c27b04 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/sensorstream.cpp @@ -0,0 +1,155 @@ +#include +#include +#include "sensorstream.h" +//#define LINEBUFFER_SIZE 1000000 //for not Cyrill to unbless me, it is better to exagerate :-)) +// Can't declare a buffer that big on the stack. So we'll risk Cyrill's +// unblessing, and make it smaller. +#define LINEBUFFER_SIZE 8192 + +namespace GMapping { + +using namespace std; + +//SensorStream +SensorStream::SensorStream(const SensorMap& sensorMap) :m_sensorMap(sensorMap){} + +SensorStream::~SensorStream(){} + +SensorReading* SensorStream::parseReading(std::istream& is, const SensorMap& smap){ + SensorReading* reading=0; + if (is){ + char buf[LINEBUFFER_SIZE]; + is.getline(buf, LINEBUFFER_SIZE); + istringstream lis(buf); + + string sensorname; + + if (lis){ + lis >>sensorname; + } else + return 0; + + SensorMap::const_iterator it=smap.find(sensorname); + if (it==smap.end()){ + return 0; + } + + Sensor* sensor=it->second; + + OdometrySensor* odometry=dynamic_cast(sensor); + if (odometry) + reading=parseOdometry(lis, odometry); + + RangeSensor* range=dynamic_cast(sensor); + if (range) + reading=parseRange(lis, range); + } + return reading; +} + +OdometryReading* SensorStream::parseOdometry(std::istream& is, const OdometrySensor* osen ){ + OdometryReading* reading=new OdometryReading(osen); + OrientedPoint pose; + OrientedPoint speed; + OrientedPoint accel; + is >> pose.x >> pose.y >> pose.theta; + is >> speed.x >>speed.theta; + speed.y=0; + is >> accel.x; + accel.y=accel.theta=0; + reading->setPose(pose); reading->setSpeed(speed); reading->setAcceleration(accel); + double timestamp, reltimestamp; + string s; + is >> timestamp >>s >> reltimestamp; + reading->setTime(timestamp); + return reading; +} + +RangeReading* SensorStream::parseRange(std::istream& is, const RangeSensor* rs){ + //cerr << __PRETTY_FUNCTION__ << endl; + if(rs->newFormat){ + string laser_type, start_angle, field_of_view, angular_resolution, maximum_range, accuracy, remission_mode; + is >> laser_type>> start_angle>> field_of_view>> angular_resolution>> maximum_range>> accuracy>> remission_mode; + //cerr << " New format laser msg" << endl; + } + unsigned int size; + is >> size; + assert(size==rs->beams().size()); + RangeReading* reading=new RangeReading(rs); + reading->resize(size); + for (unsigned int i=0; i> (*reading)[i]; + } + if (rs->newFormat){ + int reflectionBeams; + is >> reflectionBeams; + double reflection; + for (int i=0; i> reflection; + } + OrientedPoint laserPose; + is >> laserPose.x >> laserPose.y >> laserPose.theta; + OrientedPoint pose; + is >> pose.x >> pose.y >> pose.theta; + reading->setPose(pose); + + if (rs->newFormat){ + string laser_tv, laser_rv, forward_safety_dist, side_safty_dist, turn_axis; + is >> laser_tv >> laser_rv >> forward_safety_dist >> side_safty_dist >> turn_axis; + } +// else { +// double a,b,c; +// is >> a >> b >> c; +// } + double timestamp, reltimestamp; + string s; + is >> timestamp >>s >> reltimestamp; + reading->setTime(timestamp); + return reading; + +} + +//LogSensorStream +LogSensorStream::LogSensorStream(const SensorMap& sensorMap, const SensorLog* log): + SensorStream(sensorMap){ + m_log=log; + assert(m_log); + m_cursor=log->begin(); +} + +LogSensorStream::operator bool() const{ + return m_cursor==m_log->end(); +} + +bool LogSensorStream::rewind(){ + m_cursor=m_log->begin(); + return true; +} + +SensorStream& LogSensorStream::operator >>(const SensorReading*& rd){ + rd=*m_cursor; + m_cursor++; + return *this; +} + +//InputSensorStream +InputSensorStream::InputSensorStream(const SensorMap& sensorMap, std::istream& is): + SensorStream(sensorMap), m_inputStream(is){ +} + +InputSensorStream::operator bool() const{ + return (bool) m_inputStream; +} + +bool InputSensorStream::rewind(){ + //m_inputStream.rewind(); + return false; +} + +SensorStream& InputSensorStream::operator >>(const SensorReading*& reading){ + reading=parseReading(m_inputStream, m_sensorMap); + return *this; +} + +}; + diff --git a/slam_gmapping/openslam_gmapping/log/sensorstream.h b/slam_gmapping/openslam_gmapping/log/sensorstream.h new file mode 100644 index 0000000..5e25507 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/log/sensorstream.h @@ -0,0 +1,47 @@ +#ifndef SENSORSTREAM_H +#define SENSORSTREAM_H + +#include +#include "sensorlog.h" + +namespace GMapping { +class SensorStream{ + public: + SensorStream(const SensorMap& sensorMap); + virtual ~SensorStream(); + virtual operator bool() const=0; + virtual bool rewind() = 0 ; + virtual SensorStream& operator >>(const SensorReading*&) = 0; + inline const SensorMap& getSensorMap() const {return m_sensorMap; } + protected: + const SensorMap& m_sensorMap; + static SensorReading* parseReading(std::istream& is, const SensorMap& smap); + static OdometryReading* parseOdometry(std::istream& is, const OdometrySensor* ); + static RangeReading* parseRange(std::istream& is, const RangeSensor* ); +}; + +class InputSensorStream: public SensorStream{ + public: + InputSensorStream(const SensorMap& sensorMap, std::istream& is); + virtual operator bool() const; + virtual bool rewind(); + virtual SensorStream& operator >>(const SensorReading*&); + + //virtual SensorStream& operator >>(SensorLog*& log); + protected: + std::istream& m_inputStream; +}; + +class LogSensorStream: public SensorStream{ + public: + LogSensorStream(const SensorMap& sensorMap, const SensorLog* log); + virtual operator bool() const; + virtual bool rewind(); + virtual SensorStream& operator >>(const SensorReading*&); + protected: + const SensorLog* m_log; + SensorLog::const_iterator m_cursor; +}; + +}; +#endif diff --git a/slam_gmapping/openslam_gmapping/package.xml b/slam_gmapping/openslam_gmapping/package.xml new file mode 100644 index 0000000..23ef119 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/package.xml @@ -0,0 +1,25 @@ + + + + openslam_gmapping + 0.1.2 + ROS-ified version of gmapping SLAM. Forked from https://openslam.informatik.uni-freiburg.de/data/svn/gmapping/trunk/ + Vincent Rabaud + CreativeCommons-by-nc-sa-2.0 + + http://openslam.org/gmapping + https://github.com/ros-perception/openslam_gmapping + https://github.com/ros-perception/openslam_gmapping/issues + + Giorgio Grisetti + Cyrill Stachniss + Wolfram Burgard + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/slam_gmapping/openslam_gmapping/particlefilter/particlefilter.cpp b/slam_gmapping/openslam_gmapping/particlefilter/particlefilter.cpp new file mode 100644 index 0000000..e49999a --- /dev/null +++ b/slam_gmapping/openslam_gmapping/particlefilter/particlefilter.cpp @@ -0,0 +1,133 @@ +std::vector sistematicResampler::resample(const vector& particles) const{ + Numeric cweight=0; + + //compute the cumulative weights + unsigned int n=0; + for (vector::const_iterator it=particles.begin(); it!=particles.end(); ++it){ + cweight+=it->weight; + n++; + } + + //compute the interval + Numeric interval=cweight/n; + + //compute the initial target weight + Numeric target= + //compute the resampled indexes + + cweight=0; + std::vector indexes(n); + n=0; + unsigned int i=0; + for (vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){ + cweight+=it->weight; + while(cweight>target){ + indexes[n++]=i; + target+=interval; + } + } + return indexes; + } + +template +std::vector indexResampler::resample(const vector >& weights) const{ + Numeric cweight=0; + + //compute the cumulative weights + unsigned int n=0; + for (vector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ + cweight+=*it; + n++; + } + + //compute the interval + Numeric interval=cweight/n; + + //compute the initial target weight + Numeric target= + //compute the resampled indexes + + cweight=0; + std::vector indexes(n); + n=0; + unsigned int i=0; + for (vector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ + cweight+=it->weight; + while(cweight>target){ + indexes[n++]=i; + target+=interval; + } + } + return indexes; +} + +/* + +The following are patterns for the evolution and the observation classes +The user should implement classes having the specified meaning + +template +struct observer{ + Observation& observation + Numeric observe(const class State&) const; +}; + +template +struct evolver{ + Input& input; + State& evolve(const State& s); +}; +*/ + +template +void evolver::evolve(std::vector& particles) const{ + for (std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it) + *it=evolutionModel.evolve(*it); +} + +void evolver::evolve(std::vector& dest, const std::vector& src) const{ + dest.clear(); + for (std::vector::const_iterator it=src.begin(); it!=src.end(); ++it) + dest.push_back(evolutionModel.evolve(*it)); +} + +template +struct auxiliaryEvolver{ + typedef particle Particle; + + EvolutionModel evolutionModel; + QualificationModel qualificationModel; + LikelyhoodModel likelyhoodModel; + indexResampler resampler; + +void auxiliaryEvolver::evolve + (std::vector&particles){ + std::vector observationWeights(particles.size()); + unsigned int i=0; + for (std::vector::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){ + observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); + } + std::vector indexes(indexResampler.resample(observationWeights)); + for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ + Particle & particle=particles[*it]; + particle=evolutionModel.evolve(particle); + particle.weight*=lykelyhoodModel.lykelyhood(particle)/observationWeights[*it]; + } +} + +void auxiliaryEvolver::evolve + (std::vector& dest, const std::vector& src){ + dest.clear(); + std::vector observationWeights(particles.size()); + unsigned int i=0; + for (std::vector::const_iterator it=src.begin(); it!=src.end(); ++it, i++){ + observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it)); + } + std::vector indexes(indexResampler.resample(observationWeights)); + for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); it++){ + Particle & particle=src[*it]; + dest.push_back(evolutionModel.evolve(particle)); + dest.back().weight*=likelyhoodModel.lykelyhood(particle)/observationWeights[*it]; + } + return dest(); +} diff --git a/slam_gmapping/openslam_gmapping/particlefilter/particlefilter_test.cpp b/slam_gmapping/openslam_gmapping/particlefilter/particlefilter_test.cpp new file mode 100644 index 0000000..28defed --- /dev/null +++ b/slam_gmapping/openslam_gmapping/particlefilter/particlefilter_test.cpp @@ -0,0 +1,98 @@ +#include +#include +#include +#include "particlefilter.h" + +using namespace std; + +#define test(s) {cout << s << " " << flush;} +#define testOk() {cout << "OK" << endl;} + +struct Particle{ + double p; + double w; + inline operator double() const {return w;} + inline void setWeight(double _w) {w=_w;} +}; + +ostream& printParticles(ostream& os, const vector& p) +{ + for (vector::const_iterator it=p.begin(); it!=p.end(); ++it) { + os << it->p<< " " << (double)*it << endl; + } + return os; +} + +struct EvolutionModel{ + Particle evolve(const Particle& p){ + Particle pn(p); + pn.p+=.5*(drand48()-.5); + return pn; + } +}; + +struct QualificationModel{ + Particle evolve(const Particle& p){ + return p; + } +}; + +struct LikelyhoodModel{ + double likelyhood(const Particle& p) const{ + double v = 1./(0.1+10*(p.p-2)*(p.p-2))+0.5/(0.1+10*(p.p-8)*(p.p-8)); + return v; + } +}; + +int main (unsigned int argc, const char * const * argv){ + int nparticles=100; + if (argc>1) + nparticles=atoi(argv[1]); + vector particles(nparticles); + LikelyhoodModel likelyhoodModel; + uniform_resampler resampler; + auxiliary_evolver auxevolver; + evolver evolver; + + for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ + it->w=1; + it->p=10*(drand48()); + } + + vector sirparticles(particles); + vector auxparticles(particles); + + /*sir step*/ + while (1){ + char buf[2]; + cin.getline(buf,2); + vector newgeneration; + + cout << "# SIR step" << endl; + evolver.evolve(sirparticles); + for (vector::iterator it=sirparticles.begin(); it!=sirparticles.end(); it++){ + it->setWeight(likelyhoodModel.likelyhood(*it)); + } + ofstream os("sir.dat"); + printParticles(os, sirparticles); + os.close(); + newgeneration=resampler.resample(sirparticles); + sirparticles=newgeneration; + + cout << "# AUX step" << endl; + auxevolver.evolve(auxparticles); + for (vector::iterator it=auxparticles.begin(); it!=auxparticles.end(); it++){ + it->setWeight(likelyhoodModel.likelyhood(*it)); + } + os.open("aux.dat"); + printParticles(os, auxparticles); + os.close(); + newgeneration=resampler.resample(auxparticles); + auxparticles=newgeneration; + cout << "plot [0:10][0:10]\"sir.dat\" w impulses" << endl; + cout << "replot 1./(0.1+10*(x-2)*(x-2))+0.5/(0.1+10*(x-8)*(x-8))" << endl; + +// cout << "replot \"aux.dat\" w p" << endl; + } +} + diff --git a/slam_gmapping/openslam_gmapping/particlefilter/pf.h b/slam_gmapping/openslam_gmapping/particlefilter/pf.h new file mode 100644 index 0000000..374b0d8 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/particlefilter/pf.h @@ -0,0 +1,175 @@ +#ifndef PARTICLEFILTER_H +#define PARTICLEFILTER_H +#include +#include +#include +#include + + + +/** +the particle class has to be convertible into numeric data type; +That means that a particle must define the Numeric conversion operator; + operator Numeric() const. +that returns the weight, and the method + setWeight(Numeric) +that sets the weight. + +*/ + +typedef std::pair UIntPair; + +template +double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){ + //determine the maximum + double lmax=-MAXDOUBLE; + for (Iterator it=begin; it!=end; it++){ + lmax=lmax>((double)(*it))? lmax: (double)(*it); + } + //convert to raw form + for (Iterator it=begin; it!=end; it++){ + *out=exp((double)(*it)-lmax); + out++; + } + return lmax; +} + +template +void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){ + //determine the maximum + for (Iterator it=begin; it!=end; it++){ + *out=log((Numeric)(*it))-lmax; + out++; + } + return lmax; +} + +template +void resample(std::vector& indexes, const WeightVector& weights, unsigned int nparticles=0){ + double cweight=0; + + //compute the cumulative weights + unsigned int n=0; + for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){ + cweight+=(double)*it; + n++; + } + + if (nparticles>0) + n=nparticles; + + //compute the interval + double interval=cweight/n; + + //compute the initial target weight + double target=interval*::drand48(); + //compute the resampled indexes + + cweight=0; + indexes.resize(n); + + n=0; + unsigned int i=0; + for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){ + cweight+=(double)* it; + while(cweight>target){ + indexes[n++]=i; + target+=interval; + } + } +} + +template +void normalizeWeights(WeightVector& weights, unsigned int size, double minWeight){ + double wmin=MAXDOUBLE; + double wmax=-MAXDOUBLE; + for (uint i=0; iweights[i]?wmax:weights[i]; + } + double min_normalized_value=log(minWeight); + double max_normalized_value=log(1.); + double dn=max_normalized_value-min_normalized_value; + double dw=wmax-wmin; + if (dw==0) dw=1; + double scale=dn/dw; + double offset=-wmax*scale; + for (uint i=0; i +void repeatIndexes(Vector& dest, const std::vector& indexes, const Vector& particles){ +/*<<<<<<< .mine + assert(indexes.size()==particles.size()); + if (dest.size()!=particles.size()) + dest.resize(particles.size()); +=======*/ + //assert(indexes.size()==particles.size()); //DIEGO non ne vedo il senso, anzi è sbagliata + //dest.resize(particles.size()); // è sbagliato anche questo + dest.resize(indexes.size()); +// >>>>>>> .r2534 + unsigned int i=0; + for (std::vector::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){ + dest[i]=particles[*it]; + i++; + } +} + +template +void repeatIndexes(Vector& dest, const std::vector& indexes2, const Vector& particles, const std::vector& indexes){ + // assert(indexes.size()==indexes2.size()); + dest=particles; + unsigned int i=0; + for (std::vector::const_iterator it=indexes2.begin(); it!=indexes2.end(); ++it){ + dest[indexes[i]]=particles[*it]; + i++; + } +} + + +template +double neff(const Iterator& begin, const Iterator& end){ + double sum=0; + for (Iterator it=begin; it!=end; ++it){ + sum+=*it; + } + double cum=0; + for (Iterator it=begin; it!=end; ++it){ + double w=*it/sum; + cum+=w*w; + } + return 1./cum; +} + + + +template +void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){ + unsigned int current=0; + unsigned int count=0; + for (Iterator it=begin; it!=end; it++){ + if (it==begin){ + current=*it; + count=1; + continue; + } + if (((uint)*it) ==current) + count++; + if (((uint)*it)!=current){ + *out=std::make_pair(current,count); + out++; + current=*it; + count=1; + } + } + if (count>0) + *out=std::make_pair(current,count); + out++; +} + +#endif + diff --git a/slam_gmapping/openslam_gmapping/particlefilter/range_bearing.cpp b/slam_gmapping/openslam_gmapping/particlefilter/range_bearing.cpp new file mode 100644 index 0000000..44f99e0 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/particlefilter/range_bearing.cpp @@ -0,0 +1,99 @@ +#include +#include +#include +#include +#include "particlefilter.h" + +using namespace std; +using namespace GMapping; + +#define test(s) {cout << s << " " << flush;} +#define testOk() {cout << "OK" << endl;} + +struct Particle{ + Particle(): p(0,0), w(0){} + Point p; + double w; + operator double() const {return w; } + void setWeight(double _w) {w=_w;} +}; + +ostream& printParticles(ostream& os, const vector& p) +{ + for (vector::const_iterator it=p.begin(); it!=p.end(); ++it) { + os << it->p.x << " " << it->p.y << endl; + } + return os; +} + +struct EvolutionModel{ + Particle evolve(const Particle& p){ + Particle pn(p); + pn.p.x+=10*(drand48()-.5); + pn.p.y+=10*(drand48()-.5); + return pn; + } +}; + + +struct LikelyhoodModel{ + std::vector observerVector; + std::vector observations; + double sigma; + double likelyhood(const Particle& p) const{ + double v=1; + std::vector::const_iterator oit=observations.begin(); + for (std::vector::const_iterator it=observerVector.begin(); it!=observerVector.end();it++){ + v*=exp(-pow(((p.p-*it)*(p.p-*it)-*oit*(*oit))/sigma, 2)); + oit++; + } + cout << "#v=" << v << endl; + return v; + } +}; + +int main (unsigned int argc, const char * const * argv){ + vector particles(1000); + LikelyhoodModel likelyhoodModel; + uniform_resampler resampler; + evolver evolver; + + for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ + it->w=1; + it->p.x=400*(drand48()-.5); + it->p.y=400*(drand48()-.5); + } + + vector sensors; + + sensors.push_back(Point(-50,0)); + sensors.push_back(Point(50,0)); + sensors.push_back(Point(0,100)); + + likelyhoodModel.sigma=1000; + likelyhoodModel.observations.push_back(70); + likelyhoodModel.observations.push_back(70); + likelyhoodModel.observations.push_back(70); + + likelyhoodModel.observerVector=sensors; + while (1){ + char buf[2]; + cin.getline(buf,2); + vector newgeneration; + + cout << "# SIR step" << endl; + evolver.evolve(particles); + for (vector::iterator it=particles.begin(); it!=particles.end(); it++){ + it->w*=likelyhoodModel.likelyhood(*it); + } + + ofstream os("sir.dat"); + printParticles(os, particles); + os.close(); + vector newpart=resampler.resample(particles); + particles=newpart; + + cout << "plot [-200:200][-200:200]\"sir.dat\" w p" << endl; + } +} + diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt b/slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt new file mode 100644 index 0000000..14993b4 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/scanmatcher/CMakeLists.txt @@ -0,0 +1,4 @@ +add_library(scanmatcher STATIC eig3.cpp scanmatcher.cpp scanmatcherprocessor.cpp smmap.cpp) +target_link_libraries(scanmatcher sensor_range utils) + +install(TARGETS scanmatcher DESTINATION lib) diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp b/slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp new file mode 100644 index 0000000..2c9e409 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/scanmatcher/eig3.cpp @@ -0,0 +1,270 @@ + +/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public + domain Java Matrix library JAMA. */ + +#include + +#ifndef MAX +#define MAX(a, b) ((a)>(b)?(a):(b)) +#endif + +#define n 3 + +static double hypot2(double x, double y) { + return sqrt(x*x+y*y); +} + +// Symmetric Householder reduction to tridiagonal form. + +static void tred2(double V[n][n], double d[n], double e[n]) { + +// This is derived from the Algol procedures tred2 by +// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for +// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding +// Fortran subroutine in EISPACK. + + int i,j,k; + double f,g,h,hh; + for (j = 0; j < n; j++) { + d[j] = V[n-1][j]; + } + + // Householder reduction to tridiagonal form. + + for (i = n-1; i > 0; i--) { + + // Scale to avoid under/overflow. + + double scale = 0.0; + double h = 0.0; + for (k = 0; k < i; k++) { + scale = scale + fabs(d[k]); + } + if (scale == 0.0) { + e[i] = d[i-1]; + for (j = 0; j < i; j++) { + d[j] = V[i-1][j]; + V[i][j] = 0.0; + V[j][i] = 0.0; + } + } else { + + // Generate Householder vector. + + for (k = 0; k < i; k++) { + d[k] /= scale; + h += d[k] * d[k]; + } + f = d[i-1]; + g = sqrt(h); + if (f > 0) { + g = -g; + } + e[i] = scale * g; + h = h - f * g; + d[i-1] = f - g; + for (j = 0; j < i; j++) { + e[j] = 0.0; + } + + // Apply similarity transformation to remaining columns. + + for (j = 0; j < i; j++) { + f = d[j]; + V[j][i] = f; + g = e[j] + V[j][j] * f; + for (k = j+1; k <= i-1; k++) { + g += V[k][j] * d[k]; + e[k] += V[k][j] * f; + } + e[j] = g; + } + f = 0.0; + for (j = 0; j < i; j++) { + e[j] /= h; + f += e[j] * d[j]; + } + hh = f / (h + h); + for (j = 0; j < i; j++) { + e[j] -= hh * d[j]; + } + for (j = 0; j < i; j++) { + f = d[j]; + g = e[j]; + for (k = j; k <= i-1; k++) { + V[k][j] -= (f * e[k] + g * d[k]); + } + d[j] = V[i-1][j]; + V[i][j] = 0.0; + } + } + d[i] = h; + } + + // Accumulate transformations. + + for (i = 0; i < n-1; i++) { + V[n-1][i] = V[i][i]; + V[i][i] = 1.0; + h = d[i+1]; + if (h != 0.0) { + for (k = 0; k <= i; k++) { + d[k] = V[k][i+1] / h; + } + for (j = 0; j <= i; j++) { + g = 0.0; + for (k = 0; k <= i; k++) { + g += V[k][i+1] * V[k][j]; + } + for (k = 0; k <= i; k++) { + V[k][j] -= g * d[k]; + } + } + } + for (k = 0; k <= i; k++) { + V[k][i+1] = 0.0; + } + } + for (j = 0; j < n; j++) { + d[j] = V[n-1][j]; + V[n-1][j] = 0.0; + } + V[n-1][n-1] = 1.0; + e[0] = 0.0; +} + +// Symmetric tridiagonal QL algorithm. + +static void tql2(double V[n][n], double d[n], double e[n]) { + +// This is derived from the Algol procedures tql2, by +// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for +// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding +// Fortran subroutine in EISPACK. + + int i,j,m,l,k; + double g,p,r,dl1,h,f,tst1,eps; + double c,c2,c3,el1,s,s2; + + for (i = 1; i < n; i++) { + e[i-1] = e[i]; + } + e[n-1] = 0.0; + + f = 0.0; + tst1 = 0.0; + eps = pow(2.0,-52.0); + for (l = 0; l < n; l++) { + + // Find small subdiagonal element + + tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l])); + m = l; + while (m < n) { + if (fabs(e[m]) <= eps*tst1) { + break; + } + m++; + } + + // If m == l, d[l] is an eigenvalue, + // otherwise, iterate. + + if (m > l) { + int iter = 0; + do { + iter = iter + 1; // (Could check iteration count here.) + + // Compute implicit shift + + g = d[l]; + p = (d[l+1] - g) / (2.0 * e[l]); + r = hypot2(p,1.0); + if (p < 0) { + r = -r; + } + d[l] = e[l] / (p + r); + d[l+1] = e[l] * (p + r); + dl1 = d[l+1]; + h = g - d[l]; + for (i = l+2; i < n; i++) { + d[i] -= h; + } + f = f + h; + + // Implicit QL transformation. + + p = d[m]; + c = 1.0; + c2 = c; + c3 = c; + el1 = e[l+1]; + s = 0.0; + s2 = 0.0; + for (i = m-1; i >= l; i--) { + c3 = c2; + c2 = c; + s2 = s; + g = c * e[i]; + h = c * p; + r = hypot2(p,e[i]); + e[i+1] = s * r; + s = e[i] / r; + c = p / r; + p = c * d[i] - s * g; + d[i+1] = h + s * (c * g + s * d[i]); + + // Accumulate transformation. + + for (k = 0; k < n; k++) { + h = V[k][i+1]; + V[k][i+1] = s * V[k][i] + c * h; + V[k][i] = c * V[k][i] - s * h; + } + } + p = -s * s2 * c3 * el1 * e[l] / dl1; + e[l] = s * p; + d[l] = c * p; + + // Check for convergence. + + } while (fabs(e[l]) > eps*tst1); + } + d[l] = d[l] + f; + e[l] = 0.0; + } + + // Sort eigenvalues and corresponding vectors. + + for (i = 0; i < n-1; i++) { + k = i; + p = d[i]; + for (j = i+1; j < n; j++) { + if (d[j] < p) { + k = j; + p = d[j]; + } + } + if (k != i) { + d[k] = d[i]; + d[i] = p; + for (j = 0; j < n; j++) { + p = V[j][i]; + V[j][i] = V[j][k]; + V[j][k] = p; + } + } + } +} + +void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) { + int i,j; + double e[n]; + for (i = 0; i < n; i++) { + for (j = 0; j < n; j++) { + V[i][j] = A[i][j]; + } + } + tred2(V, d, e); + tql2(V, d, e); +} diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/eig3.h b/slam_gmapping/openslam_gmapping/scanmatcher/eig3.h new file mode 100644 index 0000000..b708cb1 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/scanmatcher/eig3.h @@ -0,0 +1,11 @@ + +/* Eigen-decomposition for symmetric 3x3 real matrices. + Public domain, copied from the public domain Java library JAMA. */ + +#ifndef _eig_h + +/* Symmetric matrix A => eigenvectors in columns of V, corresponding + eigenvalues in d. */ +void eigen_decomposition(double A[3][3], double V[3][3], double d[3]); + +#endif diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/gridlinetraversal.h b/slam_gmapping/openslam_gmapping/scanmatcher/gridlinetraversal.h new file mode 100644 index 0000000..2eb34a8 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/scanmatcher/gridlinetraversal.h @@ -0,0 +1,128 @@ +#ifndef GRIDLINETRAVERSAL_H +#define GRIDLINETRAVERSAL_H + +#include +#include + +namespace GMapping { + +typedef struct { + int num_points; + IntPoint* points; +} GridLineTraversalLine; + +struct GridLineTraversal { + inline static void gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ; + inline static void gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) ; + +}; + +void GridLineTraversal::gridLineCore( IntPoint start, IntPoint end, GridLineTraversalLine *line ) +{ + int dx, dy, incr1, incr2, d, x, y, xend, yend, xdirflag, ydirflag; + int cnt = 0; + + dx = abs(end.x-start.x); dy = abs(end.y-start.y); + + if (dy <= dx) { + d = 2*dy - dx; incr1 = 2 * dy; incr2 = 2 * (dy - dx); + if (start.x > end.x) { + x = end.x; y = end.y; + ydirflag = (-1); + xend = start.x; + } else { + x = start.x; y = start.y; + ydirflag = 1; + xend = end.x; + } + line->points[cnt].x=x; + line->points[cnt].y=y; + cnt++; + if (((end.y - start.y) * ydirflag) > 0) { + while (x < xend) { + x++; + if (d <0) { + d+=incr1; + } else { + y++; d+=incr2; + } + line->points[cnt].x=x; + line->points[cnt].y=y; + cnt++; + } + } else { + while (x < xend) { + x++; + if (d <0) { + d+=incr1; + } else { + y--; d+=incr2; + } + line->points[cnt].x=x; + line->points[cnt].y=y; + cnt++; + } + } + } else { + d = 2*dx - dy; + incr1 = 2*dx; incr2 = 2 * (dx - dy); + if (start.y > end.y) { + y = end.y; x = end.x; + yend = start.y; + xdirflag = (-1); + } else { + y = start.y; x = start.x; + yend = end.y; + xdirflag = 1; + } + line->points[cnt].x=x; + line->points[cnt].y=y; + cnt++; + if (((end.x - start.x) * xdirflag) > 0) { + while (y < yend) { + y++; + if (d <0) { + d+=incr1; + } else { + x++; d+=incr2; + } + line->points[cnt].x=x; + line->points[cnt].y=y; + cnt++; + } + } else { + while (y < yend) { + y++; + if (d <0) { + d+=incr1; + } else { + x--; d+=incr2; + } + line->points[cnt].x=x; + line->points[cnt].y=y; + cnt++; + } + } + } + line->num_points = cnt; +} + +void GridLineTraversal::gridLine( IntPoint start, IntPoint end, GridLineTraversalLine *line ) { + int i,j; + int half; + IntPoint v; + gridLineCore( start, end, line ); + if ( start.x!=line->points[0].x || + start.y!=line->points[0].y ) { + half = line->num_points/2; + for (i=0,j=line->num_points - 1;ipoints[i]; + line->points[i] = line->points[j]; + line->points[j] = v; + } + } +} + +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/icptest.cpp b/slam_gmapping/openslam_gmapping/scanmatcher/icptest.cpp new file mode 100644 index 0000000..41c240f --- /dev/null +++ b/slam_gmapping/openslam_gmapping/scanmatcher/icptest.cpp @@ -0,0 +1,91 @@ +#include +#include +#include + +#include +#include + +using namespace GMapping; +using namespace std; + +typedef std::list PointPairList; + +PointPairList generateRandomPointPairs(int size, OrientedPoint t, double noise=0.){ + PointPairList ppl; + double s=sin(t.theta), c=cos(t.theta); + for (int i=0; i> size >> t.x >> t.y >> t.theta; + PointPairList ppl=generateRandomPointPairs(size, t, 3); + OrientedPoint tc; + OrientedPoint ttot(0.,0.,0.); + bool method=true; + while(1){ + char buf[10]; + cerr << "iterate?" << endl; + cin.getline(buf,10); + if (buf[0]=='n') + method=false; + else if (buf[0]=='l') + method=true; + else if (buf[0]!=char(0)) + break; + cout << "plot '-' w l, '-' w p, '-' w p" << endl; + for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ + cout << it->first.x << " " << it->first.y<< endl; + cout << it->second.x << " " << it->second.y<< endl; + cout << endl; + } + cout << "e" << endl; + for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ + cout << it->first.x << " " << it->first.y<< endl; + } + cout << "e" << endl; + for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ + cout << it->second.x << " " << it->second.y<< endl; + } + cout << "e" << endl; + + double error; + if (!method){ + cerr << "Nonlinear Optimization" << endl; + error=icpNonlinearStep(tc,ppl); + }else { + cerr << "Linear Optimization" << endl; + error=icpStep(tc,ppl); + } + cerr << "ICP err=" << error << " t.x=" << tc.x << " t.y=" << tc.y << " t.theta=" << tc.theta << endl; + cerr << "\t" << error << " ttot.x=" << ttot.x << " ttot.y=" << ttot.y << " ttot.theta=" << ttot.theta << endl; + double s=sin(tc.theta), c=cos(tc.theta); + for(PointPairList::iterator it=ppl.begin(); it!=ppl.end(); it++){ + Point p1(c*it->first.x-s*it->first.y+tc.x, + s*it->first.x+c*it->first.y+tc.y); + it->first=p1; + } + ttot.x+=tc.x; + ttot.y+=tc.y; + ttot.theta+=tc.theta; + ttot.theta=atan2(sin(ttot.theta), cos(ttot.theta)); + } + } + return 0; +} diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/lumiles.h b/slam_gmapping/openslam_gmapping/scanmatcher/lumiles.h new file mode 100644 index 0000000..151dd81 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/scanmatcher/lumiles.h @@ -0,0 +1,50 @@ +#ifndef LUMILESPROCESSOR +#define LUMILESPROCESSOR + +namespace GMapping{ + +class LuMilesProcessor{ + typedef std:vector PointVector; + static OrientedPoint step(const PointVector& src, const PointVector& dest); +}; + +OrientedPoint LuMilesProcessors::step(const PointVector& src, const PointVector& dest){ + assert(src.size()==dest.size()); + unsigned int size=dest.size(); + double smx=0, smy=0, dmx=0, dmy=0; + for (PointVector::const_iterator it=src.begin(); it!=src.end(); it++){ + smx+=it->x; + smy+=it->y; + } + smx/=src.size(); + smy/=src.size(); + + for (PointVector::const_iterator it=dest.begin(); it!=dest.end(); it++){ + dmx+=it->x; + dmy+=it->y; + } + dmx/=src.size(); + dmy/=src.size(); + + double sxx=0, sxy=0; + double syx=0, syy=0; + for (unsigned int i=0; i +#include +#include +#include +#include +#include +#include +#include +#include "scanmatcherprocessor.h" + +using namespace std; +using namespace GMapping; + +#define DEBUG cout << __PRETTY_FUNCTION__ +#define MAX_STRING_LENGTH 1024 + +int main(int argc, const char * const * argv){ + string filename; + string outfilename; + double xmin=-100.; + double ymin=-100.; + double xmax=100.; + double ymax=100.; + double delta=1.; + double patchDelta=0.1; + double sigma=0.02; + double maxrange=81.9; + double maxUrange=81.9; + double regscore=1e4; + double lstep=.05; + double astep=.05; + int kernelSize=0; + int iterations=4; + double critscore=0.; + double maxMove=1.; + bool computeCovariance=false; + bool readFromStdin=false; + bool useICP=false; + double laserx=.0,lasery=.0,lasertheta=.0; +// bool headingOnly=false; + + + if (argc<2){ + cout << "usage main {arglist}" << endl; + cout << "where the arguments are: " << endl; + cout << "\t -xmin " << endl; + cout << "\t -xmax " << endl; + cout << "\t -ymin " << endl; + cout << "\t -ymax " << endl; + cout << "\t -maxrange : maxmimum preception range" << endl; + cout << "\t -delta : patch size" << endl; + cout << "\t -patchDelta : patch cell size" << endl; + cout << "\t -lstep : linear serach step" << endl; + cout << "\t -astep : ìangular search step" << endl; + cout << "\t -regscore : registration scan score" << endl; + cout << "\t -filename : log filename in carmen format" << endl; + cout << "\t -sigma : convolution kernel size" << endl; + cout << "Look the code for discovering another thousand of unuseful parameters" << endl; + return -1; + } + + CMD_PARSE_BEGIN(1,argc); + parseString("-filename",filename); + parseString("-outfilename",outfilename); + parseDouble("-xmin",xmin); + parseDouble("-xmax",xmax); + parseDouble("-ymin",ymin); + parseDouble("-ymax",ymax); + parseDouble("-delta",delta); + parseDouble("-patchDelta",patchDelta); + parseDouble("-maxrange",maxrange); + parseDouble("-maxUrange",maxUrange); + parseDouble("-regscore",regscore); + parseDouble("-critscore",critscore); + parseInt("-kernelSize",kernelSize); + parseDouble("-sigma",sigma); + parseInt("-iterations",iterations); + parseDouble("-lstep",lstep); + parseDouble("-astep",astep); + parseDouble("-maxMove",maxMove); + parseFlag("-computeCovariance",computeCovariance); + parseFlag("-stdin", readFromStdin); + parseFlag("-useICP", useICP); + parseDouble("-laserx",laserx); + parseDouble("-lasery",lasery); + parseDouble("-lasertheta",lasertheta); + CMD_PARSE_END; + + if (!filename.size()){ + cout << "no filename specified" << endl; + return -1; + } + + ifstream is; + is.open(filename.c_str()); + if (! is){ + cout << "no file found" << endl; + return -1; + } + + + DEBUG << "scanmatcher processor construction" << endl; + ScanMatcherProcessor scanmatcher(xmin, ymin, xmax, ymax, delta, patchDelta); + + //double range, double sigma, int kernsize, double lopt, double aopt, int iterations + scanmatcher.setMatchingParameters(maxUrange, maxrange, sigma, kernelSize, lstep, astep, iterations, computeCovariance); + scanmatcher.setRegistrationParameters(regscore, critscore); + scanmatcher.setmaxMove(maxMove); + scanmatcher.useICP=useICP; + scanmatcher.matcher().setlaserPose(OrientedPoint(laserx,lasery,lasertheta)); + + CarmenConfiguration conf; + conf.load(is); + is.close(); + + SensorMap sensorMap=conf.computeSensorMap(); + scanmatcher.setSensorMap(sensorMap); + + InputSensorStream* input=0; + + ifstream plainStream; + if (! readFromStdin){ + plainStream.open(filename.c_str()); + input=new InputSensorStream(sensorMap, plainStream); + cout << "Plain Stream opened="<< (bool) plainStream << endl; + } else { + input=new InputSensorStream(sensorMap, cin); + cout << "Plain Stream opened on stdin" << endl; + } + +/* + SensorLog log(sensorMap); + ifstream logstream(filename); + log.load(logstream); + logstream.close(); + cout << "Log loaded " << log.size() << " records" << endl; +*/ + ostream* output; + ofstream poseStream; + if (! readFromStdin){ + if (! outfilename.size()){ + outfilename=string("scanmatched")+filename; + } + poseStream.open(outfilename.c_str()); + output=&poseStream; + } else { + output=&cout; + } + scanmatcher.init(); + ofstream odopathStream("odopath.dat"); + while (*input){ + const SensorReading* r; + (*input) >> r; + if (! r) + continue; + const RangeReading* rr=dynamic_cast(r); + if (rr){ + const RangeSensor* s=dynamic_cast(r->getSensor()); + bool isFront= s->getPose().theta==0; + + if (! readFromStdin){ + cout << "." << flush; + } + const RangeSensor* rs=dynamic_cast(rr->getSensor()); + assert (rs && rs->beams().size()==rr->size()); + odopathStream << rr->getPose().x << " " << rr->getPose().y << endl; + scanmatcher.processScan(*rr); + OrientedPoint p=scanmatcher.getPose(); + if (isFront) + *output << "FLASER "<< rr->size() << " "; + else + *output << "RLASER "<< rr->size() << " "; + for (RangeReading::const_iterator b=rr->begin(); b!=rr->end(); b++){ + *output << *b << " "; + } + *output << p.x << " " << p.y << " " << p.theta << " "; + //p=rr->getPose(); + double t=rr->getTime(); //FIXME + *output << p.x << " " << p.y << " " << p.theta << " "; + *output << t << " nohost " << t << endl; + } + } + if (! readFromStdin){ + poseStream.close(); + } +} diff --git a/slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.cpp b/slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.cpp new file mode 100644 index 0000000..8f2c3ac --- /dev/null +++ b/slam_gmapping/openslam_gmapping/scanmatcher/scanmatcher.cpp @@ -0,0 +1,725 @@ +#include +#include +#include +#include + +#include +#include "gridlinetraversal.h" +//#define GENERATE_MAPS + +namespace GMapping { + +using namespace std; + +const double ScanMatcher::nullLikelihood=-.5; + +ScanMatcher::ScanMatcher(): m_laserPose(0,0,0){ + //m_laserAngles=0; + m_laserBeams=0; + m_optRecursiveIterations=3; + m_activeAreaComputed=false; + + // This are the dafault settings for a grid map of 5 cm + m_llsamplerange=0.01; + m_llsamplestep=0.01; + m_lasamplerange=0.005; + m_lasamplestep=0.005; + m_enlargeStep=10.; + m_fullnessThreshold=0.1; + m_angularOdometryReliability=0.; + m_linearOdometryReliability=0.; + m_freeCellRatio=sqrt(2.); + m_initialBeamsSkip=0; + +/* + // This are the dafault settings for a grid map of 10 cm + m_llsamplerange=0.1; + m_llsamplestep=0.1; + m_lasamplerange=0.02; + m_lasamplestep=0.01; +*/ + // This are the dafault settings for a grid map of 20/25 cm +/* + m_llsamplerange=0.2; + m_llsamplestep=0.1; + m_lasamplerange=0.02; + m_lasamplestep=0.01; + m_generateMap=false; +*/ + + m_linePoints = new IntPoint[20000]; +} + +ScanMatcher::~ScanMatcher(){ + delete [] m_linePoints; +} + +void ScanMatcher::invalidateActiveArea(){ + m_activeAreaComputed=false; +} + +/* +void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ + if (m_activeAreaComputed) + return; + HierarchicalArray2D::PointSet activeArea; + OrientedPoint lp=p; + lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; + lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; + lp.theta+=m_laserPose.theta; + IntPoint p0=map.world2map(lp); + const double * angle=m_laserAngles; + for (const double* r=readings; rm_laserMaxRange) + continue; + if (d>m_usableRange) + d=m_usableRange; + + Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + IntPoint p1=map.world2map(phit); + + d+=map.getDelta(); + //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + //IntPoint p2=map.world2map(phit2); + IntPoint linePoints[20000] ; + GridLineTraversalLine line; + line.points=linePoints; + //GridLineTraversal::gridLine(p0, p2, &line); + GridLineTraversal::gridLine(p0, p1, &line); + for (int i=0; im_laserMaxRange||*r>m_usableRange) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + IntPoint p1=map.world2map(phit); + assert(p1.x>=0 && p1.y>=0); + IntPoint cp=map.storage().patchIndexes(p1); + assert(cp.x>=0 && cp.y>=0); + activeArea.insert(cp); + + } + //this allocates the unallocated cells in the active area of the map + //cout << "activeArea::size() " << activeArea.size() << endl; + map.storage().setActiveArea(activeArea, true); + m_activeAreaComputed=true; +} +*/ +void ScanMatcher::computeActiveArea(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ + if (m_activeAreaComputed) + return; + OrientedPoint lp=p; + lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; + lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; + lp.theta+=m_laserPose.theta; + IntPoint p0=map.world2map(lp); + + Point min(map.map2world(0,0)); + Point max(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1)); + + if (lp.xmax.x) max.x=lp.x; + if (lp.y>max.y) max.y=lp.y; + + /*determine the size of the area*/ + const double * angle=m_laserAngles+m_initialBeamsSkip; + for (const double* r=readings+m_initialBeamsSkip; rm_laserMaxRange||*r==0.0||isnan(*r)) continue; + double d=*r>m_usableRange?m_usableRange:*r; + Point phit=lp; + phit.x+=d*cos(lp.theta+*angle); + phit.y+=d*sin(lp.theta+*angle); + if (phit.xmax.x) max.x=phit.x; + if (phit.y>max.y) max.y=phit.y; + } + //min=min-Point(map.getDelta(),map.getDelta()); + //max=max+Point(map.getDelta(),map.getDelta()); + + if ( !map.isInside(min) || !map.isInside(max)){ + Point lmin(map.map2world(0,0)); + Point lmax(map.map2world(map.getMapSizeX()-1,map.getMapSizeY()-1)); + //cerr << "CURRENT MAP " << lmin.x << " " << lmin.y << " " << lmax.x << " " << lmax.y << endl; + //cerr << "BOUNDARY OVERRIDE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl; + min.x=( min.x >= lmin.x )? lmin.x: min.x-m_enlargeStep; + max.x=( max.x <= lmax.x )? lmax.x: max.x+m_enlargeStep; + min.y=( min.y >= lmin.y )? lmin.y: min.y-m_enlargeStep; + max.y=( max.y <= lmax.y )? lmax.y: max.y+m_enlargeStep; + map.resize(min.x, min.y, max.x, max.y); + //cerr << "RESIZE " << min.x << " " << min.y << " " << max.x << " " << max.y << endl; + } + + HierarchicalArray2D::PointSet activeArea; + /*allocate the active area*/ + angle=m_laserAngles+m_initialBeamsSkip; + for (const double* r=readings+m_initialBeamsSkip; rm_laserMaxRange||d==0.0||isnan(d)) + continue; + if (d>m_usableRange) + d=m_usableRange; + Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + IntPoint p0=map.world2map(lp); + IntPoint p1=map.world2map(phit); + + //IntPoint linePoints[20000] ; + GridLineTraversalLine line; + line.points=m_linePoints; + GridLineTraversal::gridLine(p0, p1, &line); + for (int i=0; i=0 && m_linePoints[i].y>=0); + } + if (d=0 && cp.y>=0); + activeArea.insert(cp); + } + } else { + if (*r>m_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + IntPoint p1=map.world2map(phit); + assert(p1.x>=0 && p1.y>=0); + IntPoint cp=map.storage().patchIndexes(p1); + assert(cp.x>=0 && cp.y>=0); + activeArea.insert(cp); + } + + //this allocates the unallocated cells in the active area of the map + //cout << "activeArea::size() " << activeArea.size() << endl; +/* + cerr << "ActiveArea="; + for (HierarchicalArray2D::PointSet::const_iterator it=activeArea.begin(); it!= activeArea.end(); it++){ + cerr << "(" << it->x <<"," << it->y << ") "; + } + cerr << endl; +*/ + map.storage().setActiveArea(activeArea, true); + m_activeAreaComputed=true; +} + +double ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ + if (!m_activeAreaComputed) + computeActiveArea(map, p, readings); + + //this operation replicates the cells that will be changed in the registration operation + map.storage().allocActiveArea(); + + OrientedPoint lp=p; + lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; + lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; + lp.theta+=m_laserPose.theta; + IntPoint p0=map.world2map(lp); + + + const double * angle=m_laserAngles+m_initialBeamsSkip; + double esum=0; + for (const double* r=readings+m_initialBeamsSkip; rm_laserMaxRange||d==0.0||isnan(d)) + continue; + if (d>m_usableRange) + d=m_usableRange; + Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + IntPoint p1=map.world2map(phit); + //IntPoint linePoints[20000] ; + GridLineTraversalLine line; + line.points=m_linePoints; + GridLineTraversal::gridLine(p0, p1, &line); + for (int i=0; im_laserMaxRange||*r>m_usableRange||*r==0.0||isnan(*r)) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + IntPoint p1=map.world2map(phit); + assert(p1.x>=0 && p1.y>=0); + map.cell(p1).update(true,phit); + } + //cout << "informationGain=" << -esum << endl; + return esum; +} + +/* +void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ + if (!m_activeAreaComputed) + computeActiveArea(map, p, readings); + + //this operation replicates the cells that will be changed in the registration operation + map.storage().allocActiveArea(); + + OrientedPoint lp=p; + lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; + lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; + lp.theta+=m_laserPose.theta; + IntPoint p0=map.world2map(lp); + const double * angle=m_laserAngles; + for (const double* r=readings; rm_laserMaxRange) + continue; + if (d>m_usableRange) + d=m_usableRange; + Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + IntPoint p1=map.world2map(phit); + + IntPoint linePoints[20000] ; + GridLineTraversalLine line; + line.points=linePoints; + GridLineTraversal::gridLine(p0, p1, &line); + for (int i=0; im_laserMaxRange||*r>m_usableRange) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + map.cell(phit).update(true,phit); + } +} + +*/ + +double ScanMatcher::icpOptimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{ + double currentScore; + double sc=score(map, init, readings);; + OrientedPoint start=init; + pnew=init; + int iterations=0; + do{ + currentScore=sc; + sc=icpStep(pnew, map, start, readings); + //cerr << "pstart=" << start.x << " " <currentScore); + cerr << "i="<< iterations << endl; + return currentScore; +} + +double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{ + double bestScore=-1; + OrientedPoint currentPose=init; + double currentScore=score(map, currentPose, readings); + double adelta=m_optAngularDelta, ldelta=m_optLinearDelta; + unsigned int refinement=0; + enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done}; +/* cout << __PRETTY_FUNCTION__<< " readings: "; + for (int i=0; i=currentScore){ + refinement++; + adelta*=.5; + ldelta*=.5; + } + bestScore=currentScore; +// cout <<"score="<< currentScore << " refinement=" << refinement; +// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl; + OrientedPoint bestLocalPose=currentPose; + OrientedPoint localPose=currentPose; + + Move move=Front; + do { + localPose=currentPose; + switch(move){ + case Front: + localPose.x+=ldelta; + move=Back; + break; + case Back: + localPose.x-=ldelta; + move=Left; + break; + case Left: + localPose.y-=ldelta; + move=Right; + break; + case Right: + localPose.y+=ldelta; + move=TurnLeft; + break; + case TurnLeft: + localPose.theta+=adelta; + move=TurnRight; + break; + case TurnRight: + localPose.theta-=adelta; + move=Done; + break; + default:; + } + + double odo_gain=1; + if (m_angularOdometryReliability>0.){ + double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth; + odo_gain*=exp(-m_angularOdometryReliability*dth); + } + if (m_linearOdometryReliability>0.){ + double dx=init.x-localPose.x; + double dy=init.y-localPose.y; + double drho=dx*dx+dy*dy; + odo_gain*=exp(-m_linearOdometryReliability*drho); + } + double localScore=odo_gain*score(map, localPose, readings); + + if (localScore>currentScore){ + currentScore=localScore; + bestLocalPose=localPose; + } + c_iterations++; + } while(move!=Done); + currentPose=bestLocalPose; +// cout << "currentScore=" << currentScore<< endl; + //here we look for the best move; + }while (currentScore>bestScore || refinement0.){ + double dth=init.theta-localPose.theta; dth=atan2(sin(dth), cos(dth)); dth*=dth; + odo_gain*=exp(-m_angularOdometryReliability*dth); + } + if (m_linearOdometryReliability>0.){ + double dx=init.x-localPose.x; + double dy=init.y-localPose.y; + double drho=dx*dx+dy*dy; + odo_gain*=exp(-m_linearOdometryReliability*drho); + } + localScore=odo_gain*score(map, localPose, readings); + //update the score + count++; + matched=likelihoodAndScore(localScore, localLikelihood, map, localPose, readings); + if (localScore>currentScore){ + currentScore=localScore; + bestLocalPose=localPose; + } + sm.score=localScore; + sm.likelihood=localLikelihood;//+log(odo_gain); + sm.pose=localPose; + moveList.push_back(sm); + //update the move list + } while(move!=Done); + currentPose=bestLocalPose; + //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl; + //here we look for the best move; + }while (currentScore>bestScore || refinement::PointSet activeArea; + OrientedPoint lp=p; + lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; + lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; + lp.theta+=m_laserPose.theta; + IntPoint p0=map.world2map(lp); + const double * angle=m_laserAngles; + for (const double* r=readings; rm_laserMaxRange) + continue; + if (d>m_usableRange) + d=m_usableRange; + + Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + IntPoint p1=map.world2map(phit); + + d+=map.getDelta(); + //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + //IntPoint p2=map.world2map(phit2); + IntPoint linePoints[20000] ; + GridLineTraversalLine line; + line.points=linePoints; + //GridLineTraversal::gridLine(p0, p2, &line); + GridLineTraversal::gridLine(p0, p1, &line); + for (int i=0; im_laserMaxRange||*r>m_usableRange) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + IntPoint p1=map.world2map(phit); + assert(p1.x>=0 && p1.y>=0); + IntPoint cp=map.storage().patchIndexes(p1); + assert(cp.x>=0 && cp.y>=0); + activeArea.insert(cp); + + } + //this allocates the unallocated cells in the active area of the map + //cout << "activeArea::size() " << activeArea.size() << endl; + map.storage().setActiveArea(activeArea, true); + m_activeAreaComputed=true; +} + +void ScanMatcher::registerScan(ScanMatcherMap& map, const OrientedPoint& p, const double* readings){ + if (!m_activeAreaComputed) + computeActiveArea(map, p, readings); + + //this operation replicates the cells that will be changed in the registration operation + map.storage().allocActiveArea(); + + OrientedPoint lp=p; + lp.x+=cos(p.theta)*m_laserPose.x-sin(p.theta)*m_laserPose.y; + lp.y+=sin(p.theta)*m_laserPose.x+cos(p.theta)*m_laserPose.y; + lp.theta+=m_laserPose.theta; + IntPoint p0=map.world2map(lp); + const double * angle=m_laserAngles; + for (const double* r=readings; rm_laserMaxRange) + continue; + if (d>m_usableRange) + d=m_usableRange; + Point phit=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + IntPoint p1=map.world2map(phit); + + d+=map.getDelta(); + //Point phit2=lp+Point(d*cos(lp.theta+*angle),d*sin(lp.theta+*angle)); + //IntPoint p2=map.world2map(phit2); + IntPoint linePoints[20000] ; + GridLineTraversalLine line; + line.points=linePoints; + //GridLineTraversal::gridLine(p0, p2, &line); + GridLineTraversal::gridLine(p0, p1, &line); + for (int i=0; im_laserMaxRange||*r>m_usableRange) continue; + Point phit=lp; + phit.x+=*r*cos(lp.theta+*angle); + phit.y+=*r*sin(lp.theta+*angle); + map.cell(phit).update(true,phit); + } +} + + + +double ScanMatcher::optimize(OrientedPoint& pnew, const ScanMatcherMap& map, const OrientedPoint& init, const double* readings) const{ + double bestScore=-1; + OrientedPoint currentPose=init; + double currentScore=score(map, currentPose, readings); + double adelta=m_optAngularDelta, ldelta=m_optLinearDelta; + unsigned int refinement=0; + enum Move{Front, Back, Left, Right, TurnLeft, TurnRight, Done}; + int c_iterations=0; + do{ + if (bestScore>=currentScore){ + refinement++; + adelta*=.5; + ldelta*=.5; + } + bestScore=currentScore; +// cout <<"score="<< currentScore << " refinement=" << refinement; +// cout << "pose=" << currentPose.x << " " << currentPose.y << " " << currentPose.theta << endl; + OrientedPoint bestLocalPose=currentPose; + OrientedPoint localPose=currentPose; + + Move move=Front; + do { + localPose=currentPose; + switch(move){ + case Front: + localPose.x+=ldelta; + move=Back; + break; + case Back: + localPose.x-=ldelta; + move=Left; + break; + case Left: + localPose.y-=ldelta; + move=Right; + break; + case Right: + localPose.y+=ldelta; + move=TurnLeft; + break; + case TurnLeft: + localPose.theta+=adelta; + move=TurnRight; + break; + case TurnRight: + localPose.theta-=adelta; + move=Done; + break; + default:; + } + double localScore=score(map, localPose, readings); + if (localScore>currentScore){ + currentScore=localScore; + bestLocalPose=localPose; + } + c_iterations++; + } while(move!=Done); + currentPose=bestLocalPose; + //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl; + //here we look for the best move; + }while (currentScore>bestScore || refinementcurrentScore){ + currentScore=localScore; + bestLocalPose=localPose; + } + sm.score=localScore; + sm.likelihood=localLikelihood; + sm.pose=localPose; + moveList.push_back(sm); + //update the move list + } while(move!=Done); + currentPose=bestLocalPose; + //cout << __PRETTY_FUNCTION__ << "currentScore=" << currentScore<< endl; + //here we look for the best move; + }while (currentScore>bestScore || refinement((laser_it->second)); + assert(rangeSensor && rangeSensor->beams().size()); + + m_beams=static_cast(rangeSensor->beams().size()); + double* angles=new double[rangeSensor->beams().size()]; + for (unsigned int i=0; ibeams()[i].pose.theta; + } + m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose()); + delete [] angles; + + +} + +void ScanMatcherProcessor::init(){ + m_first=true; + m_pose=OrientedPoint(0,0,0); + m_count=0; +} + +void ScanMatcherProcessor::processScan(const RangeReading & reading){ + /**retireve the position from the reading, and compute the odometry*/ + OrientedPoint relPose=reading.getPose(); + if (!m_count){ + m_odoPose=relPose; + } + + //compute the move in the scan m_matcher + //reference frame + + OrientedPoint move=relPose-m_odoPose; + + double dth=m_odoPose.theta-m_pose.theta; + // cout << "rel-move x="<< move.x << " y=" << move.y << " theta=" << move.theta << endl; + + double lin_move=move*move; + if (lin_move>m_maxMove){ + cerr << "Too big jump in the log file: " << lin_move << endl; + cerr << "relPose=" << relPose.x << " " < + +namespace GMapping { + +const PointAccumulator& PointAccumulator::Unknown(){ + if (! unknown_ptr) + unknown_ptr=new PointAccumulator; + return *unknown_ptr; +} + +PointAccumulator* PointAccumulator::unknown_ptr=0; + +}; + + diff --git a/slam_gmapping/openslam_gmapping/sensor/CMakeLists.txt b/slam_gmapping/openslam_gmapping/sensor/CMakeLists.txt new file mode 100644 index 0000000..93ddf7a --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/CMakeLists.txt @@ -0,0 +1,8 @@ +add_subdirectory(sensor_base) +ament_export_libraries(sensor_base) + +add_subdirectory(sensor_odometry) +ament_export_libraries(sensor_odometry) + +add_subdirectory(sensor_range) +ament_export_libraries(sensor_range) \ No newline at end of file diff --git a/slam_gmapping/openslam_gmapping/sensor/Makefile b/slam_gmapping/openslam_gmapping/sensor/Makefile new file mode 100644 index 0000000..6b9f784 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/Makefile @@ -0,0 +1,5 @@ +-include ../global.mk + +SUBDIRS=sensor_base sensor_odometry sensor_range + +-include ../build_tools/Makefile.subdirs diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_base/CMakeLists.txt b/slam_gmapping/openslam_gmapping/sensor/sensor_base/CMakeLists.txt new file mode 100644 index 0000000..1f23da6 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_base/CMakeLists.txt @@ -0,0 +1,4 @@ +add_library(sensor_base sensor.cpp sensorreading.cpp) +install(TARGETS sensor_base DESTINATION lib) + +ament_export_libraries(sensor_base) \ No newline at end of file diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensor.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensor.cpp new file mode 100644 index 0000000..d7a2ef7 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensor.cpp @@ -0,0 +1,12 @@ +#include + +namespace GMapping{ + +Sensor::Sensor(const std::string& name){ + m_name=name; +} + +Sensor::~Sensor(){ +} + +};// end namespace diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensoreading.h b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensoreading.h new file mode 100644 index 0000000..217ec6f --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensoreading.h @@ -0,0 +1,18 @@ +#ifndef SENSORREADING_H +#define SENSORREADING_H + +#include "sensor.h" +namespace GMapping{ + +class SensorReading{ + public: + SensorReading(const Sensor* s=0, double time=0); + inline double getTime() const {return m_time;} + inline const Sensor* getSensor() const {return m_sensor;} + protected: + double m_time; + const Sensor* m_sensor; +}; + +}; //end namespace +#endif diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensorreading.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensorreading.cpp new file mode 100644 index 0000000..aa703ac --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_base/sensorreading.cpp @@ -0,0 +1,15 @@ +#include + +namespace GMapping{ + +//SensorReading::SensorReading(const Sensor* s, double t){ +// m_sensor=s; +// m_time=t; +//} +// +// +//SensorReading::~SensorReading(){ +//} + +}; + diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt new file mode 100644 index 0000000..ed4cc4b --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/CMakeLists.txt @@ -0,0 +1,6 @@ +add_library(sensor_odometry odometryreading.cpp odometrysensor.cpp) +target_link_libraries(sensor_odometry sensor_base) + +install(TARGETS sensor_odometry DESTINATION lib) + +ament_export_libraries(sensor_odometry) diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp new file mode 100644 index 0000000..1d76b8b --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometryreading.cpp @@ -0,0 +1,9 @@ +#include + +namespace GMapping{ + +OdometryReading::OdometryReading(const OdometrySensor* odo, double time): + SensorReading(odo,time){} + +}; + diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp new file mode 100644 index 0000000..1cd98e0 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_odometry/odometrysensor.cpp @@ -0,0 +1,9 @@ +#include + +namespace GMapping{ + +OdometrySensor::OdometrySensor(const std::string& name, bool ideal): Sensor(name){ m_ideal=ideal;} + + +}; + diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_range/CMakeLists.txt b/slam_gmapping/openslam_gmapping/sensor/sensor_range/CMakeLists.txt new file mode 100644 index 0000000..5e1c9ba --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_range/CMakeLists.txt @@ -0,0 +1,7 @@ +add_library(sensor_range rangereading.cpp rangesensor.cpp) +target_link_libraries(sensor_range sensor_base) +#ament_target_dependencies(sensor_range sensor_base) + +install(TARGETS sensor_range DESTINATION lib) + +ament_export_libraries(sensor_range) \ No newline at end of file diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangereading.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangereading.cpp new file mode 100644 index 0000000..6bb8946 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangereading.cpp @@ -0,0 +1,114 @@ +#include +#include +#include +#include +#include +#include + +namespace GMapping{ + +using namespace std; + +RangeReading::RangeReading(const RangeSensor* rs, double time): + SensorReading(rs,time){} + +RangeReading::RangeReading(unsigned int n_beams, const double* d, const RangeSensor* rs, double time): + SensorReading(rs,time){ + assert(n_beams==rs->beams().size()); + resize(n_beams); + for (unsigned int i=0; i(getSensor()); + assert(rs); + Point lp( + cos(rs->beams()[i].pose.theta)*(*this)[i], + sin(rs->beams()[i].pose.theta)*(*this)[i]); + Point dp=lastPoint-lp; + double distance=sqrt(dp*dp); + if (distance::max(); + suppressed++; + } + else{ + lastPoint=lp; + v[i]=(*this)[i]; + } + //std::cerr<< __PRETTY_FUNCTION__ << std::endl; + //std::cerr<< "suppressed " << suppressed <<"/"<(size()); + +}; + +unsigned int RangeReading::activeBeams(double density) const{ + if (density==0.) + return size(); + int ab=0; + Point lastPoint(0,0); + uint suppressed=0; + for (unsigned int i=0; i(getSensor()); + assert(rs); + Point lp( + cos(rs->beams()[i].pose.theta)*(*this)[i], + sin(rs->beams()[i].pose.theta)*(*this)[i]); + Point dp=lastPoint-lp; + double distance=sqrt(dp*dp); + if (distance RangeReading::cartesianForm(double maxRange) const{ + const RangeSensor* rangeSensor=dynamic_cast(getSensor()); + assert(rangeSensor && rangeSensor->beams().size()); + // uint m_beams=rangeSensor->beams().size(); + uint m_beams=static_cast(rangeSensor->beams().size()); + std::vector cartesianPoints(m_beams); + double px,py,ps,pc; + px=rangeSensor->getPose().x; + py=rangeSensor->getPose().y; + ps=sin(rangeSensor->getPose().theta); + pc=cos(rangeSensor->getPose().theta); + for (unsigned int i=0; ibeams()[i].s; + const double& c=rangeSensor->beams()[i].c; + if (rho>=maxRange){ + cartesianPoints[i]=Point(0,0); + } else { + Point p=Point(rangeSensor->beams()[i].pose.x+c*rho, rangeSensor->beams()[i].pose.y+s*rho); + cartesianPoints[i].x=px+pc*p.x-ps*p.y; + cartesianPoints[i].y=py+ps*p.x+pc*p.y; + } + } + return cartesianPoints; +} + +}; + diff --git a/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangesensor.cpp b/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangesensor.cpp new file mode 100644 index 0000000..dc7ff1a --- /dev/null +++ b/slam_gmapping/openslam_gmapping/sensor/sensor_range/rangesensor.cpp @@ -0,0 +1,30 @@ +#include + +namespace GMapping{ + +RangeSensor::RangeSensor(std::string name): Sensor(name){} + +RangeSensor::RangeSensor(std::string name, unsigned int beams_num, double res, const OrientedPoint& position, double span, double maxrange):Sensor(name), + m_pose(position), m_beams(beams_num){ + double angle=-.5*res*beams_num; + for (unsigned int i=0; i +#include "autoptr.h" + +using namespace std; +using namespace GMapping; + +typedef autoptr DoubleAutoPtr; + +int main(int argc, const char * const * argv){ + double* d1=new double(10.); + double* d2=new double(20.); + cout << "Construction test" << endl; + DoubleAutoPtr pd1(d1); + DoubleAutoPtr pd2(d2); + cout << *pd1 << " " << *pd2 << endl; + cout << "Copy Construction" << endl; + DoubleAutoPtr pd3(pd1); + cout << *pd3 << endl; + cout << "assignment" << endl; + pd3=pd2; + pd1=pd2; + cout << *pd1 << " " << *pd2 << " " << *pd3 << " " << endl; + cout << "conversion operator" << endl; + DoubleAutoPtr nullPtr; + cout << "conversion operator " << !(nullPtr) << endl; + cout << "neg conversion operator " << nullPtr << endl; + cout << "conversion operator " << (int)pd1 << endl; + cout << "neg conversion operator " << !(pd1) << endl; +} diff --git a/slam_gmapping/openslam_gmapping/utils/datasmoother.h b/slam_gmapping/openslam_gmapping/utils/datasmoother.h new file mode 100644 index 0000000..ddd8553 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/datasmoother.h @@ -0,0 +1,451 @@ +#ifndef DATASMOOTHER_H +#define DATASMOOTHER_H + +#include +#include +#include +#include +#include "stat.h" +#include + +namespace GMapping { + +class DataSmoother { + public: + struct DataPoint { + DataPoint(double _x=0.0, double _y=0.0) { x=_x;y=_y;} + double x; + double y; + }; + + typedef std::vector Data; + + DataSmoother(double parzenWindow) { + init(parzenWindow); + }; + + virtual ~DataSmoother() { + m_data.clear(); + m_cummulated.clear(); + }; + + void init(double parzenWindow) { + m_data.clear(); + m_cummulated.clear(); + m_int=-1; + m_parzenWindow = parzenWindow; + m_from = MAXDOUBLE; + m_to = -MAXDOUBLE; + m_lastStep = 0.001; + }; + + + double sqr(double x) { + return x*x; + } + + + void setMinToZero() { + double minval=MAXDOUBLE; + + for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { + const DataPoint& d = *it; + if (minval > d.y) + minval = d.y; + } + + for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) { + DataPoint& d = *it; + d.y = d.y - minval; + } + + m_cummulated.clear(); + } + + void add(double x, double p) { + m_data.push_back(DataPoint(x,p)); + m_int=-1; + + if (x-3.0*m_parzenWindow < m_from) + m_from = x - 3.0*m_parzenWindow; + + if (x+3.0*m_parzenWindow > m_to) + m_to = x + 3.0*m_parzenWindow; + + m_cummulated.clear(); + } + + void integrate(double step) { + m_lastStep = step; + double sum=0; + for (double x=m_from; x<=m_to; x+=step) + sum += smoothedData(x)*step; + m_int = sum; + } + + double integral(double step, double xTo) { + double sum=0; + for (double x=m_from; x<=xTo; x+=step) + sum += smoothedData(x)*step; + return sum; + } + + + double smoothedData(double x) { + assert( m_data.size() > 0 ); + + double p=0; + double sum_y=0; + for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { + const DataPoint& d = *it; + double dist = fabs(x - d.x); + p += d.y * exp( -0.5 * sqr ( dist/m_parzenWindow ) ); + sum_y += d.y; + } + double denom = sqrt(2.0 * M_PI) * (sum_y) * m_parzenWindow; + p *= 1./denom; + + return p; + } + + double sampleNumeric(double step) { + + assert( m_data.size() > 0 ); + + if (m_int <0 || step != m_lastStep) + integrate(step); + + double r = sampleUniformDouble(0.0, m_int); + double sum2=0; + for (double x=m_from; x<=m_to; x+=step) { + sum2 += smoothedData(x)*step; + if (sum2 > r) + return x-0.5*step; + } + return m_to; + } + + void computeCummuated() { + assert( m_data.size() > 0 ); + m_cummulated.resize(m_data.size()); + std::vector::iterator cit = m_cummulated.begin(); + double sum=0; + for (Data::const_iterator it = m_data.begin(); it != m_data.end(); ++it) { + sum += it->y; + (*cit) = sum; + ++cit; + } + } + + double sample() { + + assert( m_data.size() > 0 ); + + if (m_cummulated.size() == 0) { + computeCummuated(); + } + double maxval = m_cummulated.back(); + + double random = sampleUniformDouble(0.0, maxval); + int nCum = (int) m_cummulated.size(); + double sum=0; + int i=0; + while (i= random) { + return m_data[i].x + sampleGaussian(m_parzenWindow); + } + i++; + } + assert(0); + } + + + void sampleMultiple(std::vector& samples, int num) { + + assert( m_data.size() > 0 ); + samples.clear(); + + if (m_cummulated.size() == 0) { + computeCummuated(); + } + double maxval = m_cummulated.back(); + + std::vector randoms(num); + for (int i=0; i= randoms[j] && j < num) { + samples.push_back( m_data[i].x + sampleGaussian(m_parzenWindow) ); + j++; + } + i++; + } + } + + + + void approxGauss(double step, double* mean, double* sigma) { + + assert( m_data.size() > 0 ); + + double sum=0; + double d=0; + + *mean=0; + for (double x=m_from; x<=m_to; x+=step) { + d = smoothedData(x); + sum += d; + *mean += x*d; + } + *mean /= sum; + + double var=0; + for (double x=m_from; x<=m_to; x+=step) { + d = smoothedData(x); + var += sqr(x-*mean) * d; + } + var /= sum; + + *sigma = sqrt(var); + } + + double gauss(double x, double mean, double sigma) { + return 1.0/(sqrt(2.0*M_PI)*sigma) * exp(-0.5 * sqr( (x-mean)/sigma)); + } + + double cramerVonMisesToGauss(double step, double mean, double sigma) { + + double p=0; + double s=0; + double g=0; + double sint=0; + double gint=0; + + for (double x=m_from; x<=m_to; x+=step) { + s = smoothedData(x); + sint += s * step; + + g = gauss(x, mean, sigma); + gint += g * step; + + p += sqr( (sint - gint) ); + } + + return p; + } + + double kldToGauss(double step, double mean, double sigma) { + + double p=0; + double d=0; + double g=0; + + double sd=0; + double sg=0; + + for (double x=m_from; x<=m_to; x+=step) { + + d = 1e-10 + smoothedData(x); + g = 1e-10 + gauss(x, mean, sigma); + + sd += d; + sg += g; + + p += d * log(d/g); + } + + sd *= step; + sg *= step; + + if (fabs(sd-sg) > 0.1) + assert(0); + + p *= step; + return p; + } + + + void gnuplotDumpData(FILE* fp) { + for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { + const DataPoint& d = *it; + fprintf(fp, "%f %f\n", d.x, d.y); + } + } + + void gnuplotDumpSmoothedData(FILE* fp, double step) { + for (double x=m_from; x<=m_to; x+=step) + fprintf(fp, "%f %f\n", x, smoothedData(x)); + } + + protected: + Data m_data; + std::vector m_cummulated; + double m_int; + double m_lastStep; + + double m_parzenWindow; + double m_from; + double m_to; + +}; + + + + +/* class DataSmoother3D { */ +/* public: */ +/* struct InputPoint { */ +/* InputPoint(double _x=0.0, double _y=0.0, double _t=0.0) { x=_x;y=_y;t=_t;} */ +/* double x; */ +/* double y; */ +/* double t; */ +/* }; */ + +/* struct DataPoint { */ +/* DataPoint(const InputPoint& _p, double _val=0.0;) { p=_p;val=_val;} */ +/* InputPoint p; */ +/* double val; */ +/* }; */ + +/* typedef std::list Data; */ + +/* DataSmoother(double parzenWindow) { */ +/* m_int=-1; */ +/* m_parzenWindow = parzenWindow; */ +/* m_from = InputPoint(MAXDOUBLE,MAXDOUBLE,MAXDOUBLE); */ +/* m_to = InputPoint(-MAXDOUBLE,-MAXDOUBLE,-MAXDOUBLE); */ +/* }; */ + +/* virtual ~DataSmoother() { */ +/* m_data.clear(); */ +/* }; */ + +/* double sqr(double x) { */ +/* return x*x; */ +/* } */ + + +/* void setMinToZero() { */ +/* double minval=MAXDOUBLE; */ +/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */ +/* const DataPoint& d = *it; */ +/* if (minval > d.val) */ +/* minval = d.val; */ +/* } */ + +/* for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) { */ +/* DataPoint& d = *it; */ +/* d.val = d.val - minval; */ +/* } */ + +/* } */ + +/* void add(double x, double y, double t, double v) { */ +/* m_data.push_back(DataPoint(InputPoint(x,y,t),v)); */ +/* m_int=-1; */ + +/* if (x-3.0*m_parzenWindow < m_from.x) */ +/* m_from.x = x - 3.0*m_parzenWindow.x; */ +/* if (x+3.0*m_parzenWindow.x > m_to.x) */ +/* m_to.x = x + 3.0*m_parzenWindow.x; */ + +/* if (y-3.0*m_parzenWindow < m_from.y) */ +/* m_from.y = y - 3.0*m_parzenWindow.y; */ +/* if (y+3.0*m_parzenWindow.y > m_to.y) */ +/* m_to.y = y + 3.0*m_parzenWindow.y; */ + +/* if (t-3.0*m_parzenWindow < m_from.t) */ +/* m_from.t = t - 3.0*m_parzenWindow.t; */ +/* if (t+3.0*m_parzenWindow.t > m_to.t) */ +/* m_to.t = t + 3.0*m_parzenWindow.t; */ +/* } */ + +/* void integrate(InputPoint step) { */ +/* m_lastStep = step; */ +/* double sum=0; */ +/* for (double x=m_from.x; x<=m_to.x; x+=step.x) { */ +/* for (double y=m_from.y; x<=m_to.y; y+=step.y) { */ +/* for (double t=m_from.t; t<=m_to.t; t+=step.t) { */ +/* sum += smoothedData(InputPoint(x,y,t)) * step.x * step.y * step.t; */ +/* } */ +/* } */ +/* } */ +/* m_int = sum; */ +/* } */ + + +/* double smoothedData(InputPoint pnt) { */ +/* assert( m_data.size() > 0 ); */ +/* double p=0; */ +/* double sum_y=0; */ +/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */ +/* const DataPoint& d = *it; */ +/* double u = sqr( (pnt.x-d.x)/m_parzenWindow.x) + */ +/* sqr((pnt.y-d.y)/m_parzenWindow.y) + */ +/* sqr((pnt.t-d.t)/m_parzenWindow.t); */ +/* p += d.val * exp( -0.5 * u); */ +/* sum_y += d.y; */ +/* } */ +/* double denom = sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x) * (sum_y) * */ +/* sqrt(sqr(m_parzenWindow.x) + sqr(m_parzenWindow.y) + sqr(m_parzenWindow.t)); */ +/* p *= 1./denom; */ + +/* return p; */ +/* } */ + +/* double sample(const InputPoint& step) { */ + +/* assert( m_data.size() > 0 ); */ + +/* if (m_int <0 || step != m_lastStep) */ +/* integrate(step); */ + +/* double r = sampleUniformDouble(0.0, m_int); */ +/* double sum2=0; */ +/* for (double x=m_from; x<=m_to; x+=step) { */ +/* sum2 += smoothedData(x)*step; */ +/* if (sum2 > r) */ +/* return x-0.5*step; */ +/* } */ +/* return m_to; */ +/* } */ + +/* void gnuplotDumpData(FILE* fp) { */ +/* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */ +/* const DataPoint& d = *it; */ +/* fprintf(fp, "%f %f %f %f\n", d.x, d.y, d.t, d.val); */ +/* } */ +/* } */ + +/* void gnuplotDumpSmoothedData(FILE* fp, double step) { */ +/* for (double x=m_from; x<=m_to; x+=step) */ +/* fprintf(fp, "%f %f %f %f\n", x, ,y, t, smoothedData(x,y,t)); */ +/* } */ + +/* protected: */ +/* Data m_data; */ +/* vector m_intdata; */ +/* double m_int; */ +/* double m_lastStep; */ + +/* double m_parzenWindow; */ +/* double m_from; */ +/* double m_to; */ + +/* }; */ + +} + +#endif diff --git a/slam_gmapping/openslam_gmapping/utils/dmatrix.h b/slam_gmapping/openslam_gmapping/utils/dmatrix.h new file mode 100644 index 0000000..708a1f5 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/dmatrix.h @@ -0,0 +1,232 @@ +#ifndef DMATRIX_HXX +#define DMATRIX_HXX + +#include +#include +namespace GMapping { + +class DNotInvertibleMatrixException: public std::exception {}; +class DIncompatibleMatrixException: public std::exception {}; +class DNotSquareMatrixException: public std::exception {}; + +template class DMatrix { + public: + DMatrix(int n=0,int m=0); + ~DMatrix(); + + DMatrix(const DMatrix&); + DMatrix& operator=(const DMatrix&); + + X * operator[](int i) { + if ((*shares)>1) detach(); + return mrows[i]; + } + + const X * operator[](int i) const { return mrows[i]; } + + const X det() const; + DMatrix inv() const; + DMatrix transpose() const; + DMatrix operator*(const DMatrix&) const; + DMatrix operator+(const DMatrix&) const; + DMatrix operator-(const DMatrix&) const; + DMatrix operator*(const X&) const; + + int rows() const { return nrows; } + int columns() const { return ncols; } + + void detach(); + + static DMatrix I(int); + + protected: + int nrows,ncols; + X * elems; + X ** mrows; + + int * shares; +}; + +template DMatrix::DMatrix(int n,int m) { + if (n<1) n=1; + if (m<1) m=1; + nrows=n; + ncols=m; + elems=new X[nrows*ncols]; + mrows=new X* [nrows]; + for (int i=0;i DMatrix::~DMatrix() { + if (--(*shares)) return; + delete [] elems; + delete [] mrows; + delete shares; +} + +template DMatrix::DMatrix(const DMatrix& m) { + shares=m.shares; + elems=m.elems; + nrows=m.nrows; + ncols=m.ncols; + mrows=m.mrows; + (*shares)++; +} + +template DMatrix& DMatrix::operator=(const DMatrix& m) { + if (!--(*shares)) { + delete [] elems; + delete [] mrows; + delete shares; + } + shares=m.shares; + elems=m.elems; + nrows=m.nrows; + ncols=m.ncols; + mrows=m.mrows; + (*shares)++; + return *this; +} + +template DMatrix DMatrix::inv() const { + if (nrows!=ncols) throw DNotInvertibleMatrixException(); + DMatrix aux1(*this),aux2(I(nrows)); + aux1.detach(); + for (int i=0;i=nrows) throw DNotInvertibleMatrixException(); + X val=aux1.mrows[k][i]; + for (int j=0;j const X DMatrix::det() const { + if (nrows!=ncols) throw DNotSquareMatrixException(); + DMatrix aux(*this); + X d=X(1); + aux.detach(); + for (int i=0;i=nrows) return X(0); + X val=aux.mrows[k][i]; + for (int j=0;j DMatrix DMatrix::transpose() const { + DMatrix aux(ncols, nrows); + for (int i=0; i DMatrix DMatrix::operator*(const DMatrix& m) const { + if (ncols!=m.nrows) throw DIncompatibleMatrixException(); + DMatrix aux(nrows,m.ncols); + for (int i=0;i DMatrix DMatrix::operator+(const DMatrix& m) const { + if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException(); + DMatrix aux(nrows,ncols); + for (int i=0;i DMatrix DMatrix::operator-(const DMatrix& m) const { + if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException(); + DMatrix aux(nrows,ncols); + for (int i=0;i DMatrix DMatrix::operator*(const X& e) const { + DMatrix aux(nrows,ncols); + for (int i=0;i void DMatrix::detach() { + DMatrix aux(nrows,ncols); + for (int i=0;i DMatrix DMatrix::I(int n) { + DMatrix aux(n,n); + for (int i=0;i std::ostream& operator<<(std::ostream& os, const DMatrix &m) { + os << "{"; + for (int i=0;i0) os << ","; + os << "{"; + for (int j=0;j0) os << ","; + os << m[i][j]; + } + os << "}"; + } + return os << "}"; +} + +}; //namespace GMapping +#endif diff --git a/slam_gmapping/openslam_gmapping/utils/movement.cpp b/slam_gmapping/openslam_gmapping/utils/movement.cpp new file mode 100644 index 0000000..6c66e3e --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/movement.cpp @@ -0,0 +1,108 @@ +#include "movement.h" +#include + +namespace GMapping { + + +FSRMovement::FSRMovement(double f, double s, double r) { + this->f = f; + this->s = s; + this->r = r; +} + +FSRMovement::FSRMovement(const FSRMovement& src) { + *this = src; +} + +FSRMovement::FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2) { + *this = moveBetweenPoints(pt1, pt2); +} + + +FSRMovement::FSRMovement(const FSRMovement& move1, const FSRMovement& move2) { + *this = composeMoves(move1, move2); +} + +void FSRMovement::normalize() +{ + if (r >= -M_PI && r < M_PI) + return; + + int multiplier = (int)(r / (2*M_PI)); + r = r - multiplier*2*M_PI; + if (r >= M_PI) + r -= 2*M_PI; + if (r < -M_PI) + r += 2*M_PI; +} + +OrientedPoint FSRMovement::move(const OrientedPoint& pt) const { + return movePoint(pt, *this); +} + +void FSRMovement::invert() { + *this = invertMove(*this); +} + +void FSRMovement::compose(const FSRMovement& move2) { + *this = composeMoves(*this, move2); +} + + +FSRMovement FSRMovement::composeMoves(const FSRMovement& move1, + const FSRMovement& move2) { + FSRMovement comp; + comp.f = cos(move1.r) * move2.f - sin(move1.r) * move2.s + move1.f; + comp.s = sin(move1.r) * move2.f + cos(move1.r) * move2.s + move1.s; + comp.r = (move1.r + move2.r); + comp.normalize(); + return comp; +} + +OrientedPoint FSRMovement::movePoint(const OrientedPoint& pt, const FSRMovement& move1) { + OrientedPoint pt2(pt); + pt2.x += move1.f * cos(pt.theta) - move1.s * sin(pt.theta); + pt2.y += move1.f * sin(pt.theta) + move1.s * cos(pt.theta); + pt2.theta = (move1.r + pt.theta); + pt2.normalize(); + return pt2; +} + +FSRMovement FSRMovement::moveBetweenPoints(const OrientedPoint& pt1, + const OrientedPoint& pt2) { + FSRMovement move; + move.f = (pt2.y - pt1.y) * sin(pt1.theta) + (pt2.x - pt1.x) * cos(pt1.theta); + move.s = + (pt2.y - pt1.y) * cos(pt1.theta) - (pt2.x - pt1.x) * sin(pt1.theta); + move.r = (pt2.theta - pt1.theta); + move.normalize(); + return move; + +} + +FSRMovement FSRMovement::invertMove(const FSRMovement& move1) { + FSRMovement p_inv; + p_inv.f = - cos(move1.r) * move1.f - sin(move1.r) * move1.s; + p_inv.s = sin(move1.r) * move1.f - cos(move1.r) * move1.s; + p_inv.r = (-move1.r); + p_inv.normalize(); + return p_inv; +} + + +OrientedPoint FSRMovement::frameTransformation(const OrientedPoint& reference_pt_frame1, + const OrientedPoint& reference_pt_frame2, + const OrientedPoint& pt_frame1) { + OrientedPoint zero; + + FSRMovement itrans_refp1(zero, reference_pt_frame1); + itrans_refp1.invert(); + + FSRMovement trans_refp2(zero, reference_pt_frame2); + FSRMovement trans_pt(zero, pt_frame1); + + FSRMovement tmp = composeMoves( composeMoves(trans_refp2, itrans_refp1), trans_pt); + return tmp.move(zero); +} + + +} diff --git a/slam_gmapping/openslam_gmapping/utils/movement.h b/slam_gmapping/openslam_gmapping/utils/movement.h new file mode 100644 index 0000000..19b2200 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/movement.h @@ -0,0 +1,45 @@ +#ifndef FSRMOVEMENT_H +#define FSRMOVEMENT_H + +#include + +namespace GMapping { + + /** fsr-movement (forward, sideward, rotate) **/ +class FSRMovement { + public: + FSRMovement(double f=0.0, double s=0.0, double r=0.0); + FSRMovement(const FSRMovement& src); + FSRMovement(const OrientedPoint& pt1, const OrientedPoint& pt2); + FSRMovement(const FSRMovement& move1, const FSRMovement& move2); + + + void normalize(); + void invert(); + void compose(const FSRMovement& move2); + OrientedPoint move(const OrientedPoint& pt) const; + + + /* static members */ + + static OrientedPoint movePoint(const OrientedPoint& pt, const FSRMovement& move1); + + static FSRMovement composeMoves(const FSRMovement& move1, + const FSRMovement& move2); + + static FSRMovement moveBetweenPoints(const OrientedPoint& pt1, + const OrientedPoint& pt2); + + static FSRMovement invertMove(const FSRMovement& move1); + + static OrientedPoint frameTransformation(const OrientedPoint& reference_pt_frame1, + const OrientedPoint& reference_pt_frame2, + const OrientedPoint& pt_frame1); + + public: + double f; + double s; + double r; +}; +} +#endif diff --git a/slam_gmapping/openslam_gmapping/utils/optimizer.h b/slam_gmapping/openslam_gmapping/utils/optimizer.h new file mode 100644 index 0000000..b6b1d57 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/optimizer.h @@ -0,0 +1,159 @@ +#ifndef _OPTIMIZER_H_ +#define _OPTIMIZER_H_ + +#include "point.h" + +namespace GMapping { + +struct OptimizerParams{ + double discretization; + double angularStep, linearStep; + int iterations; + double maxRange; +}; + +template +struct Optimizer { + Optimizer(const OptimizerParams& params); + OptimizerParams params; + Map lmap; + Likelihood likelihood; + OrientedPoint gradientDescent(const RangeReading& oldReading, const RangeReading& newReading); + OrientedPoint gradientDescent(const RangeReading& oldReading, const OrientedPoint& pose, OLocalMap& Map); + enum Move {Forward, Backward, Left, Right, TurnRight, TurnLeft}; +}; + +template +Optimizer::Optimizer(const OptimizerParams& p): + params(p), + lmap(p.discretization){} + +template +OrientedPoint Optimizer::gradientDescent(const RangeReading& oldReading, const RangeReading& newReading){ + lmap.clear(); + lmap.update(oldReading, OrientedPoint(0,0,0), params.maxRange); + OrientedPoint delta=absoluteDifference(newReading.getPose(), oldReading.getPose()); + OrientedPoint bestPose=delta; + double bestScore=likelihood(lmap, newReading, bestPose, params.maxRange); + int it=0; + double lstep=params.linearStep, astep=params.angularStep; + bool increase; +/* cerr << "bestScore=" << bestScore << endl;;*/ + do { + increase=false; + OrientedPoint itBestPose=bestPose; + double itBestScore=bestScore; + bool itIncrease; + do { + itIncrease=false; + OrientedPoint testBestPose=itBestPose; + double testBestScore=itBestScore; + for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){ + OrientedPoint testPose=itBestPose; + switch(move){ + case Forward: testPose.x+=lstep; + break; + case Backward: testPose.x-=lstep; + break; + case Left: testPose.y+=lstep; + break; + case Right: testPose.y-=lstep; + break; + case TurnRight: testPose.theta-=astep; + break; + case TurnLeft: testPose.theta+=astep; + break; + } + double score=likelihood(lmap, newReading, testPose, params.maxRange); + if (score>testBestScore){ + testBestScore=score; + testBestPose=testPose; + } + } + if (testBestScore > itBestScore){ + itBestScore=testBestScore; + itBestPose=testBestPose; +/* cerr << "s=" << itBestScore << " ";*/ + itIncrease=true; + } + } while(itIncrease); + if (itBestScore > bestScore){ +/* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/ + bestScore=itBestScore; + bestPose=itBestPose; + increase=true; + } else { + it++; + lstep*=0.5; + astep*=0.5; + } + } while (it +OrientedPoint Optimizer::gradientDescent(const RangeReading& reading, const OrientedPoint& pose, OLocalMap& lmap){ + OrientedPoint bestPose=pose; + double bestScore=likelihood(lmap, reading, bestPose, params.maxRange); + int it=0; + double lstep=params.linearStep, astep=params.angularStep; + bool increase; +/* cerr << "bestScore=" << bestScore << endl;;*/ + do { + increase=false; + OrientedPoint itBestPose=bestPose; + double itBestScore=bestScore; + bool itIncrease; + do { + itIncrease=false; + OrientedPoint testBestPose=itBestPose; + double testBestScore=itBestScore; + for (Move move=Forward; move<=TurnLeft; move=(Move)((int)move+1)){ + OrientedPoint testPose=itBestPose; + switch(move){ + case Forward: testPose.x+=lstep; + break; + case Backward: testPose.x-=lstep; + break; + case Left: testPose.y+=lstep; + break; + case Right: testPose.y-=lstep; + break; + case TurnRight: testPose.theta-=astep; + break; + case TurnLeft: testPose.theta+=astep; + break; + } + double score=likelihood(lmap, reading, testPose, params.maxRange); + if (score>testBestScore){ + testBestScore=score; + testBestPose=testPose; + } + } + if (testBestScore > itBestScore){ + itBestScore=testBestScore; + itBestPose=testBestPose; +/* cerr << "s=" << itBestScore << " ";*/ + itIncrease=true; + } + } while(itIncrease); + if (itBestScore > bestScore){ +/* cerr << "S(" << itBestScore << "," << bestScore<< ")";*/ + bestScore=itBestScore; + bestPose=itBestPose; + increase=true; + } else { + it++; + lstep*=0.5; + astep*=0.5; + } + } while (it +#include + +#include + +namespace GMapping{ + +template +class OrientedBoundingBox { + + public: + OrientedBoundingBox(std::vector< point > p); + double area(); + + protected: + Point ul; + Point ur; + Point ll; + Point lr; +}; + +#include "orientedboundingbox.hxx" + +};// end namespace + +#endif + diff --git a/slam_gmapping/openslam_gmapping/utils/orientedboundingbox.hxx b/slam_gmapping/openslam_gmapping/utils/orientedboundingbox.hxx new file mode 100644 index 0000000..2b66e6b --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/orientedboundingbox.hxx @@ -0,0 +1,116 @@ +template +double OrientedBoundingBox::area() { + return sqrt((ul.x - ll.x)*(ul.x - ll.x) + (ul.y - ll.y)*(ul.y - ll.y)) * + sqrt((ul.x - ur.x)*(ul.x - ur.x) + (ul.y - ur.y)*(ul.y - ur.y)) ; +} + +template +OrientedBoundingBox::OrientedBoundingBox(std::vector< point > p) { + + int nOfPoints = (int) p.size(); + + // calculate the center of all points (schwerpunkt) + // ------------------------------------------------- + double centerx = 0; + double centery = 0; + for (int i=0; i < nOfPoints; i++) { + centerx += p[i].x; + centery += p[i].y; + } + centerx /= (double) nOfPoints; + centery /= (double) nOfPoints; + + + + // calcutae the covariance matrix + // ------------------------------- + // covariance matrix (x1 x2, x3 x4) + double x1 = 0.0; + double x2 = 0.0; + double x3 = 0.0; + double x4 = 0.0; + + for (int i=0; i < nOfPoints; i++) { + double cix = p[i].x - centerx; + double ciy = p[i].y - centery; + + x1 += cix*cix; + x2 += cix*ciy; + x4 += ciy*ciy; + } + x1 /= (double) nOfPoints; + x2 /= (double) nOfPoints; + x3 = x2; + x4 /= (double) nOfPoints; + // covariance & center done + + + // calculate the eigenvectors + // --------------------------- + // catch 1/0 or sqrt(<0) + if ((x3 == 0) || (x2 == 0)|| (x4*x4-2*x1*x4+x1*x1+4*x2*x3 < 0 )) { + fprintf(stderr,"error computing the Eigenvectors (%s, line %d)\nx3=%lf, x2=%lf, term=%lf\n\n", + __FILE__, __LINE__, x3,x2, (x4*x4-2*x1*x4+x1*x1+4*x2*x3) ); + + ul.x = 0; + ul.y = 0; + ur.x = 0; + ur.y = 0; + ll.x = 0; + ll.y = 0; + lr.x = 0; + lr.y = 0; + } + + // eigenvalues + double lamda1 = 0.5* (x4 + x1 + sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3)); + double lamda2 = 0.5* (x4 + x1 - sqrt(x4*x4 - 2.0*x1*x4 + x1*x1 + 4.0*x2*x3)); + + // eigenvector 1 with (x,y) + double v1x = - (x4-lamda1) * (x4-lamda1) * (x1-lamda1) / (x2 * x3 * x3); + double v1y = (x4-lamda1) * (x1-lamda1) / (x2 * x3); + // eigenvector 2 with (x,y) + double v2x = - (x4-lamda2) * (x4-lamda2) * (x1-lamda2) / (x2 * x3 * x3); + double v2y = (x4-lamda2) * (x1-lamda2) / (x2 * x3); + + // norm the eigenvectors + double lv1 = sqrt ( (v1x*v1x) + (v1y*v1y) ); + double lv2 = sqrt ( (v2x*v2x) + (v2y*v2y) ); + v1x /= lv1; + v1y /= lv1; + v2x /= lv2; + v2y /= lv2; + // eigenvectors done + + // get the points with maximal dot-product + double x = 0.0; + double y = 0.0; + double xmin = 1e20; + double xmax = -1e20; + double ymin = 1e20; + double ymax = -1e20; + for(int i = 0; i< nOfPoints; i++) { + // dot-product of relativ coordinates of every point + x = (p[i].x - centerx) * v1x + (p[i].y - centery) * v1y; + y = (p[i].x - centerx) * v2x + (p[i].y - centery) * v2y; + + if( x > xmax) xmax = x; + if( x < xmin) xmin = x; + if( y > ymax) ymax = y; + if( y < ymin) ymin = y; + } + + // now we can compute the corners of the bounding box + ul.x = centerx + xmin * v1x + ymin * v2x; + ul.y = centery + xmin * v1y + ymin * v2y; + + ur.x = centerx + xmax * v1x + ymin * v2x; + ur.y = centery + xmax * v1y + ymin * v2y; + + ll.x = centerx + xmin * v1x + ymax * v2x; + ll.y = centery + xmin * v1y + ymax * v2y; + + lr.x = centerx + xmax * v1x + ymax * v2x; + lr.y = centery + xmax * v1y + ymax * v2y; + +} diff --git a/slam_gmapping/openslam_gmapping/utils/printmemusage.cpp b/slam_gmapping/openslam_gmapping/utils/printmemusage.cpp new file mode 100644 index 0000000..44f4664 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/printmemusage.cpp @@ -0,0 +1,27 @@ +#include "printmemusage.h" + +namespace GMapping{ + +using namespace std; +void printmemusage(){ + pid_t pid=getpid(); + char procfilename[1000]; + sprintf(procfilename, "/proc/%d/status", pid); + ifstream is(procfilename); + string line; + while (is){ + is >> line; + if (line=="VmData:"){ + is >> line; + cerr << "#VmData:\t" << line << endl; + } + if (line=="VmSize:"){ + is >> line; + cerr << "#VmSize:\t" << line << endl; + } + + } +} + +}; + diff --git a/slam_gmapping/openslam_gmapping/utils/printmemusage.h b/slam_gmapping/openslam_gmapping/utils/printmemusage.h new file mode 100644 index 0000000..fc7fd91 --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/printmemusage.h @@ -0,0 +1,13 @@ +#ifndef PRINTMEMUSAGE_H +#define PRINTMEMUSAGE_H +#include +#include +#include +#include +#include + +namespace GMapping{ + void printmemusage(); +}; + +#endif diff --git a/slam_gmapping/openslam_gmapping/utils/printpgm.h b/slam_gmapping/openslam_gmapping/utils/printpgm.h new file mode 100644 index 0000000..d9a3b8a --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/printpgm.h @@ -0,0 +1,19 @@ +#include +#include +#include +#include + + +using namespace std; +ostream& printpgm(ostream& os, int xsize, int ysize, const double * const * matrix){ + if (!os) + return os; + os<< "P5" << endl << xsize << endl << ysize << endl << 255 << endl; + for (int y=ysize-1; y>=0; y--){ + for (int x=0;x + +//#include +//#include +//#include +//#include +#include +#include +#include + +namespace GMapping { + +#if 0 + +int sampleUniformInt(int max) +{ + return (int)(max*(rand()/(RAND_MAX+1.0))); +} + +double sampleUniformDouble(double min, double max) +{ + return min + (rand() / (double)RAND_MAX) * (max - min); +} + + +#endif + +// Draw randomly from a zero-mean Gaussian distribution, with standard +// deviation sigma. +// We use the polar form of the Box-Muller transformation, explained here: +// http://www.taygeta.com/random/gaussian.html +double pf_ran_gaussian(double sigma) +{ + double x1, x2, w; + double r; + + do + { + do { r = drand48(); } while (r == 0.0); + x1 = 2.0 * r - 1.0; + do { r = drand48(); } while (r == 0.0); + x2 = 2.0 * drand48() - 1.0; + w = x1*x1 + x2*x2; + } while(w > 1.0 || w==0.0); + + return(sigma * x2 * sqrt(-2.0*log(w)/w)); +} + +double sampleGaussian(double sigma, unsigned int S) { + /* + static gsl_rng * r = NULL; + if(r==NULL) { + gsl_rng_env_setup(); + r = gsl_rng_alloc (gsl_rng_default); + } + */ + if (S!=0) + { + //gsl_rng_set(r, S); + srand(S); + } + if (sigma==0) + return 0; + //return gsl_ran_gaussian (r,sigma); + return pf_ran_gaussian (sigma); +} +#if 0 + +double evalGaussian(double sigmaSquare, double delta){ + if (sigmaSquare<=0) + sigmaSquare=1e-4; + return exp(-.5*delta*delta/sigmaSquare)/sqrt(2*M_PI*sigmaSquare); +} + +#endif +double evalLogGaussian(double sigmaSquare, double delta){ + if (sigmaSquare<=0) + sigmaSquare=1e-4; + return -.5*delta*delta/sigmaSquare-.5*log(2*M_PI*sigmaSquare); +} +#if 0 + + +Covariance3 Covariance3::zero={0.,0.,0.,0.,0.,0.}; + +Covariance3 Covariance3::operator + (const Covariance3 & cov) const{ + Covariance3 r(*this); + r.xx+=cov.xx; + r.yy+=cov.yy; + r.tt+=cov.tt; + r.xy+=cov.xy; + r.yt+=cov.yt; + r.xt+=cov.xt; + return r; +} + +EigenCovariance3::EigenCovariance3(){} + +EigenCovariance3::EigenCovariance3(const Covariance3& cov){ + static gsl_eigen_symmv_workspace * m_eigenspace=NULL; + static gsl_matrix * m_cmat=NULL; + static gsl_matrix * m_evec=NULL; + static gsl_vector * m_eval=NULL; + static gsl_vector * m_noise=NULL; + static gsl_vector * m_pnoise=NULL; + + if (m_eigenspace==NULL){ + m_eigenspace=gsl_eigen_symmv_alloc(3); + m_cmat=gsl_matrix_alloc(3,3); + m_evec=gsl_matrix_alloc(3,3); + m_eval=gsl_vector_alloc(3); + m_noise=gsl_vector_alloc(3); + m_pnoise=gsl_vector_alloc(3); + } + + gsl_matrix_set(m_cmat,0,0,cov.xx); gsl_matrix_set(m_cmat,0,1,cov.xy); gsl_matrix_set(m_cmat,0,2,cov.xt); + gsl_matrix_set(m_cmat,1,0,cov.xy); gsl_matrix_set(m_cmat,1,1,cov.yy); gsl_matrix_set(m_cmat,1,2,cov.yt); + gsl_matrix_set(m_cmat,2,0,cov.xt); gsl_matrix_set(m_cmat,2,1,cov.yt); gsl_matrix_set(m_cmat,2,2,cov.tt); + gsl_eigen_symmv (m_cmat, m_eval, m_evec, m_eigenspace); + for (int i=0; i<3; i++){ + eval[i]=gsl_vector_get(m_eval,i); + for (int j=0; j<3; j++) + evec[i][j]=gsl_matrix_get(m_evec,i,j); + } +} + +EigenCovariance3 EigenCovariance3::rotate(double angle) const{ + static gsl_matrix * m_rmat=NULL; + static gsl_matrix * m_vmat=NULL; + static gsl_matrix * m_result=NULL; + if (m_rmat==NULL){ + m_rmat=gsl_matrix_alloc(3,3); + m_vmat=gsl_matrix_alloc(3,3); + m_result=gsl_matrix_alloc(3,3); + } + + double c=cos(angle); + double s=sin(angle); + gsl_matrix_set(m_rmat,0,0, c ); gsl_matrix_set(m_rmat,0,1, -s); gsl_matrix_set(m_rmat,0,2, 0.); + gsl_matrix_set(m_rmat,1,0, s ); gsl_matrix_set(m_rmat,1,1, c); gsl_matrix_set(m_rmat,1,2, 0.); + gsl_matrix_set(m_rmat,2,0, 0.); gsl_matrix_set(m_rmat,2,1, 0.); gsl_matrix_set(m_rmat,2,2, 1.); + + for (unsigned int i=0; i<3; i++) + for (unsigned int j=0; j<3; j++) + gsl_matrix_set(m_vmat,i,j,evec[i][j]); + gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m_rmat, m_vmat, 0., m_result); + EigenCovariance3 ecov(*this); + for (int i=0; i<3; i++){ + for (int j=0; j<3; j++) + ecov.evec[i][j]=gsl_matrix_get(m_result,i,j); + } + return ecov; +} + +OrientedPoint EigenCovariance3::sample() const{ + static gsl_matrix * m_evec=NULL; + static gsl_vector * m_noise=NULL; + static gsl_vector * m_pnoise=NULL; + if (m_evec==NULL){ + m_evec=gsl_matrix_alloc(3,3); + m_noise=gsl_vector_alloc(3); + m_pnoise=gsl_vector_alloc(3); + } + for (int i=0; i<3; i++){ + for (int j=0; j<3; j++) + gsl_matrix_set(m_evec,i,j, evec[i][j]); + } + for (int i=0; i<3; i++){ + double v=sampleGaussian(sqrt(eval[i])); + if(isnan(v)) + v=0; + gsl_vector_set(m_pnoise,i, v); + } + gsl_blas_dgemv (CblasNoTrans, 1., m_evec, m_pnoise, 0, m_noise); + OrientedPoint ret(gsl_vector_get(m_noise,0),gsl_vector_get(m_noise,1),gsl_vector_get(m_noise,2)); + ret.theta=atan2(sin(ret.theta), cos(ret.theta)); + return ret; +} + +#endif + +double Gaussian3::eval(const OrientedPoint& p) const{ + OrientedPoint q=p-mean; + q.theta=atan2(sin(p.theta-mean.theta),cos(p.theta-mean.theta)); + double v1,v2,v3; + v1 = covariance.evec[0][0]*q.x+covariance.evec[1][0]*q.y+covariance.evec[2][0]*q.theta; + v2 = covariance.evec[0][1]*q.x+covariance.evec[1][1]*q.y+covariance.evec[2][1]*q.theta; + v3 = covariance.evec[0][2]*q.x+covariance.evec[1][2]*q.y+covariance.evec[2][2]*q.theta; + return evalLogGaussian(covariance.eval[0], v1)+evalLogGaussian(covariance.eval[1], v2)+evalLogGaussian(covariance.eval[2], v3); +} + +#if 0 +void Gaussian3::computeFromSamples(const std::vector & poses, const std::vector& weights ){ + OrientedPoint mean=OrientedPoint(0,0,0); + double wcum=0; + double s=0, c=0; + std::vector::const_iterator w=weights.begin(); + for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ + s+=*w*sin(p->theta); + c+=*w*cos(p->theta); + mean.x+=*w*p->x; + mean.y+=*w*p->y; + wcum+=*w; + w++; + } + mean.x/=wcum; + mean.y/=wcum; + s/=wcum; + c/=wcum; + mean.theta=atan2(s,c); + + Covariance3 cov=Covariance3::zero; + w=weights.begin(); + for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ + OrientedPoint delta=(*p)-mean; + delta.theta=atan2(sin(delta.theta),cos(delta.theta)); + cov.xx+=*w*delta.x*delta.x; + cov.yy+=*w*delta.y*delta.y; + cov.tt+=*w*delta.theta*delta.theta; + cov.xy+=*w*delta.x*delta.y; + cov.yt+=*w*delta.y*delta.theta; + cov.xt+=*w*delta.x*delta.theta; + w++; + } + cov.xx/=wcum; + cov.yy/=wcum; + cov.tt/=wcum; + cov.xy/=wcum; + cov.yt/=wcum; + cov.xt/=wcum; + EigenCovariance3 ecov(cov); + this->mean=mean; + this->covariance=ecov; + this->cov=cov; +} + +void Gaussian3::computeFromSamples(const std::vector & poses){ + OrientedPoint mean=OrientedPoint(0,0,0); + double wcum=1; + double s=0, c=0; + for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ + s+=sin(p->theta); + c+=cos(p->theta); + mean.x+=p->x; + mean.y+=p->y; + wcum+=1.; + } + mean.x/=wcum; + mean.y/=wcum; + s/=wcum; + c/=wcum; + mean.theta=atan2(s,c); + + Covariance3 cov=Covariance3::zero; + for (std::vector::const_iterator p=poses.begin(); p!=poses.end(); p++){ + OrientedPoint delta=(*p)-mean; + delta.theta=atan2(sin(delta.theta),cos(delta.theta)); + cov.xx+=delta.x*delta.x; + cov.yy+=delta.y*delta.y; + cov.tt+=delta.theta*delta.theta; + cov.xy+=delta.x*delta.y; + cov.yt+=delta.y*delta.theta; + cov.xt+=delta.x*delta.theta; + } + cov.xx/=wcum; + cov.yy/=wcum; + cov.tt/=wcum; + cov.xy/=wcum; + cov.yt/=wcum; + cov.xt/=wcum; + EigenCovariance3 ecov(cov); + this->mean=mean; + this->covariance=ecov; + this->cov=cov; +} +#endif + +}// end namespace + diff --git a/slam_gmapping/openslam_gmapping/utils/stat_test.cpp b/slam_gmapping/openslam_gmapping/utils/stat_test.cpp new file mode 100644 index 0000000..a49dccb --- /dev/null +++ b/slam_gmapping/openslam_gmapping/utils/stat_test.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include "stat.h" + +using namespace std; +using namespace GMapping; + +// struct Covariance3{ +// double xx, yy, tt, xy, xt, yt; +// }; + +#define SAMPLES_NUMBER 10000 + +int main(int argc, char** argv){ + Covariance3 cov={1.,0.01,0.01,0,0,0}; + EigenCovariance3 ecov(cov); + cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl; + + cout << "EigenVectors:" < points; + for (unsigned int i=0; i::iterator b = points.begin(); + std::vector::iterator e = points.end(); + Gaussian3 gaussian=computeGaussianFromSamples(b, e); + cov=gaussian.cov; + ecov=gaussian.covariance; + cout << "*************** Estimated with Templates ***************" << endl; + cout << "EigenValues: " << ecov.eval[0] << " "<< ecov.eval[1] << " " << ecov.eval[2] << endl; + cout << "EigenVectors:" < +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "std_msgs/msg/float64.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/map_meta_data.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2/utils.h" +#include "message_filters/subscriber.h" +#include "tf2_ros/message_filter.h" + +#include "gmapping/gridfastslam/gridslamprocessor.h" +#include "gmapping/sensor/sensor_base/sensor.h" +#include "gmapping/sensor/sensor_range/rangesensor.h" +#include "gmapping/sensor/sensor_odometry/odometrysensor.h" + +class SlamGmapping : public rclcpp::Node{ +public: + SlamGmapping(); + ~SlamGmapping() override; + + void init(); + void startLiveSlam(); + void publishTransform(); + void laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); + void publishLoop(double transform_publish_period); + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr entropy_publisher_; + rclcpp::Publisher::SharedPtr sst_; + rclcpp::Publisher::SharedPtr sstm_; + + std::shared_ptr buffer_; + std::shared_ptr tfl_; + + std::shared_ptr> scan_filter_sub_; + std::shared_ptr> scan_filter_; + std::shared_ptr tfB_; + + GMapping::GridSlamProcessor* gsp_; + GMapping::RangeSensor* gsp_laser_; + // The angles in the laser, going from -x to x (adjustment is made to get the laser between + // symmetrical bounds as that's what gmapping expects) + std::vector laser_angles_; + // The pose, in the original laser frame, of the corresponding centered laser with z facing up + geometry_msgs::msg::PoseStamped centered_laser_pose_; + // Depending on the order of the elements in the scan and the orientation of the scan frame, + // We might need to change the order of the scan + bool do_reverse_range_; + unsigned int gsp_laser_beam_count_; + GMapping::OdometrySensor* gsp_odom_; + + bool got_first_scan_; + + bool got_map_; + nav_msgs::msg::OccupancyGrid map_; + + tf2::Duration map_update_interval_; + tf2::Transform map_to_odom_; + std::mutex map_to_odom_mutex_; + std::mutex map_mutex_; + + int laser_count_; + int throttle_scans_; + + std::shared_ptr transform_thread_; + + std::string base_frame_; + std::string laser_frame_; + std::string map_frame_; + std::string odom_frame_; + + void updateMap(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); + bool getOdomPose(GMapping::OrientedPoint& gmap_pose, const rclcpp::Time& t); + bool initMapper(sensor_msgs::msg::LaserScan::ConstSharedPtr scan); + bool addScan(sensor_msgs::msg::LaserScan::ConstSharedPtr scan, GMapping::OrientedPoint& gmap_pose); + double computePoseEntropy(); + + // Parameters used by GMapping + double maxRange_; + double maxUrange_; + double maxrange_; + double minimum_score_; + double sigma_; + int kernelSize_; + double lstep_; + double astep_; + int iterations_; + double lsigma_; + double ogain_; + int lskip_; + double srr_; + double srt_; + double str_; + double stt_; + double linearUpdate_; + double angularUpdate_; + double temporalUpdate_; + double resampleThreshold_; + int particles_; + double xmin_; + double ymin_; + double xmax_; + double ymax_; + double delta_; + double occ_thresh_; + double llsamplerange_; + double llsamplestep_; + double lasamplerange_; + double lasamplestep_; + + unsigned long int seed_; + + double transform_publish_period_; + double tf_delay_; +}; + +#endif //SLAM_GMAPPING_SLAM_GMAPPING_H_ diff --git a/slam_gmapping/slam_gmapping/launch/slam_gmapping.launch.py b/slam_gmapping/slam_gmapping/launch/slam_gmapping.launch.py new file mode 100644 index 0000000..77b361a --- /dev/null +++ b/slam_gmapping/slam_gmapping/launch/slam_gmapping.launch.py @@ -0,0 +1,31 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.substitutions import EnvironmentVariable +import launch.actions +import launch_ros.actions + + +def generate_launch_description(): + use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='true') + + rviz_config_dir = os.path.join( + get_package_share_directory('slam_gmapping'), + 'rviz', + 'gmapping.rviz') + + return LaunchDescription([ + launch_ros.actions.Node( + package='slam_gmapping', executable='slam_gmapping', output='screen', parameters=[{'use_sim_time':use_sim_time}]), + + launch_ros.actions.Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_dir], + parameters=[{'use_sim_time': use_sim_time}], + output='screen'), + + ]) diff --git a/slam_gmapping/slam_gmapping/package.xml b/slam_gmapping/slam_gmapping/package.xml new file mode 100644 index 0000000..66e0ac7 --- /dev/null +++ b/slam_gmapping/slam_gmapping/package.xml @@ -0,0 +1,36 @@ + + + + slam_gmapping + 0.0.0 + This package contains a ROS2 Crystal Clemmys wrapper for OpenSlam's Gmapping. + The gmapping package provides laser-based SLAM (Simultaneous Localization and Mapping), + as a ROS node called slam_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. + + Brian Gerkey + Vincent Rabaud + CreativeCommons-by-nc-sa-2.0 + + http://ros.org/wiki/gmapping + + ament_cmake + + std_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + message_filters + rclcpp + sensor_msgs + visualization_msgs + openslam_gmapping + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/slam_gmapping/slam_gmapping/rviz/gmapping.rviz b/slam_gmapping/slam_gmapping/rviz/gmapping.rviz new file mode 100644 index 0000000..9ad494f --- /dev/null +++ b/slam_gmapping/slam_gmapping/rviz/gmapping.rviz @@ -0,0 +1,227 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + - /TF1/Tree1 + Splitter Ratio: 0.5191489458084106 + Tree Height: 576 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_footprint: + Value: true + base_link: + Value: true + camera_link: + Value: true + imu_link: + Value: true + laser_frame: + Value: true + map: + Value: true + odom: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_footprint: + base_link: + {} + camera_link: + {} + imu_link: + {} + laser_frame: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 5.522627830505371 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.741522490978241 + Y: 0.08230660855770111 + Z: -0.0012204855447635055 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.863584041595459 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 873 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001d8000002cbfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002cb000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005100000003efc0100000002fb0000000800540069006d0065010000000000000510000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000332000002cb00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1296 + X: 30 + Y: 94 \ No newline at end of file diff --git a/slam_gmapping/slam_gmapping/src/slam_gmapping.cpp b/slam_gmapping/slam_gmapping/src/slam_gmapping.cpp new file mode 100644 index 0000000..193108f --- /dev/null +++ b/slam_gmapping/slam_gmapping/src/slam_gmapping.cpp @@ -0,0 +1,541 @@ +/* + * slam_gmapping + * Copyright (c) 2008, Willow Garage, Inc. + * + * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE + * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY + * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS + * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED. + * + * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO + * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS + * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND + * CONDITIONS. + * + */ + +/* Author: Brian Gerkey */ +/* Modified by: Charles DuHadway */ + +// +// Created by shivesh on 29/10/18. +// + +#include "slam_gmapping/slam_gmapping.h" + +#include "tf2_ros/create_timer_ros.h" + +#define MAP_IDX(sx, i, j) ((sx) * (j) + (i)) + +using std::placeholders::_1; + +SlamGmapping::SlamGmapping(): + Node("slam_gmapping"), + scan_filter_sub_(nullptr), + scan_filter_(nullptr), + laser_count_(0), + transform_thread_(nullptr) +{ + buffer_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), + get_node_timers_interface()); + buffer_->setCreateTimerInterface(timer_interface); + tfl_ = std::make_shared(*buffer_); + node_ = std::shared_ptr(this, [](rclcpp::Node *) {}); + tfB_ = std::make_shared(node_); + map_to_odom_.setIdentity(); + seed_ = static_cast(time(nullptr)); + init(); + startLiveSlam(); +} + +void SlamGmapping::init() { + gsp_ = new GMapping::GridSlamProcessor(); + + gsp_laser_ = nullptr; + gsp_odom_ = nullptr; + got_first_scan_ = false; + got_map_ = false; + + throttle_scans_ = 1; + base_frame_ = "base_footprint"; + map_frame_ = "map"; + odom_frame_ = "odom"; + transform_publish_period_ = 0.05; + + map_update_interval_ = tf2::durationFromSec(0.5); + maxUrange_ = 80.0; maxRange_ = 0.0; + minimum_score_ = 0; + sigma_ = 0.05; + kernelSize_ = 1; + lstep_ = 0.05; + astep_ = 0.05; + iterations_ = 5; + lsigma_ = 0.075; + ogain_ = 3.0; + lskip_ = 0; + srr_ = 0.1; + srt_ = 0.2; + str_ = 0.1; + stt_ = 0.2; + linearUpdate_ = 1.0; + angularUpdate_ = 0.5; + temporalUpdate_ = 1.0; + resampleThreshold_ = 0.5; + particles_ = 30; + xmin_ = -10.0; + ymin_ = -10.0; + xmax_ = 10.0; + ymax_ = 10.0; + delta_ = 0.05; + occ_thresh_ = 0.25; + llsamplerange_ = 0.01; + llsamplestep_ = 0.01; + lasamplerange_ = 0.005; + lasamplestep_ = 0.005; + tf_delay_ = transform_publish_period_; +} + +void SlamGmapping::startLiveSlam() { + entropy_publisher_ = this->create_publisher("entropy", rclcpp::SystemDefaultsQoS()); + sst_ = this->create_publisher("map", rclcpp::SystemDefaultsQoS()); + sstm_ = this->create_publisher("map_metadata", rclcpp::SystemDefaultsQoS()); + scan_filter_sub_ = std::make_shared> + (node_, "scan", rclcpp::SensorDataQoS().get_rmw_qos_profile()); +// sub_ = this->create_subscription( +// "scan", rclcpp::SensorDataQoS(), +// std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1)); + scan_filter_ = std::make_shared> + (*scan_filter_sub_, *buffer_, odom_frame_, 10, node_); + scan_filter_->registerCallback(std::bind(&SlamGmapping::laserCallback, this, std::placeholders::_1)); + transform_thread_ = std::make_shared + (std::bind(&SlamGmapping::publishLoop, this, transform_publish_period_)); +} + +void SlamGmapping::publishLoop(double transform_publish_period){ + if (transform_publish_period == 0) + return; + rclcpp::Rate r(1.0 / transform_publish_period); + while (rclcpp::ok()) { + publishTransform(); + r.sleep(); + } +} + +SlamGmapping::~SlamGmapping() +{ + if(transform_thread_){ + transform_thread_->join(); + } + + delete gsp_; + delete gsp_laser_; + delete gsp_odom_; +} + +bool SlamGmapping::getOdomPose(GMapping::OrientedPoint& gmap_pose, const rclcpp::Time& t) +{ + // Get the pose of the centered laser at the right time + centered_laser_pose_.header.stamp = t; + // Get the laser's pose that is centered + geometry_msgs::msg::PoseStamped odom_pose; + try + { + buffer_->transform(centered_laser_pose_, odom_pose, odom_frame_, tf2::durationFromSec(1.0)); + } + catch(tf2::TransformException& e) + { + RCLCPP_WARN(this->get_logger(), "Failed to compute odom pose, skipping scan (%s)", e.what()); + return false; + } + + double yaw = tf2::getYaw(odom_pose.pose.orientation); + + gmap_pose = GMapping::OrientedPoint(odom_pose.pose.position.x, + odom_pose.pose.position.y, + yaw); + return true; +} + +bool SlamGmapping::initMapper(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan) +{ + laser_frame_ = scan->header.frame_id; + // Get the laser's pose, relative to base. + geometry_msgs::msg::PoseStamped ident; + geometry_msgs::msg::PoseStamped laser_pose; + + try{ + ident.header.frame_id = laser_frame_; + ident.header.stamp = scan->header.stamp; + tf2::Transform transform; + transform.setIdentity(); + tf2::toMsg(transform, ident.pose); + buffer_->transform(ident, laser_pose, base_frame_); + } + catch (tf2::TransformException& e){ + RCLCPP_WARN(this->get_logger(), "Failed to compute laser pose, aborting initialization (%s)", e.what()); + return false; + } + + // create a point 1m above the laser position and transform it into the laser-frame + geometry_msgs::msg::PointStamped up; + up.header.stamp = scan->header.stamp; + up.header.frame_id = base_frame_; + up.point.x = up.point.y = 0; + up.point.z = 1 + laser_pose.pose.position.z; + try + { + buffer_->transform(up, up, laser_frame_); + } + catch(tf2::TransformException& e) + { + RCLCPP_WARN(this->get_logger(), "Unable to determine orientation of laser: %s", e.what()); + return false; + } + + // gmapping doesnt take roll or pitch into account. So check for correct sensor alignment. + if (fabs(fabs(up.point.z) - 1) > 0.001) + { + RCLCPP_INFO(this->get_logger(), + "Laser has to be mounted planar! Z-coordinate has to be 1 or -1, but gave: %.5f", up.point.z); + return false; + } + + gsp_laser_beam_count_ = static_cast(scan->ranges.size()); + + double angle_center = (scan->angle_min + scan->angle_max)/2; + + centered_laser_pose_.header.frame_id = laser_frame_; + centered_laser_pose_.header.stamp = get_clock()->now(); + tf2::Quaternion q; + + if (up.point.z > 0) + { + do_reverse_range_ = scan->angle_min > scan->angle_max; + q.setEuler(angle_center, 0, 0); + RCLCPP_INFO(this->get_logger(),"Laser is mounted upwards."); + } + else + { + do_reverse_range_ = scan->angle_min < scan->angle_max; + q.setEuler(-angle_center, 0, M_PI); + RCLCPP_INFO(this->get_logger(), "Laser is mounted upside down."); + } + + centered_laser_pose_.pose.position.x = 0; + centered_laser_pose_.pose.position.y = 0; + centered_laser_pose_.pose.position.z = 0; + + centered_laser_pose_.pose.orientation.w = q.getW(); + centered_laser_pose_.pose.orientation.x = q.getX(); + centered_laser_pose_.pose.orientation.y = q.getY(); + centered_laser_pose_.pose.orientation.z = q.getZ(); + + // Compute the angles of the laser from -x to x, basically symmetric and in increasing order + laser_angles_.resize(scan->ranges.size()); + // Make sure angles are started so that they are centered + double theta = - std::fabs(scan->angle_min - scan->angle_max)/2; + for(unsigned int i=0; iranges.size(); ++i) + { + laser_angles_[i]=theta; + theta += std::fabs(scan->angle_increment); + } + + RCLCPP_DEBUG(this->get_logger(), "Laser angles in laser-frame: min: %.3f max: %.3f inc: %.3f", + scan->angle_min, scan->angle_max, scan->angle_increment); + RCLCPP_DEBUG(this->get_logger(), "Laser angles in top-down centered laser-frame: min: %.3f max: %.3f inc: %.3f", + laser_angles_.front(), laser_angles_.back(), std::fabs(scan->angle_increment)); + + GMapping::OrientedPoint gmap_pose(0, 0, 0); + + // setting maxRange and maxUrange here so we can set a reasonable default + maxRange_ = scan->range_max - 0.01; + maxUrange_ = maxRange_; + + // The laser must be called "FLASER". + // We pass in the absolute value of the computed angle increment, on the + // assumption that GMapping requires a positive angle increment. If the + // actual increment is negative, we'll swap the order of ranges before + // feeding each scan to GMapping. + gsp_laser_ = new GMapping::RangeSensor("FLASER", gsp_laser_beam_count_, fabs(scan->angle_increment), gmap_pose, + 0.0, maxRange_); + + GMapping::SensorMap smap; + smap.insert(make_pair(gsp_laser_->getName(), gsp_laser_)); + gsp_->setSensorMap(smap); + + gsp_odom_ = new GMapping::OdometrySensor(odom_frame_); + + /// @todo Expose setting an initial pose + GMapping::OrientedPoint initialPose; + if(!getOdomPose(initialPose, scan->header.stamp)) + { + RCLCPP_WARN(this->get_logger(), "Unable to determine inital pose of laser! Starting point will be set to zero."); + initialPose = GMapping::OrientedPoint(0.0, 0.0, 0.0); + } + + gsp_->setMatchingParameters(maxUrange_, maxRange_, sigma_, + kernelSize_, lstep_, astep_, iterations_, + lsigma_, ogain_, static_cast(lskip_)); + + gsp_->setMotionModelParameters(srr_, srt_, str_, stt_); + gsp_->setUpdateDistances(linearUpdate_, angularUpdate_, resampleThreshold_); + gsp_->setUpdatePeriod(temporalUpdate_); + gsp_->setgenerateMap(false); + gsp_->GridSlamProcessor::init(static_cast(particles_), xmin_, ymin_, xmax_, ymax_, + delta_, initialPose); + gsp_->setllsamplerange(llsamplerange_); + gsp_->setllsamplestep(llsamplestep_); + /// @todo Check these calls; in the gmapping gui, they use + /// llsamplestep and llsamplerange intead of lasamplestep and + /// lasamplerange. It was probably a typo, but who knows. + gsp_->setlasamplerange(lasamplerange_); + gsp_->setlasamplestep(lasamplestep_); + gsp_->setminimumScore(minimum_score_); + + // Call the sampling function once to set the seed. + GMapping::sampleGaussian(1, static_cast(seed_)); + + RCLCPP_INFO(this->get_logger(), "Initialization complete"); + + return true; +} + +bool SlamGmapping::addScan(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan, GMapping::OrientedPoint& gmap_pose) { + if (!getOdomPose(gmap_pose, scan->header.stamp)) + return false; + + if (scan->ranges.size() != gsp_laser_beam_count_) + return false; + + // GMapping wants an array of doubles... + auto *ranges_double = new double[scan->ranges.size()]; + // If the angle increment is negative, we have to invert the order of the readings. + if (do_reverse_range_) { + RCLCPP_DEBUG(this->get_logger(), "Inverting scan"); + int num_ranges = static_cast(scan->ranges.size()); + for (int i = 0; i < num_ranges; i++) { + // Must filter out short readings, because the mapper won't + if (scan->ranges[num_ranges - i - 1] < scan->range_min) + ranges_double[i] = (double) scan->range_max; + else + ranges_double[i] = (double) scan->ranges[num_ranges - i - 1]; + } + } else { + for (unsigned int i = 0; i < scan->ranges.size(); i++) { + // Must filter out short readings, because the mapper won't + if (scan->ranges[i] < scan->range_min) + ranges_double[i] = (double) scan->range_max; + else + ranges_double[i] = (double) scan->ranges[i]; + } + } + + GMapping::RangeReading reading(static_cast(scan->ranges.size()), + ranges_double, + gsp_laser_, + scan->header.stamp.sec); + + // ...but it deep copies them in RangeReading constructor, so we don't + // need to keep our array around. + delete[] ranges_double; + + reading.setPose(gmap_pose); + + RCLCPP_DEBUG(this->get_logger(), "processing scan"); + + return gsp_->processScan(reading); +} + + +void SlamGmapping::laserCallback(sensor_msgs::msg::LaserScan::ConstSharedPtr scan) { + laser_count_++; + if ((laser_count_ % throttle_scans_) != 0) + return; + + tf2::TimePoint last_map_update = tf2::TimePointZero; + + // We can't initialize the mapper until we've got the first scan + if(!got_first_scan_) + { + if(!initMapper(scan)) + return; + got_first_scan_ = true; + } + + GMapping::OrientedPoint odom_pose; + + if(addScan(scan, odom_pose)) + { + GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose; + + tf2::Quaternion q; + q.setRPY(0, 0, mpose.theta); + tf2::Transform laser_to_map = tf2::Transform(q, tf2::Vector3(mpose.x, mpose.y, 0.0)).inverse(); + q.setRPY(0, 0, odom_pose.theta); + tf2::Transform odom_to_laser = tf2::Transform(q, tf2::Vector3(odom_pose.x, odom_pose.y, 0.0)); + + map_to_odom_mutex_.lock(); + map_to_odom_ = (odom_to_laser * laser_to_map).inverse(); + map_to_odom_mutex_.unlock(); + + tf2::TimePoint timestamp = tf2_ros::fromMsg(scan->header.stamp); + if(!got_map_ || (timestamp - last_map_update) > map_update_interval_) + { + updateMap(scan); + last_map_update = tf2_ros::fromMsg(scan->header.stamp); + } + } +} + +double SlamGmapping::computePoseEntropy() +{ + double weight_total=0.0; + for (const auto &it : gsp_->getParticles()) { + weight_total += it.weight; + } + double entropy = 0.0; + for (const auto &it : gsp_->getParticles()) { + if(it.weight/weight_total > 0.0) + entropy += it.weight/weight_total * log(it.weight/weight_total); + } + return -entropy; +} + +void SlamGmapping::updateMap(const sensor_msgs::msg::LaserScan::ConstSharedPtr scan) +{ + RCLCPP_DEBUG(this->get_logger(), "Update map"); + map_mutex_.lock(); + GMapping::ScanMatcher matcher; + + matcher.setLaserParameters(static_cast(scan->ranges.size()), &(laser_angles_[0]), + gsp_laser_->getPose()); + + matcher.setlaserMaxRange(maxRange_); + matcher.setusableRange(maxUrange_); + matcher.setgenerateMap(true); + + GMapping::GridSlamProcessor::Particle best = + gsp_->getParticles()[gsp_->getBestParticleIndex()]; + std_msgs::msg::Float64 entropy; + entropy.data = computePoseEntropy(); + if(entropy.data > 0.0) + entropy_publisher_->publish(entropy); + + if(!got_map_) { + map_.info.resolution = static_cast(delta_); + map_.info.origin.position.x = 0.0; + map_.info.origin.position.y = 0.0; + map_.info.origin.position.z = 0.0; + map_.info.origin.orientation.x = 0.0; + map_.info.origin.orientation.y = 0.0; + map_.info.origin.orientation.z = 0.0; + map_.info.origin.orientation.w = 1.0; + } + + GMapping::Point center; + center.x=(xmin_ + xmax_) / 2.0; + center.y=(ymin_ + ymax_) / 2.0; + + GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, + delta_); + + RCLCPP_DEBUG(this->get_logger(), "Trajectory tree:"); + for(GMapping::GridSlamProcessor::TNode* n = best.node; + n; + n = n->parent) + { + RCLCPP_DEBUG(this->get_logger(), " %.3f %.3f %.3f", + n->pose.x, + n->pose.y, + n->pose.theta); + if(!n->reading) + { + RCLCPP_DEBUG(this->get_logger(), "Reading is NULL"); + continue; + } + matcher.invalidateActiveArea(); + matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0])); + matcher.registerScan(smap, n->pose, &((*n->reading)[0])); + } + + // the map may have expanded, so resize ros message as well + if(map_.info.width != (unsigned int) smap.getMapSizeX() || map_.info.height != (unsigned int) smap.getMapSizeY()) { + + // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor + // so we must obtain the bounding box in a different way + GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0)); + GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY())); + xmin_ = wmin.x; ymin_ = wmin.y; + xmax_ = wmax.x; ymax_ = wmax.y; + + RCLCPP_DEBUG(this->get_logger(), "map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(), + xmin_, ymin_, xmax_, ymax_); + + map_.info.width = static_cast(smap.getMapSizeX()); + map_.info.height = static_cast(smap.getMapSizeY()); + map_.info.origin.position.x = xmin_; + map_.info.origin.position.y = ymin_; + map_.data.resize(map_.info.width * map_.info.height); + + RCLCPP_DEBUG(this->get_logger(), "map origin: (%f, %f)", map_.info.origin.position.x, map_.info.origin.position.y); + } + + for(int x=0; x < smap.getMapSizeX(); x++) + { + for(int y=0; y < smap.getMapSizeY(); y++) + { + /// @todo Sort out the unknown vs. free vs. obstacle thresholding + GMapping::IntPoint p(x, y); + double occ=smap.cell(p); + assert(occ <= 1.0); + if(occ < 0) + map_.data[MAP_IDX(map_.info.width, x, y)] = -1; + else if(occ > occ_thresh_) + { + //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0); + map_.data[MAP_IDX(map_.info.width, x, y)] = 100; + } + else + map_.data[MAP_IDX(map_.info.width, x, y)] = 0; + } + } + got_map_ = true; + + //make sure to set the header information on the map + map_.header.stamp = get_clock()->now(); + map_.header.frame_id = map_frame_; + + sst_->publish(map_); + sstm_->publish(map_.info); + map_mutex_.unlock(); +} + +void SlamGmapping::publishTransform() +{ + map_to_odom_mutex_.lock(); + rclcpp::Time tf_expiration = get_clock()->now() + rclcpp::Duration( + static_cast(static_cast(tf_delay_)), 0); + geometry_msgs::msg::TransformStamped transform; + transform.header.frame_id = map_frame_; + transform.header.stamp = tf_expiration; + transform.child_frame_id = odom_frame_; + try { + transform.transform = tf2::toMsg(map_to_odom_); + tfB_->sendTransform(transform); + } + catch (tf2::LookupException& te){ + RCLCPP_INFO(this->get_logger(), te.what()); + } + map_to_odom_mutex_.unlock(); +} + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + + auto slam_gmapping_node = std::make_shared(); + rclcpp::spin(slam_gmapping_node); + return(0); +}