PatchMap.cpp

Go to the documentation of this file.

/*
 * PatchMap.cpp
 *
 *  Created on: 14.11.2011
 *      Author: hess
 */

#include <surfacelet/PatchMap.h>

#include <iostream>
#include <limits>
#include <cmath>
#include <cstdlib>

#include <boost/thread/thread.hpp>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>

#include <ros/ros.h>
#include <pcl_ros/transforms.h>
#include <sensor_msgs/PointCloud2.h>

#include <tf/tf.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <eigen3/Eigen/src/Core/EigenBase.h>
#include <eigen3/Eigen/src/Geometry/Quaternion.h>

using namespace surfacelet;

tf::Pose tfPoseFromPatch(const SurfacePatch& patch) {

    tf::Pose pose;
    pose.setOrigin(tf::Point(patch.x, patch.y, patch.z));

    //compute angle-axis representation
    //compute cross product of normal and z-axis and get the rotation with smallest angle
    Eigen::Vector3f patch_normal(patch.normal_x, patch.normal_y, patch.normal_z);
    patch_normal.normalize();
    Eigen::Vector3f cross_product = Eigen::Vector3f(0, 0, 1).cross(patch_normal);
    Eigen::Matrix3f rot_matrix;
    cross_product.normalize();

    float angle = 0;
    if (cross_product.norm() > 0.000000001) { //everything ok
        angle = acos(patch_normal.dot(Eigen::Vector3f(0, 0, 1)));
        rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
    } else { //cross product 0 - normal either in or exactly opposite of z-axis
        double tmp = patch_normal.dot(Eigen::Vector3f(0, 0, 1));
        if (tmp > 0) {
            rot_matrix = Eigen::Matrix3f::Identity();
        } else {
            angle = M_PI;
            rot_matrix = Eigen::AngleAxis<float>(angle, Eigen::Vector3f(0, 1, 0)).toRotationMatrix();
        }
    }
    Eigen::Quaternion<float> quat(rot_matrix);
    tf::Quaternion tf_quat(quat.x(), quat.y(), quat.z(), quat.w());
    pose.setRotation(tf_quat);
    return pose;

}

void eigenToTf(const Eigen::Vector3f& origin, const Eigen::Matrix3f& rot_matrix, tf::Pose& pose) {

    pose.setOrigin(tf::Point(origin[0], origin[1], origin[2]));
    Eigen::Quaternion<float> quat(rot_matrix);

    tf::Quaternion tf_quat(quat.x(), quat.y(), quat.z(), quat.w());
    pose.setRotation(tf_quat);

}

SurfacePatch pointNormal2SurfacePatch(const pcl::PointNormal& p1) {
    SurfacePatch p2;
    p2.x = p1.x;
    p2.y = p1.y;
    p2.z = p1.z;

    p2.normal_x = p1.x;
    p2.normal_y = p1.y;
    p2.normal_z = p1.z;

    return p2;

}

SurfacePatch point2SurfacePatch(const pcl::PointXYZ& p1) {
    SurfacePatch p2;
    p2.x = p1.x;
    p2.y = p1.y;
    p2.z = p1.z;

    p2.normal_x = 0;
    p2.normal_y = 0;
    p2.normal_z = 1;

    return p2;

}

PatchMap::PatchMap() :
    first_insert(true), radius_(0.03), ransac_distance_(0.01), ransac_iterations_(50) {

}

