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);
}
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) );
}
