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