//std::vector<SurfacePatch>  PatchMap::getMap(){
//    return map_;
//}
//void  PatchMap::setMap(std::vector<SurfacePatch> patches){
//    map_=patches;
//}
void PatchMap::insertScan(pcl::PointCloud<pcl::PointXYZ>& cloud, pcl::PointCloud<pcl::PointXYZ>& not_used_cloud) {
    pcl::PointXYZ view_point(0, 0, 0);
    insertScan(cloud, not_used_cloud, view_point);
}
void PatchMap::insertScan(pcl::PointCloud<pcl::PointXYZ>& cloud, pcl::PointCloud<pcl::PointXYZ>& not_used_cloud,
    pcl::PointXYZ view_point) {

    if (first_insert) {

        //compute patches
        computePatchesFromCloud(cloud, view_point, map_, not_used_cloud);

        //get new kdtree for patches
        pcl::PointCloud<SurfacePatch> patch_cloud;
        patch_cloud.height = 1;
        patch_cloud.width = map_.size();
        patch_cloud.points.insert(patch_cloud.points.begin(), map_.begin(), map_.end());
        tree_.setInputCloud(patch_cloud.makeShared());

        first_insert = false;
    } else {

        //compute normals
        pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
        pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
        pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
        ne.setViewPoint(view_point.x, view_point.y, view_point.z);
        ne.setSearchMethod(tree);
        ne.setInputCloud(cloud.makeShared());
        ne.setRadiusSearch(radius_);
        ne.compute(*cloud_normals);

        //construct pointcloud including the normals
        pcl::PointCloud<pcl::PointNormal> pn_cloud;
        pcl::concatenateFields(cloud, *cloud_normals, pn_cloud);

        //remove already represented points
        pcl::PointCloud<pcl::PointXYZ> tmp_cloud;
        for (unsigned int i = 0; i < cloud.size(); ++i) {
            if (!pointRepresented(pn_cloud.points[i]))
                tmp_cloud.points.push_back(cloud[i]);
        }
        tmp_cloud.height = 1;
        tmp_cloud.width = tmp_cloud.points.size();

        //compute patches for the rest of the points
        std::vector<SurfacePatch> new_patches;
        computePatchesFromCloud(tmp_cloud, view_point, new_patches, not_used_cloud);

        //add patches to the current map and pointcloud
        map_.insert(map_.end(), new_patches.begin(), new_patches.end());
        pcl::PointCloud<SurfacePatch> patch_cloud;
        patch_cloud.height = 1;
        patch_cloud.width = map_.size();
        patch_cloud.points.insert(patch_cloud.points.begin(), map_.begin(), map_.end());

        //update the kdtree for patch searching
        tree_.setInputCloud(patch_cloud.makeShared());
    }
}

void PatchMap::computePatchesFromCloud(pcl::PointCloud<pcl::PointXYZ>& cloud, const pcl::PointXYZ view_point,
    std::vector<SurfacePatch>& patches, pcl::PointCloud<pcl::PointXYZ>& not_used_cloud) {

    if (cloud.points.size() == 0) {
        ROS_ERROR("Pointcloud is empty.");
    }

    patches.clear();

    pcl::search::KdTree<pcl::PointXYZ> tree;
    tree.setInputCloud(cloud.makeShared());

    std::set<int> usable_indexes;
    for (unsigned int i = 0; i < cloud.points.size(); i++) {
        usable_indexes.insert(i);
    }
    std::vector<unsigned int> mask(cloud.points.size(), 0);

    int j = 0;
    int pr = 0;
    while ((usable_indexes.size() > 0)) {
        std::set<int>::iterator it = usable_indexes.begin();
        std::advance(it, rand() % usable_indexes.size());
        int index = *it;

        std::vector<int> inlier_indices;
        std::vector<int> patch_cloud_indices;
        std::vector<int> removed_points;
        std::vector<int> inlier_mask;
        inlier_indices.clear();
        patch_cloud_indices.clear();
        removed_points.clear();
        inlier_mask.clear();

        std::vector<float> dist;
        tree.radiusSearch(index, radius_, inlier_indices, dist);

        //build patch cloud
        pcl::PointCloud<pcl::PointXYZ> patch_cloud;
        for (unsigned int k = 0; k < inlier_indices.size(); ++k) {
//            if (mask[inlier_indices[k]] == 0) {
            patch_cloud_indices.push_back(inlier_indices[k]);
            inlier_mask.push_back(mask[inlier_indices[k]]);
            patch_cloud.points.push_back(cloud.points[inlier_indices[k]]);
//            }
        }
        patch_cloud.height = 1;
        patch_cloud.width = patch_cloud.points.size();

        SurfacePatch patch;
        bool flat = false;
        if (patch_cloud.points.size() > 2) {
            flat = generatePatch(patch_cloud, view_point, cloud.points[index], inlier_mask, patch, removed_points);
        }

        if (flat) {

            for (unsigned int i = 0; i < removed_points.size(); ++i) {
                unsigned int removed_point_index = patch_cloud_indices[removed_points[i]];
                mask[removed_point_index] = 1;
                pr++;

                //remove from set
                it = usable_indexes.find(removed_point_index);
                if (!(it == usable_indexes.end()))
                    usable_indexes.erase(it);
            }
            patches.push_back(patch);
        }

        it = usable_indexes.find(index);
        if (!(it == usable_indexes.end()))
            usable_indexes.erase(it);
        ROS_DEBUG_STREAM("PatchMap::computePatchesFromCloud: iteration " << j);
        j++;
    }

    ROS_DEBUG_STREAM("PatchMap::computePatchesFromCloud: Total points " << cloud.size() << " removed point " << pr);
    pcl::PointCloud<SurfacePatch> patch_cloud;
    patch_cloud.height = 1;
    patch_cloud.width = map_.size();
    patch_cloud.points.insert(patch_cloud.points.begin(), patches.begin(), patches.end());
    tree_.setInputCloud(patch_cloud.makeShared());

    //collect the points which were not inserted into a patch
    not_used_cloud.points.clear();
    for (unsigned int i = 0; i < mask.size(); ++i) {
        if (mask[i] == 0) {
            //try to match outlier point
            if (!searchPatchForPoint(cloud.points[i])) {
                not_used_cloud.points.push_back(cloud.points[i]);
            }
        }
    }
    not_used_cloud.height = 1;
    not_used_cloud.width = not_used_cloud.points.size();
}

