SubmodMap.cpp

Go to the documentation of this file.

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

#include <surfacelet/SubmodMap.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;

}

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

}

//std::vector<SurfacePatch>  SubmodMap::getMap(){
//    return map_;
//}
//void  SubmodMap::setMap(std::vector<SurfacePatch> patches){
//    map_=patches;
//}
void SubmodMap::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 SubmodMap::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 SubmodMap::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);

    std::vector<SurfacePatch> all_patches;
    std::vector<std::vector<int> > all_inlier_indices;
    std::vector<int> all_inlier_num;

    ROS_INFO("SubmodMap::computePatchesFromCloud: construction all patches");
    //construct one patch for each point in the cloud
    for (unsigned int i = 0; i < cloud.points.size(); ++i) {

        if ((i%100)==0)  ROS_INFO_STREAM("SubmodMap::computePatchesFromCloud: patch " << i << " of " << cloud.points.size());
        //search for nearest neighbors
        std::vector<int> inlier_indices;
        std::vector<float> dist;
        int index=i;
        tree.radiusSearch(index, radius_, inlier_indices, dist);

        //build patch cloud
        std::vector<int> inlier_mask;
        std::vector<int> patch_cloud_indices;
        pcl::PointCloud<pcl::PointXYZ> patch_cloud;
        for (unsigned int k = 0; k < inlier_indices.size(); ++k) {
            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;
        std::vector<int> removed_points;
        if (patch_cloud.points.size() > 2) {
            flat = generatePatch(patch_cloud, view_point, cloud.points[index], inlier_mask, patch, removed_points);
        }

        if (flat) {
            //save the patch
            all_patches.push_back(patch);

            //iterate over all points that are now represented by the patch --> Note: might only be a subset of the inliers from the distance search
            std::vector<int> patch_inlier;
            for (unsigned int j = 0; j < removed_points.size(); ++j) {
                int removed_point_index = patch_cloud_indices[removed_points[j]];
                patch_inlier.push_back(removed_point_index);
            }

            //save the indices to the represented points
            all_inlier_indices.push_back(patch_inlier);

            //for convenience save the number of points represented
            all_inlier_num.push_back(patch_inlier.size());
            if ((i%100)==0)    ROS_INFO_STREAM("SubmodMap::computePatchesFromCloud:inliers" << patch_inlier.size());
        }
    }

    ROS_INFO("SubmodMap::computePatchesFromCloud: starting patch selection");
    //while there are still points not represented
    bool done = false; // we are done if there are no more patches left or no more points
    int loop_c=0;
    while (!done) {

        //find the index of the patch which represents the largest set of points
        //Don't you think its odd that there is still no c++ built-in function to determine the index and the language is still alive...
        unsigned int max_idx = 0;
        int max_val = 0;
        for (unsigned int i = 0; i < all_patches.size(); ++i) {
            if (all_inlier_num[i] > max_val) {
                max_val = all_inlier_num[i];
                max_idx = i;
            }
        }

        //save the patch
        patches.push_back(all_patches[max_idx]);

        //mark point indices used in the mask
        for (unsigned int i = 0; i < all_inlier_indices[max_idx].size(); ++i) {
            mask[all_inlier_indices[max_idx][i]]=1;
        }

        //delete the patch
        all_patches.erase (all_patches.begin()+max_idx);
        all_inlier_indices.erase (all_inlier_indices.begin()+max_idx);
        all_inlier_num.erase (all_inlier_num.begin()+max_idx);

        //update the points now represented by the rest of the patches
        std::vector<int> ind_zero_p;
        for (unsigned int i = 0; i < all_patches.size(); ++i) {
            int num_p=0;
            for (unsigned int j = 0; j < all_inlier_indices[i].size(); ++j) {
                if (mask[all_inlier_indices[i][j]]==0) num_p++;
            }
            all_inlier_num[i]=num_p;
        }

        //done? no more points
        done = true;
        int mask_p=0;
        for (unsigned int i = 0; i < mask.size(); ++i) {
            if (mask[i]==0) done =false;
            if (mask[i]==1 )mask_p++;
        }

        //done? no more patches?
        if (all_patches.size()==0) done=true;
        loop_c++;
//        if ((loop_c%100)==0)
            ROS_INFO_STREAM("SubmodMap::computePatchesFromCloud: patch selection state " << all_patches.size() << " " <<mask_p << " " << max_val << " " <<max_idx);
            ROS_INFO_STREAM("SubmodMap::computePatchesFromCloud: patch selection state " << all_inlier_indices.size() << " " << all_inlier_num.size());
    }

    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) {
                not_used_cloud.points.push_back(cloud.points[i]);
        }
    }
    not_used_cloud.height = 1;
    not_used_cloud.width = not_used_cloud.points.size();
}

bool SubmodMap::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 SubmodMap::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 SubmodMap::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("SubmodMap::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=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
            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("SubmodMap::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(
            "SubmodMap::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 SubmodMap::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(
                "SubmodMap::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 = 1;
        marker.color.g=std::fabs(1.0 - ((double) i / (double) map_.size()));
        marker.color.b=std::fabs(1.0 - ((double) i / (double) map_.size()));
        marker.color.a = 0.8;
        array.markers.push_back(marker);
    }
}

void SubmodMap::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(
                "SubmodMap::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 SubmodMap::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(
                "SubmodMap::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 SubmodMap::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);
    }
}