/**
 * \brief   Recusrive function performing DFS
 */
bool DirectedDFS::searchForPath(const Point2d &start, const Point2d &goal,
                                uint32_t depth, std::vector<bool> &visited, bool in_obstacle_space) {

    //std::cout << start.x << " " << start.y << std::endl;

    // Termination crit
    if (start == goal) {
        return true;
    }
    if (depth == 0) {
        return false;
    }

    uint32_t start_idx = MAP_IDX(map_.info.width, start.x, start.y);
    visited[start_idx] = true;

    std::vector<Point2d> neighbours;
    getOrderedNeighbours(start, goal, visited, neighbours, in_obstacle_space);
    for (size_t i = 0; i < neighbours.size(); ++i) {
        Point2d& n = neighbours[i];
        // Check if it has been visited again - quite likely that one of the
        // previous loop iterations have covered this already
        uint32_t n_idx = MAP_IDX(map_.info.width, n.x, n.y);
        if (visited[n_idx]) {
            continue;
        }
        bool success = searchForPath(n, goal, depth - 1, visited);
        if (success)
            return true;
    }

    return false; // disconnected components
}
Exemple #2
0
  bool locationsInDirectLineOfSight(const Point2f& pt1, const Point2f& pt2,
      const nav_msgs::OccupancyGrid map) {

    int x0 = lrint(pt1.x), y0 = lrint(pt1.y);
    int x1 = lrint(pt2.x), y1 = lrint(pt2.y);
    int dx = abs(x1-x0), dy = abs(y1-y0);
    int sx = (x0 < x1) ? 1 : -1;
    int sy = (y0 < y1) ? 1 : -1;
    float err = dx - dy;
    bool is_occupied = false;
    while (!is_occupied) {
      is_occupied = map.data[MAP_IDX(map.info.width, x0, y0)] > 50;
      if (x0 == x1 && y0 == y1) break;
      float e2 = 2 * err;
      if (e2 > -dy) {
        err -= dy;
        x0 += sx;
      }
      if (x0 == x1 && y0 == y1) {
        is_occupied = map.data[MAP_IDX(map.info.width, x0, y0)] > 50;
        break;
      }
      if (e2 < dx) {
        err += dx;
        y0 += sy;
      }
    }
    return !is_occupied;
  }
void CostMapCalculation::sysMessageCallback(const std_msgs::String& string)
{
    ROS_DEBUG("HectorCM: sysMsgCallback, msg contents: %s", string.data.c_str());

    if (string.data == "reset")
    {
        ROS_INFO("HectorCM: reset");

        // set default values
        cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
        elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
        cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);

        // loop through starting area
        min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
        min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
        max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
        max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
        for (int v = min_index(1); v < max_index(1); ++v)
            for (int u = min_index(0); u < max_index(0); ++u)
                cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;

        received_elevation_map = false;
        received_grid_map      = false;
        received_point_cloud   = false;
    }
}
  /**
   * \brief   draws colored regions corresponding to each critical region
   *          onto a given image starting at (orig_x, orig_y)
   */
  void TopologicalMapper::drawConnectedComponents(cv::Mat &image,
      uint32_t orig_x, uint32_t orig_y) {

    // Figure out different colors - 1st color should always be black
    size_t num_colors = num_components_;
    component_colors_.resize(num_colors);
    for (size_t i = 0; i < num_colors; ++i) {
      component_colors_[i] = 
        cv::Vec3b(160 + rand() % 64, 160 + rand() % 64, 160 + rand() % 64);
    }

    // component 0 is obstacles + background. don't draw?

    // Now paint!
    for (uint32_t j = 1; j < map_resp_.map.info.height; ++j) {
      cv::Vec3b* image_row_j = image.ptr<cv::Vec3b>(j + orig_y);
      for (uint32_t i = 0; i < map_resp_.map.info.width; ++i) {
        size_t map_idx = MAP_IDX(map_resp_.map.info.width, i, j);
        if (component_map_[map_idx] == -1)
          continue;
        cv::Vec3b& pixel = image_row_j[i + orig_x];
        pixel[0] = component_colors_[component_map_[map_idx]][0];
        pixel[1] = component_colors_[component_map_[map_idx]][1];
        pixel[2] = component_colors_[component_map_[map_idx]][2];

      }
    }
  }
/**
 * \brief   Gets neighbours for a given node iff they are also obstacles
 *          and have not been visited before
 */