bool PatchMap::pointRepresented(pcl::PointNormal p) {

    bool represented = false;
    //search for close patches
    std::vector<int> inlier;
    std::vector<float> dist;
    SurfacePatch tmp = pointNormal2SurfacePatch(p);
    tree_.radiusSearch(tmp, radius_, inlier, dist);

    //check if point represented by any inlier patch
    for (unsigned int i = 0; i < inlier.size(); ++i) {

        //compute angle-axis representation and compare point normal to patch normal
        SurfacePatch patch = map_[inlier[i]];
        Eigen::Vector3f patch_normal = patch.getNormalVector3fMap();
        Eigen::Vector3f patch_origin = map_[inlier[i]].getVector3fMap();
        float angle = acos(patch_normal.normalized().dot(patch_origin.normalized()));
        float dist = fabs(patch_normal.dot(patch_origin - p.getVector3fMap()));
        if ((dist < ransac_distance_) && (dist < (patch.std_dev_z)) && (angle < M_PI / 6.0)) {
            represented = true;
        }
    }

    return represented;
}

bool PatchMap::searchPatchForPoint(pcl::PointXYZ p) {

    //search for close patches
    std::vector<int> inlier;
    std::vector<float> dist;
    SurfacePatch tmp = point2SurfacePatch(p);
    tree_.radiusSearch(tmp, radius_ * 1.05, inlier, dist);

    //check if point represented by any inlier patch

    for (unsigned int i = 0; i < inlier.size(); ++i) {

        //compute angle-axis representation and compare point normal to patch normal
        SurfacePatch patch = map_[inlier[i]];
        Eigen::Vector3f patch_normal = patch.getNormalVector3fMap();
        Eigen::Vector3f patch_origin = patch.getVector3fMap();

        //compute angle-axis representation
        //compute cross product of normal and z-axis
        Eigen::Vector3f cross_product = Eigen::Vector3f(0, 0, 1).cross(patch_normal);
        Eigen::Matrix3f rot_matrix;
        if (cross_product.norm() > 0.000000001) {
            float angle = asin(cross_product.norm());
            cross_product.normalize();
            //get rotation matrix
            rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
        } else { //cross product 0 - normal either in or exactly opposite of z-axis
            double tmp = patch_normal.dot(Eigen::Vector3f(0, 0, 1));
            if (tmp > 0) {
                rot_matrix = Eigen::Matrix3f::Identity();
            } else {
                float angle = M_PI;
                rot_matrix = Eigen::AngleAxis<float>(angle, Eigen::Vector3f(0, 1, 0)).toRotationMatrix();
            }
        }

        float dist = fabs(patch_normal.dot(patch_origin - p.getVector3fMap()));
        Eigen::Vector3f new_p = rot_matrix * (p.getVector3fMap() - patch_origin); // U - rotation matrix, U^T is inverse rotation matrix
        new_p[2] = 0;
        float scale = new_p.norm();

        if ((dist < ransac_distance_) && (scale < 1.05 * radius_)) {

            //set marker scale
            if ((scale > map_[inlier[i]].scale_x / 2.0) || (scale > map_[inlier[i]].scale_y / 2.0)) {
                map_[inlier[i]].scale_x = 2 * scale;
                map_[inlier[i]].scale_y = 2 * scale;
            }
            return true;
        }
    }

    return false;
}

