Comparing two Occupancy Grids

edit

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 ...