void DirectedDFS::getOrderedNeighbours(const Point2d &from,
                                       const Point2d &goal, const std::vector<bool> &visited,
                                       std::vector<Point2d> &neighbours, bool in_obstacle_space) {

    size_t neighbour_count = 8;
    uint32_t x_offset[] = {-1, 0, 1, -1, 1, -1, 0, 1};
    uint32_t y_offset[] = {-1, -1, -1, 0, 0, 1, 1, 1};
    neighbours.clear();
    for (size_t i = 0; i < neighbour_count; ++i) {
        // Check if neighbours are still on map
        Point2d p = from + Point2d(x_offset[i],y_offset[i]);
        // covers negative case as well (unsigned)
        if (p.x >= (int) map_.info.width || p.y >= (int) map_.info.height ||
                p.x <= 0 || p.y <= 0) {
            continue;
        }
        uint32_t map_idx = MAP_IDX(map_.info.width, p.x, p.y);
        if (visited[map_idx] || (in_obstacle_space && map_.data[map_idx] == 0) ||
                (!in_obstacle_space && map_.data[map_idx] != 0)) {
            continue;
        }
        p.distance_from_ref = norm(p - goal);
        neighbours.push_back(p);
    }
    std::sort(neighbours.begin(), neighbours.end(), Point2dDistanceComp());
}
void CostMapCalculation::updateMapParamsCallback(const nav_msgs::MapMetaData& map_meta_data)
{
    ROS_DEBUG("HectorCM: received new map meta data -> overwrite old parameters");

    // check if new parameters differ and abort otherwise
    if (cost_map.info.width == map_meta_data.width &&
        cost_map.info.height == map_meta_data.height &&
        cost_map.info.resolution == map_meta_data.resolution &&
        cost_map.info.origin.position.x == map_meta_data.origin.position.x &&
        cost_map.info.origin.position.y == map_meta_data.origin.position.y &&
        cost_map.info.origin.position.z == map_meta_data.origin.position.z &&
        cost_map.info.origin.orientation.x == map_meta_data.origin.orientation.x &&
        cost_map.info.origin.orientation.y == map_meta_data.origin.orientation.y &&
        cost_map.info.origin.orientation.z == map_meta_data.origin.orientation.z &&
        cost_map.info.origin.orientation.w == map_meta_data.origin.orientation.w) {
      return;
    }

    // set new parameters
    cost_map.info.width = map_meta_data.width;
    cost_map.info.height = map_meta_data.height;
    cost_map.info.resolution = map_meta_data.resolution;

    cost_map.info.origin.position.x = map_meta_data.origin.position.x;
    cost_map.info.origin.position.y = map_meta_data.origin.position.y;
    cost_map.info.origin.position.z = map_meta_data.origin.position.z;

    cost_map.info.origin.orientation.x = map_meta_data.origin.orientation.x;
    cost_map.info.origin.orientation.y = map_meta_data.origin.orientation.y;
    cost_map.info.origin.orientation.z = map_meta_data.origin.orientation.z;
    cost_map.info.origin.orientation.w = map_meta_data.origin.orientation.w;

    world_map_transform.setTransforms(cost_map.info);

    // allocate memory
    cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
    elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
    cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);

    // loop through starting area
    min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
    min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
    max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
    max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
    for (int v = min_index(1); v < max_index(1); ++v)
        for (int u = min_index(0); u < max_index(0); ++u)
            cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;

    // update flags
    received_elevation_map = false;
    received_grid_map      = false;
    use_cloud_map          = false;
}
void CostMapCalculation::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sensor (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base_link (new pcl::PointCloud<pcl::PointXYZ>);

    ROS_DEBUG("HectorCM: received new point cloud");

    // converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
    pcl::fromROSMsg(*cloud_msg, *cloud_sensor);

    // transform cloud to /map frame
    try
    {
        listener.waitForTransform("base_stabilized", cloud_msg->header.frame_id,cloud_msg->header.stamp,ros::Duration(1.0));
        pcl_ros::transformPointCloud("base_stabilized",*cloud_sensor,*cloud_base_link,listener);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s",ex.what());
        ROS_DEBUG("HectorCM: pointcloud transform failed");
        return;
    }

    // compute region of intereset
    if(!computeWindowIndices(cloud_msg->header.stamp, update_radius_world))
        return;

    Eigen::Vector2f world_coords;
    // define a cube with two points in space:
    Eigen::Vector4f minPoint;
    world_coords = world_map_transform.getC1Coords(min_index.cast<float>());
    minPoint[0]=world_coords(0);  // define minimum point x
    minPoint[1]=world_coords(1);  // define minimum point y
    minPoint[2]=slize_min_height; // define minimum point z

    Eigen::Vector4f maxPoint;
    world_coords = world_map_transform.getC1Coords(max_index.cast<float>());
    maxPoint[0]=world_coords(0);  // define max point x
    maxPoint[1]=world_coords(1);  // define max point y
    maxPoint[2]=slize_max_height; // define max point z

    pcl::CropBox<pcl::PointXYZ> cropFilter;
    cropFilter.setInputCloud (cloud_base_link);
    cropFilter.setMin(minPoint);
    cropFilter.setMax(maxPoint);
    cropFilter.filter (*sliced_cloud);

    ROS_DEBUG("HectorCM: slice size: %d", (int)sliced_cloud->size());
    pub_cloud_slice.publish(sliced_cloud);

    cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);

    // iterate trough all points
    for (unsigned int k = 0; k < sliced_cloud->size(); ++k)
    {
        const pcl::PointXYZ& pt_cloud = sliced_cloud->points[k];

        // allign grid points
        Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
        Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));

        cloud_cost_map.data[MAP_IDX(cost_map.info.width, (int)round(index_map(0)), (int)round(index_map(1)))] = OCCUPIED_CELL;
    }

    // set point cloud received flag
    received_point_cloud = true;

    // calculate cost map
    if(received_grid_map)
    {
        if(received_elevation_map)
        {
            calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
        }
        else
        {
            calculateCostMap(USE_GRID_AND_CLOUD_MAP);
        }
    }
}
void CostMapCalculation::callbackElevationMap(const hector_elevation_msgs::ElevationGridConstPtr& elevation_map_msg)
{
    ROS_DEBUG("HectorCM: received new elevation map");

    // check header
    if((int)(1000*elevation_map_msg->info.resolution_xy) != (int)(1000*cost_map.info.resolution) &&  // Magic number 1000 -> min grid size should not be less than 1mm
            elevation_map_msg->info.height != cost_map.info.height &&
            elevation_map_msg->info.width != cost_map.info.width)
    {
        ROS_ERROR("HectorCM: elevation map resolution and or size incorrect!");
        return;
    }

    // store elevation_map_msg in local variable
    elevation_map_ = cv::Mat(elevation_map_msg->info.height, elevation_map_msg->info.width, CV_16S, const_cast<int16_t*>(&elevation_map_msg->data[0]), 2*(size_t)elevation_map_msg->info.width);

    // store elevation map zero level
    elevation_zero_level = elevation_map_msg->info.zero_elevation;

    // compute region of intereset
    if(!computeWindowIndices(elevation_map_msg->header.stamp, update_radius_world))
        return;

    // loop through each element
    int filtered_cell, filtered_cell_x, filtered_cell_y;
    for (int v = min_index(1); v < max_index(1); ++v)
    {
        for (int u = min_index(0); u < max_index(0); ++u)
        {
            // compute cost_map_index
            unsigned int cost_map_index = MAP_IDX(cost_map.info.width, u, v);

            // check if neighbouring cells are known
            if(elevation_map_.at<int16_t>(v+1,u) == (-elevation_zero_level) ||
                    elevation_map_.at<int16_t>(v-1,u) == (-elevation_zero_level) ||
                    elevation_map_.at<int16_t>(v,u+1) == (-elevation_zero_level) ||
                    elevation_map_.at<int16_t>(v,u-1) == (-elevation_zero_level))
                continue;

            // edge filter
            filtered_cell_y = abs(elevation_map_.at<int16_t>(v,u-1) - elevation_map_.at<int16_t>(v,u+1));
            filtered_cell_x = abs(elevation_map_.at<int16_t>(v-1,u) - elevation_map_.at<int16_t>(v+1,u));


            if(filtered_cell_x > filtered_cell_y)
                filtered_cell = filtered_cell_x;
            else
                filtered_cell =  filtered_cell_y;

            // check if cell is traversable
            if(filtered_cell > max_delta_elevation/grid_res_z)
            {
                // cell is not traversable -> mark it as occupied
                elevation_cost_map.data[cost_map_index] = OCCUPIED_CELL;
            }
            else
            {
                // cell is traversable -> mark it as free
                elevation_cost_map.data[cost_map_index] = FREE_CELL;
            }
        }
    }

    // set elevation map received flag
    received_elevation_map = true;

    // calculate cost map
    if(received_grid_map)
    {
        if(received_point_cloud)
        {
            calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
        }
        else
        {
            calculateCostMap(USE_GRID_AND_ELEVATION_MAP);
        }
    }
    else
    {
        calculateCostMap(USE_ELEVATION_MAP_ONLY);
    }
}
CostMapCalculation::CostMapCalculation() : nHandle(), pnHandle("~")
{
    int width, height;
    double grid_res_xy;

    pnHandle.param("elevation_resolution", grid_res_z, 0.01); //[m]

    pnHandle.param("max_grid_size_x", width, 1024); //[cell]
    cost_map.info.width = width;
    pnHandle.param("max_grid_size_y", height, 1024); //[cell]
    cost_map.info.height = height;
    pnHandle.param("map_resolution", grid_res_xy, 0.05); //[m]
    cost_map.info.resolution = grid_res_xy;
    pnHandle.param("origin_x",cost_map.info.origin.position.x, -(double)cost_map.info.width*(double)cost_map.info.resolution/2.0); //[m]
    pnHandle.param("origin_y",cost_map.info.origin.position.y, -(double)cost_map.info.height*(double)cost_map.info.resolution/2.0); //[m]
    pnHandle.param("origin_z",cost_map.info.origin.position.z, 0.0); //[m]
    pnHandle.param("orientation_x",cost_map.info.origin.orientation.x, 0.0); //[]
    pnHandle.param("orientation_y",cost_map.info.origin.orientation.y, 0.0); //[]
    pnHandle.param("orientation_z",cost_map.info.origin.orientation.z, 0.0); //[]
    pnHandle.param("orientation_w",cost_map.info.origin.orientation.w, 1.0); //[]

    pnHandle.param("initial_free_cells_radius", initial_free_cells_radius, 0.30); //[m]
    pnHandle.param("update_radius", update_radius_world, 4.0); //[m]
    pnHandle.param("max_delta_elevation", max_delta_elevation, 0.08); //[m]

    pnHandle.param("map_frame_id", map_frame_id,std::string("map"));
    pnHandle.param("local_transform_frame_id", local_transform_frame_id,std::string("base_footprint"));
    pnHandle.param("cost_map_topic", cost_map_topic, std::string("cost_map"));
    pnHandle.param("elevation_map_topic", elevation_map_topic, std::string("elevation_map_local"));
    pnHandle.param("grid_map_topic", grid_map_topic, std::string("scanmatcher_map"));

    pnHandle.param("use_elevation_map", use_elevation_map, true);
    pnHandle.param("use_grid_map", use_grid_map, true);
    pnHandle.param("use_cloud_map", use_cloud_map, false);
    pnHandle.param("allow_elevation_map_to_clear_occupied_cells", allow_elevation_map_to_clear_occupied_cells, true);
    pnHandle.param("max_clear_size", max_clear_size, 2);

    pnHandle.param("point_cloud_topic", point_cloud_topic,std::string("openni/depth/points"));
    pnHandle.param("slize_min_height", slize_min_height, 0.30); //[m]
    pnHandle.param("slize_max_height", slize_min_height, 0.35); //[m]

    pnHandle.param("costmap_pub_freq", costmap_pub_freq, 4.0); //[Hz]

    pnHandle.param("sys_msg_topic", sys_msg_topic, std::string("syscommand"));

    cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
    elevation_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
    cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);

    world_map_transform.setTransforms(cost_map.info);

    // loop through starting area
    min_index(0) = cost_map.info.width/2-floor(initial_free_cells_radius/cost_map.info.resolution);
    min_index(1) = cost_map.info.height/2-floor(initial_free_cells_radius/cost_map.info.resolution);
    max_index(0) = cost_map.info.width/2+floor(initial_free_cells_radius/cost_map.info.resolution);
    max_index(1) = cost_map.info.height/2+floor(initial_free_cells_radius/cost_map.info.resolution);
    for (int v = min_index(1); v < max_index(1); ++v)
        for (int u = min_index(0); u < max_index(0); ++u)
            cost_map.data[MAP_IDX(cost_map.info.width, u, v)] = FREE_CELL;

    pub_cost_map = nHandle.advertise<nav_msgs::OccupancyGrid>(cost_map_topic, 1, true);

    received_elevation_map = false;
    received_grid_map      = false;
    received_point_cloud   = false;

    sliced_cloud.reset(new pcl::PointCloud<pcl::PointXYZ>);

    if(use_elevation_map)
        sub_elevation_map = nHandle.subscribe(elevation_map_topic,10,&CostMapCalculation::callbackElevationMap,this);

    if(use_grid_map)
        sub_grid_map = nHandle.subscribe(grid_map_topic,10,&CostMapCalculation::callbackGridMap,this);

    if(use_cloud_map)
        sub_point_cloud = nHandle.subscribe(point_cloud_topic,10,&CostMapCalculation::callbackPointCloud,this);

    sub_map_info = nHandle.subscribe("map_metadata",1,&CostMapCalculation::updateMapParamsCallback,this);

    sub_sysMessage = nHandle.subscribe(sys_msg_topic, 10, &CostMapCalculation::sysMessageCallback, this);
    dyn_rec_server_.setCallback(boost::bind(&CostMapCalculation::dynRecParamCallback, this, _1, _2));

    timer = pnHandle.createTimer(ros::Duration(1.0/costmap_pub_freq), &CostMapCalculation::timerCallback,this);

    ROS_INFO("HectorCM: is up and running.");

    pub_cloud_slice = pnHandle.advertise<sensor_msgs::PointCloud2>("cloud_slice",1,true);

    ros::spin();
}
bool
SlamKarto::updateMap()
{
    boost::mutex::scoped_lock(map_mutex_);

    karto::OccupancyGrid* occ_grid =
        karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);

    if(!occ_grid)
        return false;

    if(!got_map_) {
        map_.map.info.resolution = resolution_;
        map_.map.info.origin.position.x = 0.0;
        map_.map.info.origin.position.y = 0.0;
        map_.map.info.origin.position.z = 0.0;
        map_.map.info.origin.orientation.x = 0.0;
        map_.map.info.origin.orientation.y = 0.0;
        map_.map.info.origin.orientation.z = 0.0;
        map_.map.info.origin.orientation.w = 1.0;
    }

    // Translate to ROS format
    kt_int32s width = occ_grid->GetWidth();
    kt_int32s height = occ_grid->GetHeight();
    karto::Vector2<kt_double> offset = occ_grid->GetCoordinateConverter()->GetOffset();

    if(map_.map.info.width != (unsigned int) width ||
            map_.map.info.height != (unsigned int) height ||
            map_.map.info.origin.position.x != offset.GetX() ||
            map_.map.info.origin.position.y != offset.GetY())
    {
        map_.map.info.origin.position.x = offset.GetX();
        map_.map.info.origin.position.y = offset.GetY();
        map_.map.info.width = width;
        map_.map.info.height = height;
        map_.map.data.resize(map_.map.info.width * map_.map.info.height);
    }

    for (kt_int32s y=0; y<height; y++)
    {
        for (kt_int32s x=0; x<width; x++)
        {
            // Getting the value at position x,y
            kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));

            switch (value)
            {
            case karto::GridStates_Unknown:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
                break;
            case karto::GridStates_Occupied:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
                break;
            case karto::GridStates_Free:
                map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
                break;
            default:
                ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
                break;
            }
        }
    }

    // Set the header information on the map
    map_.map.header.stamp = ros::Time::now();
    map_.map.header.frame_id = map_frame_;

    sst_.publish(map_.map);
    sstm_.publish(map_.map.info);

    delete occ_grid;

    return true;
}
	void
		loadMapFromFile(dynamic_mapping::GetDynamicMap::Response* resp,
		const char* fname, double res, bool negate,
		double occ_th, double free_th, double* origin, uint32_t *dynamic_imp)
	{
		SDL_Surface* img;

		unsigned char* pixels;
		unsigned char* p;
		int rowstride, n_channels;
		unsigned int i,j;
		int k;
		double occ;
		int color_sum;
		double color_avg;

		int fname_size = strlen(fname);

		// Load the image using SDL.  If we get NULL back, the image load failed.
		if(!(img = IMG_Load(fname)))
		{
			std::string errmsg = std::string("failed to open image file \"") + 
				std::string(fname) + std::string("\"");
			throw std::runtime_error(errmsg);
		}

		// Copy the image data into the map structure
		resp->map.info.width = img->w;
		resp->map.info.height = img->h;
		resp->map.info.resolution = res;
		resp->map.info.origin.position.x = *(origin);
		resp->map.info.origin.position.y = *(origin+1);
		resp->map.info.origin.position.z = 0.0;
		tf::Quaternion q;
		q.setRPY(0,0, *(origin+2));
		resp->map.info.origin.orientation.x = q.x();
		resp->map.info.origin.orientation.y = q.y();
		resp->map.info.origin.orientation.z = q.z();
		resp->map.info.origin.orientation.w = q.w();

		// Allocate space to hold the data
		resp->map.data.resize(resp->map.info.width * resp->map.info.height);
		resp->map.dynamic.resize(resp->map.info.width * resp->map.info.height);

		// Get values that we'll need to iterate through the pixels
		rowstride = img->pitch;
		n_channels = img->format->BytesPerPixel;
		ROS_INFO("Bytes per Pixel: %d\n", n_channels);

		// Copy pixel data into the map structure
		pixels = (unsigned char*)(img->pixels);
		int dyn_area = 1;
		for(j = 0; j < resp->map.info.height; j++)
		{
			for (i = 0; i < resp->map.info.width; i++)
			{
				static unsigned char color[3];
				// Compute mean of RGB for this pixel
				p = pixels + j*rowstride + i*n_channels;
				color_sum = 0;
				// ppm file rgb
				if(n_channels == 3)	{
					color[0] = (int) *p;  	   //red
					color[1] = (int) *(p+1);  //green
					color[2] = (int) *(p+2);  //blue

					// blue pixel
					if(color[1] < color[2])	{					
						resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = -1;
						resp->map.dynamic[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
					}
					// red pixel
					else if(color[0] > color[1])	{
						resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 100 - ((color[1]) * 5 * 100) / 255;
						resp->map.dynamic[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = dynamic_imp[resp->map.info.height*j + i];
					}
					// grey valued pixel
					else	{					
						resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 100 - ((color[1]) * 100) / 255;
						resp->map.dynamic[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = dynamic_imp[resp->map.info.height*j + i];
					}

				}
				//pgm grey valued file
				else	{
						color[0] = (int) *p;  	   //grey
						
						if(color[0] == 254)
							resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
						else if(color[0] == 0)
							resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = -1;
						else
							resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = (color[0] - 100);
			
						resp->map.dynamic[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = 0;
				}
			}
		}

		SDL_FreeSurface(img);
	}
  geometry_msgs::Pose BaseRobotPositioner::positionRobot(
      const bwi_mapper::Point2f& from,
      const bwi_mapper::Point2f& at,
      const bwi_mapper::Point2f& to) {

    geometry_msgs::Pose resp;
    bwi_mapper::Point2f from_map = 
      bwi_mapper::toMap(from, map_info_);
    bwi_mapper::Point2f at_map = 
      bwi_mapper::toMap(at, map_info_);

    // Figure out if you want to stay on the outside angle or not
    bool use_outside_angle = true;
    float yaw1 = -atan2((to - at).y, (to - at).x);
    float yaw2 = -atan2((from - at).y, (from - at).x);

    bwi_mapper::Point2f yaw1_pt(cosf(yaw1),sinf(yaw1));
    bwi_mapper::Point2f yaw2_pt(cosf(yaw2),sinf(yaw2));
    bwi_mapper::Point2f yawmid_pt = 0.5 * (yaw1_pt + yaw2_pt);

    if (bwi_mapper::getMagnitude(yawmid_pt) < 0.1) {
      use_outside_angle = false;
    }

    size_t y_test = at.y - search_distance_ / map_info_.resolution;
    float location_fitness = -1;
    bwi_mapper::Point2f test_coords;
    bwi_mapper::Point2f map_coords;

    while(y_test < at.y + search_distance_ / map_info_.resolution) {
      size_t x_test = at.x - search_distance_ / map_info_.resolution;
      while (x_test < at.x + search_distance_ / map_info_.resolution) {

        bwi_mapper::Point2f test(x_test, y_test);
        
        // Check if x_test, y_test is free.
        size_t map_idx = MAP_IDX(map_info_.width, x_test, y_test);
        if (inflated_map_.data[map_idx] != 0) {
          x_test++;
          continue;
        }

        // Check if it is on the outside or not
        if (use_outside_angle) {
          if ((from + to - 2 * at).dot(test - at) > 0) {
            x_test++;
            continue;
          }
        }

        bwi_mapper::Point2f test_loc(x_test, y_test);

        float dist1 = 
          bwi_mapper::minimumDistanceToLineSegment(from, at, test_loc);
        float dist2 = 
          bwi_mapper::minimumDistanceToLineSegment(at, to, test_loc);
        float fitness = std::min(dist1, dist2);

        if (fitness > location_fitness) {
          test_coords = test_loc;
          map_coords = bwi_mapper::toMap(test_loc, map_info_);
          resp.position.x = map_coords.x;
          resp.position.y = map_coords.y;
          location_fitness = fitness;
        }

        x_test++; 
      }
      y_test++;
    }

    // Calculate yaw
    yaw1 = atan2(resp.position.y - at_map.y, resp.position.x - at_map.x); 
    yaw2 = atan2(resp.position.y - from_map.y, resp.position.x - from_map.x);

    yaw1_pt = bwi_mapper::Point2f(cosf(yaw1),sinf(yaw1));
    yaw2_pt = bwi_mapper::Point2f(cosf(yaw2),sinf(yaw2));
    yawmid_pt = 0.5 * (yaw1_pt + yaw2_pt);

    /* float resp_yaw = atan2f(yawmid_pt.y, yawmid_pt.x);  */
    float resp_yaw = yaw2; 
    resp.orientation = tf::createQuaternionMsgFromYaw(resp_yaw);

    if (debug_) {
      cv::Mat image;
      mapper_->drawMap(image, inflated_map_);

      cv::circle(image, from, 5, cv::Scalar(255, 0, 0), -1);
      cv::circle(image, at, 5, cv::Scalar(255, 0, 255), 2);
      cv::circle(image, to, 5, cv::Scalar(0, 0, 255), -1);
      cv::circle(image, test_coords, 5, cv::Scalar(0, 255, 0), 2);

      cv::circle(image, test_coords + 
          bwi_mapper::Point2f(20 * cosf(resp_yaw), 20 * sinf(resp_yaw)), 
          3, cv::Scalar(0, 255, 0), -1);

      cv::namedWindow("Display window", CV_WINDOW_AUTOSIZE);
      cv::imshow("Display window", image);
      cv::waitKey(0);
    }

    return resp;
  }
  void TopologicalMapper::computeGraph(double merge_threshold) {

    // First for each critical point, find out the neigbouring regions
    std::vector<std::set<uint32_t> > point_neighbour_sets;
    point_neighbour_sets.resize(critical_points_.size());

    // Draw the critical lines as their indexes on the map
    cv::Mat lines(map_resp_.map.info.height, map_resp_.map.info.width, CV_16UC1,
        cv::Scalar((uint16_t)-1));
    drawCriticalLines(lines, 0, 0, true);

    // Go over all the pixels in the lines image and find neighbouring critical
    // regions
    for (int j = 0; j < lines.rows; ++j) {
      uint16_t* image_row_j = lines.ptr<uint16_t>(j);
      for (int i = 0; i < lines.cols; ++i) {
        uint16_t pixel = image_row_j[i];
        if (pixel == (uint16_t)-1) {
          continue;
        }
        int x_offset[] = {0, -1, 1, 0};
        int y_offset[] = {-1, 0, 0, 1};
        size_t num_neighbours = 4;
        for (size_t count = 0; count < num_neighbours; ++count) {
          uint32_t x_n = i + x_offset[count];
          uint32_t y_n = j + y_offset[count];
          if (x_n >= (uint16_t) lines.cols || y_n >= (uint16_t) lines.rows) {
            continue;
          }
          size_t map_idx = MAP_IDX(lines.cols, x_n, y_n);
          if (component_map_[map_idx] >= 0 && 
              component_map_[map_idx] < (int32_t) num_components_) {
            point_neighbour_sets[pixel].insert(component_map_[map_idx]);
          }
        }
      }
    }

    // Throw out any critical points that do not have 2 adjoining regions.
    // This can happen if the regions are too small
    std::vector<std::vector<uint32_t> > point_neighbours;
    point_neighbours.resize(critical_points_.size());
    std::vector<int> remove_crit_points;
    for (size_t i = 0; i < critical_points_.size(); ++i) {
      if (point_neighbour_sets[i].size() == 2) {
        point_neighbours[i] = 
          std::vector<uint32_t>(point_neighbour_sets[i].begin(),
              point_neighbour_sets[i].end());
      } else {
        remove_crit_points.push_back(i);
      }
    }

    for (int i = remove_crit_points.size() - 1; i >=0 ; --i) {
      critical_points_.erase(critical_points_.begin() + i);
      point_neighbour_sets.erase(point_neighbour_sets.begin() + i);
      point_neighbours.erase(point_neighbours.begin() + i);
    }

    // Find connectivity to remove any isolated sub-graphs
    std::vector<bool> checked_region(critical_points_.size(), false);
    std::vector<std::set<int> > graph_sets;
    for (size_t i = 0; i < num_components_; ++i) {
      if (checked_region[i]) {
        continue;
      }
      std::set<int> open_set, closed_set;
      open_set.insert(i);
      while (open_set.size() != 0) {
        int current_region = *(open_set.begin());
        open_set.erase(open_set.begin());
        closed_set.insert(current_region);
        checked_region[current_region] = true;
        for (int j = 0; j < critical_points_.size(); ++j) {
          for (int k = 0; k < 2; ++k) {
            if (point_neighbours[j][k] == current_region && 
                std::find(closed_set.begin(), closed_set.end(), 
                  point_neighbours[j][1-k]) == 
                closed_set.end()) {
              open_set.insert(point_neighbours[j][1-k]);
            }
          }
        }
      }
      graph_sets.push_back(closed_set);
    }

    std::set<int> master_region_set;
    if (graph_sets.size() != 1) {
      std::cout << "WARNING: Master graph is fragmented into " <<
        graph_sets.size() << " sub-graphs!!!" << std::endl;
      int max_idx = 0;
      int max_size = graph_sets[0].size();
      for (size_t i = 1; i < graph_sets.size(); ++i) {
        if (graph_sets[i].size() > max_size) {
          max_idx = i;
          max_size = graph_sets[i].size();
        }
      }
      master_region_set = graph_sets[max_idx];
    } else {
      master_region_set = graph_sets[0];
    }
    
    // Create the region graph next
    std::map<int, int> region_to_vertex_map;
    int vertex_count = 0;
    for (size_t r = 0; r < num_components_; ++r) { 
      if (std::find(master_region_set.begin(), master_region_set.end(), r) ==
          master_region_set.end()) {
        region_to_vertex_map[r] = -1;
        continue;
      }

      Graph::vertex_descriptor vi = boost::add_vertex(region_graph_);

      // Calculate the centroid
      uint32_t avg_i = 0, avg_j = 0, pixel_count = 0;
      for (size_t j = 0; j < map_resp_.map.info.height; ++j) {
        for (size_t i = 0; i < map_resp_.map.info.width; ++i) {
          size_t map_idx = MAP_IDX(map_resp_.map.info.width, i, j);
          if (component_map_[map_idx] == (int)r) {
            avg_j += j;
            avg_i += i;
            pixel_count++;
          }
        }
      }

      region_graph_[vi].location.x = ((float) avg_i) / pixel_count;
      region_graph_[vi].location.y = ((float) avg_j) / pixel_count;
      region_graph_[vi].pixels = floor(sqrt(pixel_count));

      // This map is only required till the point we form edges on this graph 
      region_to_vertex_map[r] = vertex_count;

      ++vertex_count;
    }

    // Now that region_to_vertex_map is complete, 
    // forward critical points to next graph
    std::map<int, std::map<int, VoronoiPoint> > 
      region_vertex_crit_points;
    for (size_t r = 0; r < num_components_; ++r) { 
      if (std::find(master_region_set.begin(), master_region_set.end(), r) ==
          master_region_set.end()) {
        region_to_vertex_map[r] = -1;
        continue;
      }
      // Store the critical points corresponding to this vertex
      for (int j = 0; j < critical_points_.size(); ++j) {
        for (int k = 0; k < 2; ++k) {
          if (point_neighbours[j][k] == r) {
            int region_vertex = region_to_vertex_map[r];
            int other_region_vertex = region_to_vertex_map[point_neighbours[j][1-k]];
            if (region_vertex == 192 && other_region_vertex == 202) {
              std::cout << "192-202: " << r << " " << point_neighbours[j][1-k] << " " << critical_points_[j] << std::endl;
            }
            if (region_vertex == 202 && other_region_vertex == 192) {
              std::cout << "192-202: " << r << " " << point_neighbours[j][1-k] << " " << critical_points_[j] << std::endl;
            }
            region_vertex_crit_points[region_vertex]
              [region_to_vertex_map[point_neighbours[j][1-k]]] =  
                critical_points_[j];
          }
        }
      }
    }

    // Create 1 edge per critical point
    for (size_t i = 0; i < critical_points_.size(); ++i) {
      int region1 = region_to_vertex_map[point_neighbours[i][0]];
      int region2 = region_to_vertex_map[point_neighbours[i][1]];
      if (region1 == -1) {
        // part of one of the sub-graphs that was discarded
        continue;
      }
      int count = 0;
      Graph::vertex_descriptor vi,vj;
      vi = boost::vertex(region1, region_graph_);
      vj = boost::vertex(region2, region_graph_);
      Graph::edge_descriptor e; bool b;
      boost::tie(e,b) = boost::edge(vi, vj, region_graph_);
      if (!b) {
        boost::tie(e,b) = boost::add_edge(vi, vj, region_graph_);
      }
      /* boost::tie(e,b) = boost::add_edge(vi, vj, region_graph_); */
      // region_graph_[e].weight = bwi_mapper::getMagnitude(
      //     region_graph_[vi].location - region_graph_[vj].location);
    }

    // Refine the region graph into the point graph:
    int pixel_threshold = merge_threshold / map_resp_.map.info.resolution;

    enum {
      PRESENT = 0,
      REMOVED_REGION_VERTEX = 1,
      CONVERT_TO_CRITICAL_POINT = 2,
      MERGE_VERTEX = 3
    };
    Graph::vertex_iterator vi, vend;

    // PASS 1 - resolve all vertices that are too big and have more than 2 
    // critical points to their underlying critical points
    std::cout << std::endl << "==============================" << std::endl;
    std::cout << "PASS 1" << std::endl;
    std::cout << "==============================" << std::endl << std::endl;
    Graph pass_0_graph = region_graph_;
    Graph pass_1_graph;
    int pass_0_count = 0;
    int pass_1_count = 0;
    std::vector<int> pass_0_vertex_status(
        boost::num_vertices(pass_0_graph), PRESENT);
    std::map<int, int> pass_0_vertex_to_pass_1_map;
    for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
        ++vi, ++pass_0_count) {

      std::cout << "Analyzing pass 0 graph vertex: " << pass_0_count << std::endl;

      std::vector<size_t> adj_vertices;
      getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
      // See if the area of this region is too big to be directly pushed into 
      // the pass 3 graph
      if (pass_0_graph[*vi].pixels >= pixel_threshold &&
          adj_vertices.size() > 2) {
        pass_0_vertex_status[pass_0_count] = CONVERT_TO_CRITICAL_POINT;
        std::cout << " - throwing it out (needs to be resolved to CPs)" <<
          std::endl;
        continue;
      }

      // Otherwise insert this as is into the point graph
      Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
      pass_1_graph[point_vi] = pass_0_graph[*vi];
      pass_0_vertex_to_pass_1_map[pass_0_count] = pass_1_count;

      ++pass_1_count;
    }

    // Now for each vertex that needs to be resolved into critical points, 
    // check neighbours recursively to convert these vertices into critical
    // points
    std::map<int, std::map<int, int> > pass_0_vertex_to_cp_map;
    pass_0_count = 0;
    for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
        ++vi, ++pass_0_count) {

      if (pass_0_vertex_status[pass_0_count] != CONVERT_TO_CRITICAL_POINT) {
        continue;
      }
      std::cout << "Converting pass1 vtx to CPs: " << pass_0_count << std::endl;

      std::vector<size_t> adj_vertices;
      getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
      std::vector<int> connect_edges;
      BOOST_FOREACH(size_t vtx, adj_vertices) {
        if (pass_0_vertex_status[vtx] == PRESENT) {
          Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
          pass_1_graph[point_vi].location =
            region_vertex_crit_points[pass_0_count][vtx];
          std::cout << " - added vtx at " << pass_1_graph[point_vi].location 
            << std::endl;
          int pass_1_vertex = pass_0_vertex_to_pass_1_map[vtx];
          std::cout << " - connecting vtx to pass_1_vertex: " << pass_1_vertex
            << " (pass0 = " << vtx << ")" << std::endl;
          Graph::vertex_descriptor vj;
          vj = boost::vertex(pass_1_vertex, pass_1_graph);
          Graph::edge_descriptor e; bool b;
          boost::tie(e,b) = boost::add_edge(point_vi, vj, pass_1_graph);
          connect_edges.push_back(pass_1_count);
          ++pass_1_count;
        } else if (vtx > pass_0_count) {
          // The CP is shared, but has not been added
          Graph::vertex_descriptor point_vi = boost::add_vertex(pass_1_graph);
          pass_1_graph[point_vi].location =
            region_vertex_crit_points[pass_0_count][vtx];
          std::cout << " - added shared cp with " << vtx << " at " 
            << pass_1_graph[point_vi].location << std::endl;
          pass_0_vertex_to_cp_map[pass_0_count][vtx] = pass_1_count;
          connect_edges.push_back(pass_1_count);
          ++pass_1_count;
        } else {
          // Retrieve existing CP
          int cp_vtx = pass_0_vertex_to_cp_map[vtx][pass_0_count];
          connect_edges.push_back(cp_vtx);
        }
      }

      /* Connect all the edges */
      for (int i = 0; i < connect_edges.size(); ++i) {
        for (int j = 0; j < i; ++j) {
          Graph::vertex_descriptor vi, vj;
          vi = boost::vertex(connect_edges[i], pass_1_graph);
          vj = boost::vertex(connect_edges[j], pass_1_graph);
          Graph::edge_descriptor e; bool b;
          boost::tie(e,b) = boost::add_edge(vi, vj, pass_1_graph);
        }
      }

    }

    // Connect all the edges as is from pass 1
    pass_0_count = 0;
    for (boost::tie(vi, vend) = boost::vertices(pass_0_graph); vi != vend;
        ++vi, ++pass_0_count) {

      if (pass_0_vertex_status[pass_0_count] != PRESENT) {
        continue;
      }
      std::cout << "Adding pass 1 edges for: " << pass_0_count << std::endl;

      std::vector<size_t> adj_vertices;
      getAdjacentNodes(pass_0_count, pass_0_graph, adj_vertices);
      BOOST_FOREACH(size_t vtx, adj_vertices) {
        if (pass_0_vertex_status[vtx] == PRESENT && vtx < pass_0_count) {
          Graph::vertex_descriptor vi, vj;
          vi = boost::vertex(
              pass_0_vertex_to_pass_1_map[pass_0_count], pass_1_graph);
          vj = boost::vertex(
              pass_0_vertex_to_pass_1_map[vtx], pass_1_graph);
          Graph::edge_descriptor e; bool b;
          boost::tie(e,b) = boost::add_edge(vi, vj, pass_1_graph);
        }
      }
    }

    pass_1_graph_ = pass_1_graph;

    // PASS 2 - remove any vertices that are adjacent to only 2 other vertices,
    // where both those vertices are visible to each other
    std::cout << std::endl << "==============================" << std::endl;
    std::cout << "PASS 2" << std::endl;
    std::cout << "==============================" << std::endl << std::endl;

    Graph pass_2_graph;

    std::vector<int> pass_1_vertex_status(boost::num_vertices(pass_1_graph), PRESENT);
    std::map<int, int> pass_1_vertex_to_pass_2_vertex_map;
    std::vector<std::pair<int, int> > rr_extra_edges;
    std::map<int, std::pair<int, int> > removed_vertex_map;

    pass_1_count = 0;
    int pass_2_count = 0;
    for (boost::tie(vi, vend) = boost::vertices(pass_1_graph); vi != vend;
        ++vi, ++pass_1_count) {

      std::cout << "Analyzing pass_1 graph vertex: " << pass_1_count << std::endl;

      if (pass_1_vertex_status[pass_1_count] != PRESENT) {
        std::cout << " - the vertex has been thrown out already " << std::endl;
        continue;
      }
      
      // See if this only has 2 neighbours, and the 2 neighbours are visible to
      // each other
      std::vector<size_t> open_vertices;
      int current_vertex = pass_1_count;
      getAdjacentNodes(current_vertex, pass_1_graph, open_vertices);
      std::vector<int> removed_vertices;
      std::pair<int, int> edge;
      if (open_vertices.size() == 2 &&
          pass_1_vertex_status[open_vertices[0]] == PRESENT &&
          pass_1_vertex_status[open_vertices[1]] == PRESENT) {
        while(true) {
          std::cout << " - has 2 adjacent vertices " << open_vertices[0] << ", " 
            << open_vertices[1] << std::endl;
          // Check if the 2 adjacent vertices are visible
          Point2f location1 = 
            getLocationFromGraphId(open_vertices[0], pass_1_graph);
          Point2f location2 = 
            getLocationFromGraphId(open_vertices[1], pass_1_graph);
          bool locations_visible = 
            locationsInDirectLineOfSight(location1, location2, map_resp_.map);
          if (locations_visible) {
            std::cout << " - the 2 adjacent vertices are visible " 
              << "- removing vertex." << std::endl; 
            pass_1_vertex_status[current_vertex] = REMOVED_REGION_VERTEX;
            removed_vertices.push_back(current_vertex);
            edge = std::make_pair(open_vertices[0], open_vertices[1]);
            bool replacement_found = false;
            for (int i = 0; i < 2; ++i) {
              std::vector<size_t> adj_vertices;
              getAdjacentNodes(open_vertices[i], pass_1_graph, 
                  adj_vertices);
              if (adj_vertices.size() == 2) {
                size_t new_vertex = (adj_vertices[0] == current_vertex) ? 
                  adj_vertices[1] : adj_vertices[0];
                if (pass_1_vertex_status[new_vertex] == PRESENT) {
                  current_vertex = open_vertices[i];
                  std::cout << " - neighbours may suffer from the same problem"
                    << ". checking vertex " << current_vertex << std::endl;
                  open_vertices[i] = new_vertex;
                  replacement_found = true;
                  break;
                }
              }
            }
            if (!replacement_found) {
              // Both neighbours on either side had 1 or more than 2 adjacent 
              // vertices
              break;
            }
          } else {
            // left and right vertices not visible
            break;
          }
        }
      }

      BOOST_FOREACH(int removed_vertex, removed_vertices) {
        removed_vertex_map[removed_vertex] = edge;
      }

      if (removed_vertices.size() != 0) {
        // Add the extra edge
        std::cout << " - adding extra edge between " << edge.first <<
          " " << edge.second << std::endl;
        rr_extra_edges.push_back(edge);
        continue;
      }

      // Otherwise insert this as is into the point graph
      Graph::vertex_descriptor point_vi = boost::add_vertex(pass_2_graph);
      pass_2_graph[point_vi] = pass_1_graph[*vi];
      pass_1_vertex_to_pass_2_vertex_map[pass_1_count] = 
        pass_2_count;

      ++pass_2_count;
    }

    // Insert all edges that can be inserted
    
    // Add edges from the pass_1 graph that can be placed as is
    pass_1_count = 0;
    for (boost::tie(vi, vend) = boost::vertices(pass_1_graph); vi != vend;
        ++vi, ++pass_1_count) {
      if (pass_1_vertex_status[pass_1_count] == PRESENT) {
        std::vector<size_t> adj_vertices;
        getAdjacentNodes(pass_1_count, pass_1_graph, adj_vertices);
        BOOST_FOREACH(size_t adj_vertex, adj_vertices) {
          if (pass_1_vertex_status[adj_vertex] == PRESENT &&
              adj_vertex > pass_1_count) {
            Graph::vertex_descriptor vi,vj;
            int vertex1 = 
              pass_1_vertex_to_pass_2_vertex_map[pass_1_count];
            int vertex2 = pass_1_vertex_to_pass_2_vertex_map[adj_vertex];
            vi = boost::vertex(vertex1, pass_2_graph);
            vj = boost::vertex(vertex2, pass_2_graph);
            Graph::edge_descriptor e; bool b;
            boost::tie(e,b) = boost::add_edge(vi, vj, pass_2_graph);
          }
        }
      }
    }
void
loadMapFromFile(nav_msgs::GetMap::Response* resp,
                const char* fname, double res, bool negate,
                double occ_th, double free_th, double* origin,
                bool trinary)
{
  SDL_Surface* img;

  unsigned char* pixels;
  unsigned char* p;
  unsigned char value;
  int rowstride, n_channels, avg_channels;
  unsigned int i,j;
  int k;
  double occ;
  int alpha;
  int color_sum;
  double color_avg;

  // Load the image using SDL.  If we get NULL back, the image load failed.
  if(!(img = IMG_Load(fname)))
  {
    std::string errmsg = std::string("failed to open image file \"") + 
            std::string(fname) + std::string("\"");
    throw std::runtime_error(errmsg);
  }

  // Copy the image data into the map structure
  resp->map.info.width = img->w;
  resp->map.info.height = img->h;
  resp->map.info.resolution = res;
  resp->map.info.origin.position.x = *(origin);
  resp->map.info.origin.position.y = *(origin+1);
  resp->map.info.origin.position.z = 0.0;
  tf::Quaternion q;
  q.setRPY(0,0, *(origin+2));
  resp->map.info.origin.orientation.x = q.x();
  resp->map.info.origin.orientation.y = q.y();
  resp->map.info.origin.orientation.z = q.z();
  resp->map.info.origin.orientation.w = q.w();

  // Allocate space to hold the data
  resp->map.data.resize(resp->map.info.width * resp->map.info.height);

  // Get values that we'll need to iterate through the pixels
  rowstride = img->pitch;
  n_channels = img->format->BytesPerPixel;

  if (trinary || n_channels == 1)
    avg_channels = n_channels;
  else
    avg_channels = n_channels - 1;

  // Copy pixel data into the map structure
  pixels = (unsigned char*)(img->pixels);
  for(j = 0; j < resp->map.info.height; j++)
  {
    for (i = 0; i < resp->map.info.width; i++)
    {
      // Compute mean of RGB for this pixel
      p = pixels + j*rowstride + i*n_channels;
      color_sum = 0;
      for(k=0;k<avg_channels;k++)
        color_sum += *(p + (k));
      color_avg = color_sum / (double)avg_channels;

      if (n_channels == 1)
          alpha = 1;
      else
          alpha = *(p+n_channels-1);

      // If negate is true, we consider blacker pixels free, and whiter
      // pixels free.  Otherwise, it's vice versa.
      if(negate)
        occ = color_avg / 255.0;
      else
        occ = (255 - color_avg) / 255.0;
      
      // Apply thresholds to RGB means to determine occupancy values for
      // map.  Note that we invert the graphics-ordering of the pixels to
      // produce a map with cell (0,0) in the lower-left corner.
      if(occ > occ_th)
        value = +100;
      else if(occ < free_th)
        value = 0;
      else if(trinary || alpha < 1.0)
        value = -1;
      else {
        double ratio = (occ - free_th) / (occ_th - free_th);
        value = 99 * ratio;
      }
           
      resp->map.data[MAP_IDX(resp->map.info.width,i,resp->map.info.height - j - 1)] = value;
    }
  }

  SDL_FreeSurface(img);
}
bool CostMapCalculation::calculateCostMap_old(char map_level)
{
    switch(map_level)
    {
    case USE_GRID_MAP_ONLY:
    {
        // cost map based on grid map only

        ROS_DEBUG("HectorCM: compute costmap based on grid map");

        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if((char)grid_map_.data[index] != UNKNOWN_CELL)
                {
                    if(grid_map_.data[index] == OCCUPIED_CELL)
                    {
                        // cell is occupied
                        cost_map.data[index] = OCCUPIED_CELL;
                    }
                    else
                    {
                        // cell is not occupied
                        cost_map.data[index] = FREE_CELL;
                    }
                }
            }
        }

        break;
    }
    case USE_ELEVATION_MAP_ONLY:
    {
        // cost map based on elevation map only

        ROS_DEBUG("HectorCM: compute costmap based on elevation map");


        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if(elevation_cost_map.data[index] != UNKNOWN_CELL)
                {
                    if(elevation_cost_map.data[index] == OCCUPIED_CELL)
                    {
                        // cell is occupied
                        cost_map.data[index] = OCCUPIED_CELL;
                    }
                    else
                    {
                        // cell is not occupied
                        cost_map.data[index] = FREE_CELL;
                    }
                }
            }
        }

        break;
    }
    case USE_GRID_AND_ELEVATION_MAP:
    {
        // cost map based on elevation and grid map

        ROS_DEBUG("HectorCM: compute costmap based on grid and elevation map");

        int checksum_grid_map;

        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
                {
                    if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL)
                    {
                        checksum_grid_map = 0;

                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u);
                        checksum_grid_map += grid_map_.at<int8_t>(v,   u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v,   u+1);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);

                        if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells)
                        {
                            if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL)
                            {
                                // cell is free
                                cost_map.data[index] = FREE_CELL;
                            }
                            else
                            {
                                // cell is occupied
                                cost_map.data[index] = OCCUPIED_CELL;
                            }
                        }
                        else
                        {
                            // cell is occupied
                            cost_map.data[index] = OCCUPIED_CELL;
                        }
                    }
                    else
                    {
                        cost_map.data[index] = FREE_CELL;
                    }
                }
            }
        }
        break;
    }
    case USE_GRID_AND_CLOUD_MAP:
    {
        // cost map based on cloud map and grid map

        ROS_DEBUG("HectorCM: compute costmap based on grid and cloud map");

        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
                {
                    if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || cloud_cost_map.data[index] == OCCUPIED_CELL)
                    {
                            // cell is occupied
                            cost_map.data[index] = OCCUPIED_CELL;
                    }
                    else
                    {
                        cost_map.data[index] = FREE_CELL;
                    }
                }
            }
        }
        break;
    }

    case USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP:
    {
        // cost map based on elevation, cloud and grid map

        ROS_DEBUG("HectorCM: compute costmap based on grid, cloud and elevation map");

        int checksum_grid_map;

        // loop through each element
        for (int v = min_index(1); v < max_index(1); ++v)
        {
            for (int u = min_index(0); u < max_index(0); ++u)
            {
                unsigned int index = MAP_IDX(cost_map.info.width, u, v);

                // check if cell is known
                if(grid_map_.at<int8_t>(v,u) != UNKNOWN_CELL)
                {
                    if(grid_map_.at<int8_t>(v,u) == OCCUPIED_CELL || elevation_cost_map.data[index] == OCCUPIED_CELL)
                    {
                        checksum_grid_map = 0;

                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u);
                        checksum_grid_map += grid_map_.at<int8_t>(v,   u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v,   u+1);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
                        checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);

                        if(elevation_cost_map.data[index] == FREE_CELL && allow_elevation_map_to_clear_occupied_cells)
                        {
                            if(checksum_grid_map <= max_clear_size*OCCUPIED_CELL)
                            {
                                // cell is free
                                cost_map.data[index] = FREE_CELL;
                            }
                            else
                            {
                                // cell is occupied
                                cost_map.data[index] = OCCUPIED_CELL;
                            }
                        }
                        else
                        {
                            // cell is occupied
                            cost_map.data[index] = OCCUPIED_CELL;
                        }
                    }
                    else
                    {
                        if(cloud_cost_map.data[index] == OCCUPIED_CELL)
                        {
                            // cell is occupied
                            cost_map.data[index] = OCCUPIED_CELL;
                        }
                        else
                        {
                            // cell is free
                            cost_map.data[index] = FREE_CELL;
                        }

                    }
                }
            }
        }
        break;
    }
    }

    ROS_DEBUG("HectorCM: computed a new costmap");

    return true;
}
void ElevationMapping::timerCallback(const ros::TimerEvent& event)
{
    tf::StampedTransform local_map_transform;

    // get local map transform
    try
    {
        listener.waitForTransform(map_frame_id, local_map_frame_id,ros::Time(0),ros::Duration(1.0));
        listener.lookupTransform(map_frame_id, local_map_frame_id,ros::Time(0), local_map_transform);
    }
    catch (tf::TransformException ex)
    {
        ROS_DEBUG("HectorEM: localmap transform in timer callback failed");
        ROS_ERROR("%s",ex.what());
        return;
    }

    // allign grid points
    Eigen::Vector2f index_world(local_map_transform.getOrigin().x(), local_map_transform.getOrigin().y());
    Eigen::Vector2f index_map = world_map_transform.getC2Coords(index_world);

    // check if elevation of current robot position is known, otherwise cancel pose update
    if(global_elevation_map.data[MAP_IDX(elevation_map_meta.width, (int)index_map(0), (int)index_map(1))] != (int16_t)-elevation_map_meta.zero_elevation)
    {
        geometry_msgs::PoseWithCovarianceStamped cell_height_average;

        int error_level  = 0;
        int pattern_cell_quantity = 4*poseupdate_used_pattern_size*poseupdate_used_pattern_size;

        /// todo check if min/max index is inside map
        // include neighbours
        for(int x=index_map(0)-poseupdate_used_pattern_size;x<index_map(0)+poseupdate_used_pattern_size;x++)
        {
            for(int y=index_map(1)-poseupdate_used_pattern_size;y<index_map(1)+poseupdate_used_pattern_size;y++)
            {
                int cell_index = MAP_IDX(elevation_map_meta.width, x, y);

                if(global_elevation_map.data[cell_index] == (int16_t)-elevation_map_meta.zero_elevation)
                {
                    // unknown cell
                    error_level++;
                }
                else
                {
                    // cell is knwon, accumulate cell hight
                    cell_height_average.pose.pose.position.z += (global_elevation_map.data[cell_index]-elevation_map_meta.zero_elevation)*elevation_map_meta.resolution_z;
                }
            }
        }

        // only publish pose update, if more than 1/2 of pattern cells are known
        if(error_level < pattern_cell_quantity/2)
        {
            pattern_cell_quantity -= error_level;

            cell_height_average.pose.pose.position.z = cell_height_average.pose.pose.position.z/(double)pattern_cell_quantity;

            cell_height_average.header.frame_id = map_frame_id;
            cell_height_average.header.stamp = ros::Time::now();

            cell_height_average.pose.covariance.at(0) = 0.0; //no x-position information
            cell_height_average.pose.covariance.at(7) = 0.0; //no y-position information
            cell_height_average.pose.covariance.at(14) = 1.0/poseupdate_height_covariance;

            pub_height_update.publish(cell_height_average);

            ROS_DEBUG("HectorEM: published height update %f",cell_height_average.pose.pose.position.z);

        }
    }
}
bool CostMapCalculation::calculateCostMap(char map_level)
{
    if (!map_level)
    {
      ROS_WARN("Invalid map selection was given to cost map calculation!");
      return false;
    }

    if (map_level & GRID_MAP) ROS_DEBUG("HectorCM: compute costmap based on grid map");
    if (map_level & ELEVATION_MAP) ROS_DEBUG("HectorCM: compute costmap based on elevation map");
    if (map_level & CLOUD_MAP) ROS_DEBUG("HectorCM: compute costmap based on cloud map");

    // loop through each element
    for (int v = min_index(1); v < max_index(1); ++v)
    {
        for (int u = min_index(0); u < max_index(0); ++u)
        {
            unsigned int index = MAP_IDX(cost_map.info.width, u, v);
            int checksum_grid_map = 0;

            cost_map.data[index] = UNKNOWN_CELL;

            // grid map
            if (map_level & GRID_MAP)
            {
                switch (grid_map_.data[index])
                {
                    case OCCUPIED_CELL:
                        if (map_level & ELEVATION_MAP && allow_elevation_map_to_clear_occupied_cells)
                        {
                            checksum_grid_map += grid_map_.at<int8_t>(v-1, u  );
                            checksum_grid_map += grid_map_.at<int8_t>(v+1, u  );
                            checksum_grid_map += grid_map_.at<int8_t>(v,   u-1);
                            checksum_grid_map += grid_map_.at<int8_t>(v,   u+1);
                            checksum_grid_map += grid_map_.at<int8_t>(v+1, u+1);
                            checksum_grid_map += grid_map_.at<int8_t>(v-1, u-1);
                            checksum_grid_map += grid_map_.at<int8_t>(v+1, u-1);
                            checksum_grid_map += grid_map_.at<int8_t>(v-1, u+1);
                        }

                        cost_map.data[index] = OCCUPIED_CELL;
                        break;
                    case FREE_CELL: cost_map.data[index] = FREE_CELL; break;
                }
            }

            // point cloud
            if (map_level & CLOUD_MAP)
            {
                if (cost_map.data[index] != OCCUPIED_CELL)
                {
                    switch (cloud_cost_map.data[index])
                    {
                        case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break;
                        case FREE_CELL:     cost_map.data[index] = FREE_CELL;     break;
                    }
                }
            }

            // elevation map
            if (map_level & ELEVATION_MAP)
            {
                if (cost_map.data[index] != OCCUPIED_CELL || (allow_elevation_map_to_clear_occupied_cells && checksum_grid_map <= max_clear_size*OCCUPIED_CELL))
                {
                    switch (elevation_cost_map.data[index])
                    {
                        case OCCUPIED_CELL: cost_map.data[index] = OCCUPIED_CELL; break;
                        case FREE_CELL:     cost_map.data[index] = FREE_CELL;     break;
                    }
                }
            }
        }
    }

    ROS_DEBUG("HectorCM: computed a new costmap");

    return true;
}
void ElevationMapping::cloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointcloud2_sensor_msg)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud2_map_pcl (new pcl::PointCloud<pcl::PointXYZ> ()),
    pointcloud2_sensor_pcl (new pcl::PointCloud<pcl::PointXYZ> ());
    tf::StampedTransform local_map_transform;

    ROS_DEBUG("HectorEM received a point cloud.");

    // converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
    pcl::fromROSMsg(*pointcloud2_sensor_msg, *pointcloud2_sensor_pcl);


    // transform cloud to /map frame
    try
    {
        listener.waitForTransform(map_frame_id, pointcloud2_sensor_msg->header.frame_id,pointcloud2_sensor_msg->header.stamp,ros::Duration(1.0));
        pcl_ros::transformPointCloud(map_frame_id,*pointcloud2_sensor_pcl,*pointcloud2_map_pcl,listener);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s",ex.what());
        ROS_DEBUG("HectorEM: pointcloud transform failed");
        return;
    }

    // get local map transform
    try
    {
        listener.waitForTransform(map_frame_id, local_map_frame_id,pointcloud2_sensor_msg->header.stamp,ros::Duration(1.0));
        listener.lookupTransform(map_frame_id, local_map_frame_id, pointcloud2_sensor_msg->header.stamp, local_map_transform);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s",ex.what());
        ROS_DEBUG("HectorEM: localmap transform in cloud callback failed");
        return;
    }

    bool local_map_is_subscribed = (pub_local_map.getNumSubscribers () > 0);
    bool global_map_is_subscribed = (pub_global_map.getNumSubscribers () > 0);

    if(local_map_is_subscribed)
        local_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);

    unsigned int size = (unsigned int)pointcloud2_map_pcl->points.size();

    // iterate trough all points
    for (unsigned int k = 0; k < size; ++k)
    {
        const pcl::PointXYZ& pt_cloud = pointcloud2_map_pcl->points[k];

        double measurement_distance = pointcloud2_sensor_pcl->points[k].z;

        // check for invalid measurements
        if (isnan(pt_cloud.x) || isnan(pt_cloud.y) || isnan(pt_cloud.z))
            continue;

        // check max distance (manhatten norm)
        if(max_observable_distance < measurement_distance)
            continue;

        // check min/max height
        if(elevation_map_meta.min_elevation+local_map_transform.getOrigin().z() > pt_cloud.z || elevation_map_meta.max_elevation+local_map_transform.getOrigin().z() < pt_cloud.z)
            continue;

        // allign grid points
        Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
        Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));

        unsigned int cell_index = MAP_IDX(elevation_map_meta.width, (int)round(index_map(0)), (int)round(index_map(1)));

        int16_t* pt_local_map = &local_elevation_map.data[cell_index];
        int16_t* pt_global_map = &global_elevation_map.data[cell_index];
        double*  pt_var = &cell_variance[cell_index];


        if(local_map_is_subscribed)
        {
            // elevation in current cell in meter
            double cell_elevation = elevation_map_meta.resolution_z*(*pt_local_map-elevation_map_meta.zero_elevation);

            // store maximum of each cell
            if(pt_cloud.z > cell_elevation)
                *pt_local_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);

            // filter each cell localy
