next_best_view.cpp
Go to the documentation of this file.
00001 #include <pluginlib/class_list_macros.h>
00002 #include <ros/ros.h>
00003 #include <sensor_msgs/PointCloud2.h>
00004 
00005 #include <geometry_msgs/PoseArray.h>
00006 
00007 #include "octomap/octomap.h"
00008 #include "octomap_ros/conversions.h"
00009 #include <octomap_ros/OctomapBinary.h>
00010 #include "pcl_to_octree/octree/OcTreePCL.h"
00011 #include "pcl_to_octree/octree/OcTreeNodePCL.h"
00012 #include "pcl_to_octree/octree/OcTreeServerPCL.h"
00013 
00014 #include <pcl/io/pcd_io.h>
00015 #include <pcl/point_types.h>
00016 #include <pcl/features/normal_3d.h>
00017 #include <pcl/features/boundary.h>
00018 #include <pcl/segmentation/extract_clusters.h>
00019 #include <pcl/filters/extract_indices.h>
00020 #include <pcl/filters/passthrough.h>
00021 
00022 #include <pcl_ros/publisher.h>
00023 #include <pcl_ros/pcl_nodelet.h>
00024 
00025 #include <tf/transform_listener.h>
00026 
00027 #include <visualization_msgs/MarkerArray.h>
00028 
00029 #include <vector>
00030 
00031 #include <nodelet/nodelet.h>
00032 #include <math.h>
00033 
00034 #include <iostream>
00035 #include <fstream>
00036 #include <sstream>
00037 #include <nav_msgs/OccupancyGrid.h>
00055 namespace autonomous_mapping
00056 {
00057 
00058 double pi=3.141;
00059 int counter=0;
00060 typedef struct
00061 {
00062         double x,y;
00063         int weight;
00064 }point_2d;
00065 
00066 void write_pgm (std::string filename, std::vector<std::vector<int> > cm)
00067 {
00068         std::ofstream myfile;
00069         myfile.open (filename.c_str());
00070         myfile << "P2" << std::endl;
00071         myfile << (int)cm[0].size() << " " << (int)cm.size() << std::endl;
00072         int max = 0;
00073         for (std::vector<std::vector<int> >::iterator it = cm.begin (); it != cm.end (); it++)
00074                 for (std::vector<int>::iterator jt = it->begin(); jt != it->end (); jt++)
00075                         if (max < (*jt))
00076                                 max = (*jt);
00077         myfile << max << std::endl;
00078 
00079         for (std::vector<std::vector<int> >::iterator it = cm.begin (); it != cm.end (); it++)
00080         {
00081                 for (std::vector<int>::iterator jt = it->begin(); jt != it->end (); jt++)
00082                         myfile << (*jt) << " ";
00083                 myfile << std::endl;
00084         }
00085         myfile.close();
00086 }
00087 /*void write_kernel_pgm (std::string filename, std::vector<std::vector<point_2d> > ker)
00088 {
00089         std::ofstream myfile;
00090         myfile.open (filename.c_str());
00091         myfile << "P2" << std::endl;
00092         myfile << (int)ker[0].size() << " " << (int)ker.size() << std::endl;
00093         point_2d max;
00094         for (std::vector<std::vector<point_2d> >::iterator it = ker.begin (); it != ker.end (); it++)
00095                 for (std::vector<point_2d>::iterator jt = it->begin(); jt != it->end (); jt++)
00096                 {
00097                         if (max.x < jt->x)
00098                                 max.x = jt->x;
00099                 if (max.y<jt->y)
00100                         max.y-jt->y;
00101                 if (max.weight<jt->weight)
00102                         max.weight=jt->weight;
00103                 }
00104         myfile << max.x<<max.y << max.weight<<std::endl;
00105 
00106         for (std::vector<std::vector<point_2d> >::iterator it = ker.begin (); it != ker.end (); it++)
00107         {
00108                 for (std::vector<point_2d>::iterator jt = it->begin(); jt != it->end (); jt++)
00109                         myfile <<jt-it->begin() << " ";
00110                 myfile << std::endl;
00111         }
00112         myfile.close();
00113 }
00114 */
00115 class NextBestView : public pcl_ros::PCLNodelet
00116 {
00117 
00118 public:
00119         void onInit ();
00120 protected:
00121         using pcl_ros::PCLNodelet::pnh_;
00122         // parameters:
00123         // topic namesros/ros.h
00124         std::string input_cloud_topic_;
00125         std::string output_octree_topic_;
00126         std::string output_pose_topic_;
00127         std::string laser_frame_;
00128         std::string ogrid_topic_;
00129         std::string ogrid_sub_topic_;
00130 
00131         // octree parameters
00132         double octree_res_, octree_maxrange_;
00133         int level_, free_label_, occupied_label_, unknown_label_, fringe_label_;
00134         bool check_centroids_;
00135         bool visualize_octree_;
00136 
00137         // search parameters
00138         double normal_search_radius_;
00139         int min_pts_per_cluster_;
00140         double eps_angle_;
00141         double tolerance_;
00142         double boundary_angle_threshold_;
00143 
00144         // costmap related stuff
00145         int nr_costmap_dirs_;
00146         double sensor_d_min_;
00147         double sensor_d_max_;
00148         double kernel_degree_step_;
00149         double kernel_radial_step_;
00150         double sensor_opening_angle_;
00151         double costmap_grid_cell_size_;
00152         double sensor_horizontal_angle_;
00153         double sensor_vertical_angle_;
00154         double sensor_horizontal_resolution_;
00155         double sensor_vertical_resolution_;
00156         double sensor_preferred_opening_angle_;
00157         double sensor_preferred_d_min_;
00158         double sensor_preferred_d_max_;
00159         double nr_pose_samples_;
00160         bool received_map_;
00161         int number;
00162         int fringe_nr_,free_nr_,occupied_nr_;
00163         int fringe_threashhold_;
00164         //geometry_msgs::Point min,max,minoc,maxoc;
00165         geometry_msgs::Pose p;
00166         //std::vector<std::vector<std::vector<int> > > border_costmap;
00167         // kernels for every (discretized) direction
00168         std::vector<std::vector<point_2d> > vis_kernel;
00169 
00170         //objects needed
00171         tf::TransformListener tf_listener_;
00172 
00173         //datasets
00174         octomap::OcTreePCL* octree_;
00175         octomap::ScanGraph* octomap_graph_;
00176 
00177         octomap::KeyRay ray;
00178         //pcl::PointCloud<pcl::PointXYZ> pointcloud2_pcl_total;
00179         sensor_msgs::PointCloud2 cloud_in_;
00180         geometry_msgs::PoseArray nbv_pose_array_;
00181         visualization_msgs::MarkerArray marker;
00182         visualization_msgs::Marker marker_bound_;
00183         visualization_msgs::Marker marker_occ_;
00184         nav_msgs::OccupancyGrid map_;
00185         // ROS communications / Publishers / Subscribers
00186         ros::Subscriber cloud_sub_;
00187         ros::Subscriber grid_sub_;
00188         ros::Publisher octree_pub_;
00189         //pcl_ros::Publisher<pcl::PointXYZ> border_cloud_pub_;
00190         ros::Publisher pose_pub_;
00191         ros::Publisher pose_marker_pub_, pose_marker_array_pub_;
00192         ros::Publisher pose_boundary_pub_;
00193         ros::Publisher pose_occupied_pub_;
00194         ros::Publisher ogrid_pub_;
00195         ros::Publisher marker_bound_pub_;
00196         ros::Publisher marker_occ_pub_;
00197         ros::Publisher aggregated_pub_;
00198         ros::Publisher border_cloud_pub_;
00199         // Publishes the octree in MarkerArray format so that it can be visualized in rviz
00200         ros::Publisher octree_marker_array_publisher_;
00201         /* The following publisher, even though not required, is used because otherwise rviz
00202          * cannot visualize the MarkerArray format without advertising the Marker format*/
00203         ros::Publisher octree_marker_publisher_;
00204         // Marker array to visualize the octree. It displays the occupied cells of the octree
00205         visualization_msgs::MarkerArray octree_marker_array_msg_;
00206 
00207         static bool compareClusters(pcl::PointIndices c1, pcl::PointIndices c2) { return (c1.indices.size() < c2.indices.size()); }
00208         void grid_cb (const nav_msgs::OccupancyGridConstPtr& grid_msg);
00209         void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pointcloud2_msg);
00210         void createOctree (pcl::PointCloud<pcl::PointXYZ>& pointcloud2_pcl, octomap::pose6d laser_pose);
00211         void visualizeOctree (const sensor_msgs::PointCloud2ConstPtr& pointcloud2_msg, geometry_msgs::Point viewpoint);
00212         void castRayAndLabel (pcl::PointCloud<pcl::PointXYZ>& cloud, octomap::pose6d origin);
00213         void findBorderPoints (pcl::PointCloud<pcl::PointXYZ>& border_cloud, std::string frame_id);
00214         void findOccupiedPoints(pcl::PointCloud<pcl::PointXYZ>& occupied_cloud, std::string frame_id);
00215         void computeBoundaryPoints (pcl::PointCloud<pcl::PointXYZ>& border_cloud, pcl::PointCloud<pcl::Normal>& border_normals, std::vector<pcl::PointIndices>& clusters, pcl::PointCloud<pcl::PointXYZ>* cluster_clouds);
00216 
00217         void extractClusters (const pcl::PointCloud<pcl::PointXYZ> &cloud,
00218                         const pcl::PointCloud<pcl::Normal> &normals,
00219                         float tolerance,
00220                         const boost::shared_ptr<pcl::KdTree<pcl::PointXYZ> > &tree,
00221                         std::vector<pcl::PointIndices> &clusters, double eps_angle,
00222                         unsigned int min_pts_per_cluster = 1,
00223                         unsigned int max_pts_per_cluster = std::numeric_limits<int>::max ());
00224 
00225         void create_kernels ();
00226         void find_min_max(pcl::PointCloud<pcl::PointXYZ> border_cloud, geometry_msgs::Point &min, geometry_msgs::Point &max);
00227         std::vector<std::vector<std::vector<int> > > create_costmap(double x_dim,double y_dim, int nr_dirs, pcl::PointCloud<pcl::PointXYZ> border_cloud,geometry_msgs::Point &min,geometry_msgs::Point &max);
00228         std::vector<std::vector<std::vector<int> > > create_empty_costmap (double x_dim, double y_dim, int nr_dirs);
00229         geometry_msgs::Pose find_best_pose(int best_i,int best_j,int best_k,int max_reward,geometry_msgs::Point &min,geometry_msgs::Point &max);
00230         geometry_msgs::Pose sample_from_costmap (std::vector<std::vector<std::vector<int> > > costmap, int max_reward, int nr_dirs, int x_dim, int y_dim, geometry_msgs::Point min, geometry_msgs::Point max, int &reward, double &score);
00231         //geometry_msgs::PoseArray computedirections(double x, double y, double theta, int reward);
00232         double compute_overlap_score(double x, double y, double theta, int reward);
00233         std::vector<std::vector<std::vector<int> > > reduced_costmap(std::vector<std::vector<std::vector<int> > > costmap, int best_j, int best_k,geometry_msgs::Point &min,geometry_msgs::Point &max,double &x_dim,double &y_dim);
00234         void find_max_indices (int &best_i, int &best_j, int &best_k,
00235                                 int nr_dirs, int x_dim, int y_dim, int &max_reward,
00236                                 std::vector<std::vector<std::vector<int > > > costmap);
00237 
00238         // save costmap stack to files
00239         void save_costmaps (int nr_dirs, std::vector<std::vector<std::vector<int> > > costmap, std::string file_prefix, ros::Time stamp);
00240         //void save_kernel(int nr_dirs,std::vector<std::vector<point_2d> > kernel, std::string file_prefix, ros::Time stamp);
00241 
00242 public:
00243         NextBestView();
00244 
00245         ~NextBestView();
00246 };
00247 
00248 
00249 NextBestView::NextBestView () : received_map_(false)
00250 {
00251 
00252 }
00253 
00254 NextBestView::~NextBestView()
00255 {
00256         ROS_INFO("Shutting down NextBestView!");
00257 
00258         octree_marker_array_publisher_.shutdown();
00259         octree_marker_publisher_.shutdown();
00260 }
00261 
00262 void NextBestView::onInit()
00263 {
00264         pcl_ros::PCLNodelet::onInit ();
00265         // costmap params
00266         pnh_->param("nr_costmap_dirs", nr_costmap_dirs_, 8);
00267         pnh_->param("sensor_d_min", sensor_d_min_, 1.0);
00268         pnh_->param("sensor_d_max", sensor_d_max_, 4.0);
00269         pnh_->param("kernel_radial_step", kernel_radial_step_, 0.1);
00270         pnh_->param("kernel_degree_step", kernel_degree_step_, 10.0);
00271         kernel_degree_step_=pi*kernel_degree_step_/180.0;
00272         pnh_->param("sensor_opening_angle", sensor_opening_angle_, 180.0);
00273         sensor_opening_angle_=pi*sensor_opening_angle_/180.0;
00274         pnh_->param("costmap_grid_cell_size", costmap_grid_cell_size_, 0.15);
00275         pnh_->param("sensor_horizontal_angle", sensor_horizontal_angle_,180.0);//180 degrees
00276         sensor_horizontal_angle_=pi*sensor_horizontal_angle_/180.0;
00277         pnh_->param("sensor_vertical_angle", sensor_vertical_angle_,91.71);
00278         sensor_vertical_angle_=sensor_vertical_angle_*pi/180.0;
00279         pnh_->param("sensor_horizontal_resolution", sensor_horizontal_resolution_,1.0);//1 degree
00280         sensor_horizontal_resolution_=sensor_horizontal_resolution_*pi/180.0;
00281         pnh_->param("sensor_vertical_resolution", sensor_vertical_resolution_,1.0);//1 degree
00282         sensor_vertical_resolution_=sensor_vertical_resolution_*pi/180.0;
00283         pnh_->param("sensor_preferred_opening_angle", sensor_preferred_opening_angle_,120.0);
00284         sensor_preferred_opening_angle_=sensor_preferred_opening_angle_*pi/180.0;
00285         pnh_->param("sensor_preferred_d_min", sensor_preferred_d_min_,1.0);
00286         pnh_->param("sensor_preferred_d_max", sensor_preferred_d_max_,3.0);
00287         pnh_->param("nr_pose_samples", nr_pose_samples_,100.0);//1 degree
00288         // topic names
00289         pnh_->param("ogrid_sub_topic", ogrid_sub_topic_, std::string("/map"));
00290         pnh_->param("ogrid_topic", ogrid_topic_, std::string("/nbv_map"));
00291         pnh_->param("input_cloud_topic", input_cloud_topic_, std::string("/cloud_pcd"));
00292         pnh_->param("output_octree_topic", output_octree_topic_, std::string("/nbv_octree"));
00293         pnh_->param("output_pose_topic", output_pose_topic_, std::string("/nbv_pose"));
00294         pnh_->param("laser_frame", laser_frame_, std::string("/laser_tilt_link"));
00295         pnh_->param("octree_resolution", octree_res_, 0.1);
00296 
00297         // octree stuff
00298         pnh_->param("octree_maxrange", octree_maxrange_, -1.0);
00299         pnh_->param("level", level_, 0);
00300         pnh_->param("check_centroids", check_centroids_, false);
00301         pnh_->param("free_label", free_label_, 0);
00302         pnh_->param("occupied_label", occupied_label_, 1);
00303         pnh_->param("unknown_label", unknown_label_, -1);
00304         pnh_->param("fringe_label", fringe_label_, 2);
00305         pnh_->param("visualize_octree", visualize_octree_, true);
00306 
00307         // search stuff
00308         pnh_->param("normal_search_radius", normal_search_radius_, 0.6);
00309         pnh_->param("min_pts_per_cluster", min_pts_per_cluster_, 10);
00310         pnh_->param("eps_angle", eps_angle_, 0.25);
00311         pnh_->param("tolerance", tolerance_, 0.3);
00312         pnh_->param("boundary_angle_threshold", boundary_angle_threshold_, 2.5);
00313         //ending criterion stuff
00314         pnh_->param("fringe_threashold",fringe_threashhold_,100);
00315 
00316         // create subs and pubs
00317         cloud_sub_ = pnh_->subscribe (input_cloud_topic_, 1, &NextBestView::cloud_cb, this);
00318         octree_pub_ = pnh_->advertise<octomap_ros::OctomapBinary> (output_octree_topic_, 1);
00319         ogrid_pub_ = pnh_->advertise<nav_msgs::OccupancyGrid> (ogrid_topic_, 1);
00320         grid_sub_=pnh_->subscribe(ogrid_sub_topic_,1,&NextBestView::grid_cb,this);
00321         //border_cloud_pub_ = pcl_ros::Publisher<pcl::PointXYZ> (*pnh_, "border_cloud", 1);
00322         pose_pub_ = pnh_->advertise<geometry_msgs::PoseArray> (output_pose_topic_, 1);
00323         pose_marker_pub_=pnh_->advertise<visualization_msgs::Marker>("pose_marker", 100);
00324         pose_marker_array_pub_=pnh_->advertise<visualization_msgs::MarkerArray>("pose_marker_array", 100);
00325         pose_boundary_pub_ = pnh_->advertise<geometry_msgs::PoseArray> ("boundary_pose", 1);
00326         pose_occupied_pub_ = pnh_->advertise<geometry_msgs::PoseArray> ("occupied_pose", 1);
00327         octree_marker_array_publisher_ = pnh_->advertise<visualization_msgs::MarkerArray>("visualization_marker_array",100);
00328         octree_marker_publisher_ = pnh_->advertise<visualization_msgs::Marker>("visualization_marker", 100);
00329         marker_bound_pub_=pnh_->advertise<visualization_msgs::Marker>("visualization_boundary_marker", 100);
00330         marker_occ_pub_=pnh_->advertise<visualization_msgs::Marker>("visualization_occupancy_marker", 100);
00331         aggregated_pub_=pnh_->advertise<sensor_msgs::PointCloud2>("aggregated_point_cloud",1);
00332         border_cloud_pub_=pnh_->advertise<sensor_msgs::PointCloud2>("border_cloud",1);
00333         octree_ = NULL;
00334         // finally, precompute visibility kernel points
00335         create_kernels ();
00336 }
00337 
00338 void NextBestView::find_max_indices (int &best_i, int &best_j, int &best_k,
00339                         int nr_dirs, int x_dim, int y_dim, int &max_reward,
00340                         std::vector<std::vector<std::vector<int > > > costmap)
00341 {
00342         int bla=0;
00343         std::ofstream file;
00344         file.open("/home/ghitzarus/Desktop/rewards.txt");
00345         for (int i = 0; i < nr_dirs; ++i)
00346         {
00347                 //              std::stringstream ss;
00348                 //              ss << "costmap_" << border_cloud.header.stamp << "_" << i;
00349                 //              write_pgm (ss.str(), costmap[i]);
00350                 for (int j = 0; j < x_dim; ++j)
00351                         for (int k = 0; k < y_dim; k++)
00352                         {
00353                                 int reward = costmap[i][j][k];
00354                                 if (reward!=0) bla++;
00355                                 file<<reward<<std::endl;
00356                                 //ROS_INFO("The reward is: %d",reward);
00357                                 if (reward > max_reward)
00358                                 {
00359 
00360                                         max_reward = reward;
00361                                         best_i = i;
00362                                         best_j = j;
00363                                         best_k = k;
00364                                 }
00365                         }
00366         }
00367         file.close();
00368         //ROS_INFO("THE NUMBER OF REWARDS DIFFERNT THAN 0 is: %d",bla);
00369         //ROS_INFO("max revard is :%d ",max_reward);
00370 }
00371 void NextBestView::save_costmaps (int nr_dirs, std::vector<std::vector<std::vector<int> > > costmap, std::string file_prefix, ros::Time stamp)
00372 {
00373         for (int i = 0; i < nr_dirs; ++i)
00374         {
00375                 std::stringstream ss;
00376                 ss << file_prefix << "_" << stamp << "_" << i << ".pgm";
00377                 write_pgm (ss.str(), costmap[i]);
00378         }
00379 }
00380 /*void NextBestView::save_kernel(int nr_dirs,std::vector<std::vector<point_2d> > kernel, std::string file_prefix, ros::Time stamp)
00381 {
00382      for(int i=0;i<nr_dirs;i++)
00383      {
00384                         std::stringstream ss;
00385                         ss << file_prefix << "_" << stamp << "_" << i << ".pgm";
00386                         write_kernel_pgm (ss.str(), kernel);
00387      }
00388 }
00389 */
00390 void NextBestView::find_min_max(pcl::PointCloud<pcl::PointXYZ> border_cloud,geometry_msgs::Point &min, geometry_msgs::Point &max)
00391 {
00392                 for (unsigned int i=1;i<border_cloud.points.size();i++)
00393                 {
00394                         if (min.x > border_cloud.points[i].x) min.x = border_cloud.points[i].x;
00395                         if (min.y > border_cloud.points[i].y) min.y = border_cloud.points[i].y;
00396                         if (max.x < border_cloud.points[i].x) max.x = border_cloud.points[i].x;
00397                         if (max.y < border_cloud.points[i].y) max.y = border_cloud.points[i].y;
00398                 }
00399 }
00400 
00401 std::vector<std::vector<std::vector<int> > > NextBestView::create_empty_costmap (double x_dim, double y_dim, int nr_dirs)
00402 {
00403         std::vector<std::vector<std::vector<int> > > costmap;
00404         costmap.resize (nr_dirs);
00405         for (int i = 0; i < nr_dirs; ++i) {
00406                 costmap[i].resize(x_dim);
00407 
00408                 for (int j = 0; j < x_dim; ++j)
00409                         costmap[i][j].resize(y_dim);
00410         }
00411         return costmap;
00412 }
00413 
00414 
00415 std::vector<std::vector<std::vector<int> > > NextBestView::create_costmap (double x_dim, double y_dim, int nr_dirs, pcl::PointCloud<pcl::PointXYZ> cloud,geometry_msgs::Point &min,geometry_msgs::Point &max)
00416 {
00417         // initialize vector of costmaps
00418         std::vector<std::vector<std::vector<int> > > costmap = create_empty_costmap (x_dim, y_dim, nr_dirs);
00419 
00420                 // compute costmaps -- convolute with the different kernels
00421                 for (int dir=0;dir<nr_dirs;dir++)
00422                 {
00423                         for (unsigned int i=0;i<cloud.points.size();i++)
00424                         {
00425                                 for (unsigned int j=0;j<vis_kernel[dir].size();j++)
00426                                 {
00427                                         double x=(double)cloud.points[i].x + vis_kernel[dir][j].x;
00428                                         double y=(double)cloud.points[i].y + vis_kernel[dir][j].y;
00429                                         int id_x=(int)(x_dim*((x-min.x)/(max.x-min.x)));
00430                                         int id_y=(int)(y_dim*((y-min.y)/(max.y-min.y)));
00431 
00432 
00433                                         if (received_map_)
00434                                         {
00435                                                 //  // NOTE: this assumes the "/map" message had no rotation (0,0,0,1)..
00436                                                 double min_map_x = (double)map_.info.origin.position.x;
00437                                                 double min_map_y = (double)map_.info.origin.position.y;
00438                                                 double max_map_x = (double)(map_.info.origin.position.x + map_.info.resolution * map_.info.width);
00439                                                 double max_map_y = (double)(map_.info.origin.position.y + map_.info.resolution * map_.info.height);
00440 
00441                                                 int id_x_m=(int)(map_.info.width*((x-min_map_x)/(max_map_x-min_map_x)));
00442                                                 int id_y_m=(int)(map_.info.height*((y-min_map_y)/(max_map_y-min_map_y)));
00443                                                 if (map_.data[id_y_m*map_.info.width+id_x_m] == 0)
00444                                                         if (id_x >= 0 && id_x < x_dim &&
00445                                                                         id_y >= 0 && id_y < y_dim)
00446                                                                 costmap[dir][id_x][id_y]+=vis_kernel[dir][j].weight;
00447                                         }
00448                                         else
00449                                                 if (id_x >= 0 && id_x < x_dim &&
00450                                                                 id_y >= 0 && id_y < y_dim)
00451                                                         costmap[dir][id_x][id_y]+=vis_kernel[dir][j].weight;
00452                                 }
00453                         }
00454                 }
00455                 return costmap;
00456 }
00457 geometry_msgs::Pose NextBestView::find_best_pose(int best_i,int best_j,int best_k,int max_reward,geometry_msgs::Point &min,geometry_msgs::Point &max)
00458 {
00459             p.position.x = min.x + (best_j + 0.5) * costmap_grid_cell_size_;
00460                 p.position.y = min.y + (best_k + 0.5) * costmap_grid_cell_size_;
00461                 p.position.z = 0;
00462                 btVector3 axis(0, 0, 1);
00463                 btQuaternion quat (axis, pi+((double)best_i)*2.0*pi/(double)nr_costmap_dirs_);
00464                 ROS_INFO("BEST I,J,K::: %i %i %i (reward %i)", best_i, best_j, best_k, max_reward);
00465                 ROS_INFO("BEST robot pose::: x,y = [%f , %f], theta = %f", p.position.x, p.position.y, best_i*2.0*pi/nr_costmap_dirs_);
00466 
00467                 geometry_msgs::Quaternion quat_msg;
00468                 tf::quaternionTFToMsg(quat, quat_msg);
00469                 p.orientation = quat_msg;
00470                 return p;
00471 }
00472 std::vector<std::vector<std::vector<int> > >  NextBestView::reduced_costmap(std::vector<std::vector<std::vector<int> > > costmap, int best_j, int best_k,geometry_msgs::Point &min,geometry_msgs::Point &max,double &x_dim,double &y_dim)
00473 {
00474           p.position.x = min.x + (best_j + 0.5) * costmap_grid_cell_size_;
00475           p.position.y = min.y + (best_k + 0.5) * costmap_grid_cell_size_;
00476           p.position.z = 0;
00477                 octomap::OcTreeNodePCL *octree_node = octree_->search(p.position.x,p.position.y,p.position.y);
00478                 for (int dir=0;dir<nr_costmap_dirs_;dir++)
00479                         for (int i=0;i<x_dim;i++)
00480                                 for (int j=0;j<y_dim;j++)
00481                                 {
00482                                         if (octree_node != NULL )
00483                                         {
00484                                                 if (octree_node->getLabel()!=occupied_label_)
00485                                                    costmap[dir][i][j]=0;
00486                                         }
00487                                 }
00488         return costmap;
00489 }
00490 /*geometry_msgs::PoseArray NextBestView::computedirections(double x, double y, double theta, int reward)
00491 {
00492         geometry_msgs::PoseArray ret_poses;
00493         std::vector<std::vector<int> > node_map;
00494         double node_map_x=(int)(pi / 0.00436332313)+1;
00495         double node_map_y=(int)(1.6/0.0174532925)+1;
00496         node_map.resize (node_map_x);
00497         for (int i = 0; i<node_map_x; i++)
00498                 node_map[i].resize(node_map_y);
00499 
00500         geometry_msgs::Pose pose;
00501         pose.position.x = x;
00502         pose.position.y = y;
00503         pose.position.z = 1.35;
00504         octomap::point3d origin (x, y, pose.position.z);
00505         int hit = 0, miss = 0, fringe = 0, known = 0;
00506         // given a vector, along x axis.
00507         btVector3 x_axis (1, 0, 0);
00508         // rotate it "down" around y:
00509         btQuaternion rot_down (btVector3(0,1,0), 0.3);
00510         // rotate it in xy plane:
00511         btQuaternion rot_in_xy (btVector3(0,0,1), theta);
00512         int countx,county;
00513         for (double pid=-pi*0.5,countx=0;pid<pi*0.5,countx<node_map_x;pid+=0.00436332313,countx++)  // 0.25 degrees steps
00514         {
00515                 btQuaternion rot_in_laser_plane (btVector3 (0,0,1), pid);
00516                 for (double sid=-0.8,county=0;sid<0.8,county<node_map_y;sid+=0.0174532925,county++) // 1.0 degree steps
00517                 {
00518 
00519                         btQuaternion rot_laser_plane (btVector3 (0,1,0), sid);
00520                         btVector3 laser_ray (1, 0, 0);
00521                         btMatrix3x3 rot (rot_in_xy * rot_down * rot_laser_plane * rot_in_laser_plane);
00522                         btVector3 dir = rot * laser_ray;
00523                         octomap::point3d direction = octomap::point3d (dir.x(), dir.y(), dir.z());
00524                         octomap::point3d obstacle(0,0,0);
00525 
00526                         if (!octree_->castRay(origin, direction, obstacle, true, sensor_d_max_))
00527                           {
00528                                 miss++;
00529                           }
00530                         else {
00531                                 hit++;
00532                                 octomap::OcTreeNodePCL *octree_node = octree_->search(obstacle);
00533 
00534                                 // if occupied voxel
00535                                 if (octree_node != NULL)
00536                                 {
00537                                         if (octree_node->getLabel() == occupied_label_)
00538                                         {
00539                                                 known++;
00540                                                 node_map[countx][county] = 1;
00541                                         }
00542                                         else //if (octree_node->getLabel() == fringe_label_)
00543                                         {
00544                                                 fringe++;
00545                                                 node_map[countx][county] = 2;
00546                                         }
00547                                 }
00548 
00549                         }
00550 
00551                         //geometry_msgs::Quaternion dir_tic;
00552                         //tf::quaternionTFToMsg(rot_in_xy * rot_down * rot_laser_plane * rot_in_laser_plane, dir_tic);
00553                         //pose.orientation = dir_tic;
00554                 //ret_poses.poses.push_back (pose);
00555                 }
00556 
00557         }
00558         double p_f = (double)fringe/hit;
00559         double p_k = (double)known/hit;
00560         double entropy = - p_f * log (p_f) - p_k * log (p_k);
00561         ROS_INFO ("found %i fringe points, %i occupied points, ratios: %f, %f, Entropy = %f, score = %f",
00562                                         fringe, known, p_f, p_k, entropy, reward * entropy);
00563         return ret_poses;
00564 }
00565 */
00566 geometry_msgs::Pose NextBestView::sample_from_costmap (std::vector<std::vector<std::vector<int> > > costmap, int max_reward, int nr_dirs, int x_dim, int y_dim, geometry_msgs::Point min, geometry_msgs::Point max, int &reward, double &score)
00567 {
00568         int dir, x, y;
00569         int c;
00570         do
00571         {
00572         //ROS_INFO("Infinite loop :(");
00573                 dir = rand () * ((double)nr_dirs / RAND_MAX);
00574                 x = rand () * ((double)x_dim / RAND_MAX);
00575                 y = rand () * ((double)y_dim / RAND_MAX);
00576         //max_reward=max_reward*0.89;
00577                 c = 0.5 * (double)max_reward + rand () * ((double)max_reward * 0.5 / RAND_MAX);
00578                 c = sqrt((double)max_reward*max_reward - (double)c*c); // quarter circular transfer function
00579                 //c =rand () * ((double)max_reward/ RAND_MAX);
00580                 //c = sqrt((double)max_reward*max_reward/2 - (double)c*c);
00581                 //ROS_INFO("The c value is: %d",c);
00582         } while (costmap [dir][x][y] < c);
00583         reward = costmap[dir][x][y];
00584 
00585         geometry_msgs::Pose pose = find_best_pose(dir, x, y, reward, min, max);
00586 
00587         double real_x = min.x + (x + 0.5) * costmap_grid_cell_size_;
00588         double real_y = min.y + (y + 0.5) * costmap_grid_cell_size_;
00589         double theta = pi+((double)dir)*2.0*pi/(double)nr_costmap_dirs_;
00590         score = compute_overlap_score (real_x, real_y, theta, reward);
00591      pose.position.z = (double)score / 100;
00592         //pose.position.z=0;
00593         return (pose);
00594 }
00595 double NextBestView::compute_overlap_score(double x, double y, double theta, int reward)
00596 {
00597         std::vector<std::vector<int> > node_map;
00598                 double node_map_x=(int)(sensor_horizontal_angle_ / sensor_horizontal_resolution_)+1;
00599                 double node_map_y=(int)(sensor_vertical_angle_/sensor_vertical_resolution_)+1;
00600                 node_map.resize (node_map_x);
00601                 for (int i = 0; i<node_map_x; i++)
00602                         node_map[i].resize(node_map_y);
00603 
00604                 geometry_msgs::Pose pose;
00605                 pose.position.x = x;
00606                 pose.position.y = y;
00607                 pose.position.z = 1.35;
00608                 octomap::point3d origin (x, y, pose.position.z);
00609                 int hit = 0, miss = 0, fringe = 0, known = 0;
00610                 // given a vector, along x axis.
00611                 btVector3 x_axis (1, 0, 0);
00612                 // rotate it "down" around y:
00613                 btQuaternion rot_down (btVector3(0,1,0), 0.3);
00614                 // rotate it in xy plane:
00615                 btQuaternion rot_in_xy (btVector3(0,0,1), theta);
00616                 int countx = 0, county;
00617                 for (double pid=-sensor_horizontal_angle_*0.5;pid<sensor_horizontal_angle_*0.5 && countx<node_map_x;pid+=sensor_horizontal_resolution_,countx++)  // 1 degrees steps
00618                 {
00619                         btQuaternion rot_in_laser_plane (btVector3 (0,0,1), pid);
00620                         county = 0;
00621                         for (double sid=-sensor_vertical_angle_/2;sid<sensor_vertical_angle_/2 && county<node_map_y;sid+=sensor_vertical_resolution_,county++) // 1.0 degree steps
00622                         {
00623 
00624                                 btQuaternion rot_laser_plane (btVector3 (0,1,0), sid);
00625                                 btVector3 laser_ray (1, 0, 0);
00626                                 btMatrix3x3 rot (rot_in_xy * rot_down * rot_laser_plane * rot_in_laser_plane);
00627                                 btVector3 dir = rot * laser_ray;
00628                                 octomap::point3d direction = octomap::point3d (dir.x(), dir.y(), dir.z());
00629                                 octomap::point3d obstacle(0,0,0);
00630 
00631                                 if (!octree_->castRay(origin, direction, obstacle, true, sensor_d_max_))
00632                                   {
00633                                         miss++;
00634                                   }
00635                                 else {
00636                                         hit++;
00637                                         octomap::OcTreeNodePCL *octree_node = octree_->search(obstacle);
00638 
00639                                         // if occupied voxel
00640                                         if (octree_node != NULL)
00641                                         {
00642                                                 if (octree_node->getLabel() == occupied_label_)
00643                                                 {
00644                                                         known++;
00645                                                         node_map[countx][county] = 1;
00646                                                 }
00647                                                 else if (octree_node->getLabel() == fringe_label_)
00648                                                 {
00649                                                         fringe++;
00650                                                         node_map[countx][county] = 2;
00651                                                 }
00652                                         }
00653 
00654                                 }
00655 
00656                                 //geometry_msgs::Quaternion dir_tic;
00657                                 //tf::quaternionTFToMsg(rot_in_xy * rot_down * rot_laser_plane * rot_in_laser_plane, dir_tic);
00658                                 //pose.orientation = dir_tic;
00659                         //ret_poses.poses.push_back (pose);
00660                         }
00661 
00662                 }
00663                 double p_f = (double)fringe/hit;
00664                 double p_k = (double)known/hit;
00665                 double entropy;
00666                 if (p_f == 0 || p_k == 0)
00667                         entropy = 0;
00668                 else
00669                         entropy = pow((-0.5*p_f * log (p_f) -1.5*p_k * log (p_k)),1);
00670                 ROS_INFO ("found %i fringe points, %i occupied points, ratios: %f, %f, Entropy = %f, score = %f",
00671                                 fringe, known, p_f, p_k, entropy, reward * entropy);
00672                 return reward * entropy;
00673 }
00674 void NextBestView::grid_cb(const nav_msgs::OccupancyGridConstPtr& grid_msg)
00675 {
00676         map_.header = grid_msg->header;
00677         map_.info = grid_msg->info;
00678         map_.data.resize(grid_msg->data.size());
00679 
00680         // dilate the map..
00681         double dilation_radius = 0.5;
00682         int dilation_radius_grid_cells = (dilation_radius / map_.info.resolution);
00683         int n = dilation_radius_grid_cells * 2 + 1;
00684 
00685         std::vector<std::vector<int> > dil_kernel;
00686         ROS_INFO ("creating dilation kernel for map with size %i x %i... ", n, n);
00687         dil_kernel.resize(n);
00688         for (int i = 0; i < n; i++)
00689         {
00690                 dil_kernel[i].resize(n);
00691                 for (int j = 0; j < n; j++)
00692                 {
00693                         double sqr_dist = (i - (dilation_radius_grid_cells+1))*(i - (dilation_radius_grid_cells+1))
00694                                                         + (j - (dilation_radius_grid_cells+1))*(j - (dilation_radius_grid_cells+1));
00695                         if (sqr_dist < dilation_radius_grid_cells*dilation_radius_grid_cells)
00696                                 dil_kernel[i][j] = 1;
00697                         else
00698                                 dil_kernel[i][j] = 0;
00699                 }
00700         }
00701         ROS_INFO ("done.");
00702 
00703 
00704         ROS_INFO ("dilating map... ");
00705         for (unsigned int p = 0; p < grid_msg->data.size(); p++)
00706                 map_.data[p] = grid_msg->data[p];
00707         int w = grid_msg->info.width;
00708         int h = grid_msg->info.height;
00709         for (unsigned int p = 0; p < grid_msg->data.size(); p++)
00710         {
00711                 if (grid_msg->data[p] == 0) // free point in map
00712                 {
00713                         // find index in data... set it to 100
00714                         int row = p / w;
00715                         int col = (p - row * w) - (dilation_radius_grid_cells+1);
00716                         row -= (dilation_radius_grid_cells+1);
00717 
00718                         bool found_occupied_cell = false;
00719                         for (int i = 0; i < n && !found_occupied_cell; i++)
00720                                 for (int j = 0; j < n && !found_occupied_cell; j++)
00721                                         if (dil_kernel[i][j] == 1)
00722                                         {
00723                                                 int x = col + i;
00724                                                 int y = row + j;
00725                                                 if (x >= 0 && x < w && y >= 0 && y < h &&
00726                                                                 grid_msg->data[y*w+x] != 0)
00727                                                 {
00728                                                         map_.data[p] = 100;
00729                                                         found_occupied_cell = true;
00730                                                 }
00731                                         }
00732                 }
00733         }
00734         ROS_INFO ("done.");
00735 
00736         received_map_=true;
00737 }
00738 
00739 
00740 void NextBestView::create_kernels ()
00741 {
00742         vis_kernel.resize(nr_costmap_dirs_);
00743         point_2d ker_pt;
00744         ROS_INFO ("creating visibility kernels... ");
00745         for(double d = sensor_d_min_; d < sensor_d_max_; d += kernel_radial_step_)
00746         {
00747                 for (double phi2 = -sensor_opening_angle_/2; phi2 < sensor_opening_angle_/2; phi2 += kernel_degree_step_)
00748                 {
00749                         double x = d*cos(phi2);
00750                         double y = d*sin(phi2);
00751 
00752                         for (int i = 0; i < nr_costmap_dirs_; i++)
00753                         {
00754                                 double theta=i*2*pi/nr_costmap_dirs_;
00755                                 ker_pt.x = cos(theta)*x - sin(theta)*y;
00756                                 ker_pt.y = sin(theta)*x + cos(theta)*y;
00757                                 if (d>sensor_preferred_d_min_ && d<sensor_preferred_d_max_
00758                                                 && fabs(phi2)<sensor_preferred_opening_angle_/2)
00759                                         ker_pt.weight = 2;
00760                                 else
00761                                         ker_pt.weight = 1;
00762                                 vis_kernel[i].push_back(ker_pt);
00763                         }
00764                 }
00765         }
00766         ROS_INFO ("done.");
00767 }
00768 
00774 void NextBestView::cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pointcloud2_msg)
00775 {
00776         fringe_nr_=0;
00777         free_nr_=0;
00778         occupied_nr_=0;
00779         counter++;
00780         //sensor_msgs::PointCloud2ConstPtr& pointcloud2_msg_total;
00781         ros::Time start_time = ros::Time::now();
00782         pcl::PointCloud<pcl::PointXYZ> pointcloud2_pcl;
00783         pcl::fromROSMsg(*pointcloud2_msg, pointcloud2_pcl);
00784         //pointcloud2_pcl_total.header.frame_id="/map";
00785         //pointcloud2_pcl_total.header.stamp=start_time;
00786         //pointcloud2_pcl_total+=pointcloud2_pcl;
00787         //number+=pointcloud2_pcl.points.size();
00788     //pointcloud2_pcl_total.points.resize(number);
00789 //uncommnet if you want to get clouds incrementally
00790         //if (counter<13)
00791                 //return;
00792 
00793 
00794         //pointcloud2_pcl_total.points.resize(number);
00795 
00796 
00797     //aggregated_pub_.publish(pointcloud2_pcl_total);
00798         //ROS_INFO("Number is %d);
00799         octomap::point3d octomap_point3d;
00800         ROS_INFO("Received point cloud with number of points %ld",pointcloud2_pcl.points.size());
00801         //get the latest parameters
00802         pnh_->getParam("normal_search_radius", normal_search_radius_);
00803         pnh_->getParam("min_pts_per_cluster", min_pts_per_cluster_);
00804         pnh_->getParam("eps_angle", eps_angle_);
00805         pnh_->getParam("tolerance", tolerance_);
00806         pnh_->getParam("boundary_angle_threshold", boundary_angle_threshold_);
00807 
00808         //get the viewpoint (position of laser) from tf
00809         tf::StampedTransform transform;
00810         try {
00811 
00812                 ros::Time acquisition_time = pointcloud2_pcl.header.stamp;
00813 
00814                 ros::Duration timeout(1.0 / 30);
00815 
00816                 tf_listener_.waitForTransform(pointcloud2_pcl.header.frame_id, laser_frame_, acquisition_time, timeout);
00817 
00818                 tf_listener_.lookupTransform(pointcloud2_pcl.header.frame_id, laser_frame_, acquisition_time, transform);
00819         }
00820         catch (tf::TransformException& ex) {
00821                 ROS_WARN("[next_best_view] TF exception:\n%s", ex.what());
00822         }
00823         tf::Point pt = transform.getOrigin();
00824         octomap::pose6d laser_pose (pt.x(), pt.y(), pt.z(),0,0,0);
00825         ROS_INFO("viewpoint [%f %f %f]", pt.x(), pt.y(), pt.z());
00826 
00827 
00828         //Converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
00829         //pcl::fromROSMsg(*pointcloud2_msg, pointcloud2_pcl);
00830 
00831         // create or update the octree
00832         if (octree_ == NULL) {
00833                 octomap_graph_ = new octomap::ScanGraph();
00834                 octree_ = new octomap::OcTreePCL(octree_res_);
00835         }
00836         createOctree(pointcloud2_pcl, laser_pose);
00837 
00838         /*
00839          * assign new points to Leaf Nodes  and cast rays from laser pos to point
00840          */
00841         castRayAndLabel(pointcloud2_pcl, laser_pose);
00842 
00843         /*
00844          * find unknown voxels with free neighbors and add them to a pointcloud
00845          */
00846         pcl::PointCloud<pcl::PointXYZ> border_cloud,border_cloud_filtered;
00847 
00848         findBorderPoints(border_cloud,pointcloud2_msg->header.frame_id);
00849         fringe_nr_=border_cloud.points.size();
00850     ROS_WARN("Number of free points is: %d and number of fringe points is: %d",free_nr_,fringe_nr_);
00851         pcl::PointCloud<pcl::PointXYZ> occupied_cloud,occupied_cloud_filtered;
00852         findOccupiedPoints(occupied_cloud,pointcloud2_msg->header.frame_id);
00853     ROS_WARN("Number of occupied points is: %d ",occupied_nr_);
00854     // the fringe_threashold_ value was determined after counting the number of fringe points  from IROS01.bag file (1707),
00855     // then the number of fringe points from IROS01.bag + IROS02.bag (1224), and in the and the number of fringe points
00856     // from IROS01.bag + IROS02.bag + IROS.03.bag (955)
00857     if (fringe_nr_>=fringe_threashhold_)
00858     {
00859         // Create the filterin objects
00860         pcl::PassThrough<pcl::PointXYZ> pass;
00861 
00862         //creating datasets
00863         pcl::PointCloud<pcl::Normal> border_normals;
00864 
00865         //filter out ceiling
00866         pass.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (border_cloud));
00867         pass.setFilterFieldName ("z");
00868         pass.setFilterLimits (0, 2.2);
00869         pass.filter(border_cloud_filtered);
00870         pass.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (occupied_cloud));
00871         pass.filter(occupied_cloud_filtered);
00872 
00873     geometry_msgs::Point min,max;
00874         min.x=FLT_MAX;
00875         min.y=FLT_MAX;
00876         max.x=-FLT_MAX;
00877         max.y=-FLT_MAX;
00878 
00879         find_min_max(border_cloud_filtered,min,max);
00880         find_min_max(occupied_cloud_filtered,min,max);
00881 
00882         min.x -= sensor_d_max_;
00883         min.y -= sensor_d_max_;
00884         max.x += sensor_d_max_;
00885         max.y += sensor_d_max_;
00886 
00887         double x_dim=(int)abs((max.x-min.x)/costmap_grid_cell_size_)+1;
00888         double y_dim=(int)abs((max.y-min.y)/costmap_grid_cell_size_)+1;
00889         ROS_INFO("X,Y min, max: %f, %f, %f, %f", min.x, min.y, max.x, max.y);
00890         ROS_INFO("X,Y DIMENSIONS OF THE COSTMAP: %f, %f",x_dim,y_dim);
00891         std::vector<std::vector<std::vector<int> > > boundary_costmap, occupied_costmap;
00892         boundary_costmap = create_costmap(x_dim, y_dim, nr_costmap_dirs_, border_cloud_filtered, min, max);
00893         occupied_costmap = create_costmap(x_dim, y_dim, nr_costmap_dirs_, occupied_cloud_filtered, min, max);
00894 
00895         // combine both costmaps using histogram intersection
00896         std::vector<std::vector<std::vector<int> > > bound_occ_costmap = create_empty_costmap(x_dim, y_dim, nr_costmap_dirs_);
00897         for (int i = 0; i < nr_costmap_dirs_; ++i)
00898                 for (int j = 0; j < x_dim; ++j)
00899                         for (int k = 0; k < y_dim; k++)
00900                         {
00901                                 // should use min function.. :D
00902                                 if (boundary_costmap[i][j][k] < occupied_costmap[i][j][k])
00903                                         bound_occ_costmap[i][j][k] =  boundary_costmap[i][j][k];
00904                                 else
00905                                         bound_occ_costmap[i][j][k] =  occupied_costmap[i][j][k];
00906                         }
00907 
00908         //save_costmaps (nr_costmap_dirs_, boundary_costmap, "boundary", pointcloud2_msg->header.stamp);
00909         //save_costmaps (nr_costmap_dirs_, occupied_costmap, "occupied", pointcloud2_msg->header.stamp);
00910         //save_costmaps (nr_costmap_dirs_, bound_occ_costmap, "combined", pointcloud2_msg->header.stamp);
00911         //save_kernel  (nr_costmap_dirs_, vis_kernel, "vis_kernel", pointcloud2_msg->header.stamp);
00912         // also publish the best found pose(s)
00913         nbv_pose_array_.header.frame_id = border_cloud_filtered.header.frame_id;
00914         nbv_pose_array_.header.stamp = ros::Time::now();
00915 
00916         // find best scanning pose -- simply the maximum
00917         int max_reward = 0;
00918         int best_combined_i=0, best_combined_j=0, best_combined_k=0;
00919 
00920         find_max_indices (best_combined_i, best_combined_j, best_combined_k, nr_costmap_dirs_, x_dim, y_dim, max_reward, bound_occ_costmap);
00921 
00922         nbv_pose_array_.poses.resize(0);
00923         //nbv_pose_array_.poses.push_back (find_best_pose(best_combined_i,best_combined_j,best_combined_k,max_reward,min,max));
00924         //geometry_msgs::Pose combined_bose=find_best_pose(best_combined_i,best_combined_j,best_combined_k,max_reward,min,max);
00925         std::vector<double> scores;
00926         int reward;
00927         double score;
00928 
00929          marker.markers.resize(nr_pose_samples_);
00930         bound_occ_costmap=reduced_costmap(bound_occ_costmap,best_combined_j,best_combined_k,min,max,x_dim,y_dim);
00931 
00932         for (int i = 0; i < nr_pose_samples_; i++)
00933         {
00934 
00935                 geometry_msgs::Pose pose = sample_from_costmap(bound_occ_costmap, max_reward, nr_costmap_dirs_, x_dim, y_dim, min, max, reward,score);
00936                 if (score == 0)
00937                         continue;
00938                 if (scores.size () == 0)
00939                 {
00940                         scores.insert (scores.begin(), score);
00941                         
00942                         nbv_pose_array_.poses.insert (nbv_pose_array_.poses.begin(), pose);
00943                 }
00944                 else
00945                 {
00946                 for (unsigned int j = 0; j < scores.size(); j++)
00947                         if (scores[j] < score)
00948                         {
00949                                 scores.insert (scores.begin() + j, score);
00950                               
00951                                 nbv_pose_array_.poses.insert (nbv_pose_array_.poses.begin() + j, pose);
00952                                 break;
00953                         }
00954                 }
00955         }
00956 
00957         for (unsigned int i = 0; i < scores.size (); i++)
00958                 ROS_INFO ("pose %i: %f", i, scores[i]);
00959     double threshold=scores[0] *0.85;
00960     int p=0;
00961     for (unsigned j=0;j<scores.size ();j++)
00962         if (scores[j]>threshold)
00963                 p=j+1;
00964         else break;
00965     nbv_pose_array_.poses.resize(p);
00966     ROS_INFO("resizing marker array for goto poses");
00967     marker.markers.resize(p);
00968         ROS_INFO("populating marker array for goto poses");
00969     for (int i = 0; i < p; i++)
00970     {
00971         marker.markers[i].header.frame_id = pointcloud2_msg->header.frame_id;
00972         marker.markers[i].header.stamp=ros::Time::now();
00973         marker.markers[i].type = visualization_msgs::Marker::ARROW;
00974         marker.markers[i].action = visualization_msgs::Marker::ADD;
00975         marker.markers[i].ns="autonomous_mapping";
00976         marker.markers[i].pose= nbv_pose_array_.poses[i];
00977         nbv_pose_array_.poses[i].position.z=0;
00978         marker.markers[i].scale.x=0.5;
00979         marker.markers[i].scale.y=4.0;
00980         marker.markers[i].scale.z=1.0;
00981         marker.markers[i].id = i;
00982         marker.markers[i].color.r = 0.0f;
00983         marker.markers[i].color.g = 0.0f;
00984         marker.markers[i].color.b = 1.0f;
00985         marker.markers[i].color.a = 1.0f;
00986 
00987         marker.markers[i].lifetime = ros::Duration::Duration();
00988     }
00989         ROS_INFO("populated marker array for goto poses");
00990     
00991     pose_pub_.publish(nbv_pose_array_);
00992         ROS_INFO("publishing marker array for goto poses");
00993     pose_marker_array_pub_.publish(marker);
00994     ROS_INFO("published marker array for goto poses,%ld",marker.markers.size());
00995         /*double x, y, theta;
00996     x = min.x + (best_combined_j + 0.5) * costmap_grid_cell_size_;
00997         y = min.y + (best_combined_k + 0.5) * costmap_grid_cell_size_;
00998         theta = pi+((double)best_combined_i)*2.0*pi/(double)nr_costmap_dirs_;
00999         //geometry_msgs::PoseArray scan_poses = computedirections(x, y, theta, bound_occ_costmap[best_combined_i][best_combined_j][best_combined_k]);
01000         scan_poses.header = nbv_pose_array_.header;
01001         pose_pub_.publish(scan_poses);
01002 
01003 
01004         nbv_pose_array_.poses.resize(0);
01005         nbv_pose_array_.poses.push_back (find_best_pose(best_combined_i,best_combined_j,best_combined_k,max_reward,min,max));
01006         pose_boundary_pub_.publish(nbv_pose_array_);*/
01007 
01008         max_reward = 0;
01009         int best_boundary_i=0, best_boundary_j=0, best_boundary_k=0;
01010         find_max_indices (best_boundary_i, best_boundary_j, best_boundary_k, nr_costmap_dirs_, x_dim, y_dim, max_reward, boundary_costmap);
01011         nbv_pose_array_.poses.resize(0);
01012         nbv_pose_array_.poses.push_back (find_best_pose(best_boundary_i,best_boundary_j,best_boundary_k,max_reward,min,max));
01013 
01014         //pose_boundary_pub_.publish(nbv_pose_array_);
01015         marker_bound_.header.frame_id=pointcloud2_msg->header.frame_id;
01016         marker_bound_.header.frame_id = pointcloud2_msg->header.frame_id;
01017         marker_bound_.header.stamp=ros::Time::now();
01018         marker_bound_.type = visualization_msgs::Marker::ARROW;
01019         marker_bound_.action = visualization_msgs::Marker::ADD;
01020         marker_bound_.ns="autonomous_mapping";
01021         marker_bound_.pose=find_best_pose(best_boundary_i,best_boundary_j,best_boundary_k,max_reward,min,max);
01022         marker_bound_.scale.x=0.5;
01023         marker_bound_.scale.y=4.0;
01024         marker_bound_.scale.z=1.0;
01025         marker_bound_.id =0;
01026         marker_bound_.color.r = 0.0f;
01027         marker_bound_.color.g = 1.0f;
01028         marker_bound_.color.b = 0.0f;
01029         marker_bound_.color.a = 1.0f;
01030         marker_bound_pub_.publish(marker_bound_);
01031         max_reward = 0;
01032         int best_occupied_i=0, best_occupied_j=0, best_occupied_k=0;
01033         find_max_indices (best_occupied_i, best_occupied_j, best_occupied_k, nr_costmap_dirs_, x_dim, y_dim, max_reward, occupied_costmap);
01034         nbv_pose_array_.poses.resize(0);
01035         nbv_pose_array_.poses.push_back (find_best_pose(best_occupied_i,best_occupied_j,best_occupied_k,max_reward,min,max));
01036         //pose_occupied_pub_.publish(nbv_pose_array_);
01037         marker_occ_.header.frame_id=pointcloud2_msg->header.frame_id;
01038         marker_occ_.header.frame_id = pointcloud2_msg->header.frame_id;
01039         marker_occ_.header.stamp=ros::Time::now();
01040         marker_occ_.type = visualization_msgs::Marker::ARROW;
01041         marker_occ_.action = visualization_msgs::Marker::ADD;
01042         marker_occ_.ns="autonomous_mapping";
01043         marker_occ_.pose=find_best_pose(best_occupied_i,best_occupied_j,best_occupied_k,max_reward,min,max);
01044         marker_occ_.scale.x=0.5;
01045         marker_occ_.scale.y=4.0;
01046         marker_occ_.scale.z=1.0;
01047         marker_occ_.id =0;
01048         marker_occ_.color.r = 1.0f;
01049         marker_occ_.color.g = 0.0f;
01050         marker_occ_.color.b = 0.0f;
01051         marker_occ_.color.a = 1.0f;
01052         marker_occ_pub_.publish(marker_occ_);
01053         // prepare a occupancy grid to be published to rviz
01054         nav_msgs::OccupancyGrid ogrid;
01055         ogrid.header.frame_id = "/map";
01056         ogrid.info.resolution = costmap_grid_cell_size_;
01057         ogrid.info.width = x_dim;
01058         ogrid.info.height = y_dim;
01059         geometry_msgs::Pose gridmap_pose;
01060         gridmap_pose.position.x = min.x;
01061         gridmap_pose.position.y = min.y;
01062         gridmap_pose.position.z = 0;
01063         ogrid.info.origin = gridmap_pose;
01064 
01065         // prepare a single costmap...
01066         std::vector<std::vector<int> > final_costmap;
01067         final_costmap.resize(x_dim);
01068         for (int i=0;i<x_dim;i++)
01069                 final_costmap[i].resize(y_dim);
01070 
01071         // assign the best costmap to be the final costmap
01072         int max_final_reward = 0;
01073         {
01074                 int k = best_boundary_i;
01075                 for (int i=0;i<x_dim;i++)
01076                         for(int j=0;j<y_dim;j++)
01077                         {
01078                                 final_costmap[i][j]+=boundary_costmap[k][i][j];
01079                                 if (final_costmap[i][j] > max_final_reward)
01080                                         max_final_reward = final_costmap[i][j];
01081                         }
01082         }
01083 
01084         // fill the occupancy grid message, scaled to 0...255
01085         for (int j=0;j<y_dim;j++)
01086                 for (int i=0;i<x_dim;i++)
01087                         ogrid.data.push_back (255 * (double)final_costmap[i][j] / (double)max_final_reward);
01088 
01089         ogrid_pub_.publish (ogrid);
01090 
01091         //std::stringstream sss;
01092         //sss << "final_costmap_" << border_cloud.header.stamp;
01093         //write_pgm (sss.str(), final_costmap);
01094 
01095 
01096         //publish border cloud for visualization
01097         border_cloud_pub_.publish(border_cloud);
01098 
01099         // publish binary octree
01100         if (0) {
01101                 octomap_ros::OctomapBinary octree_msg;
01102                 octomap_server::octomapMapToMsg(*octree_, octree_msg);
01103                 octree_pub_.publish(octree_msg);
01104         }
01105 
01106         ROS_INFO("All computed and published in %f seconds.", (ros::Time::now () - start_time).toSec());
01107         counter=0;
01108     }
01109     else
01110         {
01111          
01112         ROS_WARN("The number of fringe points is to small to continue scanning");
01113         exit(1);
01114         }
01115 
01116         //**********************************************************************************
01117         //Visualization
01118         //**********************************************************************************
01119         if (visualize_octree_)
01120         {
01121                 geometry_msgs::Point viewpoint;
01122                 viewpoint.x = pt.x();
01123                 viewpoint.y = pt.y();
01124                 viewpoint.z = pt.z();
01125                 visualizeOctree(pointcloud2_msg, viewpoint);
01126         }
01127 }
01128 
01129 void NextBestView::computeBoundaryPoints(pcl::PointCloud<pcl::PointXYZ>& border_cloud, pcl::PointCloud<pcl::Normal>& border_normals, std::vector<pcl::PointIndices>& clusters, pcl::PointCloud<pcl::PointXYZ>* cluster_clouds) {
01130         //clear old poses
01131         nbv_pose_array_.poses.clear();
01132 
01133         if (clusters.size() > 0) {
01134                 ROS_INFO ("%d clusters found.", (int)clusters.size());
01135                 // sort the clusters according to number of points they contain
01136                 std::sort(clusters.begin(), clusters.end(), compareClusters);
01137 
01138                 pcl::ExtractIndices<pcl::PointXYZ> extract;
01139                 pcl::ExtractIndices<pcl::Normal> nextract;
01140 
01141                 for (unsigned int nc = 0; nc < clusters.size(); nc++) {
01142                         //extract maximum of 3 biggest clusters
01143                         if (nc == 3)
01144                                 break;
01145 
01146                         //extract a cluster
01147                         pcl::PointCloud<pcl::PointXYZ> cluster_cloud;
01148                         extract.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (border_cloud));
01149                         extract.setIndices(boost::make_shared<pcl::PointIndices> (clusters.back()));
01150                         extract.setNegative(false);
01151                         extract.filter(cluster_cloud);
01152                         ROS_INFO ("PointCloud representing the cluster %d: %d data points.", nc, cluster_cloud.width * cluster_cloud.height);
01153 
01154                         //extract normals of cluster
01155                         pcl::PointCloud<pcl::Normal> cluster_normals;
01156                         nextract.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::Normal> > (border_normals));
01157                         nextract.setIndices(boost::make_shared<pcl::PointIndices> (clusters.back()));
01158                         nextract.setNegative(false);
01159                         nextract.filter(cluster_normals);
01160 
01161                         // find boundary points of cluster
01162                         pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree3 = boost::make_shared<pcl::KdTreeFLANN<pcl::PointXYZ> > ();
01163                         pcl::PointCloud<pcl::Boundary> boundary_cloud;
01164                         pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> be;
01165                         be.setSearchMethod(tree3);
01166                         be.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cluster_cloud));
01167                         be.setInputNormals(boost::make_shared<pcl::PointCloud<pcl::Normal> > (cluster_normals));
01168                         be.setRadiusSearch(.5);
01169                         be.angle_threshold_ = boundary_angle_threshold_;
01170                         be.compute(boundary_cloud);
01171 
01172                         geometry_msgs::Pose nbv_pose;
01173                         unsigned int nbp = 0;
01174                         /*for (unsigned int i = 0; i < boundary_cloud.points.size(); ++i) {
01175                                 if (boundary_cloud.points[i].boundary_point) {
01176                                         nbv_pose.position.x = cluster_cloud.points[i].x;
01177                                         nbv_pose.position.y = cluster_cloud.points[i].y;
01178                                         nbv_pose.position.z = cluster_cloud.points[i].z;
01179                                         btVector3 axis(0, -cluster_normals.points[i].normal[2], cluster_normals.points[i].normal[1]);
01180                                         btQuaternion quat(axis, axis.length());
01181                                         geometry_msgs::Quaternion quat_msg;
01182                                         tf::quaternionTFToMsg(quat, quat_msg);
01183                                         nbv_pose.orientation = quat_msg;
01184                                         nbv_pose_array_.poses.push_back(nbv_pose);
01185                                         nbp++;
01186                                 }
01187                         }*/
01188                         geometry_msgs::Point minim,maxim,geometric_center;
01189                         bool first_boundary = true;
01190                         minim.x=(double)cluster_cloud.points[0].x;
01191                         minim.y=(double)cluster_cloud.points[0].y;
01192                         minim.z=(double)cluster_cloud.points[0].z;
01193                         maxim.x=minim.x;
01194                         maxim.y=minim.y;
01195                         maxim.z=minim.z;
01196                         for (unsigned int i=1; i<boundary_cloud.points.size();++i)
01197                         {
01198                                 if (boundary_cloud.points[i].boundary_point)
01199                                 {
01200                                         if (first_boundary)
01201                                         {
01202                                                 minim.x=(double)cluster_cloud.points[i].x;
01203                                                 minim.y=(double)cluster_cloud.points[i].y;
01204                                                 minim.z=(double)cluster_cloud.points[i].z;
01205                                                 maxim.x=minim.x;
01206                                                 maxim.y=minim.y;
01207                                                 maxim.z=minim.z;
01208                                                 first_boundary = false;
01209                                         }
01210                                         else
01211                                         {
01212                                                 if (cluster_cloud.points[i].x<minim.x)
01213                                                         minim.x=(double)cluster_cloud.points[i].x;
01214                                                 if (cluster_cloud.points[i].y<minim.y)
01215                                                         minim.y=(double)cluster_cloud.points[i].y;
01216                                                 if (cluster_cloud.points[i].z<minim.z)
01217                                                         minim.z=(double)cluster_cloud.points[i].z;
01218                                                 if (cluster_cloud.points[i].x>maxim.x)
01219                                                         maxim.x=(double)cluster_cloud.points[i].x;
01220                                                 if (cluster_cloud.points[i].y>maxim.y)
01221                                                         maxim.y=(double)cluster_cloud.points[i].y;
01222                                                 if (cluster_cloud.points[i].z>maxim.z)
01223                                                         maxim.z=(double)cluster_cloud.points[i].z;
01224                                         }
01225                                 }
01226                         }
01227                         geometric_center.x=minim.x+(maxim.x-minim.x)/2.0;
01228                         geometric_center.y=minim.y+(maxim.y-minim.y)/2.0;
01229                         geometric_center.z=minim.z+(maxim.z-minim.z)/2.0;
01230                         ROS_INFO("the coordinates of the geometric center are %f, %f, %f",geometric_center.x,geometric_center.y,geometric_center.z);
01231                         pcl::PointCloud<pcl::PointXYZ> cluster_cloud_2,cluster_cloud_final;
01232                         cluster_cloud_2.header = boundary_cloud.header;
01233                         cluster_cloud_2.points.resize(boundary_cloud.points.size());
01234                         cluster_cloud_final.header = boundary_cloud.header;
01235                         cluster_cloud_final.points.resize(boundary_cloud.points.size());
01236                         double fi,dist;
01237                         double dx, dy;
01238                         double factor = 0.1;
01239 
01240                         for (unsigned i=0; i< boundary_cloud.points.size(); ++i)
01241                         {
01242 
01243                                 if (boundary_cloud.points[i].boundary_point)
01244                                 {
01245                                         fi=atan2(((double)cluster_cloud.points[i].x-geometric_center.x),
01246                                                         ((double)cluster_cloud.points[i].y-geometric_center.y));
01247                                         dist=abs((((double)cluster_cloud.points[i].x-geometric_center.x) / sin(fi)));
01248                                         dx = (double)cluster_cloud.points[i].x-geometric_center.x;
01249                                         dy = (double)cluster_cloud.points[i].y-geometric_center.y;
01250                                         //todo? normalize(dx,dy) to unit length, then factor is in meters..
01251                                         cluster_cloud_2.points[i].x = cluster_cloud.points[i].x + factor * dx; //(geometric_center.x+factor*dist*sin(fi));
01252                                         cluster_cloud_2.points[i].y = cluster_cloud.points[i].y + factor * dy; //(geometric_center.y+factor*dist*cos(fi));
01253                                 }
01254                         }
01255                         //ROS_INFO("TEST MESAGE %f",cluster_cloud_2.points[3].x);
01256                         int pt_counter = 0;
01257                         geometry_msgs::Point d,d_norm;
01258                         for (unsigned int i=0; i<boundary_cloud.points.size();++i)
01259                         {
01260                                 if (boundary_cloud.points[i].boundary_point)
01261                                 {
01262                                         octomap::OcTreeNodePCL * free_node = octree_->search(cluster_cloud_2.points[i].x,cluster_cloud_2.points[i].y,cluster_cloud_2.points[i].y);
01263                                         if ((free_node != NULL) && (free_node->getLabel() != occupied_label_))
01264                                         {
01265 
01266                                                 nbv_pose.position.x = cluster_cloud_2.points[i].x;
01267                                                 nbv_pose.position.y = cluster_cloud_2.points[i].y;
01268                                                 nbv_pose.position.z = cluster_cloud_2.points[i].z;
01269                                                 cluster_cloud_final.points[pt_counter].x=cluster_cloud_2.points[i].x;
01270                                                 cluster_cloud_final.points[pt_counter].y=cluster_cloud_2.points[i].y;
01271                                                 cluster_cloud_final.points[pt_counter].z=cluster_cloud_2.points[i].z;
01272                                                 pt_counter++;
01273                                                 //btVector3 axis(0, -cluster_normals.points[i].normal[2], cluster_normals.points[i].normal[1]);
01274                                                 d.x=cluster_cloud_2.points[i].x-geometric_center.x;
01275                                                 d.y=cluster_cloud_2.points[i].y-geometric_center.y;
01276                                                 d.z=cluster_cloud_2.points[i].z-geometric_center.z;
01277                                                 double norm=sqrt(pow(d.x,2)+pow(d.y,2)+pow(d.z,2));
01278                                                 d_norm.x=d.x/norm;
01279                                                 d_norm.y=d.y/norm;
01280                                                 d_norm.z=d.z/norm;
01281                                                 btVector3 axis(0, 0, 1);
01282                                                 btQuaternion quat (axis, acos(d_norm.x));
01283                                                 //ROS_INFO("ANGLE IS::: %f",cluster_cloud_2.points[i].z-geometric_center.z);
01284                                                 geometry_msgs::Quaternion quat_msg;
01285                                                 tf::quaternionTFToMsg(quat, quat_msg);
01286                                                 nbv_pose.orientation = quat_msg;
01287                                                 nbv_pose_array_.poses.push_back(nbv_pose);
01288                                                 nbp++;
01289                                         }
01290                                 }
01291                         }
01292                         ROS_INFO ("%d boundary points in cluster %d.", nbp, nc);
01293                         cluster_cloud_final.points.resize (pt_counter);
01294                         //save this cluster pointcloud for visualization
01295                         cluster_clouds[nc] = cluster_cloud_final;
01296                         //pop the just used cluster from indices
01297                         clusters.pop_back();
01298                 }
01299                 nbv_pose_array_.header.frame_id = border_cloud.header.frame_id;
01300                 nbv_pose_array_.header.stamp = ros::Time::now();
01301         }
01302         else {
01303                 ROS_INFO ("No clusters found!");
01304         }
01305 }
01306 
01307 void NextBestView::castRayAndLabel(pcl::PointCloud<pcl::PointXYZ>& cloud, octomap::pose6d origin) {
01308         octomap::point3d octomap_point3d;
01309 
01310         BOOST_FOREACH (const pcl::PointXYZ& pcl_pt, cloud.points) {
01311                 octomap_point3d(0) = pcl_pt.x;
01312                 octomap_point3d(1) = pcl_pt.y;
01313                 octomap_point3d(2) = pcl_pt.z;
01314                 octomap::OcTreeNodePCL * octree_end_node = octree_->search(octomap_point3d);
01315                 if (octree_end_node != NULL) {
01316                         // Get the nodes along the ray and label them as free
01317                         if (octree_->computeRayKeys(origin.trans(), octomap_point3d, ray)) {
01318                                 for(octomap::KeyRay::iterator it=ray.begin(); it != ray.end(); it++) {
01319                                         octomap::OcTreeNodePCL * free_node = octree_->search(*it);
01320                                         if (free_node != NULL) {
01321                                                 if (free_node->getLabel() != occupied_label_)
01322                                                 {
01323                                                         free_node->setLabel(free_label_);
01324 
01325                                                 }
01326                                         }
01327 
01328                                         else
01329                                                 ROS_DEBUG("node in ray not found!");
01330                                 }
01331                         }
01332                         else {
01333                                 ROS_DEBUG("could not compute ray from [%f %f %f] to [%f %f %f]", origin.x(), origin.y(), origin.z(), pcl_pt.x, pcl_pt.y, pcl_pt.z);
01334                         }
01335                         octree_end_node->set3DPointInliers(0);
01336                         octree_end_node->setLabel(occupied_label_);
01337                 }
01338                 else {
01339                         ROS_DEBUG("ERROR: node at [%f %f %f] not found", pcl_pt.x, pcl_pt.y, pcl_pt.z);
01340                 }
01341         }
01342 
01343 
01344 
01345 
01346 }
01347 void NextBestView::findOccupiedPoints(pcl::PointCloud<pcl::PointXYZ>& occupied_cloud, std::string frame_id)
01348 {
01349             occupied_cloud.header.frame_id = frame_id;
01350                 occupied_cloud.header.stamp = ros::Time::now();
01351                 std::list<octomap::OcTreeVolume> leaves;
01352                 octree_->getLeafNodes(leaves);
01353                 BOOST_FOREACH(octomap::OcTreeVolume vol, leaves) {
01354                         octomap::point3d centroid;
01355                         centroid(0) = vol.first.x(),  centroid(1) = vol.first.y(),  centroid(2) = vol.first.z();
01356                         octomap::OcTreeNodePCL *octree_node = octree_->search(centroid);
01357 
01358                         // if occupied voxel
01359                         if (octree_node != NULL && octree_node->getLabel() == occupied_label_)
01360                         {
01361                                 occupied_nr_++;
01362                                 pcl::PointXYZ occupied_pt (centroid.x(), centroid.y(), centroid.z());
01363                         occupied_cloud.points.push_back(occupied_pt);
01364                         }
01365                 }
01366 }
01367 
01368 void NextBestView::findBorderPoints(pcl::PointCloud<pcl::PointXYZ>& border_cloud, std::string frame_id)
01369 {
01370         border_cloud.header.frame_id = frame_id;
01371         border_cloud.header.stamp = ros::Time::now();
01372         std::list<octomap::OcTreeVolume> leaves;
01373         octree_->getLeafNodes(leaves);
01374         BOOST_FOREACH(octomap::OcTreeVolume vol, leaves)
01375         {
01376                 octomap::point3d centroid;
01377                 centroid(0) = vol.first.x(),  centroid(1) = vol.first.y(),  centroid(2) = vol.first.z();
01378                 octomap::OcTreeNodePCL *octree_node = octree_->search(centroid);
01379                 int found = 0;
01380                 // if free voxel -> check for unknown neighbors
01381                 if (octree_node != NULL && octree_node->getLabel() == free_label_)
01382                 {
01383 
01384                         free_nr_++;
01385                         for (int x = -1; x <= 1; x++)
01386                         {
01387                                 for (int y = -1; y <= 1; y++)
01388                                 {
01389                                         for (int z = -1; z <= 1; z++)
01390                                         {
01391                                                 if (x == 0 && y == 0 && z == 0)
01392                                                         continue;
01393                                                 octomap::point3d neighbor_centroid (
01394                                                                 centroid.x() + x * octree_res_,
01395                                                                 centroid.y() + y * octree_res_,
01396                                                                 centroid.z() + z * octree_res_);
01397 
01398                                                 octomap::OcTreeNodePCL *neighbor = octree_->search(neighbor_centroid);
01399                                                 if (neighbor != NULL && neighbor->getLabel() == unknown_label_) {
01400                                                         // add to list of border voxels
01401                                                         found++;
01402                                                         if (found > 7)
01403                                                         {
01404                                                                 pcl::PointXYZ border_pt (centroid.x(), centroid.y(), centroid.z());
01405                                                                 border_cloud.points.push_back(border_pt);
01406                                                                 octree_node->setLogOdds (CLAMPING_THRES_MAX);
01407                                                                 octree_node->setLabel(fringe_label_);
01408 
01409                                                                 break;
01410                                                         }
01411                                                 }
01412                                         }
01413                                         if (found > 7)
01414                                                 break;
01415                                 }
01416                                 if (found > 7)
01417                                         break;
01418                         }
01419                 }
01420         }
01421         ROS_INFO("%d points in border cloud", (int)border_cloud.points.size());
01422 }
01423 
01427 void NextBestView::createOctree (pcl::PointCloud<pcl::PointXYZ>& pointcloud2_pcl, octomap::pose6d laser_pose) {
01428 
01429         octomap::point3d octomap_3d_point;
01430         octomap::Pointcloud octomap_pointcloud;
01431 
01432         //Reading from pcl point cloud and saving it into octomap point cloud
01433         BOOST_FOREACH (const pcl::PointXYZ& pt, pointcloud2_pcl.points) {
01434                 octomap_3d_point(0) = pt.x;
01435                 octomap_3d_point(1) = pt.y;
01436                 octomap_3d_point(2) = pt.z;
01437                 octomap_pointcloud.push_back(octomap_3d_point);
01438         }
01439 
01440         // Converting from octomap point cloud to octomap graph
01441         ROS_INFO ("Number ofpoints in octomap_pointcloud %ld",octomap_pointcloud.size());
01442         octomap_pointcloud.transform(laser_pose.inv());
01443         octomap::ScanNode* scan_node = octomap_graph_->addNode(&octomap_pointcloud, laser_pose);
01444 
01445         //ROS_INFO("Number of points in scene graph: %d", octomap_graph_->getNumPoints());
01446 
01447         // Converting from octomap graph to octomap tree (octree)
01448         octree_->insertScan(*scan_node, octree_maxrange_, false);
01449 
01450         octree_->expand();
01451 
01452         //
01453         // create nodes that are unknown
01454         //
01455         //octomap::point3d min, max;
01456         double min[3], max[3];
01457         octree_->getMetricMin(min[0], min[1], min[2]);
01458         octree_->getMetricMax(max[0], max[1], max[2]);
01459         //ROS_DEBUG("octree min bounds [%f %f %f]", min(0), min(1), min(2));
01460         //ROS_DEBUG("octree max bounds [%f %f %f]", max(0), max(1), max(2));
01461 
01462         double x,y,z;
01463         for (x = min[0]+octree_res_/2; x < max[0]-octree_res_/2; x+=octree_res_) {
01464                 for (y = min[1]+octree_res_/2; y < max[1]-octree_res_/2; y+=octree_res_) {
01465                         for (z = min[2]+octree_res_/2; z < max[2]-octree_res_/2; z+=octree_res_) {
01466                                 octomap::point3d centroid (x, y, z);
01467                                 if (z > max[2])
01468                                         ROS_INFO("ahrg node at [%f %f %f]", centroid(0), centroid(1), centroid(2));
01469                                 octomap::OcTreeNodePCL *octree_node = octree_->search(centroid);
01470                                 if (octree_node != NULL) {
01471                                         octree_node->setCentroid(centroid);
01472                                 } else {
01473                                         //ROS_INFO("creating node at [%f %f %f]", centroid(0), centroid(1), centroid(2));
01474                                         //corresponding node doesn't exist yet -> create it
01475                                         octomap::OcTreeNodePCL *new_node = octree_->updateNode(centroid, false);
01476                                         new_node->setCentroid(centroid);
01477                                         new_node->setLabel(unknown_label_);
01478 
01479                                 }
01480                         }
01481                 }
01482         }
01483 
01484 
01485         if (check_centroids_) {
01486                 //int cnt = 0;
01487                 // get all existing leaves
01488                 std::list<octomap::OcTreeVolume> leaves;
01489                 //octree_->getLeafNodes(leaves);
01490 
01491                 //find Leaf Nodes' centroids, assign controid coordinates to Leaf Node
01492                 BOOST_FOREACH(octomap::OcTreeVolume vol, leaves) {
01493                         //ROS_DEBUG("Leaf Node %d : x = %f y = %f z = %f side length = %f ", cnt++, it1->first.x(), it1->first.y(), it1->first.z(), it1->second);
01494                         octomap::point3d centroid;
01495                         centroid(0) = vol.first.x(), centroid(1) = vol.first.y(), centroid(2) = vol.first.z();
01496                         octomap::OcTreeNodePCL *octree_node = octree_->search(centroid);
01497                         if (octree_node != NULL) {
01498                                 octomap::point3d test_centroid;
01499                                 test_centroid = octree_node->getCentroid();
01500                                 if (centroid.distance(test_centroid) > octree_res_/4)
01501                                         ROS_INFO("node at [%f %f %f] has a wrong centroid: [%f %f %f]", centroid(0), centroid(1), centroid(2), test_centroid(0), test_centroid(1), test_centroid(2));
01502                         }
01503                         else {
01504                                 ROS_INFO("node at [%f %f %f] not found", centroid(0), centroid(1), centroid(2));
01505                         }
01506                 }
01507         }
01508 
01509 }
01510 
01511 
01512 void NextBestView::visualizeOctree(const sensor_msgs::PointCloud2ConstPtr& pointcloud2_msg, geometry_msgs::Point viewpoint) {
01513         // each array stores all cubes of a different size, one for each depth level:
01514         octree_marker_array_msg_.markers.resize(3);
01515         double lowestRes = octree_->getResolution();
01516         //ROS_INFO_STREAM("lowest resolution: " << lowestRes);
01517 
01518         std::list<octomap::OcTreeVolume> all_cells;
01519         //getting the cells at level 0
01520         octree_->getLeafNodes(all_cells, 0);
01521         BOOST_FOREACH(octomap::OcTreeVolume vol, all_cells)
01522         {
01523                 geometry_msgs::Point cube_center;
01524                 cube_center.x = vol.first.x();
01525                 cube_center.y = vol.first.y();
01526                 cube_center.z = vol.first.z();
01527                 octomap::point3d octo_point (cube_center.x, cube_center.y, cube_center.z);
01528                 octomap::OcTreeNodePCL * node = octree_->search(octo_point);
01529                 if (node != NULL) {
01530                         if (occupied_label_ == node->getLabel())
01531                                 octree_marker_array_msg_.markers[0].points.push_back(cube_center);
01532                         else if (free_label_ == node->getLabel())
01533                                 octree_marker_array_msg_.markers[1].points.push_back(cube_center);
01534                         else if (unknown_label_ == node->getLabel())
01535                                 octree_marker_array_msg_.markers[2].points.push_back(cube_center);
01536 
01537                 }
01538         }
01539 
01540         //octree_marker_array_msg_.markers[3].points.push_back(viewpoint);
01541 
01542         // occupied cells
01543         octree_marker_array_msg_.markers[0].ns = "Occupied cells";
01544         octree_marker_array_msg_.markers[0].color.r = 1.0f;
01545         octree_marker_array_msg_.markers[0].color.g = 0.0f;
01546         octree_marker_array_msg_.markers[0].color.b = 0.0f;
01547         octree_marker_array_msg_.markers[0].color.a = 0.5f;
01548 
01549         // free cells
01550         octree_marker_array_msg_.markers[1].ns ="Free cells";
01551         octree_marker_array_msg_.markers[1].color.r = 0.0f;
01552         octree_marker_array_msg_.markers[1].color.g = 1.0f;
01553         octree_marker_array_msg_.markers[1].color.b = 0.0f;
01554         octree_marker_array_msg_.markers[1].color.a = 0.5f;
01555 
01556         // unknown cells
01557         octree_marker_array_msg_.markers[2].ns = "Unknown cells";
01558         octree_marker_array_msg_.markers[2].color.r = 0.0f;
01559         octree_marker_array_msg_.markers[2].color.g = 0.0f;
01560         octree_marker_array_msg_.markers[2].color.b = 1.0f;
01561         octree_marker_array_msg_.markers[2].color.a = 0.05f;
01562 
01563         // viewpoint
01564         /*octree_marker_array_msg_.markers[3].ns = "Fringe cells";
01565         octree_marker_array_msg_.markers[3].color.r = 1.0f;
01566         octree_marker_array_msg_.markers[3].color.g = 1.0f;
01567         octree_marker_array_msg_.markers[3].color.b = 0.0f;
01568         octree_marker_array_msg_.markers[3].color.a = 0.5f;*/
01569 
01570         for (unsigned i = 0; i < octree_marker_array_msg_.markers.size(); ++i)
01571         {
01572                 octree_marker_array_msg_.markers[i].header.frame_id = pointcloud2_msg->header.frame_id;
01573                 octree_marker_array_msg_.markers[i].header.stamp = ros::Time::now();
01574                 octree_marker_array_msg_.markers[i].id = i;
01575                 octree_marker_array_msg_.markers[i].lifetime = ros::Duration::Duration();
01576                 octree_marker_array_msg_.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
01577                 octree_marker_array_msg_.markers[i].scale.x = lowestRes;
01578                 octree_marker_array_msg_.markers[i].scale.y = lowestRes;
01579                 octree_marker_array_msg_.markers[i].scale.z = lowestRes;
01580 
01581                 if (octree_marker_array_msg_.markers[i].points.size() > 0)
01582                         octree_marker_array_msg_.markers[i].action = visualization_msgs::Marker::ADD;
01583                 else
01584                         octree_marker_array_msg_.markers[i].action = visualization_msgs::Marker::DELETE;
01585         }
01586 
01587         octree_marker_array_publisher_.publish(octree_marker_array_msg_);
01588 
01589         for (unsigned int i = 0; i < octree_marker_array_msg_.markers.size(); i++)
01590         {
01591                 if (!octree_marker_array_msg_.markers[i].points.empty())
01592                 {
01593                         octree_marker_array_msg_.markers[i].points.clear();
01594                 }
01595         }
01596         octree_marker_array_msg_.markers.clear();
01597 }
01598 }
01599 
01600 PLUGINLIB_DECLARE_CLASS(autonomous_mapping, NextBestView, autonomous_mapping::NextBestView, nodelet::Nodelet);
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


autonomous_mapping
Author(s): Gheorghe Rus
autogenerated on Sun Oct 6 2013 12:04:23