bool PatchMap::generatePatch(const pcl::PointCloud<pcl::PointXYZ>& cloud, const pcl::PointXYZ view_point,
    const pcl::PointXYZ center, const std::vector<int>& mask, SurfacePatch& patch, std::vector<int>& removed_points) {

    ROS_DEBUG_STREAM("PatchMap::generatePatch: view point " << view_point.getVector3fMap());
    removed_points.clear();
    Eigen::Vector3f best_plane_normal(0, 0, 0);
    Eigen::Vector3f best_plane_p(0, 0, 0);
    bool plane_found = false;
    double best_plane_d;
    best_plane_d = 0;
    double best_mse = std::numeric_limits<double>::max();

    for (int iter = 0; iter < ransac_iterations_; iter++) {

        double tmp_mse = 0;

        Eigen::Vector3f p1 = cloud.points[rand() % cloud.points.size()].getVector3fMap();
        Eigen::Vector3f p2 = cloud.points[rand() % cloud.points.size()].getVector3fMap();
        Eigen::Vector3f p3 = cloud.points[rand() % cloud.points.size()].getVector3fMap();

        if (cloud.points.size() == 3) {
            // sample 3 points TODO:check
            p1 = cloud.points[0].getVector3fMap();
            p2 = cloud.points[1].getVector3fMap();
            p3 = cloud.points[2].getVector3fMap();

        }

        if ((p1 == p2) || (p1 == p3) || (p2 == p3))
            continue;
        // compute plane normal
        Eigen::Vector3f plane_normal = (p2 - p1).cross(p3 - p1);
        plane_normal.normalize();
        float plane_d = plane_normal.dot(p1);

        if (isinf(plane_d) || isnan(plane_d))
            continue;

        for (unsigned int i = 0; i < cloud.points.size(); i++) {

            double tmp = fabs(plane_normal.dot(p1 - cloud.points[i].getVector3fMap()));
            tmp_mse += (tmp * tmp);
        }
        if (tmp_mse < best_mse) {
            best_plane_normal = plane_normal;
            best_plane_d = plane_d;
            best_plane_p = p1;
            best_mse = tmp_mse;
            plane_found = true;
        }
        if (cloud.points.size() == 3)
            break;
    }

    if (!plane_found)
        return false;

    // recompute model coefficients
    //get viewing direction

    Eigen::Vector3f viewing_dir = best_plane_p - view_point.getVector3fMap();
    viewing_dir.normalize();
    if (best_plane_normal.dot(viewing_dir) > 0) // flip towards viewing direction
        best_plane_normal = -best_plane_normal;

    best_plane_normal.normalize();
    Eigen::Vector3f cross_product = Eigen::Vector3f(0, 0, 1).cross(best_plane_normal);
    Eigen::Matrix3f rot_matrix;
    cross_product.normalize();

    float angle = 0;
    if (cross_product.norm() > 0.000000001) { //everything ok
        angle = acos(best_plane_normal.dot(Eigen::Vector3f(0, 0, 1)));
        rot_matrix = Eigen::AngleAxis<float>(angle, cross_product).toRotationMatrix();
    } else { //cross product 0 - normal either in or exactly opposite of z-axis
        double tmp = best_plane_normal.dot(Eigen::Vector3f(0, 0, 1));
        if (tmp > 0) {
            rot_matrix = Eigen::Matrix3f::Identity();
        } else {
            angle = M_PI;
            rot_matrix = Eigen::AngleAxis<float>(angle, Eigen::Vector3f(0, 1, 0)).toRotationMatrix();
        }
    }

    pcl::PointCloud<pcl::PointXYZ> tmp_cloud;

    for (unsigned int i = 0; i < cloud.points.size(); ++i) {

        float dist = fabs(best_plane_normal.dot(best_plane_p - cloud.points[i].getVector3fMap()));
        if ((dist < ransac_distance_) && (mask[i] == 0)) {
            tmp_cloud.points.push_back(cloud.points[i]);
            removed_points.push_back(i);
        }
    }

    ROS_DEBUG_STREAM("PatchMap::generatePatch: tmp point size " << tmp_cloud.points.size());
    if (tmp_cloud.points.size() < 3) {
        tmp_cloud.clear();
        removed_points.clear();

        for (unsigned int i = 0; i < cloud.points.size(); ++i) {

            float dist = fabs(best_plane_normal.dot(best_plane_p - cloud.points[i].getVector3fMap()));
            if ((dist < ransac_distance_)) {
                tmp_cloud.points.push_back(cloud.points[i]);
                removed_points.push_back(i);
            }
        }
        if (tmp_cloud.points.size() < 3) {
            removed_points.clear();
            return false;
        }
    }

    tmp_cloud.height = 1;
    tmp_cloud.width = tmp_cloud.points.size();

    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(tmp_cloud, centroid);

    // Subtract the centroid
    Eigen::Vector4f center_in_plane = centroid;
    Eigen::MatrixXf cloud_demean;
    demeanPointCloud(tmp_cloud, center_in_plane, cloud_demean);

    Eigen::MatrixXf new_points = rot_matrix.transpose() * cloud_demean.topRows(3); // U - rotation matrix, U^T is inverse rotation matrix
    new_points.transposeInPlace();
    new_points = new_points.array().abs().matrix();

    //set marker scale
    double scale = new_points.leftCols(2).rowwise().norm().maxCoeff();

    patch.x = center_in_plane[0];
    patch.y = center_in_plane[1];
    patch.z = center_in_plane[2];
    patch.normal_x = best_plane_normal[0];
    patch.normal_y = best_plane_normal[1];
    patch.normal_z = best_plane_normal[2];
    patch.scale_x = 2 * scale;
    patch.scale_y = 2 * scale;
    patch.scale_z = 2 * new_points.col(2).maxCoeff();
    if (isnan(patch.scale_z) || isinf(patch.scale_z)) {
        ROS_WARN_STREAM(
            "PatchMap::generatePatch: patch scale in z direction is nan.  " << std::endl <<"new points  coeff " << new_points.col(0).maxCoeff() << " " << new_points.col(1).maxCoeff() << " " << new_points.col(2).maxCoeff());
    }

    return true;
}

