void GeospatialBoundingBox::exportInformationFile() { FILE *file_ptr = fopen(_format("%s.info",file_name.c_str()).c_str(), "w"); fprintf(file_ptr, "centroid: %f %f %f\n", centroid(0), centroid(1), centroid(2)); fprintf(file_ptr, "resolution: %f %f %f\n",resolution(0), resolution(1), resolution(2)); fprintf(file_ptr, "min_pt: %f %f %f\n", min_pt(0), min_pt(1), min_pt(2)); fprintf(file_ptr, "max_pt: %f %f %f\n", max_pt(0), max_pt(1), max_pt(2)); fprintf(file_ptr, "points: %d\n", number_of_points); fclose(file_ptr); return; }
template<typename Point> void TableObjectCluster<Point>::calculateBoundingBoxesOld(pcl::PointIndices::Ptr& pc_roi, std::vector<PointCloudPtr >& object_clusters, std::vector<pcl::PointCloud<pcl::PointXYZ> >& bounding_boxes) { ROS_INFO("roi: %d", pc_roi->indices.size()); #ifdef PCL_VERSION_COMPARE //fuerte typename pcl::search::KdTree<Point>::Ptr clusters_tree (new pcl::search::KdTree<Point>()); //typename pcl::search::OrganizedNeighbor<Point>::Ptr clusters_tree( new pcl::search::OrganizedNeighbor<Point>()); #else //electric typename pcl::KdTreeFLANN<Point>::Ptr clusters_tree (new pcl::KdTreeFLANN<Point> ()); #endif pcl::EuclideanClusterExtraction<Point> cluster_obj; // Table clustering parameters cluster_obj.setClusterTolerance (cluster_tolerance_); cluster_obj.setMinClusterSize (min_cluster_size_); cluster_obj.setInputCloud (input_); cluster_obj.setIndices(pc_roi); //cluster_obj.setSearchMethod (clusters_tree); std::vector<pcl::PointIndices> object_cluster_indices; cluster_obj.extract (object_cluster_indices); pcl::ExtractIndices<Point> ei; ei.setInputCloud(input_); for(unsigned int i = 0; i < object_cluster_indices.size(); ++i) { boost::shared_ptr<pcl::PointIndices> ind_ptr(&object_cluster_indices[i], null_deleter()); ei.setIndices(ind_ptr); PointCloudPtr cluster_ptr(new pcl::PointCloud<Point>); ei.filter(*cluster_ptr); object_clusters.push_back(cluster_ptr); pcl::PointCloud<pcl::PointXYZ> bb; Eigen::Vector4f min_pt, max_pt; pcl::getMinMax3D(*input_, object_cluster_indices[i], min_pt, max_pt); //if(fabs(max_pt(2)-min_pt(2))<0.03) continue; pcl::PointXYZ p; p.x = min_pt(0); p.y = min_pt(1); p.z = min_pt(2); bb.push_back(p); p.x = max_pt(0); p.y = max_pt(1); p.z = max_pt(2); bb.push_back(p); bounding_boxes.push_back(bb); } }
void GeospatialBoundingBox::load(int index, std::string *base_file_name) { if (base_file_name) { file_name = (*base_file_name); } else { file_name = _format("geo_bbox_%d.xyz.info",index); } FILE *file_ptr = fopen(file_name.c_str(), "r"); fscanf(file_ptr,"centroid: %f %f %f\n",¢roid(0), ¢roid(1), ¢roid(2)); fscanf(file_ptr,"resolution: %f %f %f\n",&resolution(0), &resolution(1), &resolution(2)); fscanf(file_ptr,"min_pt: %f %f %f\n",&min_pt(0), &min_pt(1), &min_pt(2)); fscanf(file_ptr,"max_pt: %f %f %f\n",&max_pt(0), &max_pt(1), &max_pt(2)); fscanf(file_ptr,"points: %d\n", &number_of_points); fclose(file_ptr); return; }
// Subsample cloud for faster matching and processing, while filling in normals. void PointcloudProc::reduceCloud(const PointCloud<PointXYZRGB>& input, PointCloud<PointXYZRGBNormal>& output) const { PointCloud<PointXYZRGB> cloud_nan_filtered, cloud_box_filtered, cloud_voxel_reduced; PointCloud<Normal> normals; PointCloud<PointXYZRGBNormal> cloud_normals; std::vector<int> indices; // Filter out nans. removeNaNFromPointCloud(input, cloud_nan_filtered, indices); indices.clear(); // Filter out everything outside a [200x200x200] box. Eigen::Vector4f min_pt(-100, -100, -100, -100); Eigen::Vector4f max_pt(100, 100, 100, 100); getPointsInBox(cloud_nan_filtered, min_pt, max_pt, indices); ExtractIndices<PointXYZRGB> boxfilter; boxfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_nan_filtered)); boxfilter.setIndices (boost::make_shared<vector<int> > (indices)); boxfilter.filter(cloud_box_filtered); // Reduce pointcloud VoxelGrid<PointXYZRGB> voxelfilter; voxelfilter.setInputCloud (boost::make_shared<const PointCloud<PointXYZRGB> > (cloud_box_filtered)); voxelfilter.setLeafSize (0.05, 0.05, 0.05); // voxelfilter.setLeafSize (0.1, 0.1, 0.1); voxelfilter.filter (cloud_voxel_reduced); // Compute normals NormalEstimation<PointXYZRGB, Normal> normalest; normalest.setViewPoint(0, 0, 0); normalest.setSearchMethod (boost::make_shared<search::KdTree<PointXYZRGB> > ()); //normalest.setKSearch (10); normalest.setRadiusSearch (0.25); // normalest.setRadiusSearch (0.4); normalest.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGB> >(cloud_voxel_reduced)); normalest.compute(normals); pcl::concatenateFields (cloud_voxel_reduced, normals, cloud_normals); // Filter based on curvature PassThrough<PointXYZRGBNormal> normalfilter; normalfilter.setFilterFieldName("curvature"); // normalfilter.setFilterLimits(0.0, 0.2); normalfilter.setFilterLimits(0.0, 0.2); normalfilter.setInputCloud(boost::make_shared<const PointCloud<PointXYZRGBNormal> >(cloud_normals)); normalfilter.filter(output); }
Map Map::generate_random( const MapRandomConstraints& c, MapRandomResults& res ) { const int x_dimension = 75, y_dimension = 20; Map m( y_dimension, x_dimension ); vector< Room > room_vector; size_t n_rooms = hack_range( 5, 12 ); for( size_t i = 0; i < n_rooms; i++){ Vector2d dim( hack_range( 5, 9 ), hack_range( 4, 8 ) ); size_t tries = 0, j; while( tries < 20 ) { ++ tries; Vector2d min_pt( hack_range( 1, x_dimension-dim.x-2 ), hack_range( 1, y_dimension-dim.y-2 ) ); Vector2d max_pt = min_pt + dim; Room r( min_pt, max_pt ); if( i != 0 ) { for( j = 0; j < room_vector.size(); j++ ) { if( r.overlaps( room_vector[j] ) ) break; } if( j != room_vector.size() ) continue; } if( !room_vector.size() ) res.player_location = r.min_pt + Vector2d( 2, 2 ); room_vector.push_back( r ); m.generate_room( r ); break; } } return m; }
void FacadeGeography::CreateGridFromPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr facade_cloud, double resolution) { pcl::PointXYZ min_extent, max_extent; pcl::getMinMax3D(*facade_cloud, min_extent, max_extent); resolution_ = resolution; extents_min_.x = min_extent.x; extents_min_.y = min_extent.y; extents_range_.x = max_extent.x - min_extent.x; extents_range_.y = max_extent.y - min_extent.y; width_ = (int)(extents_range_.x / resolution_ + 0.5); height_ = (int)(extents_range_.y / resolution_ + 0.5); convert_ratio_ = cv::Point2d((width_-1.0) / extents_range_.x, (height_-1.0) / extents_range_.y); depth_ = facade_grammar_->get_terminal_symbol_size(); grd_.resize(depth_); for ( int i = 0; i < depth_; ++i) { grd_[i] = new double*[height_]; for ( int j = 0; j < height_; ++j) { grd_[i][j] = new double[width_]; for (int k = 0; k < width_; ++k) { grd_[i][j][k] = 0.0; } } } boost::progress_display display(width_ * height_); Eigen::Vector4f max_pt, min_pt; for (int i = 0; i < width_; ++i) { for ( int j = 0; j < height_; ++j) { min_pt(0) = i * resolution_ + extents_min_.x; min_pt(1) = j * resolution_ + extents_min_.y; min_pt(2) = min_extent.z; min_pt(3) = 1.0; max_pt(0) = min_pt(0) + resolution_; max_pt(1) = min_pt(1) + resolution_; max_pt(2) = max_extent.z; max_pt(3) = 1.0; std::vector<int> indices; pcl::getPointsInBox(*facade_cloud, min_pt, max_pt, indices); assert(indices.size() >= 0); #if 0 if (indices.size() <= 1) // window { SetProperty(i, j, 1, 1); } else // wall { double height = 0; for (int k = 0; k < indices.size(); ++k) { height += facade_cloud->points[k].z; } height /= indices.size(); if (height < -0.5) { SetProperty(i, j, 1, 1); // window } else if (height > 0.5) { SetProperty(i, j, 2, 1); // over wall } else { SetProperty(i, j, 0, 1); // wall } } #endif indices.clear(); ++display; } } }
void FacadeGeography::ComputeFeature(pcl::PointCloud<pcl::PointXYZ>::Ptr facade_cloud) { // First down sample the dense cloud std::cout << "down sampling ... \n"; pcl::VoxelGrid<pcl::PointXYZ> filter; filter.setInputCloud(facade_cloud); filter.setLeafSize(0.1f, 0.1f, 0.1f); pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>); filter.filter(*filtered_cloud); std::cout << "down sampling done. After filter, " << filtered_cloud->width * filtered_cloud->height << " points left.\n"; // pcl::search::KdTree<pcl::PointXYZ> search; pcl::PointXYZ cloud_min, cloud_max; pcl::getMinMax3D(*filtered_cloud, cloud_min, cloud_max); boost::progress_display display(height_ + width_); Eigen::Vector4f min_pt, max_pt; for (int i = 0; i < width_; ++i) // for each width, compute the average z value { min_pt(0) = extents_min_.x + i * resolution_; min_pt(1) = extents_min_.y; min_pt(2) = cloud_min.x; min_pt(3) = 1.0; max_pt(0) = min_pt(0) + resolution_; max_pt(1) = min_pt(1) + resolution_ * height_; max_pt(2) = cloud_max.x; max_pt(3) = 1.0; std::vector<int> indices; pcl::getPointsInBox(*filtered_cloud, min_pt, max_pt, indices); assert(indices.size() >= 0); double sum = 0.0; for (int k = 0; k < indices.size(); ++k) { sum += filtered_cloud->points[indices[k]].z; } sum /= indices.size(); horizonal_feature.push_back(sum); ++display; } for (int i = 0; i < height_; ++i) // for each height, compute the average z value { min_pt(0) = extents_min_.x; min_pt(1) = extents_min_.y + i * resolution_; min_pt(2) = cloud_min.x; min_pt(3) = 1.0; max_pt(0) = min_pt(0) + resolution_ * width_; max_pt(1) = min_pt(1) + resolution_; max_pt(2) = cloud_max.x; max_pt(3) = 1.0; std::vector<int> indices; pcl::getPointsInBox(*filtered_cloud, min_pt, max_pt, indices); assert(indices.size() >= 0); double sum = 0.0; for (int k = 0; k < indices.size(); ++k) { sum += filtered_cloud->points[indices[k]].z; } sum /= indices.size(); vertical_feature.push_back(sum); ++display; } }
bool GeospatialBoundingBox::liesInside(Vector3f const &point) { return ( (point(0) + EPSILON >= min_pt(0)) && (point(0) <= max_pt(0) + EPSILON) && (point(1) + EPSILON >= min_pt(1)) && (point(1) <= max_pt(1) + EPSILON) && (point(2) + EPSILON >= min_pt(2)) && (point(2) <= max_pt(2) + EPSILON) ); }
int main(int argc, const char *argv[]) { // parse arguments /////////////////////////////////////////// // Declare the supported options. po::options_description desc("Visualize data"); desc.add_options() ("help", "produce help message") ("width", po::value<double>()->required(), "Width ") ("height", po::value<double>()->required(), "Height ") ("dir", po::value<std::string>()->default_value("."), "Data directory") ; po::positional_options_description pos; pos.add("width", 1); pos.add("height", 1); pos.add("dir", 1); po::variables_map vm; po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm); po::notify(vm); double width = vm["width"].as<double>(); double height = vm["height"].as<double>(); std::string datadirectory = vm["dir"].as<std::string>(); // end of parse arguments //////////////////////////////////// cv::Mat laser_pose, laser_ranges, scan_angles; loadMat(laser_pose, datadirectory + "/laser_pose_all.bin"); loadMat(laser_ranges, datadirectory + "/laser_range_all.bin"); loadMat(scan_angles, datadirectory + "/scan_angles_all.bin"); cv::Mat laser_reflectance(laser_ranges.rows, laser_ranges.cols, CV_8U); std::string floorplanfile = datadirectory + "/floorplan.png"; cv::Mat floorplan = cv::imread(floorplanfile, cv::IMREAD_GRAYSCALE); if(! floorplan.data ) // Check for invalid input { std::cout << "Could not open or find the image" << std::endl ; return -1; } cv::transpose(floorplan, floorplan); cv::flip(floorplan, floorplan, 1); cv::Vec2d size_bitmap(width, height); cv::Vec2d margin(1, 1); cv::Vec2d min_pt(- margin(0) - size_bitmap(0)/2, - margin(1) - size_bitmap(1)/2); double max_range = 8; cv::Vec2i gridsize(floorplan.size[0], floorplan.size[1]); cv::Vec2d cellsize; cv::divide(size_bitmap , gridsize, cellsize); //std::cout << cellsize(0) << cellsize(1) << std::endl; cv::Vec2i ncells; cv::divide(min_pt, cellsize, ncells, -2); OccupancyGrid2D<double, int> map( min_pt(0), min_pt(1), cellsize(0), cellsize(1), ncells(0), ncells(1)); // initialize map with occupancy with floorplan for (int r = 0; r < ncells(0); ++r) { for (int c = 0; c < ncells(1); ++c) { int fp_r = r - margin(0) / cellsize(0); int fp_c = c - margin(1) / cellsize(1); if ((0 <= fp_r) && (fp_r < floorplan.rows) && (0 <= fp_c) && (fp_c < floorplan.cols)) { map.og_.at<uint8_t>(r, c) = (floorplan.at<uint8_t>(fp_r, fp_c) > 127) ? map.FREE : map.OCCUPIED; } else { map.og_.at<uint8_t>(r, c) = map.OCCUPIED; } } } boost::mt19937 gen; boost::normal_distribution<> norm_dist(1, NOISE_VARIANCE); boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > norm_rand(gen, norm_dist); for (int r = 0; r < scan_angles.rows; r++) { double* pose = laser_pose.ptr<double>(r); double* angles = scan_angles.ptr<double>(r); double robot_angle = pose[2]; for (int c = 0; c < scan_angles.cols; c++) { double total_angle = robot_angle + angles[c]; cv::Vec2d final_pos; bool refl; double range = map.ray_trace(pose[0], pose[1], total_angle, max_range, final_pos, refl); range *= norm_rand(); laser_ranges.at<double>(r, c) = range; laser_reflectance.at<uint8_t>(r, c) = (uint8_t) refl; } // draw input cv::Mat visin; cv::cvtColor(map.og_, visin, cv::COLOR_GRAY2BGR); cv::Vec2d position(pose[0], pose[1]); map.draw_lasers(visin, position, robot_angle, angles, laser_ranges.ptr<double>(r), laser_reflectance.ptr<uint8_t>(r), scan_angles.cols, CV_RGB(0, 255, 0)); cv::imshow("c", visin); cv::waitKey(33); } saveMat(laser_ranges, "laser_range_all.bin"); saveMat(laser_reflectance, "laser_reflectance_all.bin"); }