In this project we have been looking at merging maps from different robots. In the end we wanted to be able to build a common map using all dikus scorpion robots. It would be nice if a number of autonomous robot could cooperate in order to make a map, explore unexplored areas, locate "interesting" things, ect.
How we started
We had a computer with ERSP installed and four scorpions at our disposal. The first step was to get an overview of the ERSP software. Which turned out to be more difficult than expected. The tutorial and user manual was too high-level and the doxygen manual was too low-level. A clear description of the design strategy behind the ERSP software platform would have helped alot. It seemed like the people at evolution weren't interested in extensions of their software.
We had a couple of articles describing the functionality of vSLAM, and our first idea was to intercept all calls to and from the database, as described in figure 9 in the UserGuide.pdf (provided with the ERSP installation). In this way we could make a network layer between the robots local database and a global database, or otherwise intervene in the creation and recognition of landmarks. The problem was that the actual implementation did not follow that description. There was no clean interface to the landmark database, in fact there was not really a landmark-database at all. There was a database that could keep information about the look of a landmark, whereas the information about the location, time stamp and camera parameters were kept else where (only God knows where!).
So after a little fiddling around we discarded our first idea, and settled with saving and loading whole databases in a server-client manner. Again this was easier said than done. Even though there was a build-in save and load database functionality in ERSP, there was a lot of initialization problems (see Pitt falls below), and it seemed impossible to add a landmark from one database to another database in a proper way.
So we degraded our design, to merging the two maps outside ERSP and use gnuplot to visualize the 3D-points obtained from each landmark.
A landmark is not a disk or a cola-can, or any other object as such. A landmark is a view -- a picture of all the things a robot sees from a certain position. This position is then the position of that landmark, and is given with x,y coordinates and the angle in witch the robot was looking at the time.
There is always two coordinate system in use. A global describing the positions of the landmarks, and a local where the robot always look out the x-axis despite the position in the global.
The initialization order of the database and other object i important (!see email!)
Current state & results
Well how far did we get? We found it impossible to add landmarks from a database, either send to us from another robot or saved at a previous run, so we settled with a solution where we used gnuplot to plot landmark location and 3D-points.
On this image is a plot of the "foreign" map that has to be rotated and translated to fit the local map. This is a map of the image lab at diku, the x-axis is pointing towards the camera sled.
The red dots are positions of feature points in landmarks while the green are the robots position when the landmark was created.
This is the local map of the robot. The x-axis is pointing towards the corner with the white board.
This is the rotated and translated "foreign" map (Map1).
Here is the plot of both maps clarifying that the rotation and translation was correct. It first becomes really useful when the two maps only have a little overlap, so the final map covers a larger area.
The red dots are positions of feature points while the green are the robots position from the "foreign" map. The blue dots are positions of feature points while the purple are robot positions from the local map.
The source code can be found in the subversion repository in the Merge_Maps directory: http://image.diku.dk/svn/slam/trunk/Merge_Maps/
You can see how to acces the svn server on this page: svn at image.diku.dk
Directory listing * .deps/ * .libs/ * Makefile * SLAMCorrection.cpp * SLAMCorrection.hpp * SLAMVar.hpp * dbplot.gp * joystick_drive.cpp * joystick_drive.hpp * main.cpp * plot.gp * protocol.cpp * protocol.hpp
Directories .deps and .libs is par of ERSP dependencies.
SLAMCorrection is inherited from OdometryCorrector and allows us to access landmanrk and measurement events.
SLAMVar is global configuration and defines.
joystick_drive constructs a thread for handeling joystick input.
main: (our program) coVarToNum: converts a covarians matrix to fitness value. initialize_interfaces: initializes ERSP interfaces clean_up: releases ERSP interfaces initialize_slam: sets up the SLAM correcter clean_up_slam: releases SLAMCorrector vslam_add_odometry: adds positions to vslam-odometry buffer vslam_add_keypoints: creates a LandmarkEvidenceObjRec if there is enough features in the key point list. query_user: waits for user input. printDBstatus: prints the landmarks from the database on screen. getPivotPos: calculates dead pivot new position and the rotation angle. rotatePoint: rotates any given point. mergeMaps: intented to merge two databases into a single live databse, does not work! compareSLAM: find a best matching landmarks in two databases. printMap: prints a landmark-database to a file, so it can be read by gnuplot. printMergeMaps: prints the rotated map to a file. blockTillJoyButton: breaks when joystick button 0 is pressed netMain: collect landmarks untill joystick button 0 is pressed, tries to exchange database with a specified other robot, and merge their databases. main: glues it all together.
dbplot.gp and plot.pg is gnuplot scripts for visualizing program out-data.
How to call this program?
Standalone creates one database and saves it, then it creates another one and tries to merge them.
Database reads two databases from files and tries to merge them.
<ip-address> requires two robots both running the same programm, given the ip-address of the other robot it wil run netMain until it is asked to request the other robots database, it then tries to merge them.
Each time the program is called it will generate two data-files for each database (map.data, landmark.data). In one file all the 3D points is written and in the other the landmark positions. If there is a match the program will generate two new files containing the rotated version of one of the databases. These datafiles can be plottet using gnuplot.