void PatchMap::constructMarker(const std::string frame_id, visualization_msgs::MarkerArray& array) {

    array.markers.clear();

    ros::Time marker_time = ros::Time::now() - ros::Duration(0.2);
    for (unsigned int i = 0; i < map_.size(); ++i) {
        visualization_msgs::Marker marker;
        // Set the frame ID and timestamp.  See the TF tutorials for information on these.
        marker.header.frame_id = frame_id;
        marker.header.stamp = marker_time;

        // Set the namespace and id for this marker.  This serves to create a unique ID
        // Any marker sent with the same namespace and id will overwrite the old one
        marker.ns = "coverage";
        marker.id = i;

        // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
        marker.type = visualization_msgs::Marker::CYLINDER;

        // Set the marker action.  Options are ADD and DELETE
        marker.action = visualization_msgs::Marker::MODIFY;

        // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
        tf::poseTFToMsg(tfPoseFromPatch(map_[i]), marker.pose);

        if (isnan(marker.pose.position.x) || isnan(marker.pose.position.y) || isnan(marker.pose.position.z)
            || isnan(marker.pose.orientation.x) || isnan(marker.scale.x)) {
            ROS_WARN_STREAM(
                "PatchMap::constructMarker: something in the pose of the marker is nan. See output below" <<std::endl << map_[i].x << " " << map_[i].y << " " << map_[i].z << " " << map_[i].normal_x << " " << map_[i].normal_y << " " << map_[i].normal_z << " " << map_[i].scale_x << " " << map_[i].scale_y << " " << map_[i].scale_z << " " << map_[i].std_dev_z);
        }

        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        marker.scale.x = map_[i].scale_x;
        marker.scale.y = map_[i].scale_y;
        marker.scale.z = map_[i].scale_z;

        // Set the color -- be sure to set alpha to something non-zero!
        marker.color.r = 0;
        marker.color.g = 0;
        marker.color.b = 0.9;
        marker.color.a = 0.5;
        array.markers.push_back(marker);
    }
}