//            double measurement_variance = sensor_variance*(measurement_distance*measurement_distance);
//            if(*pt_local_map == (int16_t)-elevation_map_meta.zero_elevation)
//            {
//                // unknown cell -> use current measurement
//                *pt_local_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
//                *pt_var = measurement_variance;
//            }
//            else
//            {
//                // fuse cell_elevation with measurement
//                *pt_local_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
//                *pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
//            }
        }

        if(publish_poseupdate || global_map_is_subscribed)
        {
            // fuse new measurements with existing map

            // elevation in current cell in meter
            double cell_elevation = elevation_map_meta.resolution_z*(*pt_global_map-elevation_map_meta.zero_elevation);

            // measurement variance
            double measurement_variance = sensor_variance*(measurement_distance*measurement_distance);

            // mahalanobis distance
            double mahalanobis_distance = sqrt((pt_cloud.z - cell_elevation)*(pt_cloud.z - cell_elevation)/(measurement_variance*measurement_variance));

            if(pt_cloud.z > cell_elevation && (mahalanobis_distance > 5.0))
            {
                *pt_global_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
                *pt_var = measurement_variance;
                continue;
            }

            if((pt_cloud.z < cell_elevation) && (mahalanobis_distance > 5.0))
            {
                *pt_global_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
                //*pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
                *pt_var = measurement_variance;
                continue;
            }

            *pt_global_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
            *pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
        }
    }


    if(local_map_is_subscribed)
    {
        // set the header information on the map
        local_elevation_map.header.stamp = pointcloud2_sensor_msg->header.stamp;
        local_elevation_map.header.frame_id = map_frame_id;

        pub_local_map.publish(local_elevation_map);
    }

    if(global_map_is_subscribed)
    {
        // set the header information on the map
        global_elevation_map.header.stamp = pointcloud2_sensor_msg->header.stamp;
        global_elevation_map.header.frame_id = map_frame_id;

        pub_global_map.publish(global_elevation_map);
    }

}
void
SlamGMapping::updateMap(const sensor_msgs::LaserScan& scan)
{
  boost::mutex::scoped_lock map_lock (map_mutex_);
  GMapping::ScanMatcher matcher;
  double* laser_angles = new double[scan.ranges.size()];
  double theta = angle_min_;
  for(unsigned int i=0; i<scan.ranges.size(); i++)
  {
    if (gsp_laser_angle_increment_ < 0)
        laser_angles[scan.ranges.size()-i-1]=theta;
    else
        laser_angles[i]=theta;
    theta += gsp_laser_angle_increment_;
  }

  matcher.setLaserParameters(scan.ranges.size(), laser_angles,
                             gsp_laser_->getPose());

  delete[] laser_angles;
  matcher.setlaserMaxRange(maxRange_);
  matcher.setusableRange(maxUrange_);
  matcher.setgenerateMap(true);

  GMapping::GridSlamProcessor::Particle best =
          gsp_->getParticles()[gsp_->getBestParticleIndex()];
  std_msgs::Float64 entropy;
  entropy.data = computePoseEntropy();
  if(entropy.data > 0.0)
    entropy_publisher_.publish(entropy);

  if(!got_map_) {
    map_.map.info.resolution = delta_;
    map_.map.info.origin.position.x = 0.0;
    map_.map.info.origin.position.y = 0.0;
    map_.map.info.origin.position.z = 0.0;
    map_.map.info.origin.orientation.x = 0.0;
    map_.map.info.origin.orientation.y = 0.0;
    map_.map.info.origin.orientation.z = 0.0;
    map_.map.info.origin.orientation.w = 1.0;
  } 

  GMapping::Point center;
  center.x=(xmin_ + xmax_) / 2.0;
  center.y=(ymin_ + ymax_) / 2.0;

  GMapping::ScanMatcherMap smap(center, xmin_, ymin_, xmax_, ymax_, 
                                delta_);

  ROS_DEBUG("Trajectory tree:");
  for(GMapping::GridSlamProcessor::TNode* n = best.node;
      n;
      n = n->parent)
  {
    ROS_DEBUG("  %.3f %.3f %.3f",
              n->pose.x,
              n->pose.y,
              n->pose.theta);
    if(!n->reading)
    {
      ROS_DEBUG("Reading is NULL");
      continue;
    }
    matcher.invalidateActiveArea();
    matcher.computeActiveArea(smap, n->pose, &((*n->reading)[0]));
    matcher.registerScan(smap, n->pose, &((*n->reading)[0]));
  }

  // the map may have expanded, so resize ros message as well
  if(map_.map.info.width != (unsigned int) smap.getMapSizeX() || map_.map.info.height != (unsigned int) smap.getMapSizeY()) {

    // NOTE: The results of ScanMatcherMap::getSize() are different from the parameters given to the constructor
    //       so we must obtain the bounding box in a different way
    GMapping::Point wmin = smap.map2world(GMapping::IntPoint(0, 0));
    GMapping::Point wmax = smap.map2world(GMapping::IntPoint(smap.getMapSizeX(), smap.getMapSizeY()));
    xmin_ = wmin.x; ymin_ = wmin.y;
    xmax_ = wmax.x; ymax_ = wmax.y;
    
    ROS_DEBUG("map size is now %dx%d pixels (%f,%f)-(%f, %f)", smap.getMapSizeX(), smap.getMapSizeY(),
              xmin_, ymin_, xmax_, ymax_);

    map_.map.info.width = smap.getMapSizeX();
    map_.map.info.height = smap.getMapSizeY();
    map_.map.info.origin.position.x = xmin_;
    map_.map.info.origin.position.y = ymin_;
    map_.map.data.resize(map_.map.info.width * map_.map.info.height);

    ROS_DEBUG("map origin: (%f, %f)", map_.map.info.origin.position.x, map_.map.info.origin.position.y);
  }

  for(int x=0; x < smap.getMapSizeX(); x++)
  {
    for(int y=0; y < smap.getMapSizeY(); y++)
    {
      /// @todo Sort out the unknown vs. free vs. obstacle thresholding
      GMapping::IntPoint p(x, y);
      double occ=smap.cell(p);
      assert(occ <= 1.0);
      if(occ < 0)
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
      else if(occ > occ_thresh_)
      {
        //map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = (int)round(occ*100.0);
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
      }
      else
        map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
    }
  }
  got_map_ = true;

  //make sure to set the header information on the map
  map_.map.header.stamp = ros::Time::now();
  map_.map.header.frame_id = tf_.resolve( map_frame_ );

  sst_.publish(map_.map);
  sstm_.publish(map_.map.info);
}
void ElevationVisualization::visualize_map(const hector_elevation_msgs::ElevationGrid& elevation_map, tf::StampedTransform local_map_transform)
{
    int current_height_level;

    for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
    {
        map_marker_array_msg.markers[i].points.clear();
    }
    map_marker_array_msg.markers.clear();


    // each array stores all cubes of one height level:
    map_marker_array_msg.markers.resize(max_height_levels+1);

    for (int index_y = 0; index_y < (int)elevation_map.info.height; ++index_y)
    {
        for (int index_x = 0; index_x < (int)elevation_map.info.width; ++index_x)
        {
            // visualize only known cells
            if(elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)] != (int16_t)-elevation_map.info.zero_elevation)
            {
                geometry_msgs::Point cube_center;
                Eigen::Vector2f index_map(index_x, index_y);
                Eigen::Vector2f index_world = world_map_transform.getC1Coords(index_map);

                cube_center.x = index_world(0);//+elevation_map.info.resolution_xy/2.0;
                cube_center.y = index_world(1);//+elevation_map.info.resolution_xy/2.0;
                cube_center.z = (elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)]-elevation_map.info.zero_elevation)*elevation_map.info.resolution_z;
                current_height_level = max_height_levels/2+(int)round(std::min(std::max((double)cube_center.z+local_map_transform.getOrigin().z(), -(double)max_height), (double)max_height)*(double)max_height_levels/((double)max_height*2.0f));
                map_marker_array_msg.markers[current_height_level].points.push_back(cube_center);

                if(use_color_map)
                {
                    double h = (1.0 - std::min(std::max((cube_center.z-min_height)/ (max_height - min_height), 0.0), 1.0)) *color_factor;
                    map_marker_array_msg.markers[current_height_level].colors.push_back(heightMapColor(h));
                }
            }
        }
    }

    for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
    {
        std::stringstream ss;
        ss <<"Level "<<i;
        map_marker_array_msg.markers[i].ns = ss.str();
        map_marker_array_msg.markers[i].id = i;
        map_marker_array_msg.markers[i].header.frame_id = "/map";
        map_marker_array_msg.markers[i].header.stamp =  elevation_map.header.stamp;
        map_marker_array_msg.markers[i].lifetime = ros::Duration();
        map_marker_array_msg.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
        map_marker_array_msg.markers[i].scale.x = elevation_map.info.resolution_xy;
        map_marker_array_msg.markers[i].scale.y = elevation_map.info.resolution_xy;
        map_marker_array_msg.markers[i].scale.z = elevation_map.info.resolution_z;
        map_marker_array_msg.markers[i].color = marker_color;

        if (map_marker_array_msg.markers[i].points.size() > 0)
            map_marker_array_msg.markers[i].action = visualization_msgs::Marker::ADD;
        else
            map_marker_array_msg.markers[i].action = visualization_msgs::Marker::DELETE;
    }

}