I found a somewhat convoluted solution to my problem using the above answer. First, I used the map_saver to store my slam gmapping map. Then, I created a copy of map_saver that listens on the world topic instead of the map topic. I programmed a publisher publishing the real world map on the world topic. As a result, I have both my slam map and world map with the same size and resolution stored as pgm. However, they are not aligned. Hence, I modified the get_stitch function in mapstitch.cpp by commenting out addWeighted. That way, the first map can be aligned with the second without them being merged. With both maps aligned, I can compare them -I use Graphics Magick. I will post my code once it's cleaned up and commented.
However, I'd be curious to know how to make mapstitch work the way it's supposed to (using rosrun). I think that might produce more accurate results.
[Edit] Here is the code I use. It might not work out of the box so I haven't created a repo. Sorry if it's a bit messy, it was my first real c++ program.
generator.cpp:
#include <ros/ros.h>
#include <nav_msgs/GetMap.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <navfn/navfn_ros.h>
#include <mapfetch.h>
int main(int argc, char** argv){
std::vector<uint8_t> realMap; //Real world map.
ros::init(argc, argv, "compare");
/*Publishers for the real map on the /world and /align topics.*/
ros::NodeHandle nodeH;
ros::Publisher worldPub = nodeH.advertise<nav_msgs::OccupancyGrid>("world", 1000);
ros::Publisher alignPub = nodeH.advertise<nav_msgs::OccupancyGrid>("align", 1000);
ros::Rate loop_rate(1);
MapFetcher fetcher(argc, argv, argv[argc - 4]);
ROS_INFO("Map width: %d, height: %d, resolution: %g", fetcher.width, fetcher.height, fetcher.resolution);
/*Get the occupancy grid of the real map with the same
width, height and resolution as the gmapping map.*/
realMap = fetcher.getGroundTruth();
//Convert the map to a standard ros occupancy grid.
std::vector<int8_t> map;
for(int i = 0; i < +realMap.size(); i++){
if(+realMap[i] == 1){ //Occupied cell.
map.push_back(100);
}
else if (+realMap[i] == 0){ //Free cell.
map.push_back(0);
}
else{
map.push_back(-1); //Unknown cell.
}
}
//Publish the occupancy grid.
while (ros::ok()){
nav_msgs::OccupancyGrid mapGrid;
nav_msgs::MapMetaData metaData;
metaData.resolution = fetcher.resolution;
metaData.width = fetcher.width;
metaData.height = fetcher.height;
mapGrid.data = map;
mapGrid.info = metaData;
worldPub.publish(mapGrid);
alignPub.publish(mapGrid);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
mapfetcher.cpp
#include <ros/ros.h>
#include <nav_msgs/GetMap.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <navfn/navfn_ros.h>
#include <sys/stat.h>
#include <nav_msgs/MapMetaData.h>
#include "std_msgs/String.h"
#include <iostream>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <rosgraph_msgs/Clock.h>
// libstage
#include <stage.hh>
#define USAGE "stageros <worldfile>"
#define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
using namespace std;
class MapFetcher {
private:
int argc;
char **argv;
char *fname;
ros::Time sim_time;
ros::Publisher clock_pub_;
std::vector<ros::Publisher> ground_truth_pubs_;
rosgraph_msgs::Clock clockMsg;
boost::mutex msg_lock;
ros::Subscriber mapSub;
ros::Subscriber metaSub;
Stg::Model *groundModel;
/**Map data**/
std::vector<uint8_t> realMap;
std::vector<int8_t> builtMap;
void metaDataCallback(const nav_msgs::MapMetaData::ConstPtr& msg);
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
void setGroundTruth();
public:
MapFetcher(int argc, char** argv, char* fname);
~MapFetcher();
std::vector<int8_t> getSlamMap();
std::vector<uint8_t> getGroundTruth();
float resolution; //Map resolution
unsigned int width; // Map width [cells]
unsigned int height; //Map height [cells]
};
//Constructor.
MapFetcher::MapFetcher(int argc, char** argv, char* fname) :
argc(argc),
argv(argv),
fname(fname),
resolution(0),
width(0),
height(0) {
ros::NodeHandle node;
//Get the slam map's occupancy grid and its metadata.
metaSub = node.subscribe("map_metadata", 1, &MapFetcher::metaDataCallback, this);
mapSub = node.subscribe("map", 1, &MapFetcher::mapCallback, this);
width = atoi(argv[argc - 2]);
height = atoi(argv[argc - 1]);
//Wait for the map data to be fetched.
while(resolution == 0 && ros::ok){
ros::spinOnce();
sleep(2);
}
//Set the real map.
setGroundTruth();
}
//Destructor
MapFetcher::~MapFetcher(){
//DESTROY EVERYTHING BOOM
}
//Return the real map.
std::vector<uint8_t> MapFetcher::getGroundTruth(){
return realMap;
}
//Return the map built by slam gmapping.
std::vector<int8_t> MapFetcher::getSlamMap(){
return builtMap;
}
/**
* HELPER FUNCTIONS
* **/
//Get the occupancy grid for the gmapping map.
void MapFetcher::mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
this->builtMap = msg->data;
}
//Get the occupancy grid metadata.
void MapFetcher::metaDataCallback(const nav_msgs::MapMetaData::ConstPtr& msg) {
/*this->width = msg->width;
this->height = msg->height;*/
/*I changed the above two values to constants because executing this code
when the slam map was not complete caused cropping issues.*/
this->resolution = msg->resolution;
}
//Set the real map as displayed on Stage.
void MapFetcher::setGroundTruth(){
ros::NodeHandle node2;
struct stat s; //roscpp-related bookkeeping
//Timing parameters.
sim_time.fromSec(0.0);
node2.setParam("/use_sim_time", true);
clock_pub_ = node2.advertise<rosgraph_msgs::Clock>("/clock",10);
ros::WallRate r(10.0);
// We'll check the existence of the world file, because libstage doesn't
// expose its failure to open it.
if(stat(fname, &s) != 0){
ROS_FATAL("The world file %s does not exist.", fname);
ROS_BREAK();
}
//Initialize libstage.
Stg::Init(&argc-1, &argv);
//Load the world.
Stg::World *world = new Stg::World;
(*world).Load(fname);
(*world).Start();
groundModel = (*world).GetModel(argv[argc - 3]);
//Rasterize the floor model.
uint8_t raster[this->width * this->height];
Stg::meters_t cellwidth = this->resolution;
Stg::meters_t cellheight = this->resolution;
(*groundModel).Rasterize(raster, this->width, this->height, cellwidth, cellheight);
std::vector<uint8_t> temp(raster, raster + sizeof(raster) / sizeof(raster[0]));
this->realMap = temp;
}
I also found out that the GIMP Image Registration Plugin works quite well where mapstitch fails.
Originally posted by Zayin with karma: 343 on 2013-07-11
This answer was NOT ACCEPTED on the original site
Post score: 2
Original comments
Comment by dornhege on 2013-07-11:
I guess that's up to the darmstadt guys to explain. If you are interested I'd suggest opening a new question directly aimed at that.
Comment by Zayin on 2013-07-11:
I might do that if my results are not good enough. Thanks!
Comment by Zayin on 2013-07-15:
I asked the mapstitch author about my problem and received a reply here: https://github.com/tu-darmstadt-ros-pkg/mapstitch/issues/1
Comment by sagi0910 on 2016-11-06:
Can you please share your final solution ? and I'm also interested on what metrics did you use to estimate the error after the alignment of the two maps .