void PatchMap::constructMarker(const std::string frame_id, const std::vector<bool>& reachable,
    visualization_msgs::MarkerArray& array) {

    ROS_ASSERT(reachable.size()==map_.size());
    array.markers.clear();

    ros::Time marker_time = ros::Time::now() - ros::Duration(0.2);
    for (unsigned int i = 0; i < map_.size(); ++i) {
        visualization_msgs::Marker marker;
        // Set the frame ID and timestamp.  See the TF tutorials for information on these.
        marker.header.frame_id = frame_id;
        marker.header.stamp = marker_time;

        // Set the namespace and id for this marker.  This serves to create a unique ID
        // Any marker sent with the same namespace and id will overwrite the old one
        marker.ns = "coverage";
        marker.id = i;

        // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
        marker.type = visualization_msgs::Marker::CYLINDER;

        // Set the marker action.  Options are ADD and DELETE
        marker.action = visualization_msgs::Marker::MODIFY;

        // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
        tf::poseTFToMsg(tfPoseFromPatch(map_[i]), marker.pose);

        if (isnan(marker.pose.position.x) || isnan(marker.pose.position.y) || isnan(marker.pose.position.z)
            || isnan(marker.pose.orientation.x) || isnan(marker.scale.x)) {
            ROS_WARN_STREAM(
                "PatchMap::constructMarker: something in the pose of the marker is nan. See output below" <<std::endl << map_[i].x << " " << map_[i].y << " " << map_[i].z << " " << map_[i].normal_x << " " << map_[i].normal_y << " " << map_[i].normal_z << " " << map_[i].scale_x << " " << map_[i].scale_y << " " << map_[i].scale_z << " " << map_[i].std_dev_z);
        }

        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        marker.scale.x = map_[i].scale_x;
        marker.scale.y = map_[i].scale_y;
        marker.scale.z = map_[i].scale_z;
        // Set the color -- be sure to set alpha to something non-zero!
        if (reachable[i]) {
            marker.color.r = 0; //(1.0/patches.size())*i;
            marker.color.g = 0; //(1.0/patches.size())*i;
            marker.color.b = 0.9;
        } else {
            marker.color.r = 0.7; //(1.0/patches.size())*i;
            marker.color.g = 0; //(1.0/patches.size())*i;
            marker.color.b = 0;

        }
        marker.color.a = 0.7;
        array.markers.push_back(marker);
    }
}

