Ask Your Question
1

Comparing two Occupancy Grids

asked 2013-07-08 09:01:20 -0500

Zayin gravatar image

updated 2014-01-28 17:17:10 -0500

ngrennan gravatar image

I am attempting to find a way of comparing two stage occupancy grids: the first one is generated by slam gmapping (published on the "map" topic) and the second comes from the rasterization of the stage floor model (myModel.Rasterize(myRasterArray, this->width, this->height, cellwidth, cellheight) - see this page http://rtv.github.io/Stage/classStg_1_1Model.html). I managed to access both of these grids, and I generated both grids with the same height, width and resolution. Now, I am wondering if both grids possess the same coordinates; for instance, would comparing the cells at position (1,2) correspond to the same cell on both maps? If not, is there a way of superimposing both grids?

[Edit] I printed the occupancy grids obtained, and the two aren't even oriented in the same way (see picture here http://tinypic.com/r/x2ph5l/5). #1 is the slam map, #2 is the map extracted from the Stage model, and the image to the right is the bitmap used to build the Stage map (yes, a smiley face!). Note that the robot is wandering inside the white area, which explains why image #1 is just the inside of the smiley face. All in all, I need to find a way of aligning the grids... Please help?

[Edit2] I'm attempting to use the debug option of mapstitch which is supposed to "save the current_stitch.pgm and current_map.pgm, i.e. the stitched maps and the current map as seen on /map.", but there is nothing being outputted. Is this a bug? Here's my launch file:

<launch>
  <master auto="start"/>
  <param name="/use_sim_time" value="true"/>  
  <node pkg="mapstitch" type="ros_mapstitch" name="ros_mapstitch">    
    <param name="debug" value ="true" />
  </node>  
</launch>
edit retag flag offensive close merge delete

Comments

So may I ask if you know a way to do things around - having two occupancy and compare them to get coordinates? Please advice me. Thank you.

Avonic gravatar imageAvonic ( 2016-02-26 22:05:11 -0500 )edit

@Avonic: that sounds like you have a different question. You should ask a new question.

ahendrix gravatar imageahendrix ( 2016-02-27 00:57:16 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
3

answered 2013-07-09 01:44:32 -0500

dornhege gravatar image

Have a look at the mapstitch package.

edit flag offensive delete link more

Comments

Thanks! That might be just what I needed! I'll keep you informed.

Zayin gravatar imageZayin ( 2013-07-09 05:04:50 -0500 )edit

My map is not being transformed as it should; it seems like mapstitch isn't doing anything. Could it be because nothing is publishing on the world topic despite the fact that mapstitch is listening to it? Which node should be publishing on world? I'm using stage and there's no such topic as world.

Zayin gravatar imageZayin ( 2013-07-09 08:58:05 -0500 )edit

I managed to publish the world used by stage on the /world and /align topics. However, mapstitch still doesn't seem to be doing anything.

Zayin gravatar imageZayin ( 2013-07-10 10:32:58 -0500 )edit
2

answered 2013-07-11 06:32:26 -0500

Zayin gravatar image

updated 2013-07-31 05:08:48 -0500

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 ...
(more)
edit flag offensive delete link more

Comments

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.

dornhege gravatar imagedornhege ( 2013-07-11 08:14:58 -0500 )edit

I might do that if my results are not good enough. Thanks!

Zayin gravatar imageZayin ( 2013-07-11 10:11:13 -0500 )edit
1

I asked the mapstitch author about my problem and received a reply here: https://github.com/tu-darmstadt-ros-pkg/mapstitch/issues/1

Zayin gravatar imageZayin ( 2013-07-15 05:14:17 -0500 )edit
1

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 .

sagi0910 gravatar imagesagi0910 ( 2016-11-06 07:05:19 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-07-08 09:01:20 -0500

Seen: 4,235 times

Last updated: Jul 31 '13