int
main(int argc, char** argv)
{
// initialize PointClouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
// populate our PointCloud with points
cloud->width = 500;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if (i % 5 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
else if(i % 2 == 0)
cloud->points[i].z = sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
else
cloud->points[i].z = - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
}
else
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if( i % 2 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
else
cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
}
}
std::vector<int> inliers;
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
if(pcl::console::find_argument (argc, argv, "-f") >= 0)
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
// creates the visualization object and adds either our orignial cloud or all of the inliers
// depending on the command line arguments specified.
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
viewer = simpleVis(final);
else
// Methods bound to input/inlets
t_jit_err jit_pcl_segeuclidean_matrix_calc(t_jit_pcl_segeuclidean *x, void *inputs, void *outputs)
{
t_jit_err err = JIT_ERR_NONE;
long in_savelock;
long out_savelock;
t_jit_matrix_info in_minfo;
t_jit_matrix_info out_minfo;
char *in_bp;
char *out_bp;
long i, j;
long dimcount;
long planecount;
long dim[JIT_MATRIX_MAX_DIMCOUNT];
void *in_matrix;
void *out_matrix;
long rowstride;
float *fip, *fop;
in_matrix = jit_object_method(inputs,_jit_sym_getindex,0);
out_matrix = jit_object_method(outputs,_jit_sym_getindex,0);
if (x && in_matrix && out_matrix)
{
in_savelock = (long) jit_object_method(in_matrix, _jit_sym_lock, 1);
out_savelock = (long) jit_object_method(out_matrix, _jit_sym_lock, 1);
jit_object_method(in_matrix, _jit_sym_getinfo, &in_minfo);
jit_object_method(in_matrix, _jit_sym_getdata, &in_bp);
if (!in_bp) {
err=JIT_ERR_INVALID_INPUT;
goto out;
}
//get dimensions/planecount
dimcount = in_minfo.dimcount;
planecount = in_minfo.planecount;
if( planecount < 3 )
{
object_error((t_object *)x, "jit.pcl requires a 3 plane matrix (xyz)");
goto out;
}
if( in_minfo.type != _jit_sym_float32)
{
object_error((t_object *)x, "received: %s jit.pcl uses only float32 matrixes", in_minfo.type->s_name );
goto out;
}
//if dimsize is 1, treat as infinite domain across that dimension.
//otherwise truncate if less than the output dimsize
for (i=0; i<dimcount; i++) {
dim[i] = in_minfo.dim[i];
if ( in_minfo.dim[i]<dim[i] && in_minfo.dim[i]>1) {
dim[i] = in_minfo.dim[i];
}
}
//******
// PCL stuff
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud->width = (uint32_t)dim[0];
cloud->height = (uint32_t)dim[1];
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
rowstride = in_minfo.dimstride[1];// >> 2L;
size_t count = 0;
for (j = 0; j < dim[0]; j++)
{
fip = (float *)(in_bp + j * in_minfo.dimstride[0]);
for( i = 0; i < dim[1]; i++)
{
cloud->points[count].x = ((float *)fip)[0];
cloud->points[count].y = ((float *)fip)[1];
cloud->points[count].z = ((float *)fip)[2];
// cloud->points[count].r = (uint8_t)(((float *)fip)[3] * 255.0);
// cloud->points[count].g = (uint8_t)(((float *)fip)[4] * 255.0);
// cloud->points[count].b = (uint8_t)(((float *)fip)[5] * 255.0);
count++;
fip += rowstride;
}
}
{
/*
//filter
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_voxel_ (new pcl::PointCloud<pcl::PointXYZRGB>);
grid.setLeafSize (x->leafsize, x->leafsize, x->leafsize);
grid.setInputCloud (cloud);
grid.filter (*cloud_voxel_);
pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_sor_ (new pcl::PointCloud<pcl::PointXYZRGB>);
sor.setInputCloud(cloud_voxel_);
sor.setMeanK( (uint32_t)x->npoints );
sor.setStddevMulThresh(x->stdthresh);
sor.filter(*cloud_sor_);
*/
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree(new pcl::search::KdTree<pcl::PointXYZ>);
kdtree->setInputCloud(cloud);
// Euclidean clustering object.
pcl::EuclideanClusterExtraction<pcl::PointXYZ> clustering;
// Set cluster tolerance to 2cm (small values may cause objects to be divided
// in several clusters, whereas big values may join objects in a same cluster).
clustering.setClusterTolerance(x->cluster_tol);
// Set the minimum and maximum number of points that a cluster can have.
clustering.setMinClusterSize(10);
clustering.setMaxClusterSize(25000);
clustering.setSearchMethod(kdtree);
clustering.setInputCloud(cloud);
std::vector<pcl::PointIndices> clusters;
clustering.extract(clusters);
// For every cluster...
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZRGB>);
cluster->points.resize(cloud->size());
cluster->width = (uint32_t)cloud->points.size();
cluster->height = 1;
cluster->is_dense = true;
if( clusters.size() > 0 )
{
double segment_inc = 255. / clusters.size();
int count = 0;
int clusterN = 0;
for (std::vector<pcl::PointIndices>::const_iterator i = clusters.begin(); i != clusters.end(); ++i)
{
for (std::vector<int>::const_iterator p = i->indices.begin(); p != i->indices.end(); ++p)
{
cluster->points[count].x = cloud->points[ *p ].x;
cluster->points[count].y = cloud->points[ *p ].y;
cluster->points[count].z = cloud->points[ *p ].z;
cluster->points[count].r = (uint8_t)(segment_inc * clusterN);
cluster->points[count].g = (uint8_t)(segment_inc * clusterN);
cluster->points[count].b = 255;
count++;
}
clusterN++;
}
}
err = jit_xyzrgb2jit(x, cluster, &out_minfo, &out_matrix );
if( err != JIT_ERR_NONE )
goto out;
}
// unable to make use of jitter's parallel methods since we need all the data together
//jit_parallel_ndim_simplecalc2((method)jit_pcl_segeuclidean_calculate_ndim,
// x, dimcount, dim, planecount, &in_minfo, in_bp, &out_minfo, out_bp,
// 0 /* flags1 */, 0 /* flags2 */);
}
else
return JIT_ERR_INVALID_PTR;
out:
jit_object_method( out_matrix, _jit_sym_lock, out_savelock );
jit_object_method( in_matrix, _jit_sym_lock, in_savelock );
return err;
}
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PCDWriter writer;
pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>);
std::vector<int> filenames;
filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
reader.read (argv[filenames[0]], *cloud);
std::string pcd_filename;
std::string png_filename = argv[filenames[0]];
// Take the origional png image out
png::image<png::rgb_pixel> origin_image(cloud->width, cloud->height);
int origin_index = 0;
for (size_t y = 0; y < origin_image.get_height (); ++y) {
for (size_t x = 0; x < origin_image.get_width (); ++x) {
const PointType & p = cloud->points[origin_index++];
origin_image[y][x] = png::rgb_pixel(p.r, p.g, p.b);
}
}
png_filename.replace(png_filename.length () - 4, 4, ".png");
origin_image.write(png_filename);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
float min_depth = 0.1;
pcl::console::parse_argument (argc, argv, "-min_depth", min_depth);
float max_depth = 3.0;
pcl::console::parse_argument (argc, argv, "-max_depth", max_depth);
float max_x = 1.0;
pcl::console::parse_argument (argc, argv, "-max_x", max_x);
float min_x = -1.0;
pcl::console::parse_argument (argc, argv, "-min_x", min_x);
float max_y = 1.0;
pcl::console::parse_argument (argc, argv, "-max_y", max_y);
float min_y = -1.0;
pcl::console::parse_argument (argc, argv, "-min_y", min_y);
int plane_seg_times = 1;
pcl::console::parse_argument (argc, argv, "-plane_seg_times", plane_seg_times);
for (int i = 0; i < plane_seg_times; i++) {
remove_plane(cloud, min_depth, max_depth, min_x, max_x, min_y, max_y);
}
pcd_filename = argv[filenames[0]];
pcd_filename.replace(pcd_filename.length () - 4, 8, "plane.pcd");
pcl::io::savePCDFile(pcd_filename, *cloud);
// std::cout << "PointCloud after removing the plane has: " << cloud->points.size () << " data points." << std::endl; //*
uint32_t xmin = 1000, xmax = 0, ymin = 1000, ymax = 0;
pcl::PointCloud<PointXYZRGBUV>::Ptr cloud_uv (new pcl::PointCloud<PointXYZRGBUV>);
for (size_t index = 0; index < cloud->points.size(); index++) {
const pcl::PointXYZRGB & p = cloud->points[index];
if (p.x != p.x || p.y != p.y || p.z != p.z) { // if current point is invalid
continue;
}
PointXYZRGBUV cp = PointXYZRGBUV(p, index % cloud-> width, index / cloud->width);
cloud_uv->points.push_back (cp);
}
cloud_uv->width = cloud_uv->points.size ();
cloud_uv->height = 1;
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<PointXYZRGBUV>::Ptr tree (new pcl::search::KdTree<PointXYZRGBUV>);
tree->setInputCloud (cloud_uv);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<PointXYZRGBUV> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (500);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_uv);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
xmin = 1000;
xmax = 0;
ymin = 1000;
ymax = 0;
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit) {
PointXYZRGBUV& p = cloud_uv->points[*pit];
pcl::PointXYZRGB cp_rgb;
cp_rgb.x = p.x; cp_rgb.y = p.y; cp_rgb.z = p.z;
cp_rgb.rgb = p.rgb;
cloud_cluster->points.push_back(cp_rgb);
xmin = std::min(xmin, p.u);
xmax = std::max(xmax, p.u);
ymin = std::min(ymin, p.v);
ymax = std::max(ymax, p.v);
}
cloud_cluster->is_dense = true;
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
pcd_filename = argv[filenames[0]];
std::stringstream ss;
ss << "cluster_" << j++ << ".pcd";
pcd_filename.replace(pcd_filename.length () - 4, ss.str().length(), ss.str());
pcl::io::savePCDFile(pcd_filename, *cloud_cluster);
png::image<png::rgb_pixel> image(cloud_cluster->width, cloud_cluster->height);
int i = 0;
for (size_t y = 0; y < image.get_height (); ++y) {
for (size_t x = 0; x < image.get_width (); ++x) {
const PointType & p = cloud_cluster->points[i++];
image[y][x] = png::rgb_pixel(p.r, p.g, p.b);
}
}
pcd_filename.replace(pcd_filename.length () - 4, 4, ".png");
image.write(pcd_filename);
//crop out image patch
png::image<png::rgb_pixel> image_patch(xmax - xmin + 1, ymax - ymin + 1);
for (size_t y = 0; y < image_patch.get_height (); ++y) {
for (size_t x = 0; x < image_patch.get_width (); ++x) {
image_patch[y][x] = origin_image[y+ymin][x+xmin];
}
}
pcd_filename.replace(pcd_filename.length () - 4, 7, "box.png");
image_patch.write(pcd_filename);
// writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_cluster, false); //*
}
return (0);
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
std::string base_frame("base_link");
geometry_msgs::PointStamped pout;
geometry_msgs::PointStamped pin;
pin.header.frame_id = msg->header.frame_id;
pin.point.x = 0; pin.point.y = 0; pin.point.z = 0;
geometry_msgs::Vector3Stamped vout;
geometry_msgs::Vector3Stamped vin;
vin.header.frame_id = base_frame;
vin.vector.x = 0; vin.vector.y = 0; vin.vector.z = 1;
float height;
Eigen::Vector3f normal;
try {
listener->transformPoint(base_frame, ros::Time(0), pin, msg->header.frame_id, pout);
height = pout.point.z;
listener->transformVector(msg->header.frame_id, ros::Time(0), vin, base_frame, vout);
normal = Eigen::Vector3f(vout.vector.x, vout.vector.y, vout.vector.z);
}
catch (tf::TransformException ex) {
ROS_INFO("%s",ex.what());
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(*msg, *cloud);
Eigen::Vector3f p = -height*normal; // a point in the floor plane
float d = -p.dot(normal); // = height, d in the plane equation
obstacle_cloud->points.clear();
obstacle_cloud->points.reserve(cloud->size());
floor_cloud->points.clear();
Eigen::Vector3f temp;
float floor_dist;
pcl::PointXYZ point;
for (size_t i = 0; i < cloud->size(); ++i) {
/*temp = cloud->points[i].getVector3fMap(); // DEBUG!
if (i%640 < 300 && i%640 > 200 && i < 640*200 && i > 640*100) {
temp -= 0.06*normal;
}*/
// check signed distance to floor
floor_dist = normal.dot(cloud->points[i].getVector3fMap()) + d;
//floor_dist = normal.dot(temp) + d; // DEBUG
// if enough below, consider a stair point
if (floor_dist < below_threshold) {
temp = cloud->points[i].getVector3fMap(); // RELEASE
point.getVector3fMap() = -(d/normal.dot(temp))*temp + normal*0.11;
floor_cloud->points.push_back(point);
}
else { // add as a normal obstacle or clearing point
obstacle_cloud->points.push_back(cloud->points[i]);
}
}
sensor_msgs::PointCloud2 floor_msg;
pcl::toROSMsg(*floor_cloud, floor_msg);
floor_msg.header = msg->header;
floor_pub.publish(floor_msg);
sensor_msgs::PointCloud2 obstacle_msg;
pcl::toROSMsg(*obstacle_cloud, obstacle_msg);
obstacle_msg.header = msg->header;
obstacle_pub.publish(obstacle_msg);
}
void ColorizeRandomForest::extract(const sensor_msgs::PointCloud2 pc)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());
std::vector<int> indices;
pcl::fromROSMsg(pc, *cloud);
cloud->is_dense = false;
pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
tree->setInputCloud (cloud);
pcl::PassThrough<pcl::PointXYZRGB> pass;
pass.setInputCloud (cloud);
pass.setFilterFieldName (std::string("z"));
pass.setFilterLimits (0.0, 1.5);
pass.filter (*cloud);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
ec.setClusterTolerance (0.025);
ec.setMinClusterSize (200);
ec.setMaxClusterSize (100000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud);
ec.extract (cluster_indices);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr noncloth_cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
int cluster_num = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
JSK_ROS_INFO("Calculate time %d / %ld", cluster_num , cluster_indices.size());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud->points[*pit]);
cloud_cluster->is_dense = true;
cluster_num ++ ;
pcl::NormalEstimation<pcl::PointXYZRGB, pcl::PointXYZRGBNormal> ne;
pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
ne.setInputCloud (cloud_cluster);
ne.setSearchMethod (tree);
ne.setRadiusSearch (0.02);
ne.compute (*cloud_normals);
for (int cloud_index = 0; cloud_index < cloud_normals->points.size(); cloud_index++) {
cloud_normals->points[cloud_index].x = cloud_cluster->points[cloud_index].x;
cloud_normals->points[cloud_index].y = cloud_cluster->points[cloud_index].y;
cloud_normals->points[cloud_index].z = cloud_cluster->points[cloud_index].z;
}
int result_counter=0, call_counter = 0;
pcl::PointXYZRGBNormal max_pt,min_pt;
pcl::getMinMax3D(*cloud_normals, min_pt, max_pt);
for (int i = 0 ; i < 30; i++) {
double lucky = 0, lucky2 = 0;
std::string axis("x"), other_axis("y");
int rand_xy = rand()%2;
if (rand_xy == 0) {
lucky = min_pt.x - pass_offset_ + (max_pt.x - min_pt.x - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
lucky2 = min_pt.y + (max_pt.y - min_pt.y) * 1.0 * rand() / RAND_MAX;
} else {
lucky = min_pt.y - pass_offset_ + (max_pt.y - min_pt.y - pass_offset_*2) * 1.0 * rand() / RAND_MAX;
lucky2 = min_pt.x + (max_pt.x - min_pt.x) * 1.0 * rand() / RAND_MAX;
axis = std::string("y");
other_axis = std::string("x");
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_normals_pass (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
pcl::IndicesPtr indices_x(new std::vector<int>());
pass.setInputCloud (cloud_normals);
pass.setFilterFieldName (axis);
float small = std::min(lucky, lucky + pass_offset_);
float large = std::max(lucky, lucky + pass_offset_);
pass.setFilterLimits (small, large);
pass.filter (*cloud_normals_pass);
pass.setInputCloud (cloud_normals_pass);
pass.setFilterFieldName (other_axis);
float small2 = std::min(lucky2, lucky2 + pass_offset2_);
float large2 = std::max(lucky2, lucky2 + pass_offset2_);
pass.setFilterLimits (small2, large2);
pass.filter (*cloud_normals_pass);
std::vector<int> tmp_indices;
pcl::removeNaNFromPointCloud(*cloud_normals_pass, *cloud_normals_pass, tmp_indices);
if(cloud_normals_pass->points.size() == 0)
continue;
pcl::FPFHEstimationOMP<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::FPFHSignature33> fpfh;
pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33> ());
fpfh.setNumberOfThreads(8);
fpfh.setInputCloud (cloud_normals_pass);
fpfh.setInputNormals (cloud_normals_pass);
fpfh.setSearchMethod (tree);
fpfh.setRadiusSearch (radius_search_);
fpfh.compute (*fpfhs);
if((int)fpfhs->points.size() == 0)
continue;
std::vector<double> result;
int target_id, max_value;
if ((int)fpfhs->points.size() - sum_num_ - 1 < 1) {
target_id = 0;
max_value = (int)fpfhs->points.size();
} else {
target_id = rand() % ((int)fpfhs->points.size() - sum_num_ - 1);
max_value = target_id + sum_num_;
}
bool has_nan = false;
for(int index = 0; index < 33; index++) {
float sum_hist_points = 0;
for(int kndex = target_id; kndex < max_value; kndex++)
{
sum_hist_points+=fpfhs->points[kndex].histogram[index];
}
result.push_back( sum_hist_points/sum_num_ );
}
for(int x = 0; x < result.size(); x ++) {
if(pcl_isnan(result[x]))
has_nan = true;
}
if(has_nan)
break;
call_counter++;
ros::ServiceClient client = pnh_->serviceClient<ml_classifiers::ClassifyData>("/predict");
ml_classifiers::ClassifyData srv;
ml_classifiers::ClassDataPoint class_data_point;
class_data_point.point = result;
srv.request.data.push_back(class_data_point);
if(client.call(srv))
if (atoi(srv.response.classifications[0].c_str()) == 0)
result_counter += 1;
JSK_ROS_INFO("response result : %s", srv.response.classifications[0].c_str());
}
if(result_counter >= call_counter / 2) {
JSK_ROS_INFO("Cloth %d / %d", result_counter, call_counter);
}
else {
JSK_ROS_INFO("Not Cloth %d / %d", result_counter, call_counter);
}
for (int i = 0; i < cloud_cluster->points.size(); i++) {
if(result_counter >= call_counter / 2) {
cloth_cloud_cluster->points.push_back (cloud_cluster->points[i]);
}
else {
noncloth_cloud_cluster->points.push_back (cloud_cluster->points[i]);
}
}
}
sensor_msgs::PointCloud2 cloth_pointcloud2;
pcl::toROSMsg(*cloth_cloud_cluster, cloth_pointcloud2);
cloth_pointcloud2.header = pc.header;
cloth_pointcloud2.is_dense = false;
pub_.publish(cloth_pointcloud2);
sensor_msgs::PointCloud2 noncloth_pointcloud2;
pcl::toROSMsg(*noncloth_cloud_cluster, noncloth_pointcloud2);
noncloth_pointcloud2.header = pc.header;
noncloth_pointcloud2.is_dense = false;
pub2_.publish(noncloth_pointcloud2);
}
void
pcl::apps::RenderViewsTesselatedSphere::generateViews() {
//center object
double CoM[3];
vtkIdType npts_com = 0, *ptIds_com = nullptr;
vtkSmartPointer<vtkCellArray> cells_com = polydata_->GetPolys ();
double center[3], p1_com[3], p2_com[3], p3_com[3], totalArea_com = 0;
double comx = 0, comy = 0, comz = 0;
for (cells_com->InitTraversal (); cells_com->GetNextCell (npts_com, ptIds_com);)
{
polydata_->GetPoint (ptIds_com[0], p1_com);
polydata_->GetPoint (ptIds_com[1], p2_com);
polydata_->GetPoint (ptIds_com[2], p3_com);
vtkTriangle::TriangleCenter (p1_com, p2_com, p3_com, center);
double area_com = vtkTriangle::TriangleArea (p1_com, p2_com, p3_com);
comx += center[0] * area_com;
comy += center[1] * area_com;
comz += center[2] * area_com;
totalArea_com += area_com;
}
CoM[0] = comx / totalArea_com;
CoM[1] = comy / totalArea_com;
CoM[2] = comz / totalArea_com;
vtkSmartPointer<vtkTransform> trans_center = vtkSmartPointer<vtkTransform>::New ();
trans_center->Translate (-CoM[0], -CoM[1], -CoM[2]);
vtkSmartPointer<vtkMatrix4x4> matrixCenter = trans_center->GetMatrix ();
vtkSmartPointer<vtkTransformFilter> trans_filter_center = vtkSmartPointer<vtkTransformFilter>::New ();
trans_filter_center->SetTransform (trans_center);
trans_filter_center->SetInputData (polydata_);
trans_filter_center->Update ();
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
mapper->SetInputConnection (trans_filter_center->GetOutputPort ());
mapper->Update ();
//scale so it fits in the unit sphere!
double bb[6];
mapper->GetBounds (bb);
double ms = (std::max) ((std::fabs) (bb[0] - bb[1]),
(std::max) ((std::fabs) (bb[2] - bb[3]), (std::fabs) (bb[4] - bb[5])));
double max_side = radius_sphere_ / 2.0;
double scale_factor = max_side / ms;
vtkSmartPointer<vtkTransform> trans_scale = vtkSmartPointer<vtkTransform>::New ();
trans_scale->Scale (scale_factor, scale_factor, scale_factor);
vtkSmartPointer<vtkMatrix4x4> matrixScale = trans_scale->GetMatrix ();
vtkSmartPointer<vtkTransformFilter> trans_filter_scale = vtkSmartPointer<vtkTransformFilter>::New ();
trans_filter_scale->SetTransform (trans_scale);
trans_filter_scale->SetInputConnection (trans_filter_center->GetOutputPort ());
trans_filter_scale->Update ();
mapper->SetInputConnection (trans_filter_scale->GetOutputPort ());
mapper->Update ();
//////////////////////////////
// * Compute area of the mesh
//////////////////////////////
vtkSmartPointer<vtkCellArray> cells = mapper->GetInput ()->GetPolys ();
vtkIdType npts = 0, *ptIds = nullptr;
double p1[3], p2[3], p3[3], totalArea = 0;
for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds);)
{
polydata_->GetPoint (ptIds[0], p1);
polydata_->GetPoint (ptIds[1], p2);
polydata_->GetPoint (ptIds[2], p3);
totalArea += vtkTriangle::TriangleArea (p1, p2, p3);
}
//create icosahedron
vtkSmartPointer<vtkPlatonicSolidSource> ico = vtkSmartPointer<vtkPlatonicSolidSource>::New ();
ico->SetSolidTypeToIcosahedron ();
ico->Update ();
//tessellate cells from icosahedron
vtkSmartPointer<vtkLoopSubdivisionFilter> subdivide = vtkSmartPointer<vtkLoopSubdivisionFilter>::New ();
subdivide->SetNumberOfSubdivisions (tesselation_level_);
subdivide->SetInputConnection (ico->GetOutputPort ());
subdivide->Update();
// Get camera positions
vtkPolyData *sphere = subdivide->GetOutput ();
std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > cam_positions;
if (!use_vertices_)
{
vtkSmartPointer<vtkCellArray> cells_sphere = sphere->GetPolys ();
cam_positions.resize (sphere->GetNumberOfPolys ());
size_t i=0;
for (cells_sphere->InitTraversal (); cells_sphere->GetNextCell (npts_com, ptIds_com);)
{
sphere->GetPoint (ptIds_com[0], p1_com);
sphere->GetPoint (ptIds_com[1], p2_com);
sphere->GetPoint (ptIds_com[2], p3_com);
vtkTriangle::TriangleCenter (p1_com, p2_com, p3_com, center);
cam_positions[i] = Eigen::Vector3f (float (center[0]), float (center[1]), float (center[2]));
i++;
}
}
else
{
cam_positions.resize (sphere->GetNumberOfPoints ());
for (vtkIdType i = 0; i < sphere->GetNumberOfPoints (); i++)
{
double cam_pos[3];
sphere->GetPoint (i, cam_pos);
cam_positions[i] = Eigen::Vector3f (float (cam_pos[0]), float (cam_pos[1]), float (cam_pos[2]));
}
}
/*int valid = 0;
for (size_t i = 0; i < cam_positions.size (); i++)
{
if (campos_constraints_func_ (cam_positions[i]))
{
cam_positions[valid] = cam_positions[i];
valid++;
}
}
cam_positions.resize (valid);*/
double camera_radius = radius_sphere_;
std::array<double, 3> cam_pos;
std::array<double, 3> first_cam_pos;
first_cam_pos[0] = cam_positions[0][0] * radius_sphere_;
first_cam_pos[1] = cam_positions[0][1] * radius_sphere_;
first_cam_pos[2] = cam_positions[0][2] * radius_sphere_;
//create renderer and window
vtkSmartPointer<vtkRenderWindow> render_win = vtkSmartPointer<vtkRenderWindow>::New ();
vtkSmartPointer<vtkRenderer> renderer = vtkSmartPointer<vtkRenderer>::New ();
render_win->AddRenderer (renderer);
render_win->SetSize (resolution_, resolution_);
renderer->SetBackground (1.0, 0, 0);
//create picker
vtkSmartPointer<vtkWorldPointPicker> worldPicker = vtkSmartPointer<vtkWorldPointPicker>::New ();
vtkSmartPointer<vtkCamera> cam = vtkSmartPointer<vtkCamera>::New ();
cam->SetFocalPoint (0, 0, 0);
Eigen::Vector3f cam_pos_3f = cam_positions[0];
Eigen::Vector3f perp = cam_pos_3f.cross (Eigen::Vector3f::UnitY ());
cam->SetViewUp (perp[0], perp[1], perp[2]);
cam->SetPosition (first_cam_pos.data());
cam->SetViewAngle (view_angle_);
cam->Modified ();
//For each camera position, traposesnsform the object and render view
for (const auto &cam_position : cam_positions)
{
cam_pos[0] = cam_position[0];
cam_pos[1] = cam_position[1];
cam_pos[2] = cam_position[2];
//create temporal virtual camera
vtkSmartPointer<vtkCamera> cam_tmp = vtkSmartPointer<vtkCamera>::New ();
cam_tmp->SetViewAngle (view_angle_);
Eigen::Vector3f cam_pos_3f (static_cast<float> (cam_pos[0]), static_cast<float> (cam_pos[1]), static_cast<float> (cam_pos[2]));
cam_pos_3f = cam_pos_3f.normalized ();
Eigen::Vector3f test = Eigen::Vector3f::UnitY ();
//If the view up is parallel to ray cam_pos - focalPoint then the transformation
//is singular and no points are rendered...
//make sure it is perpendicular
if (fabs (cam_pos_3f.dot (test)) == 1)
{
//parallel, create
test = cam_pos_3f.cross (Eigen::Vector3f::UnitX ());
}
cam_tmp->SetViewUp (test[0], test[1], test[2]);
for (double &cam_po : cam_pos)
{
cam_po *= camera_radius;
}
cam_tmp->SetPosition (cam_pos.data());
cam_tmp->SetFocalPoint (0, 0, 0);
cam_tmp->Modified ();
//rotate model so it looks the same as if we would look from the new position
vtkSmartPointer<vtkMatrix4x4> view_trans_inverted = vtkSmartPointer<vtkMatrix4x4>::New ();
vtkMatrix4x4::Invert (cam->GetViewTransformMatrix (), view_trans_inverted);
vtkSmartPointer<vtkTransform> trans_rot_pose = vtkSmartPointer<vtkTransform>::New ();
trans_rot_pose->Identity ();
trans_rot_pose->Concatenate (view_trans_inverted);
trans_rot_pose->Concatenate (cam_tmp->GetViewTransformMatrix ());
vtkSmartPointer<vtkTransformFilter> trans_rot_pose_filter = vtkSmartPointer<vtkTransformFilter>::New ();
trans_rot_pose_filter->SetTransform (trans_rot_pose);
trans_rot_pose_filter->SetInputConnection (trans_filter_scale->GetOutputPort ());
//translate model so we can place camera at (0,0,0)
vtkSmartPointer<vtkTransform> translation = vtkSmartPointer<vtkTransform>::New ();
translation->Translate (first_cam_pos[0] * -1, first_cam_pos[1] * -1, first_cam_pos[2] * -1);
vtkSmartPointer<vtkTransformFilter> translation_filter = vtkSmartPointer<vtkTransformFilter>::New ();
translation_filter->SetTransform (translation);
translation_filter->SetInputConnection (trans_rot_pose_filter->GetOutputPort ());
//modify camera
cam_tmp->SetPosition (0, 0, 0);
cam_tmp->SetFocalPoint (first_cam_pos[0] * -1, first_cam_pos[1] * -1, first_cam_pos[2] * -1);
cam_tmp->Modified ();
//notice transformations for final pose
vtkSmartPointer<vtkMatrix4x4> matrixRotModel = trans_rot_pose->GetMatrix ();
vtkSmartPointer<vtkMatrix4x4> matrixTranslation = translation->GetMatrix ();
mapper->SetInputConnection (translation_filter->GetOutputPort ());
mapper->Update ();
//render view
vtkSmartPointer<vtkActor> actor_view = vtkSmartPointer<vtkActor>::New ();
actor_view->SetMapper (mapper);
renderer->SetActiveCamera (cam_tmp);
renderer->AddActor (actor_view);
renderer->Modified ();
//renderer->ResetCameraClippingRange ();
render_win->Render ();
//back to real scale transform
vtkSmartPointer<vtkTransform> backToRealScale = vtkSmartPointer<vtkTransform>::New ();
backToRealScale->PostMultiply ();
backToRealScale->Identity ();
backToRealScale->Concatenate (matrixScale);
backToRealScale->Concatenate (matrixTranslation);
backToRealScale->Inverse ();
backToRealScale->Modified ();
backToRealScale->Concatenate (matrixTranslation);
backToRealScale->Modified ();
Eigen::Matrix4f backToRealScale_eigen;
backToRealScale_eigen.setIdentity ();
for (int x = 0; x < 4; x++)
for (int y = 0; y < 4; y++)
backToRealScale_eigen (x, y) = float (backToRealScale->GetMatrix ()->GetElement (x, y));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
cloud->points.resize (resolution_ * resolution_);
if (gen_organized_)
{
cloud->width = resolution_;
cloud->height = resolution_;
cloud->is_dense = false;
double coords[3];
float * depth = new float[resolution_ * resolution_];
render_win->GetZbufferData (0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));
for (int x = 0; x < resolution_; x++)
{
for (int y = 0; y < resolution_; y++)
{
float value = depth[y * resolution_ + x];
if (value == 1.0)
{
cloud->at (y, x).x = cloud->at (y, x).y = cloud->at (y, x).z = std::numeric_limits<float>::quiet_NaN ();
}
else
{
worldPicker->Pick (x, y, value, renderer);
worldPicker->GetPickPosition (coords);
cloud->at (y, x).x = static_cast<float> (coords[0]);
cloud->at (y, x).y = static_cast<float> (coords[1]);
cloud->at (y, x).z = static_cast<float> (coords[2]);
cloud->at (y, x).getVector4fMap () = backToRealScale_eigen
* cloud->at (y, x).getVector4fMap ();
}
}
}
delete[] depth;
}
else
{
cloud->width = resolution_ * resolution_;
cloud->height = 1;
double coords[3];
float * depth = new float[resolution_ * resolution_];
render_win->GetZbufferData (0, 0, resolution_ - 1, resolution_ - 1, &(depth[0]));
int count_valid_depth_pixels = 0;
for (int x = 0; x < resolution_; x++)
{
for (int y = 0; y < resolution_; y++)
{
float value = depth[y * resolution_ + x];
if (value == 1.0)
continue;
worldPicker->Pick (x, y, value, renderer);
worldPicker->GetPickPosition (coords);
cloud->points[count_valid_depth_pixels].x = static_cast<float> (coords[0]);
cloud->points[count_valid_depth_pixels].y = static_cast<float> (coords[1]);
cloud->points[count_valid_depth_pixels].z = static_cast<float> (coords[2]);
cloud->points[count_valid_depth_pixels].getVector4fMap () = backToRealScale_eigen
* cloud->points[count_valid_depth_pixels].getVector4fMap ();
count_valid_depth_pixels++;
}
}
delete[] depth;
cloud->points.resize (count_valid_depth_pixels);
cloud->width = count_valid_depth_pixels;
}
if(compute_entropy_) {
//////////////////////////////
// * Compute area of the mesh
//////////////////////////////
vtkSmartPointer<vtkPolyData> polydata = mapper->GetInput ();
polydata->BuildCells ();
vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
vtkIdType npts = 0, *ptIds = nullptr;
double p1[3], p2[3], p3[3], area, totalArea = 0;
for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds);)
{
polydata->GetPoint (ptIds[0], p1);
polydata->GetPoint (ptIds[1], p2);
polydata->GetPoint (ptIds[2], p3);
area = vtkTriangle::TriangleArea (p1, p2, p3);
totalArea += area;
}
/////////////////////////////////////
// * Select visible cells (triangles)
/////////////////////////////////////
vtkSmartPointer<vtkHardwareSelector> hardware_selector = vtkSmartPointer<vtkHardwareSelector>::New ();
hardware_selector->ClearBuffers();
vtkSmartPointer<vtkSelection> hdw_selection = vtkSmartPointer<vtkSelection>::New ();
hardware_selector->SetRenderer (renderer);
hardware_selector->SetArea (0, 0, resolution_ - 1, resolution_ - 1);
hardware_selector->SetFieldAssociation(vtkDataObject::FIELD_ASSOCIATION_CELLS);
hdw_selection = hardware_selector->Select ();
vtkSmartPointer<vtkIdTypeArray> ids = vtkSmartPointer<vtkIdTypeArray>::New ();
ids = vtkIdTypeArray::SafeDownCast(hdw_selection->GetNode(0)->GetSelectionList());
double visible_area = 0;
for (vtkIdType sel_id = 0; sel_id < (ids->GetNumberOfTuples ()); sel_id++)
{
int id_mesh = int (ids->GetValue (sel_id));
if(id_mesh >= polydata->GetNumberOfPolys())
continue;
vtkCell * cell = polydata->GetCell (id_mesh);
vtkTriangle* triangle = dynamic_cast<vtkTriangle*> (cell);
double p0[3];
double p1[3];
double p2[3];
triangle->GetPoints ()->GetPoint (0, p0);
triangle->GetPoints ()->GetPoint (1, p1);
triangle->GetPoints ()->GetPoint (2, p2);
area = vtkTriangle::TriangleArea (p0, p1, p2);
visible_area += area;
}
entropies_.push_back (float (visible_area / totalArea));
}
//transform cloud to give camera coordinates instead of world coordinates!
vtkSmartPointer<vtkMatrix4x4> view_transform = cam_tmp->GetViewTransformMatrix ();
Eigen::Matrix4f trans_view;
trans_view.setIdentity ();
for (int x = 0; x < 4; x++)
for (int y = 0; y < 4; y++)
trans_view (x, y) = float (view_transform->GetElement (x, y));
//NOTE: vtk view coordinate system is different than the standard camera coordinates (z forward, y down, x right)
//thus, the fliping in y and z
for (auto &point : cloud->points)
{
point.getVector4fMap () = trans_view * point.getVector4fMap ();
point.y *= -1.0f;
point.z *= -1.0f;
}
renderer->RemoveActor (actor_view);
generated_views_.push_back (cloud);
//create pose, from OBJECT coordinates to CAMERA coordinates!
vtkSmartPointer<vtkTransform> transOCtoCC = vtkSmartPointer<vtkTransform>::New ();
transOCtoCC->PostMultiply ();
transOCtoCC->Identity ();
transOCtoCC->Concatenate (matrixCenter);
transOCtoCC->Concatenate (matrixRotModel);
transOCtoCC->Concatenate (matrixTranslation);
transOCtoCC->Concatenate (cam_tmp->GetViewTransformMatrix ());
//NOTE: vtk view coordinate system is different than the standard camera coordinates (z forward, y down, x right)
//thus, the fliping in y and z
vtkSmartPointer<vtkMatrix4x4> cameraSTD = vtkSmartPointer<vtkMatrix4x4>::New ();
cameraSTD->Identity ();
cameraSTD->SetElement (0, 0, 1);
cameraSTD->SetElement (1, 1, -1);
cameraSTD->SetElement (2, 2, -1);
transOCtoCC->Concatenate (cameraSTD);
transOCtoCC->Modified ();
Eigen::Matrix4f pose_view;
pose_view.setIdentity ();
for (int x = 0; x < 4; x++)
for (int y = 0; y < 4; y++)
pose_view (x, y) = float (transOCtoCC->GetMatrix ()->GetElement (x, y));
poses_.push_back (pose_view);
}
}
typename pcl::PointCloud<PointT>::Ptr
get_point_cloud(int distance, bool colored)
{
//get rgb and depth data
while(!device -> getDepth(depth_map)){}
while(!device -> getRGB(rgb)){}
int depth_width = 640;
int depth_height = 480;
//create the empty Pointcloud
boost::shared_ptr<pcl::PointCloud<PointT>> cloud (new pcl::PointCloud<PointT>);
//initialize the PointCloud height and width
//cloud->height = std::max (image_height, depth_height);
//cloud->width = std::max (image_width, depth_width);
//allow infinite values for points coordinates
cloud->is_dense = false;
//set camera parameters for kinect
float focal_x_depth = 585.187492217609;//5.9421434211923247e+02;
float focal_y_depth = 585.308616340665;//5.9104053696870778e+02;
float center_x_depth = 322.714077555293;//3.3930780975300314e+02;
float center_y_depth = 248.626108676666;//2.4273913761751615e+02;
float bad_point = std::numeric_limits<float>::quiet_NaN ();
#pragma omp parallel for
for (unsigned int y = 0; y < depth_height; ++y)
for ( unsigned int x = 0; x < depth_width; ++x){
PointT ptout;
uint16_t dz = depth_map[y*depth_width + x];
if (abs(dz) < distance){
// project
Eigen::Vector3d ptd((x - center_x_depth) * dz / focal_x_depth, (y - center_y_depth) * dz/focal_y_depth, dz);
// assign output xyz
ptout.x = ptd.x()*0.001f;
ptout.y = ptd.y()*0.001f;
ptout.z = ptd.z()*0.001f;
if(colored){
uint8_t r = rgb[(y*depth_width + x)*3];
uint8_t g = rgb[(y*depth_width + x)*3 + 1];
uint8_t b = rgb[(y*depth_width + x)*3 + 2];
ptout.rgba = pcl::PointXYZRGB(r, g, b).rgba; //assign color
//ptout.rgba = pcl::PointXYZRGB(0, 0, 0).rgba;
} else
ptout.rgba = pcl::PointXYZRGB(0, 0, 0).rgba;
#pragma omp critical
cloud->points.push_back(ptout); //assigns point to cloud
}
}
cloud->height = 1;
cloud->width = cloud->points.size();
return (cloud);
}
void TorusFinder::segment(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
if (!done_initialization_) {
return;
}
boost::mutex::scoped_lock lock(mutex_);
vital_checker_->poke();
pcl::PointCloud<pcl::PointNormal>::Ptr cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*cloud_msg, *cloud);
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
if (voxel_grid_sampling_) {
pcl::PointCloud<pcl::PointNormal>::Ptr downsampled_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::VoxelGrid<pcl::PointNormal> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (voxel_size_, voxel_size_, voxel_size_);
sor.filter (*downsampled_cloud);
cloud = downsampled_cloud;
}
pcl::SACSegmentation<pcl::PointNormal> seg;
if (use_normal_) {
pcl::SACSegmentationFromNormals<pcl::PointNormal, pcl::PointNormal> seg_normal;
seg_normal.setInputNormals(cloud);
seg = seg_normal;
}
seg.setOptimizeCoefficients (true);
seg.setInputCloud(cloud);
seg.setRadiusLimits(min_radius_, max_radius_);
if (algorithm_ == "RANSAC") {
seg.setMethodType(pcl::SAC_RANSAC);
}
else if (algorithm_ == "LMEDS") {
seg.setMethodType(pcl::SAC_LMEDS);
}
else if (algorithm_ == "MSAC") {
seg.setMethodType(pcl::SAC_MSAC);
}
else if (algorithm_ == "RRANSAC") {
seg.setMethodType(pcl::SAC_RRANSAC);
}
else if (algorithm_ == "RMSAC") {
seg.setMethodType(pcl::SAC_RMSAC);
}
else if (algorithm_ == "MLESAC") {
seg.setMethodType(pcl::SAC_MLESAC);
}
else if (algorithm_ == "PROSAC") {
seg.setMethodType(pcl::SAC_PROSAC);
}
seg.setDistanceThreshold (outlier_threshold_);
seg.setMaxIterations (max_iterations_);
seg.setModelType(pcl::SACMODEL_CIRCLE3D);
if (use_hint_) {
seg.setAxis(hint_axis_);
seg.setEpsAngle(eps_hint_angle_);
}
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
seg.segment(*inliers, *coefficients);
JSK_NODELET_INFO("input points: %lu", cloud->points.size());
if (inliers->indices.size() > min_size_) {
// check direction. Torus should direct to origin of pointcloud
// always.
Eigen::Vector3f dir(coefficients->values[4],
coefficients->values[5],
coefficients->values[6]);
if (dir.dot(Eigen::Vector3f::UnitZ()) < 0) {
dir = - dir;
}
Eigen::Affine3f pose = Eigen::Affine3f::Identity();
Eigen::Vector3f pos = Eigen::Vector3f(coefficients->values[0],
coefficients->values[1],
coefficients->values[2]);
Eigen::Quaternionf rot;
rot.setFromTwoVectors(Eigen::Vector3f::UnitZ(), dir);
pose = pose * Eigen::Translation3f(pos) * Eigen::AngleAxisf(rot);
PCLIndicesMsg ros_inliers;
ros_inliers.indices = inliers->indices;
ros_inliers.header = cloud_msg->header;
pub_inliers_.publish(ros_inliers);
PCLModelCoefficientMsg ros_coefficients;
ros_coefficients.values = coefficients->values;
ros_coefficients.header = cloud_msg->header;
pub_coefficients_.publish(ros_coefficients);
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
tf::poseEigenToMsg(pose, torus_msg.pose);
torus_msg.small_radius = 0.01;
torus_msg.large_radius = coefficients->values[3];
pub_torus_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_.publish(torus_array_msg);
// publish pose stamped
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = torus_msg.header;
pose_stamped.pose = torus_msg.pose;
pub_pose_stamped_.publish(pose_stamped);
pub_torus_array_with_failure_.publish(torus_array_msg);
pub_torus_with_failure_.publish(torus_msg);
}
else {
jsk_recognition_msgs::Torus torus_msg;
torus_msg.header = cloud_msg->header;
torus_msg.failure = true;
pub_torus_with_failure_.publish(torus_msg);
jsk_recognition_msgs::TorusArray torus_array_msg;
torus_array_msg.header = cloud_msg->header;
torus_array_msg.toruses.push_back(torus_msg);
pub_torus_array_with_failure_.publish(torus_array_msg);
JSK_NODELET_INFO("failed to find torus");
}
}
int
main (int argc, char** argv)
{
if (argc < 4)
{
std::cerr << "please specify the point cloud and the command line arg '-r' or '-c' or '-s' or '-v' or '-av' or '-rg' + param\n" << std::endl;
std::cerr << "radius filter: param ==> ray + min_neighbours " << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -r 100 10 \n" << std::endl;
std::cerr << "codnitional filter: param ==> min_dist + max_dist along the axis" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -c 0 1000 x\n" << std::endl;
std::cerr << "statistical filter: param ==> num_of_neigh + std_dev" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -s 50 1\n" << std::endl;
std::cerr << "voxel grid downsampling: param ==> leaf_size" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -v 10\n" << std::endl;
std::cerr << "voxel grid (approximated) downsampling filter: param ==> leafsize" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -av 10\n" << std::endl;
std::cerr << "region growing: param ==> point_dist" << std::endl;
std::cerr << " example: " << argv[0] << " cloud.pcd -rg 10\n" << std::endl;
exit(0);
}
pcl::PointCloud<PointType>::Ptr cloud (new pcl::PointCloud<PointType>);
pcl::PointCloud<PointType>::Ptr cloud_filtered (new pcl::PointCloud<PointType>);
if (pcl::io::loadPCDFile (argv[1], *cloud))
return 0;
cloud->height = 1;
cloud->width = cloud->size();
cloud->is_dense=0;
do {
pcl::PointCloud<PointType>::Ptr cloud_filtered (new pcl::PointCloud<PointType>);
std::stringstream string;
size_t begin;
std::string buff;
string << "output/";
//if (system("mkdir output")) int a=0;
buff.assign(argv[1]);
// Check if the filename is a path
begin=buff.find_last_of("/\\");
if ( begin!=std::string::npos )
buff=buff.substr(begin+1);
begin=buff.find(".pcd");
if ( begin!=std::string::npos )
buff.assign ( buff,0,begin );
else {
std::cout << "No valid .pcd loaded" << std::endl;
return 0;
}
string << buff;
buff.assign(argv[2]);
begin=buff.find("-");
if ( begin!=std::string::npos )
buff.assign ( buff, begin+1, buff.length() );
else {
std::cout << "No valid method defined" << std::endl;
return 0;
}
string << "_" << buff;
buff.clear();
if (argc > 3)
buff.assign(argv[3]);
if ( buff.length() > 0 )
string << "_" << buff;
buff.clear();
if (argc > 4)
buff.assign(argv[4]);
if ( buff.length() > 0 )
string << "_" << buff;
buff.clear();
if (argc > 5)
buff.assign(argv[5]);
if ( buff.length() > 0 )
string << "_" << buff;
string << ".pcd";
if (!filterIt(argc, argv, cloud, cloud_filtered))
return 0;
//cloud_filtered->is_dense = 0;
// If ordered, remove the NaN pointsd
if (0) {
std::cout << "Cloud is organized and NaN points are now removed" << std::endl;
std::vector<int> unused;
cloud_filtered->is_dense = 0;
pcl::removeNaNFromPointCloud (*cloud_filtered, *cloud_filtered, unused);
}
//pcl::io::savePCDFile<PointType>(string.str(), cloud_filtered.operator*());
std::cerr << "Cloud points before filtering: " << cloud->points.size() << std::endl;
std::cerr << "Cloud points after filtering: " << cloud_filtered->points.size() << std::endl;
pcl::io::savePCDFileBinary<PointType>(string.str(), *cloud_filtered);
std::cout << "Resulting cloud saved in " << string.str() << std::endl;
std::stringstream command;
command << "pcd_viewer_int " << argv[1] << " " << string.str();
std::cout << "Displaying the result... " << argv[2] << " "<<argv[3];
if (!system(command.str().data()))
std::cout << "stop" << std::endl << std::endl;
std::cout << "repeat? [y/n]" << std::endl;
char c;
cloud->clear();
cloud = cloud_filtered;
c=getchar();
getchar();
if (c != 'y')
break;
} while (1);
return (0);
}
void
pcl::RadiusOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
output.is_dense = true;
// If fields x/y/z are not present, we cannot filter
if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
{
PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
if (search_radius_ == 0.0)
{
PCL_ERROR ("[pcl::%s::applyFilter] No radius defined!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
// Send the input dataset to the spatial locator
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input_, *cloud);
// Initialize the spatial locator
if (!tree_)
{
if (cloud->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
else
tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
}
tree_->setInputCloud (cloud);
// Allocate enough space to hold the results
std::vector<int> nn_indices (indices_->size ());
std::vector<float> nn_dists (indices_->size ());
// Copy the common fields
output.is_bigendian = input_->is_bigendian;
output.point_step = input_->point_step;
output.height = 1;
output.data.resize (input_->width * input_->point_step); // reserve enough space
removed_indices_->resize (input_->data.size ());
int nr_p = 0;
int nr_removed_p = 0;
// Go over all the points and check which doesn't have enough neighbors
for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
{
int k = tree_->radiusSearch ((*indices_)[cp], search_radius_, nn_indices, nn_dists);
// Check if the number of neighbors is larger than the user imposed limit
if (k < min_pts_radius_)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step],
output.point_step);
nr_p++;
}
output.width = nr_p;
output.height = 1;
output.data.resize (output.width * output.point_step);
output.row_step = output.point_step * output.width;
removed_indices_->resize (nr_removed_p);
}
int
main (int argc, char** argv)
{
srand (time (NULL));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
// Generate pointcloud data
cloud->width = 1000;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024.0f * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1024.0f * rand () / (RAND_MAX + 1.0f);
}
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
kdtree.setInputCloud (cloud);
pcl::PointXYZ searchPoint;
searchPoint.x = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.y = 1024.0f * rand () / (RAND_MAX + 1.0f);
searchPoint.z = 1024.0f * rand () / (RAND_MAX + 1.0f);
// K nearest neighbor search
int K = 10;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
std::cout << "K nearest neighbor search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with K=" << K << std::endl;
if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxNKNSearch[i] ].x
<< " " << cloud->points[ pointIdxNKNSearch[i] ].y
<< " " << cloud->points[ pointIdxNKNSearch[i] ].z
<< " (squared distance: " << pointNKNSquaredDistance[i] << ")" << std::endl;
}
// Neighbors within radius search
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
float radius = 256.0f * rand () / (RAND_MAX + 1.0f);
std::cout << "Neighbors within radius search at (" << searchPoint.x
<< " " << searchPoint.y
<< " " << searchPoint.z
<< ") with radius=" << radius << std::endl;
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
{
for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
std::cout << " " << cloud->points[ pointIdxRadiusSearch[i] ].x
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].y
<< " " << cloud->points[ pointIdxRadiusSearch[i] ].z
<< " (squared distance: " << pointRadiusSquaredDistance[i] << ")" << std::endl;
}
return 0;
}
void ImageProcessing::segEuclideanClusters(const char *filename) {
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
reader.read(filename, *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud(cloud);
vg.setLeafSize(0.01f, 0.01f, 0.01f);
vg.filter(*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(100);
seg.setDistanceThreshold(0.02);
int i = 0, nr_points = (int) cloud_filtered->points.size();
while (cloud_filtered->points.size() > 0.3 * nr_points) {
// Segment the largest planar component from the remaining cloud
seg.setInputCloud(cloud_filtered);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() == 0) {
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud_filtered);
extract.setIndices(inliers);
extract.setNegative(false);
// Get the points associated with the planar surface
extract.filter(*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative(true);
extract.filter(*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud(cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.02); // 2cm
ec.setMinClusterSize(100);
ec.setMaxClusterSize(25000);
ec.setSearchMethod(tree);
ec.setInputCloud(cloud_filtered);
ec.extract(cluster_indices);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr all_clusters(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB aPoint;
int j = 0;
Color myColor;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) {
myColor = Color::random(); //one color for each cluster.
//adding all points of one cluster
for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++) {
cloud_cluster->points.push_back(cloud_filtered->points[*pit]); //*
aPoint.x = cloud_cluster->points.back().x;
aPoint.y = cloud_cluster->points.back().y;
aPoint.z = cloud_cluster->points.back().z;
aPoint.r = myColor.red;
aPoint.g = myColor.green;
aPoint.b = myColor.blue;
all_clusters->points.push_back(aPoint);
}
cloud_cluster->width = cloud_cluster->points.size();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
j++;
}
//writer.write<pcl::PointXYZRGB> ("clustered_cloud.pcd", *cloud_cluster, false); //*
pcl::visualization::CloudViewer viewer("Cluster viewer");
viewer.showCloud(all_clusters);
while (!viewer.wasStopped()) {
}
}
void ImageProcessing::segRegionGrowing(const char *filename) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile(filename, *cloud) == -1) {
std::cout << "Cloud reading failed." << std::endl;
//return (-1);
} else {
std::cout << "Point cloud is loaded from " <<filename<< std::endl;
}
//visualize input cloud
//visualizePointCloud(cloud);
pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod(tree);
normal_estimator.setInputCloud(cloud);
normal_estimator.setKSearch(50);
normal_estimator.compute(*normals);
pcl::IndicesPtr indices(new std::vector <int>);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(0.0, 1.0);
pass.filter(*indices);
pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
reg.setMinClusterSize(50);
reg.setMaxClusterSize(1000000);
reg.setSearchMethod(tree);
reg.setNumberOfNeighbours(30);
reg.setInputCloud(cloud);
//reg.setIndices (indices);
reg.setInputNormals(normals);
reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
reg.setCurvatureThreshold(1.0);
std::vector <pcl::PointIndices> clusters;
reg.extract(clusters);
std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;
std::cout << "First cluster has " << clusters[0].indices.size() << " points." << endl;
std::cout << "These are the indices of the points of the initial" <<
std::endl << "cloud that belong to the first cluster:" << std::endl;
int counter = 0;
// while (counter < clusters[0].indices.size ())
// {
// std::cout << clusters[0].indices[counter] << ", ";
// counter++;
// if (counter % 10 == 0)
// std::cout << std::endl;
// }
std::cout << std::endl;
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();
pcl::visualization::CloudViewer viewer("Cluster viewer");
viewer.showCloud(colored_cloud);
while (!viewer.wasStopped()) {
}
//visualize segmented cloud
//visualizePointCloud(colored_cloud);
}
int main (int argc, char** argv)
{
if (argc != 3)
return (0);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
if (pcl::io::loadPLYFile (argv[1], *cloud) == -1)
return (-1);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::io::loadPLYFile(argv[2], *cloud2);
pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
feature_extractor.setInputCloud (cloud);
feature_extractor.compute ();
std::vector <float> moment_of_inertia;
std::vector <float> eccentricity;
pcl::PointXYZ min_point_AABB;
pcl::PointXYZ max_point_AABB;
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
float major_value, middle_value, minor_value;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
Eigen::Vector3f mass_center;
feature_extractor.getMomentOfInertia (moment_of_inertia);
feature_extractor.getEccentricity (eccentricity);
feature_extractor.getAABB (min_point_AABB, max_point_AABB);
feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
feature_extractor.getEigenValues (major_value, middle_value, minor_value);
feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
feature_extractor.getMassCenter (mass_center);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (2.0, 0);
viewer->initCameraParameters ();
viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
viewer->addPointCloud<pcl::PointXYZ> (cloud2, "object cloud");
viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
Eigen::Quaternionf quat (rotational_matrix_OBB);
viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
while(!viewer->wasStopped())
{
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
int main (int argc, char** argv)
{
std::string fileName = argv[1];
std::cout << "Reading " << fileName << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (fileName, *cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file");
return (-1);
}
std::cout << "Loaded " << cloud->points.size() << " points." << std::endl;
// Compute the normals
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normalEstimation;
normalEstimation.setInputCloud (cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
normalEstimation.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloudWithNormals (new pcl::PointCloud<pcl::Normal>);
normalEstimation.setRadiusSearch (0.03);
normalEstimation.compute (*cloudWithNormals);
std::cout << "Computed " << cloudWithNormals->points.size() << " normals." << std::endl;
// Setup the feature computation
typedef pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> VFHEstimationType;
VFHEstimationType vfhEstimation;
// Provide the original point cloud (without normals)
vfhEstimation.setInputCloud (cloud);
// Provide the point cloud with normals
vfhEstimation.setInputNormals(cloudWithNormals);
// Use the same KdTree from the normal estimation
vfhEstimation.setSearchMethod (tree);
//vfhEstimation.setRadiusSearch (0.2); // With this, error: "Both radius (.2) and K (1) defined! Set one of them to zero first and then re-run compute()"
// Actually compute the VFH features
pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhFeatures(new pcl::PointCloud<pcl::VFHSignature308>);
vfhEstimation.compute (*vfhFeatures);
std::cout << "output points.size (): " << vfhFeatures->points.size () << std::endl; // This outputs 1 - should be 397!
// Display and retrieve the shape context descriptor vector for the 0th point.
pcl::VFHSignature308 descriptor = vfhFeatures->points[0];
VFHEstimationType::PointCloudOut::PointType descriptor2 = vfhFeatures->points[0];
std::cout << "VFH:" << descriptor << std::endl;
std::cout << "Numero de Elementos del VFH = " << sizeof(descriptor.histogram)/sizeof(descriptor.histogram[0]) << std::endl;
// Create *_vfh.pcd file
std::stringstream vfh_file;
vfh_file << "# .PCD v.6 - Point Cloud Data file format" << std::endl;
vfh_file << "FIELDS vfh" << std::endl;
vfh_file << "SIZE 4" << std::endl;
vfh_file << "TYPE F" << std::endl;
vfh_file << "COUNT 308" << std::endl;
vfh_file << "WIDTH 1" << std::endl;
vfh_file << "HEIGHT 1" << std::endl;
vfh_file << "POINTS 1" << std::endl;
vfh_file << "DATA ascii" << std::endl;
int vfh_length = sizeof(descriptor.histogram)/sizeof(descriptor.histogram[0]);
for (int i = 0; i < vfh_length; i++)
{
vfh_file << descriptor.histogram[i] << " ";
}
std::ofstream outFile;
outFile.open("Prueba_vfh.pcd");
outFile << vfh_file.str();
outFile.close();
return 0;
}
int main(int argc, char *argv[]) {
if (argc != 2)
return 0;
Frame3D frames[8];
for (int i = 0; i < 8; ++i) {
frames[i].load(boost::str(boost::format("%s/%05d.3df") % argv[1] % i));
}
std::cout << "Merging point cloud" << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr model_point_cloud_norm = merge(frames);
std::cout << "Got: " << model_point_cloud_norm->size() << " points" << std::endl;
std::cout << "Generating mesh" << std::endl;
pcl::PointCloud<pcl::PointNormal>::Ptr reduced_point_cloud(new pcl::PointCloud<pcl::PointNormal>());
pcl::PassThrough<pcl::PointNormal> filter;
filter.setInputCloud(model_point_cloud_norm);
filter.filter(*reduced_point_cloud);
std::cout << "Got: " << reduced_point_cloud->size() << " points" << std::endl;
pcl::Poisson<pcl::PointNormal> rec;
rec.setDepth(10);
rec.setInputCloud(reduced_point_cloud);
pcl::PolygonMesh triangles;
rec.reconstruct(triangles);
std::cout << "Finished" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::fromPCLPointCloud2(triangles.cloud, *cloud);
for (int i = 0; i < 8; ++i) {
float focal_length = frames[i].focal_length_;
// Camera width
double sizeX = frames[i].depth_image_.cols;
// Camera height
double sizeY = frames[i].depth_image_.rows;
// Centers
double cx = sizeX / 2.0;
double cy = sizeY / 2.0;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed_cloud = transformPointCloud(cloud, frames[i].getEigenTransform().inverse());
const cv::Mat& zbuffer = computeZbuffer(*transformed_cloud, frames[i]);
int point_found = 0;
for (const pcl::Vertices& polygon : triangles.polygons) {
const pcl::PointXYZRGB& point = transformed_cloud->at(polygon.vertices[0]);
int u_unscaled = std::round(focal_length * (point.x / point.z) + cx);
int v_unscaled = std::round(focal_length * (point.y / point.z) + cy);
const cv::Vec3f& zmap_point = zbuffer.at<cv::Vec3f>(v_unscaled, u_unscaled);
const float eps = 0.000000001;
// If not visible
if (std::fabs(zmap_point[0] - point.x) > eps
|| std::fabs(zmap_point[1] - point.y) > eps
|| std::fabs(zmap_point[2] - point.z) > eps)
continue;
float u = static_cast<float>(u_unscaled / sizeX);
float v = static_cast<float>(v_unscaled / sizeY);
if (u < 0. || u >= 1 || v < 0. || v >= 1)
continue;
int x = std::floor(frames[i].rgb_image_.cols * u);
int y = std::floor(frames[i].rgb_image_.rows * v);
for (int h = 0; h < 3; ++h) {
pcl::PointXYZRGB& original_point = cloud->at(polygon.vertices[h]);
const cv::Vec3b& rgb = frames[i].rgb_image_.at<cv::Vec3b>(y, x);
if (original_point.r != 0 && original_point.g != 0 && original_point.b != 0)
continue;
original_point.b = rgb[0];
original_point.g = rgb[1];
original_point.r = rgb[2];
}
++point_found;
}
std::cout << point_found << " points found" << std::endl;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr textured_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
textured_cloud->reserve(cloud->size());
for (const pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0) {
textured_cloud->push_back(point);
}
}
// Filling gaps
pcl::KdTreeFLANN<pcl::PointXYZRGB>::Ptr tree2(new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
tree2->setInputCloud(textured_cloud);
for (pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0)
continue;
std::vector<int> k_indices;
std::vector<float> k_dist;
tree2->nearestKSearch(point, 1, k_indices, k_dist);
const pcl::PointXYZRGB& textured_point = textured_cloud->at(k_indices[0]);
point.r = textured_point.r;
point.g = textured_point.g;
point.b = textured_point.b;
}
// Smoothing
tree2->setInputCloud(cloud);
for (pcl::PointXYZRGB& point : *cloud) {
if (point.r != 0 || point.g != 0 || point.b != 0)
continue;
std::vector<int> k_indices;
std::vector<float> k_dist;
tree2->nearestKSearch(point, 5, k_indices, k_dist);
int r = 0;
int g = 0;
int b = 0;
for (int i = 0; i < 5; ++i) {
const pcl::PointXYZRGB& textured_point = textured_cloud->at(k_indices[i]);
r += textured_point.r;
g += textured_point.g;
b += textured_point.b;
}
r /= 5;
g /= 5;
b /= 5;
point.r = (uint8_t) r;
point.g = (uint8_t) g;
point.b = (uint8_t) b;
}
pcl::toPCLPointCloud2(*cloud, triangles.cloud);
std::cout << "Finished texturing" << std::endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(1, 1, 1);
viewer->addPolygonMesh(triangles, "meshes", 0);
viewer->addCoordinateSystem(1.0);
viewer->initCameraParameters();
while (!viewer->wasStopped()) {
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
return 0;
}
PointViewSet PoissonFilter::run(PointViewPtr input)
{
PointViewPtr output = input->makeNew();
PointViewSet viewSet;
viewSet.insert(output);
bool logOutput = log()->getLevel() > LogLevel::Debug1;
if (logOutput)
log()->floatPrecision(8);
log()->get(LogLevel::Debug2) << "Process PoissonFilter..." << std::endl;
BOX3D buffer_bounds;
input->calculateBounds(buffer_bounds);
// convert PointView to PointNormal
typedef pcl::PointCloud<pcl::PointXYZ> Cloud;
Cloud::Ptr cloud(new Cloud);
pclsupport::PDALtoPCD(input, *cloud, buffer_bounds);
pclsupport::setLogLevel(log()->getLevel());
// pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree;
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2;
// Create search tree
tree.reset(new pcl::search::KdTree<pcl::PointXYZ> (false));
tree->setInputCloud(cloud);
// Normal estimation
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal> ());
n.setInputCloud(cloud);
n.setSearchMethod(tree);
n.setKSearch(20);
n.compute(*normals);
// Concatenate XYZ and normal information
pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);
// Create search tree
tree2.reset(new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud(cloud_with_normals);
// initial setup
pcl::Poisson<pcl::PointNormal> p;
p.setInputCloud(cloud_with_normals);
p.setSearchMethod(tree2);
p.setDepth(m_depth);
p.setPointWeight(m_point_weight);
// create PointCloud for results
pcl::PolygonMesh grid;
p.reconstruct(grid);
Cloud::Ptr cloud_f(new Cloud);
pcl::fromPCLPointCloud2(grid.cloud, *cloud_f);
if (cloud_f->points.empty())
{
log()->get(LogLevel::Debug2) << "Filtered cloud has no points!" << std::endl;
return viewSet;
}
pclsupport::PCDtoPDAL(*cloud_f, output, buffer_bounds);
log()->get(LogLevel::Debug2) << cloud->points.size() << " before, " <<
cloud_f->points.size() << " after" << std::endl;
log()->get(LogLevel::Debug2) << output->size() << std::endl;
return viewSet;
}
void PoseWithCovarianceStampedToGaussianPointCloud::convert(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
Eigen::Vector2f mean;
Eigen::Matrix2f S;
if (cut_plane_ == "xy" || cut_plane_ == "flipped_xy") {
mean = Eigen::Vector2f(msg->pose.pose.position.x, msg->pose.pose.position.y);
S << msg->pose.covariance[0], msg->pose.covariance[1],
msg->pose.covariance[6], msg->pose.covariance[7];
}
else if (cut_plane_ == "yz" || cut_plane_ == "flipped_yz") {
mean = Eigen::Vector2f(msg->pose.pose.position.y, msg->pose.pose.position.z);
S << msg->pose.covariance[7], msg->pose.covariance[8],
msg->pose.covariance[13], msg->pose.covariance[14];
}
else if (cut_plane_ == "zx" || cut_plane_ == "flipped_zx") {
mean = Eigen::Vector2f(msg->pose.pose.position.z, msg->pose.pose.position.x);
S << msg->pose.covariance[14], msg->pose.covariance[12],
msg->pose.covariance[0], msg->pose.covariance[2];
}
double max_sigma = 0;
for (size_t i = 0; i < 2; i++) {
for (size_t j = 0; j < 2; j++) {
double sigma = sqrt(S(i, j));
if (max_sigma < sigma) {
max_sigma = sigma;
}
}
}
Eigen::Matrix2f S_inv = S.inverse();
double dx = 6.0 * max_sigma / sampling_num_;
double dy = 6.0 * max_sigma / sampling_num_;
for (double x = - 3.0 * max_sigma; x <= 3.0 * max_sigma; x += dx) {
for (double y = - 3.0 * max_sigma; y <= 3.0 * max_sigma; y += dy) {
Eigen::Vector2f diff(x, y);
Eigen::Vector2f input = diff + mean;
float z = gaussian(input, mean, S, S_inv);
pcl::PointXYZ p;
if (cut_plane_ == "xy") {
p.x = input[0];
p.y = input[1];
p.z = z + msg->pose.pose.position.z;
}
else if (cut_plane_ == "yz") {
p.y = input[0];
p.z = input[1];
p.x = z + msg->pose.pose.position.x;
}
else if (cut_plane_ == "zx") {
p.z = input[0];
p.x = input[1];
p.y = z + msg->pose.pose.position.y;
}
else if (cut_plane_ == "flipped_xy") {
p.x = input[0];
p.y = input[1];
p.z = -z + msg->pose.pose.position.z;
}
else if (cut_plane_ == "flipped_yz") {
p.y = input[0];
p.z = input[1];
p.x = -z + msg->pose.pose.position.x;
}
else if (cut_plane_ == "flipped_zx") {
p.z = input[0];
p.x = input[1];
p.y = -z + msg->pose.pose.position.y;
}
cloud->points.push_back(p);
}
}
sensor_msgs::PointCloud2 ros_cloud;
pcl::toROSMsg(*cloud, ros_cloud);
ros_cloud.header = msg->header;
pub_.publish(ros_cloud);
}
void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
processingData_ = true;
int quality = odom.info().inliers;
bool lost = false;
bool lostStateChanged = false;
if(odom.pose().isNull())
{
UDEBUG("odom lost"); // use last pose
lostStateChanged = imageView_->getBackgroundColor() != Qt::darkRed;
imageView_->setBackgroundColor(Qt::darkRed);
cloudView_->setBackgroundColor(Qt::darkRed);
lost = true;
}
else if(odom.info().inliers>0 &&
qualityWarningThr_ &&
odom.info().inliers < qualityWarningThr_)
{
UDEBUG("odom warn, quality(inliers)=%d thr=%d", odom.info().inliers, qualityWarningThr_);
lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
imageView_->setBackgroundColor(Qt::darkYellow);
cloudView_->setBackgroundColor(Qt::darkYellow);
}
else
{
UDEBUG("odom ok");
lostStateChanged = imageView_->getBackgroundColor() == Qt::darkRed;
imageView_->setBackgroundColor(cloudView_->getDefaultBackgroundColor());
cloudView_->setBackgroundColor(Qt::black);
}
timeLabel_->setText(QString("%1 s").arg(odom.info().timeEstimation));
if(!odom.data().imageRaw().empty() &&
!odom.data().depthOrRightRaw().empty() &&
(odom.data().stereoCameraModel().isValid() || odom.data().cameraModels().size()))
{
UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
if(!odom.data().depthRaw().empty())
{
if(odom.data().imageRaw().cols % decimationSpin_->value() == 0 &&
odom.data().imageRaw().rows % decimationSpin_->value() == 0)
{
validDecimationValue_ = decimationSpin_->value();
}
else
{
UWARN("Decimation (%d) must be a denominator of the width and height of "
"the image (%d/%d). Using last valid decimation value (%d).",
decimationSpin_->value(),
odom.data().imageRaw().cols,
odom.data().imageRaw().rows,
validDecimationValue_);
}
}
else
{
validDecimationValue_ = decimationSpin_->value();
}
// visualization: buffering the clouds
// Create the new cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = util3d::cloudRGBFromSensorData(
odom.data(),
validDecimationValue_,
0.0f,
voxelSpin_->value());
if(cloud->size())
{
if(!odom.pose().isNull())
{
if(cloudView_->getAddedClouds().contains("cloudtmp"))
{
cloudView_->removeCloud("cloudtmp");
}
while(maxCloudsSpin_->value()>0 && (int)addedClouds_.size() > maxCloudsSpin_->value())
{
UASSERT(cloudView_->removeCloud(addedClouds_.first()));
addedClouds_.pop_front();
}
odom.data().id()?id_=odom.data().id():++id_;
std::string cloudName = uFormat("cloud%d", id_);
addedClouds_.push_back(cloudName);
UASSERT(cloudView_->addCloud(cloudName, cloud, odom.pose()));
}
else
{
cloudView_->addOrUpdateCloud("cloudtmp", cloud, lastOdomPose_);
}
}
}
if(!odom.pose().isNull())
{
lastOdomPose_ = odom.pose();
cloudView_->updateCameraTargetPosition(odom.pose());
}
if(odom.info().localMap.size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
cloud->resize(odom.info().localMap.size());
int i=0;
for(std::multimap<int, cv::Point3f>::const_iterator iter=odom.info().localMap.begin(); iter!=odom.info().localMap.end(); ++iter)
{
(*cloud)[i].x = iter->second.x;
(*cloud)[i].y = iter->second.y;
(*cloud)[i++].z = iter->second.z;
}
cloudView_->addOrUpdateCloud("localmap", cloud);
}
if(!odom.data().imageRaw().empty())
{
if(odom.info().type == 0)
{
imageView_->setFeatures(odom.info().words, odom.data().depthRaw(), Qt::yellow);
}
else if(odom.info().type == 1)
{
std::vector<cv::KeyPoint> kpts;
cv::KeyPoint::convert(odom.info().refCorners, kpts);
imageView_->setFeatures(kpts, odom.data().depthRaw(), Qt::red);
}
imageView_->clearLines();
if(lost)
{
if(lostStateChanged)
{
// save state
odomImageShow_ = imageView_->isImageShown();
odomImageDepthShow_ = imageView_->isImageDepthShown();
}
imageView_->setImageDepth(uCvMat2QImage(odom.data().imageRaw()));
imageView_->setImageShown(true);
imageView_->setImageDepthShown(true);
}
else
{
if(lostStateChanged)
{
// restore state
imageView_->setImageShown(odomImageShow_);
imageView_->setImageDepthShown(odomImageDepthShow_);
}
imageView_->setImage(uCvMat2QImage(odom.data().imageRaw()));
if(imageView_->isImageDepthShown())
{
imageView_->setImageDepth(uCvMat2QImage(odom.data().depthOrRightRaw()));
}
if(odom.info().type == 0)
{
if(imageView_->isFeaturesShown())
{
for(unsigned int i=0; i<odom.info().wordMatches.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordMatches[i], Qt::red); // outliers
}
for(unsigned int i=0; i<odom.info().wordInliers.size(); ++i)
{
imageView_->setFeatureColor(odom.info().wordInliers[i], Qt::green); // inliers
}
}
}
}
if(odom.info().type == 1 && odom.info().cornerInliers.size())
{
if(imageView_->isFeaturesShown() || imageView_->isLinesShown())
{
//draw lines
UASSERT(odom.info().refCorners.size() == odom.info().newCorners.size());
for(unsigned int i=0; i<odom.info().cornerInliers.size(); ++i)
{
if(imageView_->isFeaturesShown())
{
imageView_->setFeatureColor(odom.info().cornerInliers[i], Qt::green); // inliers
}
if(imageView_->isLinesShown())
{
imageView_->addLine(
odom.info().refCorners[odom.info().cornerInliers[i]].x,
odom.info().refCorners[odom.info().cornerInliers[i]].y,
odom.info().newCorners[odom.info().cornerInliers[i]].x,
odom.info().newCorners[odom.info().cornerInliers[i]].y,
Qt::blue);
}
}
}
}
if(!odom.data().imageRaw().empty())
{
imageView_->setSceneRect(QRectF(0,0,(float)odom.data().imageRaw().cols, (float)odom.data().imageRaw().rows));
}
}
imageView_->update();
cloudView_->update();
QApplication::processEvents();
processingData_ = false;
}
NormalEstimator::NormalEstimator(Mat* m)
{
/*
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
MatToPcl(*m, cloud);
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);
// Compute the features
ne.compute (*cloud_normals);
// cloud_normals->points.size () should have the same size as the input cloud->points.size ()*
result = PclToMat(NormsToPcl(cloud_normals));
*/
// Load input file into a PointCloud<T> with an appropriate type
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
MatToPcl(*m, cloud);
// Create a KD-Tree
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
// Output has the PointNormal type in order to store the normals calculated by MLS
pcl::PointCloud<pcl::PointNormal> mls_points;
// Init object (second point type is for the normals, even if unused)
pcl::MovingLeastSquares<pcl::PointXYZ, pcl::PointNormal> mls;
mls.setComputeNormals (true);
// Set parameters
mls.setInputCloud (cloud);
mls.setPolynomialFit (true);
mls.setSearchMethod (tree);
mls.setSearchRadius (0.03);
// Reconstruct
mls.process (mls_points);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
for(int i=0; i < mls_points.points.size(); i++){
cloud2->push_back(pcl::PointXYZ(mls_points.points[i].normal_x, mls_points.points[i].normal_y, mls_points.points[i].normal_z));
}
result = PclToMat(cloud2);
}
void
DoSampleRun2 ()
{
cob_3d_mapping_filters::SpeckleFilter<PointXYZ> filter;
pcl::PointCloud<PointXYZ>::Ptr cloud (new pcl::PointCloud<PointXYZ> ());
pcl::PointCloud<PointXYZ>::Ptr cloud_out (new pcl::PointCloud<PointXYZ> ());
cloud->width = 640;
cloud->height = 480;
double x = 0, y = 0;
for (unsigned int i = 0; i < cloud->width; i++, y += 0.001)
{
x = 0;
for (unsigned int j = 0; j < cloud->height; j++, x += 0.001)
{
PointXYZ pt;
pt.x = x;
pt.y = y;
pt.z = 1;
cloud->points.push_back (pt);
}
}
boost::mt19937 rng; // I don't seed it on purpouse (it's not relevant)
boost::normal_distribution<> nd (0.0, 0.05);
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor (rng, nd);
for (unsigned int i = 0; i < 3000; i++)
cloud->points[i * 100].z += var_nor ();
//pcl::io::savePCDFileASCII("/tmp/spk_cloud.pcd", *cloud);
filter.setInputCloud (cloud);
for (unsigned int s = 10; s <= 100; s += 10)
{
std::stringstream ss;
ss << "/tmp/spk_acc_" << s << ".txt";
std::ofstream file;
file.open (ss.str ().c_str ());
file << "thr\ttp\tfn\tfp\n";
for (double c = 0.01; c <= 0.1; c += 0.01)
{
filter.setFilterParam (s, c);
filter.filter (*cloud_out);
pcl::PointIndices::Ptr ind = filter.getRemovedIndices ();
std::cout << "Cloud size " << cloud_out->size () << ", ind: " << ind->indices.size () << std::endl;
int fn_ctr = 0, tp_ctr = 0;
for (unsigned int i = 0; i < 3000; i++)
{
bool found = false;
for (unsigned int j = 0; j < ind->indices.size (); j++)
{
if (ind->indices[j] == i * 100)
{
tp_ctr++;
found = true;
break;
}
}
if (!found)
fn_ctr++;
}
int fp_ctr = ind->indices.size () - tp_ctr;
double fn_ratio = (double)fn_ctr / 3000;
double fp_ratio = (double)fp_ctr / 3000;
double tp_ratio = (double)tp_ctr / 3000;
file << c << "\t" << tp_ratio << "\t" << fn_ratio << "\t" << fp_ratio << "\n";
std::cout << "c: " << c << " fn: " << fn_ratio << ", tp: " << tp_ratio << " fp: " << fp_ratio << std::endl;
}
file.close ();
}
}
int
main (int argc, char** argv)
{
// Read in the cloud data
pcl::PCDReader reader;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
reader.read ("table_scene_lms400.pcd", *cloud);
std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<pcl::PointXYZ> vg;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
vg.setInputCloud (cloud);
vg.setLeafSize (0.01f, 0.01f, 0.01f);
vg.filter (*cloud_filtered);
std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<pcl::PointXYZ> seg;
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PCDWriter writer;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.02);
int i=0, nr_points = (int) cloud_filtered->points.size ();
while (cloud_filtered->points.size () > 0.3 * nr_points)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
break;
}
// Extract the planar inliers from the input cloud
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud (cloud_filtered);
extract.setIndices (inliers);
extract.setNegative (false);
// Write the planar inliers to disk
extract.filter (*cloud_plane);
std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
// Remove the planar inliers, extract the rest
extract.setNegative (true);
extract.filter (*cloud_f);
*cloud_filtered = *cloud_f;
}
// Creating the KdTree object for the search method of the extraction
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud_filtered);
std::vector<pcl::PointIndices> cluster_indices;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance (0.02); // 2cm
ec.setMinClusterSize (100);
ec.setMaxClusterSize (25000);
ec.setSearchMethod (tree);
ec.setInputCloud (cloud_filtered);
ec.extract (cluster_indices);
int j = 0;
for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
cloud_cluster->width = cloud_cluster->points.size ();
cloud_cluster->height = 1;
cloud_cluster->is_dense = true;
std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
std::stringstream ss;
ss << "cloud_cluster_" << j << ".pcd";
writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
j++;
}
return (0);
}
int main(int argc, char** argv)
{
double taubin_radius = 0.03; // radius of curvature-estimation neighborhood
std::string file = "/home/andreas/data/mlaffordance/round21l_reg.pcd";
PointCloud::Ptr cloud(new PointCloud);
if (pcl::io::loadPCDFile<pcl::PointXYZRGBA>(file, *cloud) == -1) //* load the file
{
PCL_ERROR("Couldn't read input PCD file\n");
return (-1);
}
pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>::Ptr organized_neighbor(
new pcl::search::OrganizedNeighbor<pcl::PointXYZRGBA>());
std::vector<int> nn_outer_indices;
std::vector<float> nn_outer_dists;
pcl::KdTreeFLANN<pcl::PointXYZRGBA>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZRGBA>());
int sample_index = 0;
if (cloud->isOrganized())
{
organized_neighbor->setInputCloud(cloud);
organized_neighbor->radiusSearch(cloud->points[sample_index], taubin_radius, nn_outer_indices, nn_outer_dists);
}
else
{
tree->setInputCloud(cloud);
tree->radiusSearch(cloud->points[sample_index], taubin_radius, nn_outer_indices, nn_outer_dists);
}
Eigen::Matrix4d T_base, T_sqrt;
T_base << 0, 0.445417, 0.895323, 0.22, 1, 0, 0, -0.02, 0, 0.895323, -0.445417, 0.24, 0, 0, 0, 1;
T_sqrt << 0.9366, -0.0162, 0.3500, -0.2863, 0.0151, 0.9999, 0.0058, 0.0058, -0.3501, -0.0002, 0.9367, 0.0554, 0, 0, 0, 1;
std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams;
T_cams.push_back(T_base * T_sqrt.inverse());
Eigen::Vector3d sample = cloud->points[sample_index].getVector3fMap().cast<double>();
Quadric quadric(T_cams, cloud, sample, true);
quadric.fitQuadric(nn_outer_indices);
Eigen::MatrixXi cam_source = Eigen::MatrixXi::Zero(1, cloud->points.size());
quadric.findTaubinNormalAxis(nn_outer_indices, cam_source);
quadric.print();
std::set<Eigen::Vector3i, VectorComparator> s;
Eigen::Matrix3i M;
M << 1,1,1,2,3,4,1,1,1;
for (int i=0; i < M.rows(); i++)
s.insert(M.row(i));
std::cout << "M:" << std::endl;
std::cout << M << std::endl;
Eigen::Matrix<int, Eigen::Dynamic, 3> N(s.size(), 3);
int i = 0;
for (std::set<Eigen::Vector3i, VectorComparator>::iterator it=s.begin(); it!=s.end(); ++it)
{
N.row(i) = *it;
i++;
}
std::cout << "N:" << std::endl;
std::cout << N << std::endl;
return 0;
}
void FSModel::convertPointCloudToSurfaceMesh()
{
// Load input file into a PointCloud<T> with an appropriate type
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
//sensor_msgs::PointCloud2 cloud_blob;
//pcl::io::loadPCDFile ("bearHigh.pcd", cloud_blob);
//pcl::fromROSMsg (cloud_blob, *cloud);
//* the data should be available in cloud
cloud->points.resize(pointCloud->size());
for (size_t i = 0; i < pointCloud->points.size(); i++) {
cloud->points[i].x = pointCloud->points[i].x;
cloud->points[i].y = pointCloud->points[i].y;
cloud->points[i].z = pointCloud->points[i].z;
}
// Normal estimation*
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
n.setInputCloud (cloud);
n.setSearchMethod (tree);
//n.setRadiusSearch(15.0);
n.setKSearch (20);
n.compute (*normals);
//* normals should not contain the point normals + surface curvatures
// Concatenate the XYZ and normal fields*
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
//* cloud_with_normals = cloud + normals
// Create search tree*
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud (cloud_with_normals);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (15.00);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (100);
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(false);
// Get result
gp3.setInputCloud (cloud_with_normals);
gp3.setSearchMethod (tree2);
gp3.reconstruct (triangles);
// Additional vertex information
std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();
pcl::io::savePLYFile("mesh.ply", triangles);
}
void
pcl::StatisticalOutlierRemoval<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
{
output.is_dense = true;
// If fields x/y/z are not present, we cannot filter
if (x_idx_ == -1 || y_idx_ == -1 || z_idx_ == -1)
{
PCL_ERROR ("[pcl::%s::applyFilter] Input dataset doesn't have x-y-z coordinates!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
if (std_mul_ == 0.0)
{
PCL_ERROR ("[pcl::%s::applyFilter] Standard deviation multipler not set!\n", getClassName ().c_str ());
output.width = output.height = 0;
output.data.clear ();
return;
}
// Send the input dataset to the spatial locator
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg (*input_, *cloud);
// Initialize the spatial locator
if (!tree_)
{
if (cloud->isOrganized ())
tree_.reset (new pcl::search::OrganizedNeighbor<pcl::PointXYZ> ());
else
tree_.reset (new pcl::search::KdTree<pcl::PointXYZ> (false));
}
tree_->setInputCloud (cloud);
// Allocate enough space to hold the results
std::vector<int> nn_indices (mean_k_);
std::vector<float> nn_dists (mean_k_);
std::vector<float> distances (indices_->size ());
// Go over all the points and calculate the mean or smallest distance
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
if (!pcl_isfinite (cloud->points[(*indices_)[cp]].x) || !pcl_isfinite (cloud->points[(*indices_)[cp]].y)
||
!pcl_isfinite (cloud->points[(*indices_)[cp]].z))
{
distances[cp] = 0;
continue;
}
if (tree_->nearestKSearch ((*indices_)[cp], mean_k_, nn_indices, nn_dists) == 0)
{
distances[cp] = 0;
PCL_WARN ("[pcl::%s::applyFilter] Searching for the closest %d neighbors failed.\n", getClassName ().c_str (), mean_k_);
continue;
}
// Minimum distance (if mean_k_ == 2) or mean distance
double dist_sum = 0;
for (int j = 1; j < mean_k_; ++j)
dist_sum += sqrt (nn_dists[j]);
distances[cp] = static_cast<float> (dist_sum / (mean_k_ - 1));
}
// Estimate the mean and the standard deviation of the distance vector
double mean, stddev;
getMeanStd (distances, mean, stddev);
double distance_threshold = mean + std_mul_ * stddev; // a distance that is bigger than this signals an outlier
// Copy the common fields
output.is_bigendian = input_->is_bigendian;
output.point_step = input_->point_step;
output.height = 1;
output.data.resize (indices_->size () * input_->point_step); // reserve enough space
removed_indices_->resize (input_->data.size ());
// Build a new cloud by neglecting outliers
int nr_p = 0;
int nr_removed_p = 0;
for (int cp = 0; cp < static_cast<int> (indices_->size ()); ++cp)
{
if (negative_)
{
if (distances[cp] <= distance_threshold)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
}
else
{
if (distances[cp] > distance_threshold)
{
if (extract_removed_indices_)
{
(*removed_indices_)[nr_removed_p] = cp;
nr_removed_p++;
}
continue;
}
}
memcpy (&output.data[nr_p * output.point_step], &input_->data[(*indices_)[cp] * output.point_step],
output.point_step);
nr_p++;
}
output.width = nr_p;
output.data.resize (output.width * output.point_step);
output.row_step = output.point_step * output.width;
removed_indices_->resize (nr_removed_p);
}
int main(int argc, char** argv)
{
if (argc < 5)
{
PCL_INFO ("Usage %s -input_dir /dir/with/models -output_dir /output/dir [options]\n", argv[0]);
PCL_INFO (" * where options are:\n"
" -height <X> : simulate scans with sensor mounted on height X\n"
" -noise_std <X> : std of gaussian noise added to pointcloud. Default value 0.0001.\n"
" -distance <X> : simulate scans with object located on a distance X. Can be used multiple times. Default value 4.\n"
" -tilt <X> : tilt sensor for X degrees. X == 0 - sensor looks strait. X < 0 Sensor looks down. X > 0 Sensor looks up . Can be used multiple times. Default value -15.\n"
" -shift <X> : shift object from the straight line. Can be used multiple times. Default value 0.\n"
" -num_views <X> : how many times rotate the object in for every distance, tilt and shift. Default value 6.\n"
"");
return -1;
}
std::string input_dir, output_dir;
double height = 1.5;
double num_views = 6;
double noise_std = 0.0001;
std::vector<double> distances;
std::vector<double> tilt;
std::vector<double> shift;
int full_model_n_points = 30000;
pcl::console::parse_argument(argc, argv, "-input_dir", input_dir);
pcl::console::parse_argument(argc, argv, "-output_dir", output_dir);
pcl::console::parse_argument(argc, argv, "-num_views", num_views);
pcl::console::parse_argument(argc, argv, "-height", height);
pcl::console::parse_argument(argc, argv, "-noise_std", noise_std);
pcl::console::parse_multiple_arguments(argc, argv, "-distance", distances);
pcl::console::parse_multiple_arguments(argc, argv, "-tilt", tilt);
pcl::console::parse_multiple_arguments(argc, argv, "-shift", shift);
PCL_INFO("distances size: %d\n", distances.size());
for (size_t i = 0; i < distances.size(); i++)
{
PCL_INFO("distance: %f\n", distances[i]);
}
// Set default values if user didn't provide any
if (distances.size() == 0)
distances.push_back(4);
if (tilt.size() == 0)
tilt.push_back(-15);
if (shift.size() == 0)
shift.push_back(0);
// Check if input path exists
boost::filesystem::path input_path(input_dir);
if (!boost::filesystem::exists(input_path))
{
PCL_ERROR("Input directory doesnt exists.");
return -1;
}
// Check if input path is a directory
if (!boost::filesystem::is_directory(input_path))
{
PCL_ERROR("%s is not directory.", input_path.c_str());
return -1;
}
// Check if output directory exists
boost::filesystem::path output_path(output_dir);
if (!boost::filesystem::exists(output_path) || !boost::filesystem::is_directory(output_path))
{
if (!boost::filesystem::create_directories(output_path))
{
PCL_ERROR ("Error creating directory %s.\n", output_path.c_str ());
return -1;
}
}
// Find all .vtk files in the input directory
std::vector<std::string> files_to_process;
PCL_INFO("Processing following files:\n");
boost::filesystem::directory_iterator end_iter;
for (boost::filesystem::directory_iterator iter(input_path); iter != end_iter; iter++)
{
boost::filesystem::path class_dir_path(*iter);
for (boost::filesystem::directory_iterator iter2(class_dir_path); iter2 != end_iter; iter2++)
{
boost::filesystem::path file(*iter2);
if (file.extension() == ".vtk")
{
files_to_process.push_back(file.c_str());
PCL_INFO("\t%s\n", file.c_str());
}
}
}
// Check if there are any .vtk files to process
if (files_to_process.size() == 0)
{
PCL_ERROR("Directory %s has no .vtk files.", input_path.c_str());
return -1;
}
// Iterate over all files
for (size_t i = 0; i < files_to_process.size(); i++)
{
vtkSmartPointer<vtkPolyData> model;
vtkSmartPointer<vtkPolyDataReader> reader = vtkPolyDataReader::New();
vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
// Compute output directory for this model
std::vector<std::string> st;
boost::split(st, files_to_process.at(i), boost::is_any_of("/"), boost::token_compress_on);
std::string class_dirname = st.at(st.size() - 2);
std::string dirname = st.at(st.size() - 1);
dirname = dirname.substr(0, dirname.size() - 4);
dirname = output_dir + class_dirname + "/" + dirname;
// Check if output directory for this model exists. If not create
boost::filesystem::path dirpath(dirname);
if (!boost::filesystem::exists(dirpath))
{
if (!boost::filesystem::create_directories(dirpath))
{
PCL_ERROR ("Error creating directory %s.\n", dirpath.c_str ());
return -1;
}
}
// Load model from file
reader->SetFileName(files_to_process.at(i).c_str());
reader->Update();
model = reader->GetOutput();
PCL_INFO("Number of points %d\n",model->GetNumberOfPoints());
// Coumpute bounds and center of the model
double bounds[6];
model->GetBounds(bounds);
double min_z_value = bounds[4];
double center[3];
model->GetCenter(center);
createFullModelPointcloud(model, full_model_n_points, *cloud);
pcl::io::savePCDFile(dirname + "/full.pcd", *cloud);
// Initialize PCLVisualizer. Add model to scene.
pcl::visualization::PCLVisualizer viz;
viz.initCameraParameters();
viz.updateCamera();
viz.setCameraPosition(0, 0, 0, 1, 0, 0, 0, 0, 1);
viz.addModelFromPolyData(model, transform);
viz.setRepresentationToSurfaceForAllActors();
// Iterate over all shifts
for (size_t shift_index = 0; shift_index < shift.size(); shift_index++)
{
// Iterate over all tilts
for (size_t tilt_index = 0; tilt_index < tilt.size(); tilt_index++)
{
// Iterate over all distances
for (size_t distance_index = 0; distance_index < distances.size(); distance_index++)
{
// Iterate over all angles
double angle = 0;
double angle_step = 360.0 / num_views;
for (int angle_index = 0; angle_index < num_views; angle_index++)
{
// Set transformation with distance, shift, tilt and angle parameters.
transform->Identity();
transform->RotateY(tilt[tilt_index]);
transform->Translate(distances[distance_index], shift[shift_index], -(height + min_z_value));
transform->RotateZ(angle);
/*
// Render pointcloud
viz.renderView(640, 480, cloud);
//Add noise
addNoise(cloud, noise_std);
// Compute new coordinates of the model center
double new_center[3];
transform->TransformPoint(center, new_center);
// Shift origin of the poincloud to the model center and align with initial coordinate system.
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>());
moveToNewCenterAndAlign(cloud, cloud_transformed, new_center, tilt[tilt_index]);
// Compute file name for this pointcloud and save it
std::stringstream ss;
ss << dirname << "/rotation" << angle << "_distance" << distances[distance_index] << "_tilt"
<< tilt[tilt_index] << "_shift" << shift[shift_index] << ".pcd";
pcl_INFO("Writing %d points to file %s\n", cloud->points.size(), ss.str().c_str());
pcl::io::savePCDFile(ss.str(), *cloud_transformed);
*/
// increment angle by step
angle += angle_step;
}
}
}
}
}
return 0;
}
int
main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the cloud data
cloud->width = 15;
cloud->height = 1;
cloud->points.resize (cloud->width * cloud->height);
// Generate the data
for (size_t i = 0; i < cloud->points.size (); ++i)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
cloud->points[i].z = 1.0;
}
// Set a few outliers
cloud->points[0].z = 2.0;
cloud->points[3].z = -2.0;
cloud->points[6].z = 4.0;
std::cerr << "Point cloud data: " << cloud->points.size () << " points" << std::endl;
for (size_t i = 0; i < cloud->points.size (); ++i)
std::cerr << " " << cloud->points[i].x << " "
<< cloud->points[i].y << " "
<< cloud->points[i].z << std::endl;
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud);
seg.segment (*inliers, *coefficients);
if (inliers->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model for the given dataset.");
return (-1);
}
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << std::endl;
std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;
for (size_t i = 0; i < inliers->indices.size (); ++i)
std::cerr << inliers->indices[i] << " " << cloud->points[inliers->indices[i]].x << " "
<< cloud->points[inliers->indices[i]].y << " "
<< cloud->points[inliers->indices[i]].z << std::endl;
return (0);
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "points_and_lines");
ros::NodeHandle n;
ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
ros::Rate r(30);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
float f = 0.0;
while (ros::ok())
{
// %Tag(MARKER_INIT)%
visualization_msgs::Marker points, line_strip, line_list;
points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
points.ns = line_strip.ns = line_list.ns = "points_and_lines";
points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;
// %EndTag(MARKER_INIT)%
// %Tag(ID)%
points.id = 0;
line_strip.id = 1;
line_list.id = 2;
// %EndTag(ID)%
// %Tag(TYPE)%
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
line_list.type = visualization_msgs::Marker::LINE_LIST;
// %EndTag(TYPE)%
// %Tag(SCALE)%
// POINTS markers use x and y scale for width/height respectively
points.scale.x = 0.2;
points.scale.y = 0.2;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
line_strip.scale.x = 0.1;
line_list.scale.x = 0.1;
// %EndTag(SCALE)%
// %Tag(COLOR)%
// Points are green
points.color.g = 1.0f;
points.color.a = 1.0;
// Line strip is blue
line_strip.color.b = 1.0;
line_strip.color.a = 1.0;
// Line list is red
line_list.color.r = 1.0;
line_list.color.a = 1.0;
// %EndTag(COLOR)%
// %Tag(HELIX)%
// Create the vertices for the points and lines
for (uint32_t i = 0; i < 100; ++i)
{
float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
float z = 5 * cos(f + i / 100.0f * 2 * M_PI);
geometry_msgs::Point p;
p.x = (int32_t)i - 50;
p.y = y;
p.z = z;
points.points.push_back(p);
line_strip.points.push_back(p);
// The line list needs two points for each line
line_list.points.push_back(p);
p.z += 1.0;
line_list.points.push_back(p);
}
// %EndTag(HELIX)%
marker_pub.publish(points);
marker_pub.publish(line_strip);
marker_pub.publish(line_list);
r.sleep();
f += 0.04;
}
}
void HintedHandleEstimator::estimate(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
const geometry_msgs::PointStampedConstPtr &point_msg)
{
boost::mutex::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
pcl::PassThrough<pcl::PointXYZ> pass;
int K = 1;
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
pcl::search::KdTree<pcl::PointXYZ>::Ptr kd_tree(new pcl::search::KdTree<pcl::PointXYZ>);
pcl::fromROSMsg(*cloud_msg, *cloud);
geometry_msgs::PointStamped transed_point;
ros::Time now = ros::Time::now();
try
{
listener_.waitForTransform(cloud->header.frame_id, point_msg->header.frame_id, now, ros::Duration(1.0));
listener_.transformPoint(cloud->header.frame_id, now, *point_msg, point_msg->header.frame_id, transed_point);
}
catch(tf::TransformException ex)
{
JSK_ROS_ERROR("%s", ex.what());
return;
}
pcl::PointXYZ searchPoint;
searchPoint.x = transed_point.point.x;
searchPoint.y = transed_point.point.y;
searchPoint.z = transed_point.point.z;
//remove too far cloud
pass.setInputCloud(cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(searchPoint.x - 3*handle.arm_w, searchPoint.x + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(searchPoint.y - 3*handle.arm_w, searchPoint.y + 3*handle.arm_w);
pass.filter(*cloud);
pass.setInputCloud(cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(searchPoint.z - 3*handle.arm_w, searchPoint.z + 3*handle.arm_w);
pass.filter(*cloud);
if(cloud->points.size() < 10){
JSK_ROS_INFO("points are too small");
return;
}
if(1){ //estimate_normal
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setSearchMethod(kd_tree);
ne.setRadiusSearch(0.02);
ne.setViewPoint(0, 0, 0);
ne.compute(*cloud_normals);
}
else{ //use normal of msg
}
if(! (kd_tree->nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)){
JSK_ROS_INFO("kdtree failed");
return;
}
float x = cloud->points[pointIdxNKNSearch[0]].x;
float y = cloud->points[pointIdxNKNSearch[0]].y;
float z = cloud->points[pointIdxNKNSearch[0]].z;
float v_x = cloud_normals->points[pointIdxNKNSearch[0]].normal_x;
float v_y = cloud_normals->points[pointIdxNKNSearch[0]].normal_y;
float v_z = cloud_normals->points[pointIdxNKNSearch[0]].normal_z;
double theta = acos(v_x);
// use normal for estimating handle direction
tf::Quaternion normal(0, v_z/NORM(0, v_y, v_z) * cos(theta/2), -v_y/NORM(0, v_y, v_z) * cos(theta/2), sin(theta/2));
tf::Quaternion final_quaternion = normal;
double min_theta_index = 0;
double min_width = 100;
tf::Quaternion min_qua(0, 0, 0, 1);
visualization_msgs::Marker debug_hand_marker;
debug_hand_marker.header = cloud_msg->header;
debug_hand_marker.ns = string("debug_grasp");
debug_hand_marker.id = 0;
debug_hand_marker.type = visualization_msgs::Marker::LINE_LIST;
debug_hand_marker.pose.orientation.w = 1;
debug_hand_marker.scale.x=0.003;
tf::Matrix3x3 best_mat;
//search 180 degree and calc the shortest direction
for(double theta_=0; theta_<3.14/2;
theta_+=3.14/2/30){
tf::Quaternion rotate_(sin(theta_), 0, 0, cos(theta_));
tf::Quaternion temp_qua = normal * rotate_;
tf::Matrix3x3 temp_mat(temp_qua);
geometry_msgs::Pose pose_respected_to_tf;
pose_respected_to_tf.position.x = x;
pose_respected_to_tf.position.y = y;
pose_respected_to_tf.position.z = z;
pose_respected_to_tf.orientation.x = temp_qua.getX();
pose_respected_to_tf.orientation.y = temp_qua.getY();
pose_respected_to_tf.orientation.z = temp_qua.getZ();
pose_respected_to_tf.orientation.w = temp_qua.getW();
Eigen::Affine3d box_pose_respected_to_cloud_eigend;
tf::poseMsgToEigen(pose_respected_to_tf, box_pose_respected_to_cloud_eigend);
Eigen::Affine3d box_pose_respected_to_cloud_eigend_inversed
= box_pose_respected_to_cloud_eigend.inverse();
Eigen::Matrix4f box_pose_respected_to_cloud_eigen_inversed_matrixf;
Eigen::Matrix4d box_pose_respected_to_cloud_eigen_inversed_matrixd
= box_pose_respected_to_cloud_eigend_inversed.matrix();
jsk_pcl_ros::convertMatrix4<Eigen::Matrix4d, Eigen::Matrix4f>(
box_pose_respected_to_cloud_eigen_inversed_matrixd,
box_pose_respected_to_cloud_eigen_inversed_matrixf);
Eigen::Affine3f offset = Eigen::Affine3f(box_pose_respected_to_cloud_eigen_inversed_matrixf);
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud(*cloud, *output_cloud, offset);
pcl::PassThrough<pcl::PointXYZ> pass;
pcl::PointCloud<pcl::PointXYZ>::Ptr points_z(new pcl::PointCloud<pcl::PointXYZ>), points_yz(new pcl::PointCloud<pcl::PointXYZ>), points_xyz(new pcl::PointCloud<pcl::PointXYZ>);
pass.setInputCloud(output_cloud);
pass.setFilterFieldName("y");
pass.setFilterLimits(-handle.arm_w*2, handle.arm_w*2);
pass.filter(*points_z);
pass.setInputCloud(points_z);
pass.setFilterFieldName("z");
pass.setFilterLimits(-handle.finger_d, handle.finger_d);
pass.filter(*points_yz);
pass.setInputCloud(points_yz);
pass.setFilterFieldName("x");
pass.setFilterLimits(-(handle.arm_l-handle.finger_l), handle.finger_l);
pass.filter(*points_xyz);
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
for(size_t index=0; index<points_xyz->size(); index++){
points_xyz->points[index].x = points_xyz->points[index].z = 0;
}
if(points_xyz->points.size() == 0){JSK_ROS_INFO("points are empty");return;}
kdtree.setInputCloud(points_xyz);
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
pcl::PointXYZ search_point_tree;
search_point_tree.x=search_point_tree.y=search_point_tree.z=0;
if( kdtree.radiusSearch(search_point_tree, 10, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){
double before_w=10, temp_w;
for(size_t index = 0; index < pointIdxRadiusSearch.size(); ++index){
temp_w =sqrt(pointRadiusSquaredDistance[index]);
if(temp_w - before_w > handle.finger_w*2){
break; // there are small space for finger
}
before_w=temp_w;
}
if(before_w < min_width){
min_theta_index = theta_;
min_width = before_w;
min_qua = temp_qua;
best_mat = temp_mat;
}
//for debug view
geometry_msgs::Point temp_point;
std_msgs::ColorRGBA temp_color;
temp_color.r=0; temp_color.g=0; temp_color.b=1; temp_color.a=1;
temp_point.x=x-temp_mat.getColumn(1)[0] * before_w;
temp_point.y=y-temp_mat.getColumn(1)[1] * before_w;
temp_point.z=z-temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
temp_point.x+=2*temp_mat.getColumn(1)[0] * before_w;
temp_point.y+=2*temp_mat.getColumn(1)[1] * before_w;
temp_point.z+=2*temp_mat.getColumn(1)[2] * before_w;
debug_hand_marker.points.push_back(temp_point);
debug_hand_marker.colors.push_back(temp_color);
}
}
geometry_msgs::PoseStamped handle_pose_stamped;
handle_pose_stamped.header = cloud_msg->header;
handle_pose_stamped.pose.position.x = x;
handle_pose_stamped.pose.position.y = y;
handle_pose_stamped.pose.position.z = z;
handle_pose_stamped.pose.orientation.x = min_qua.getX();
handle_pose_stamped.pose.orientation.y = min_qua.getY();
handle_pose_stamped.pose.orientation.z = min_qua.getZ();
handle_pose_stamped.pose.orientation.w = min_qua.getW();
std_msgs::Float64 min_width_msg;
min_width_msg.data = min_width;
pub_pose_.publish(handle_pose_stamped);
pub_debug_marker_.publish(debug_hand_marker);
pub_debug_marker_array_.publish(make_handle_array(handle_pose_stamped, handle));
jsk_recognition_msgs::SimpleHandle simple_handle;
simple_handle.header = handle_pose_stamped.header;
simple_handle.pose = handle_pose_stamped.pose;
simple_handle.handle_width = min_width;
pub_handle_.publish(simple_handle);
}
int main(int argc, char** argv) {
//check if number of arguments is proper
if(argc!=3){
help();
exit(0);
}
// initialize PointClouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) == -1) //* load the file
{
cerr << "Couldn't read file :" << argv[1] << "\n";
return (-1);
}
std::vector<int> inliers;
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
//RANSAC for Line model
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<PointT>());
if (pcl::console::find_argument(argc, argv, "-l") >= 0) {
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_l(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_LINE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.05);
seg.setInputCloud(cloud);
seg.segment(*inliers_l, *coefficients);
// Write the line inliers to disk
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers_l);
extract.setNegative(false);
extract.filter(*cloud_line);
}
if (pcl::console::find_argument(argc, argv, "-f") >= 0) {
//RANSAC for Plane model
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
ransac.setDistanceThreshold(.01);
ransac.computeModel();
ransac.getInliers(inliers);
} else if (pcl::console::find_argument(argc, argv, "-sf") >= 0) {
//RANSAC for Sphere model
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
ransac.setDistanceThreshold(.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
// creates the visualization object and adds either our orignial cloud or all of the inliers
// depending on the command line arguments specified.
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0) {
viewer = simpleVis(final);
} else if (pcl::console::find_argument(argc, argv, "-l") >= 0) {