void PatchMap::constructMarker(const std::string frame_id, const std::vector<bool>& reachable,
    const std::vector<bool>& collision, visualization_msgs::MarkerArray& array) {

    ROS_ASSERT(reachable.size()==map_.size());
    array.markers.clear();

    ros::Time marker_time = ros::Time::now() - ros::Duration(0.2);
    for (unsigned int i = 0; i < map_.size(); ++i) {
        visualization_msgs::Marker marker;
        // Set the frame ID and timestamp.  See the TF tutorials for information on these.
        marker.header.frame_id = frame_id;
        marker.header.stamp = marker_time;

        // Set the namespace and id for this marker.  This serves to create a unique ID
        // Any marker sent with the same namespace and id will overwrite the old one
        marker.ns = "coverage";
        marker.id = i;

        // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
        marker.type = visualization_msgs::Marker::CYLINDER;

        // Set the marker action.  Options are ADD and DELETE
        marker.action = visualization_msgs::Marker::MODIFY;

        // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
        tf::poseTFToMsg(tfPoseFromPatch(map_[i]), marker.pose);

        if (isnan(marker.pose.position.x) || isnan(marker.pose.position.y) || isnan(marker.pose.position.z)
            || isnan(marker.pose.orientation.x) || isnan(marker.scale.x)) {
            ROS_WARN_STREAM(
                "PatchMap::constructMarker: something in the pose of the marker is nan. See output below" <<std::endl << map_[i].x << " " << map_[i].y << " " << map_[i].z << " " << map_[i].normal_x << " " << map_[i].normal_y << " " << map_[i].normal_z << " " << map_[i].scale_x << " " << map_[i].scale_y << " " << map_[i].scale_z << " " << map_[i].std_dev_z);
        }

        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        marker.scale.x = map_[i].scale_x;
        marker.scale.y = map_[i].scale_y;
        marker.scale.z = map_[i].scale_z;

        // Set the color -- be sure to set alpha to something non-zero!
        if (reachable[i]) {
            marker.color.r = 0; //(1.0/patches.size())*i;
            marker.color.g = 0; //(1.0/patches.size())*i;
            marker.color.b = 0.9;
        } else if (collision[i]) {
            marker.color.r = 0.9; //(1.0/patches.size())*i;
            marker.color.g = 0.9; //(1.0/patches.size())*i;
            marker.color.b = 0.3;
        } else {
            marker.color.r = 0.7; //(1.0/patches.size())*i;
            marker.color.g = 0; //(1.0/patches.size())*i;
            marker.color.b = 0;

        }
        marker.color.a = 0.9;
        array.markers.push_back(marker);
    }
}

void PatchMap::constructNormalMarker(const std::string frame_id, visualization_msgs::MarkerArray& array) const {

    array.markers.clear();
    ros::Time marker_time = ros::Time::now() - ros::Duration(0.2);
    for (unsigned int i = 0; i < map_.size(); ++i) {
        visualization_msgs::Marker marker;

        // Set the frame ID and timestamp.  See the TF tutorials for information on these.
        marker.header.frame_id = frame_id;
        marker.header.stamp = marker_time;

        // Set the namespace and id for this marker.  This serves to create a unique ID
        // Any marker sent with the same namespace and id will overwrite the old one
        marker.ns = "coverage";
        marker.id = i;

        // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
        marker.type = visualization_msgs::Marker::ARROW;

        // Set the marker action.  Options are ADD and DELETE
        marker.action = visualization_msgs::Marker::MODIFY;

        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        marker.scale.x = 0.01;
        marker.scale.y = 0.01;
        marker.scale.z = 0.01;

        // Set the color -- be sure to set alpha to something non-zero!
        marker.color.r = 1.0; //(1.0/patches.size())*i;
        marker.color.g = 0; //(1.0/patches.size())*i;
        marker.color.b = 0;
        marker.color.a = 1.0;

        marker.points.clear();

        // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
        geometry_msgs::Point start;
        start.x = map_[i].x;
        start.y = map_[i].y;
        start.z = map_[i].z;

        geometry_msgs::Point end;
        end.x = map_[i].x + 0.05 * map_[i].normal_x;
        end.y = map_[i].y + 0.05 * map_[i].normal_y;
        end.z = map_[i].z + 0.05 * map_[i].normal_z;

        marker.points.push_back(start);
        marker.points.push_back(end);
        array.markers.push_back(marker);
    }
}