Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [127] pkg/trunk/gmapping
|
From: <ge...@us...> - 2008-04-18 00:50:50
|
Revision: 127
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=127&view=rev
Author: gerkey
Date: 2008-04-17 17:50:56 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
working on gmapping
Modified Paths:
--------------
pkg/trunk/gmapping/Makefile
pkg/trunk/gmapping/build_tools/testlib
pkg/trunk/gmapping/gridfastslam/Makefile
pkg/trunk/gmapping/gridfastslam/gridslamprocessor_tree.cpp
pkg/trunk/gmapping/gui/Makefile
pkg/trunk/gmapping/gui/gfs2img.cpp
pkg/trunk/gmapping/gui/gsp_thread.cpp
pkg/trunk/gmapping/gui/gsp_thread.h
pkg/trunk/gmapping/manual.mk
pkg/trunk/gmapping/playerwrapper/Makefile
pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp
pkg/trunk/gmapping/playerwrapper/playergfswrapper.h
pkg/trunk/gmapping/scanmatcher/Makefile
pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.cpp
pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.h
pkg/trunk/gmapping/utils/Makefile
pkg/trunk/gmapping/utils/stat.cpp
Added Paths:
-----------
pkg/trunk/gmapping/playerwrapper/examples/
pkg/trunk/gmapping/playerwrapper/examples/readlog.cfg
pkg/trunk/gmapping/playerwrapper/examples/writelog_2008_04_17_13_15_20.log.gz
pkg/trunk/gmapping/scanmatcher/eig3.c
pkg/trunk/gmapping/scanmatcher/eig3.h
Modified: pkg/trunk/gmapping/Makefile
===================================================================
--- pkg/trunk/gmapping/Makefile 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/Makefile 2008-04-18 00:50:56 UTC (rev 127)
@@ -3,12 +3,16 @@
ifeq ($(CARMENSUPPORT),1)
SUBDIRS=utils sensor log configfile scanmatcher carmenwrapper gridfastslam gui gfs-carmen
else
+ifeq ($(PLAYERSUPPORT),1)
+SUBDIRS=utils sensor log configfile scanmatcher gridfastslam playerwrapper gui
+else
ifeq ($(MACOSX),1)
SUBDIRS=utils sensor log configfile scanmatcher gridfastslam
else
SUBDIRS=utils sensor log configfile scanmatcher gridfastslam gui
endif
endif
+endif
LDFLAGS+=
CPPFLAGS+= -I../sensor
Modified: pkg/trunk/gmapping/build_tools/testlib
===================================================================
--- pkg/trunk/gmapping/build_tools/testlib 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/build_tools/testlib 2008-04-18 00:50:56 UTC (rev 127)
@@ -3,6 +3,9 @@
echo "Syntax: rtestlib <library>"
exit 1
fi
+
+exit 0
+
FNAME=`mktemp rtestlibXXXXXX`
echo "int main() { return 0; }" > $FNAME.cpp
Modified: pkg/trunk/gmapping/gridfastslam/Makefile
===================================================================
--- pkg/trunk/gmapping/gridfastslam/Makefile 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/gridfastslam/Makefile 2008-04-18 00:50:56 UTC (rev 127)
@@ -1,8 +1,10 @@
OBJS= gridslamprocessor_tree.o motionmodel.o gridslamprocessor.o gfsreader.o
-APPS= gfs2log gfs2rec gfs2neff gfs2stat
+APPS= gfs2log gfs2rec gfs2neff #gfs2stat
-LDFLAGS+= -lutils -lsensor_range -llog -lscanmatcher -lsensor_base -lsensor_odometry $(GSL_LIB)
-CPPFLAGS+=-I../sensor $(GSL_INCLUDE)
+#LDFLAGS+= -lutils -lsensor_range -llog -lscanmatcher -lsensor_base -lsensor_odometry $(GSL_LIB)
+LDFLAGS+= -lutils -lsensor_range -llog -lscanmatcher -lsensor_base -lsensor_odometry
+#CPPFLAGS+=-I../sensor $(GSL_INCLUDE)
+CPPFLAGS+=-I../sensor
-include ../global.mk
-include ../build_tools/Makefile.generic-shared-object
Modified: pkg/trunk/gmapping/gridfastslam/gridslamprocessor_tree.cpp
===================================================================
--- pkg/trunk/gmapping/gridfastslam/gridslamprocessor_tree.cpp 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/gridfastslam/gridslamprocessor_tree.cpp 2008-04-18 00:50:56 UTC (rev 127)
@@ -4,7 +4,7 @@
#include <map>
#include <set>
#include <fstream>
-#include <gsl/gsl_blas.h>
+//#include <gsl/gsl_blas.h>
#include <utils/stat.h>
#include "gridslamprocessor.h"
Modified: pkg/trunk/gmapping/gui/Makefile
===================================================================
--- pkg/trunk/gmapping/gui/Makefile 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/gui/Makefile 2008-04-18 00:50:56 UTC (rev 127)
@@ -9,6 +9,11 @@
LDFLAGS+= -lcarmenwrapper
endif
+ifeq ($(PLAYERSUPPORT),1)
+CPPFLAGS+= `pkg-config --cflags playercore`
+LDFLAGS+= -lplayerwrapper `pkg-config --libs playerxdr playercore`
+endif
+
CPPFLAGS+= -I../sensor $(QT_INCLUDE) $(KDE_INCLUDE) -I$(CARMEN_HOME)/include
Modified: pkg/trunk/gmapping/gui/gfs2img.cpp
===================================================================
--- pkg/trunk/gmapping/gui/gfs2img.cpp 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/gui/gfs2img.cpp 2008-04-18 00:50:56 UTC (rev 127)
@@ -203,6 +203,7 @@
}
}
+ /*
cout << "painting trajectories" << endl;
for (int p=0; p<particles; p++){
painter.setPen(QColor(Qt::red));
@@ -240,6 +241,7 @@
}
}
paths[bestIdx].destroyReferences();;
+ */
cout << " DONE" << endl;
cout << "writing image" << endl;
QImage img=pixmap.convertToImage();
Modified: pkg/trunk/gmapping/gui/gsp_thread.cpp
===================================================================
--- pkg/trunk/gmapping/gui/gsp_thread.cpp 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/gui/gsp_thread.cpp 2008-04-18 00:50:56 UTC (rev 127)
@@ -20,6 +20,9 @@
*
*****************************************************************/
+#ifdef PLAYER_SUPPORT
+ #include <playerwrapper/playergfswrapper.h>
+#endif
#include "gsp_thread.h"
#include <utils/commandline.h>
@@ -45,7 +48,7 @@
CMD_PARSE_END_SILENT;
if (configfilename.length()>0){
- ConfigFile cfg(configfilename);
+ GMapping::ConfigFile cfg(configfilename);
filename = (std::string) cfg.value("gfs","filename",filename);
outfilename = (std::string) cfg.value("gfs","outfilename",outfilename);
@@ -281,8 +284,9 @@
//if started online retrieve the settings from the connection
+ if (gpt->onLine)
+ {
#ifdef CARMEN_SUPPORT
- if (gpt->onLine){
cout << "starting the process:" << endl;
CarmenWrapper::initializeIPC(gpt->m_argv[0]);
CarmenWrapper::start(gpt->m_argv[0]);
@@ -293,15 +297,25 @@
}
gpt->sensorMap=CarmenWrapper::sensorMap();
cout << "Connected " << endl;
- }
#else
- if (gpt->onLine){
+ #ifdef PLAYER_SUPPORT
+ cout << "Waiting for retrieving the sensor map:" << endl;
+ while (!gpt->m_pwrapper->sensorMapComputed()){
+ usleep(100000);
+ cout << "." << flush;
+ }
+ gpt->sensorMap=gpt->m_pwrapper->getSensorMap();
+ cout << "Connected " << endl;
+
+ #else
+
cout << "FATAL ERROR: cannot run online without the carmen support" << endl;
DoneEvent *done=new DoneEvent;
gpt->addEvent(done);
return 0;
+ #endif // PLAYER_SUPPORT
+#endif // CARMEN_SUPPORT
}
-#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);
@@ -412,6 +426,21 @@
}
}
}
+#else
+ #ifdef PLAYER_SUPPORT
+ if (gpt->onLine){
+ while (1){
+ RangeReading* nr;
+ while((nr = gpt->m_pwrapper->getReading()))
+ {
+ gpt->processScan(*nr);
+ delete nr;
+ }
+ usleep(1000);
+ pthread_testcancel();
+ }
+ }
+ #endif
#endif
ofstream rawpath("rawpath.dat");
if (!gpt->onLine){
@@ -504,11 +533,14 @@
pthread_create(&gfs_thread, 0, (void * (*)(void *))fastslamthread, (void *) this);
}
+#ifdef PLAYER_SUPPORT
void GridSlamProcessorThread::start(PlayerGFSWrapper* pwrapper)
{
m_pwrapper = pwrapper;
+ onLine = true;
start();
}
+#endif
void GridSlamProcessorThread::stop(){
if (! running){
@@ -517,6 +549,7 @@
}
running=false;
void * retval;
+ pthread_cancel(gfs_thread);
pthread_join(gfs_thread, &retval);
}
Modified: pkg/trunk/gmapping/gui/gsp_thread.h
===================================================================
--- pkg/trunk/gmapping/gui/gsp_thread.h 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/gui/gsp_thread.h 2008-04-18 00:50:56 UTC (rev 127)
@@ -33,15 +33,14 @@
#include <log/sensorstream.h>
#include <gridfastslam/gridslamprocessor.h>
-using namespace std;
-using namespace GMapping;
-
#ifdef PLAYER_SUPPORT
-namespace GMapping {
class PlayerGFSWrapper;
-}
#endif
+using namespace std;
+using namespace GMapping;
+
+
#define MAX_STRING_LENGTH 1024
Modified: pkg/trunk/gmapping/manual.mk
===================================================================
--- pkg/trunk/gmapping/manual.mk 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/manual.mk 2008-04-18 00:50:56 UTC (rev 127)
@@ -1,6 +1,6 @@
#CPPFLAGS+= -DNDEBUG
-CXXFLAGS+= -O3 -Wall -ffast-math
-#CXXFLAGS+= -g -O0 -Wall
+#CXXFLAGS+= -O3 -Wall -ffast-math
+CXXFLAGS+= -g -O0 -Wall
PROFILE= false
Modified: pkg/trunk/gmapping/playerwrapper/Makefile
===================================================================
--- pkg/trunk/gmapping/playerwrapper/Makefile 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/playerwrapper/Makefile 2008-04-18 00:50:56 UTC (rev 127)
@@ -1,11 +1,12 @@
-OBJS= playergfswrapper.o
+OBJS= ../gui/gsp_thread.o ../gui/qparticleviewer.o ../gui/qgraphpainter.o ../gui/qmappainter.o playergfswrapper.o
APPS=
-#LIBS+= -L$(CARMEN_HOME)/lib -lnavigator_interface -lsimulator_interface -lrobot_interface -llaser_interface -lparam_interface -lglobal -lipc -lm -lutils -lsensor_range -llog -lscanmatcher -lpthread -lz
-LIBS+= `pkg-config --libs playercore` -lsensor_range
-#-L$(CARMEN_HOME)/lib -lnavigator_interface -lsimulator_interface -lrobot_interface -llaser_interface -lparam_interface -lglobal -lipc -lm -lutils -lsensor_range -llog -lscanmatcher -lpthread -lz
-CPPFLAGS+=-I../sensor `pkg-config --cflags playercore`
+LIBS+= `pkg-config --libs playercore playerxdr` -lsensor_range -lgridfastslam
+CPPFLAGS+=-Wall -I../sensor `pkg-config --cflags playercore`
+LDFLAGS+=$(QT_LIB)
+CPPFLAGS+=$(QT_INCLUDE)
+
-include ../global.mk
ifeq ($(PLAYERSUPPORT), 0)
Added: pkg/trunk/gmapping/playerwrapper/examples/readlog.cfg
===================================================================
--- pkg/trunk/gmapping/playerwrapper/examples/readlog.cfg (rev 0)
+++ pkg/trunk/gmapping/playerwrapper/examples/readlog.cfg 2008-04-18 00:50:56 UTC (rev 127)
@@ -0,0 +1,26 @@
+driver
+(
+ name "readlog"
+ filename "writelog_2008_04_17_13_15_20.log"
+ speed 1
+ provides ["log:0" "laser:0" "position2d:0"]
+ autoplay 1
+ alwayson 0
+)
+
+driver
+(
+ name "laserposeinterpolator"
+ requires ["laser:0" "position2d:0"]
+ provides ["laser:1"]
+)
+
+driver
+(
+ name "playergfswrapper"
+ plugin "libplayerwrapper"
+ requires [ "laser:1"]
+ provides ["position2d:1"]
+ alwayson 1
+)
+
Added: pkg/trunk/gmapping/playerwrapper/examples/writelog_2008_04_17_13_15_20.log.gz
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/gmapping/playerwrapper/examples/writelog_2008_04_17_13_15_20.log.gz
___________________________________________________________________
Name: svn:mime-type
+ application/octet-stream
Modified: pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp
===================================================================
--- pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/playerwrapper/playergfswrapper.cpp 2008-04-18 00:50:56 UTC (rev 127)
@@ -22,11 +22,56 @@
*
*****************************************************************/
+#include <gui/qparticleviewer.h>
+#include <gui/qgraphpainter.h>
+#include <qapplication.h>
+#include <qframe.h>
+#include <qlabel.h>
+#include <qlayout.h>
+#include <qvbox.h>
+#include <qmainwindow.h>
+
#include "playergfswrapper.h"
-using namespace GMapping;
+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(QParticleViewer::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;
+};
+
+
PlayerGFSWrapper::PlayerGFSWrapper(ConfigFile* cf, int section)
: Driver(cf, section)
{
@@ -63,8 +108,32 @@
this->SetError(-1);
return;
}
+
+ this->laser_rs = NULL;
+ this->sensorMap_ready = false;
}
+void*
+GUI_thread(void* d)
+{
+ PlayerGFSWrapper* pd = (PlayerGFSWrapper*)d;
+ int argc;
+ char* argv[1];
+
+ argv[0] = "playergfswrapper";
+ argc=1;
+
+ QApplication app(argc,argv);
+ GFSMainWindow* mainWin=new GFSMainWindow(pd->gsp);
+ app.setMainWidget(mainWin);
+ mainWin->show();
+ //pd->gsp->setEventBufferSize(10000);
+ pd->gsp->start(pd);
+ mainWin->start(1000);
+ app.exec();
+ return(NULL);
+}
+
int
PlayerGFSWrapper::Setup()
{
@@ -86,12 +155,15 @@
this->gsp = new GridSlamProcessorThread();
- // TODO: create this->laser_rs
+ if(this->laser_rs)
+ {
+ delete this->laser_rs;
+ this->laser_rs = NULL;
+ }
- // TODO: compute GFS "sensorMap"
-
// start GFS thread
- this->gsp->start(this);
+ //this->gsp->start(this);
+ pthread_create(&this->gui_thread, 0, GUI_thread, this);
this->StartThread();
return(0);
@@ -119,24 +191,29 @@
{
for(;;)
{
+ pthread_testcancel();
+
+ this->InQueue->Wait();
+
this->ProcessMessages();
// TODO: check for new pose / map from GFS thread
}
}
-bool
-PlayerGFSWrapper::getReading(RangeReading& reading)
+GMapping::RangeReading*
+PlayerGFSWrapper::getReading()
{
- bool ret=false;
+ GMapping::RangeReading* ret = NULL;
pthread_mutex_lock(&this->rangeDeque_mutex);
if(!this->rangeDeque.empty())
{
- reading=this->rangeDeque.front();
+ ret=this->rangeDeque.front();
this->rangeDeque.pop_front();
- ret=true;
}
pthread_mutex_unlock(&this->rangeDeque_mutex);
+ if(ret)
+ puts("returned a reading");
return(ret);
}
@@ -165,14 +242,58 @@
PlayerGFSWrapper::ProcessLaser(player_msghdr_t* hdr,
player_laser_data_scanpose_t* data)
{
- RangeReading reading(this->laser_rs,hdr->timestamp);
+ if(!this->laser_rs)
+ {
+ // create this->laser_rs
+ std::string lasername = "FLASER";
+ this->laser_rs =
+ new GMapping::RangeSensor(lasername,
+ data->scan.ranges_count,
+ data->scan.resolution,
+ //OrientedPoint(0,0,laser_pose.pa-robot_pose.theta),
+ GMapping::OrientedPoint(0,0,-data->pose.pa),
+ 0, data->scan.max_range);
+ this->laser_rs->updateBeamsLookup();
+ this->sensorMap.insert(make_pair(lasername, this->laser_rs));
+ this->sensorMap_ready = true;
+ }
+
+ GMapping::RangeReading* reading = new GMapping::RangeReading(this->laser_rs,hdr->timestamp);
+ reading->resize(data->scan.ranges_count);
for(unsigned int i=0;i<data->scan.ranges_count;i++)
- reading[i] = data->scan.ranges[i];
- reading.setPose(OrientedPoint(data->pose.px,
+ (*reading)[i] = data->scan.ranges[i];
+ reading->setPose(GMapping::OrientedPoint(data->pose.px,
data->pose.py,
data->pose.pa));
pthread_mutex_lock(&this->rangeDeque_mutex);
this->rangeDeque.push_back(reading);
+ printf("queue size: %d\n", this->rangeDeque.size());
pthread_mutex_unlock(&this->rangeDeque_mutex);
}
+
+////////////////////////////////////////////////////////////////////////////////
+// Extra stuff for building a shared object.
+
+// A factory creation function, declared outside of the class so that it
+// can be invoked without any object context (alternatively, you can
+// declare it static in the class). In this function, we create and return
+// (as a generic Driver*) a pointer to a new instance of this driver.
+Driver*
+PlayerGFSWrapper_Init(ConfigFile* cf, int section)
+{
+ // Create and return a new instance of this driver
+ return((Driver*)(new PlayerGFSWrapper(cf, section)));
+}
+
+/* need the extern to avoid C++ name-mangling */
+extern "C" {
+ int player_driver_init(DriverTable* table)
+ {
+ puts("PlayerGFSWrapper driver initializing");
+ table->AddDriver("playergfswrapper", PlayerGFSWrapper_Init);
+ puts("PlayerGFSWrapper driver done");
+ return(0);
+ }
+}
+
Modified: pkg/trunk/gmapping/playerwrapper/playergfswrapper.h
===================================================================
--- pkg/trunk/gmapping/playerwrapper/playergfswrapper.h 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/playerwrapper/playergfswrapper.h 2008-04-18 00:50:56 UTC (rev 127)
@@ -26,26 +26,15 @@
#ifndef PLAYERWRAPPER_H
#define PLAYERWRAPPER_H
-//#include <iostream>
#include <deque>
#include <pthread.h>
-//#include <semaphore.h>
-//#include <carmen/carmen.h>
-//#include <carmen/global.h>
-//#include <log/carmenconfiguration.h>
-
-#include <libplayercore/playercore.h>
-
#include <gui/gsp_thread.h>
-//#include <sensor/sensor_base/sensor.h>
-//#include <log/sensorstream.h>
-//#include <log/sensorlog.h>
#include <sensor/sensor_range/rangesensor.h>
#include <sensor/sensor_range/rangereading.h>
-namespace GMapping {
+#include <libplayercore/playercore.h>
class PlayerGFSWrapper : public Driver
{
@@ -60,18 +49,20 @@
// Devices to which we subscribe
player_devaddr_t laser_id;
Device* laser;
- RangeSensor* laser_rs;
+ GMapping::RangeSensor* laser_rs;
- // The GFS object
- GridSlamProcessorThread* gsp;
+ GMapping::SensorMap sensorMap;
+ bool sensorMap_ready;
// Queue of range readings, with mutex
- std::deque<RangeReading> rangeDeque;
+ std::deque<RangeReading*> rangeDeque;
pthread_mutex_t rangeDeque_mutex;
void ProcessLaser(player_msghdr_t* hdr,
player_laser_data_scanpose_t* data);
+ pthread_t gui_thread;
+
public:
PlayerGFSWrapper(ConfigFile* cf, int section);
virtual int Setup();
@@ -80,72 +71,14 @@
player_msghdr * hdr,
void * data);
- bool getReading(RangeReading& reading);
-
- /*
- void initializeIPC(const char* name);
- bool start(const char* name);
- bool isRunning();
- void lock();
- void unlock();
- int registerLocalizationMessages();
-
- int queueLength();
- OrientedPoint getTruePos();
- bool getReading(RangeReading& reading);
- void addReading(RangeReading& reading);
- const SensorMap& sensorMap();
- bool sensorMapComputed();
- bool isStopped();
-
- // conversion function
- carmen_robot_laser_message reading2carmen(const RangeReading& reading);
- RangeReading carmen2reading(const carmen_robot_laser_message& msg);
- carmen_point_t point2carmen (const OrientedPoint& p);
- OrientedPoint carmen2point (const carmen_point_t& p);
-
-
- // carmen interaction
- void robot_frontlaser_handler(carmen_robot_laser_message* frontlaser);
- void robot_rearlaser_handler(carmen_robot_laser_message* frontlaser);
- void simulator_truepos_handler(carmen_simulator_truepos_message* truepos);
- //babsi:
- void navigator_go_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ;
- void navigator_stop_handler(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void*) ;
-
- //babsi:
- void publish_globalpos(carmen_localize_summary_p summary);
- void publish_particles(carmen_localize_particle_filter_p filter,
- carmen_localize_summary_p summary);
-
- void shutdown_module(int sig);
-
- private:
- std::deque<RangeReading> m_rangeDeque;
- sem_t m_dequeSem;
- pthread_mutex_t m_mutex, m_lock;
- pthread_t m_readingThread;
- void * m_reading_function(void*);
- bool m_threadRunning;
- SensorMap m_sensorMap;
- RangeSensor* m_frontLaser, *m_rearLaser;
- OrientedPoint m_truepos;
- bool stopped;
- */
+ // Interface used by the GFS thread
+ GMapping::RangeReading* getReading();
+ bool sensorMapComputed() { return(this->sensorMap_ready); }
+ const SensorMap& getSensorMap() { return(this->sensorMap); }
+
+ // The GFS object (public so that it can be accessed from the GUI
+ // thread)
+ GridSlamProcessorThread* gsp;
};
-} //end namespace
-
-
-
#endif
-/*
-int main (int argc, char** argv) {
-
- CarmenWrapper::init_carmen(argc, argv);
- while (true) {
- IPC_listenWait(100);
- }
- return 1;
-}
-*/
Modified: pkg/trunk/gmapping/scanmatcher/Makefile
===================================================================
--- pkg/trunk/gmapping/scanmatcher/Makefile 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/scanmatcher/Makefile 2008-04-18 00:50:56 UTC (rev 127)
@@ -1,8 +1,10 @@
-OBJS= smmap.o scanmatcher.o scanmatcherprocessor.o
+OBJS= smmap.o scanmatcher.o scanmatcherprocessor.o eig3.o
APPS= scanmatch_test icptest
-LDFLAGS+= $(GSL_LIB) -lutils -lsensor_range -llog
-CPPFLAGS+=-I../sensor $(GSL_INCLUDE)
+#LDFLAGS+= $(GSL_LIB) -lutils -lsensor_range -llog
+LDFLAGS+= -lutils -lsensor_range -llog
+#CPPFLAGS+=-I../sensor $(GSL_INCLUDE)
+CPPFLAGS+=-I../sensor
-include ../global.mk
-include ../build_tools/Makefile.generic-shared-object
Added: pkg/trunk/gmapping/scanmatcher/eig3.c
===================================================================
--- pkg/trunk/gmapping/scanmatcher/eig3.c (rev 0)
+++ pkg/trunk/gmapping/scanmatcher/eig3.c 2008-04-18 00:50:56 UTC (rev 127)
@@ -0,0 +1,271 @@
+
+/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
+ domain Java Matrix library JAMA. */
+
+#include <math.h>
+
+#ifndef MAX
+#define MAX(a, b) ((a)>(b)?(a):(b))
+#endif
+
+//#define n 3
+static int 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);
+}
Added: pkg/trunk/gmapping/scanmatcher/eig3.h
===================================================================
--- pkg/trunk/gmapping/scanmatcher/eig3.h (rev 0)
+++ pkg/trunk/gmapping/scanmatcher/eig3.h 2008-04-18 00:50:56 UTC (rev 127)
@@ -0,0 +1,19 @@
+
+/* Eigen-decomposition for symmetric 3x3 real matrices.
+ Public domain, copied from the public domain Java library JAMA. */
+
+#ifndef _eig_h
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* 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]);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
Modified: pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.cpp
===================================================================
--- pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.cpp 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.cpp 2008-04-18 00:50:56 UTC (rev 127)
@@ -1,5 +1,6 @@
#include <iostream>
#include "scanmatcherprocessor.h"
+#include "eig3.h"
//#define SCANMATHCERPROCESSOR_DEBUG
namespace GMapping {
@@ -14,7 +15,7 @@
m_maxMove=1;
m_beams=0;
m_computeCovariance=false;
- m_eigenspace=gsl_eigen_symmv_alloc(3);
+ //m_eigenspace=gsl_eigen_symmv_alloc(3);
useICP=false;
}
@@ -27,12 +28,12 @@
m_maxMove=1;
m_beams=0;
m_computeCovariance=false;
- m_eigenspace=gsl_eigen_symmv_alloc(3);
+ //m_eigenspace=gsl_eigen_symmv_alloc(3);
useICP=false;
}
ScanMatcherProcessor::~ScanMatcherProcessor (){
- gsl_eigen_symmv_free(m_eigenspace);
+ //gsl_eigen_symmv_free(m_eigenspace);
}
@@ -143,19 +144,36 @@
if(m_computeCovariance){
ScanMatcher::CovarianceMatrix cov;
score=m_matcher.optimize(newPose, cov, m_map, m_pose, plainReading);
+ /*
gsl_matrix* m=gsl_matrix_alloc(3,3);
gsl_matrix_set(m,0,0,cov.xx); gsl_matrix_set(m,0,1,cov.xy); gsl_matrix_set(m,0,2,cov.xt);
gsl_matrix_set(m,1,0,cov.xy); gsl_matrix_set(m,1,1,cov.yy); gsl_matrix_set(m,1,2,cov.yt);
gsl_matrix_set(m,2,0,cov.xt); gsl_matrix_set(m,2,1,cov.yt); gsl_matrix_set(m,2,2,cov.tt);
gsl_matrix* evec=gsl_matrix_alloc(3,3);
gsl_vector* eval=gsl_vector_alloc(3);
- gsl_eigen_symmv (m, eval, evec, m_eigenspace);
+ */
+ double m[3][3];
+ double evec[3][3];
+ double eval[3];
+ m[0][0] = cov.xx;
+ m[0][1] = cov.xy;
+ m[0][2] = cov.xt;
+ m[1][0] = cov.xy;
+ m[1][1] = cov.yy;
+ m[1][2] = cov.yt;
+ m[2][0] = cov.xt;
+ m[2][1] = cov.yt;
+ m[2][2] = cov.tt;
+
+ //gsl_eigen_symmv (m, eval, evec, m_eigenspace);
+ eigen_decomposition(m,evec,eval);
#ifdef SCANMATHCERPROCESSOR_DEBUG
- cout << "evals=" << gsl_vector_get(eval, 0) << " " << gsl_vector_get(eval, 1)<< " " << gsl_vector_get(eval, 2)<<endl;
+ //cout << "evals=" << gsl_vector_get(eval, 0) << " " << gsl_vector_get(eval, 1)<< " " << gsl_vector_get(eval, 2)<<endl;
+ cout << "evals=" << eval[0] << " " << eval[1]<< " " << eval[2]<<endl;
#endif
- gsl_matrix_free(m);
- gsl_matrix_free(evec);
- gsl_vector_free(eval);
+ //gsl_matrix_free(m);
+ //gsl_matrix_free(evec);
+ //gsl_vector_free(eval);
} else {
if (useICP){
cerr << "USING ICP" << endl;
Modified: pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.h
===================================================================
--- pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.h 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/scanmatcher/scanmatcherprocessor.h 2008-04-18 00:50:56 UTC (rev 127)
@@ -4,7 +4,7 @@
#include <log/sensorlog.h>
#include <sensor/sensor_range/rangesensor.h>
#include <sensor/sensor_range/rangereading.h>
-#include <gsl/gsl_eigen.h>
+//#include <gsl/gsl_eigen.h>
#include "scanmatcher.h"
namespace GMapping {
@@ -38,7 +38,7 @@
OrientedPoint m_pose;
OrientedPoint m_odoPose;
int m_count;
- gsl_eigen_symmv_workspace * m_eigenspace;
+ //gsl_eigen_symmv_workspace * m_eigenspace;
};
};
Modified: pkg/trunk/gmapping/utils/Makefile
===================================================================
--- pkg/trunk/gmapping/utils/Makefile 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/utils/Makefile 2008-04-18 00:50:56 UTC (rev 127)
@@ -1,8 +1,9 @@
OBJS= stat.o movement.o
-APPS= autoptr_test stat_test
+APPS= autoptr_test #stat_test
-LDFLAGS+= $(GSL_LIB)
-CPPFLAGS+= $(GSL_INCLUDE) -DFSLINE
+#LDFLAGS+= $(GSL_LIB)
+#CPPFLAGS+= $(GSL_INCLUDE) -DFSLINE
+CPPFLAGS+= -DFSLINE
-include ../global.mk
-include ../build_tools/Makefile.generic-shared-object
Modified: pkg/trunk/gmapping/utils/stat.cpp
===================================================================
--- pkg/trunk/gmapping/utils/stat.cpp 2008-04-18 00:35:41 UTC (rev 126)
+++ pkg/trunk/gmapping/utils/stat.cpp 2008-04-18 00:50:56 UTC (rev 127)
@@ -1,12 +1,15 @@
-#include <gsl/gsl_rng.h>
-#include <gsl/gsl_randist.h>
-#include <gsl/gsl_eigen.h>
-#include <gsl/gsl_blas.h>
+#include <stdlib.h>
+
+//#include <gsl/gsl_rng.h>
+//#include <gsl/gsl_randist.h>
+//#include <gsl/gsl_eigen.h>
+//#include <gsl/gsl_blas.h>
#include <math.h>
#include "utils/stat.h"
namespace GMapping {
+#if 0
int sampleUniformInt(int max)
{
@@ -19,18 +22,48 @@
}
+#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 long 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);
+ {
+ //gsl_rng_set(r, S);
+ srand48(S);
+ }
if (sigma==0)
return 0;
- return gsl_ran_gaussian (r,sigma);
+ //return gsl_ran_gaussian (r,sigma);
+ return pf_ran_gaussian (sigma);
}
+#if 0
double evalGaussian(double sigmaSquare, double delta){
if (sigmaSquare<=0)
@@ -38,11 +71,13 @@
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.};
@@ -141,6 +176,8 @@
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));
@@ -151,6 +188,7 @@
return evalLogGaussian(covariance.eval[0], v1)+evalLogGaussian(covariance.eval[1], v2)+evalLogGaussian(covariance.eval[2], v3);
}
+#if 0
void Gaussian3::computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights ){
OrientedPoint mean=OrientedPoint(0,0,0);
double wcum=0;
@@ -234,6 +272,7 @@
this->covariance=ecov;
this->cov=cov;
}
+#endif
}// end namespace
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
