void ParticleFilterTracking::initTargetModel(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud,
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &segmented_cloud)
{
std::vector<pcl::PointIndices> cluster_indices;
euclideanSegment (cloud, cluster_indices);
// select the cluster to track
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
extractSegmentCluster (cloud, cluster_indices, 0, *temp_cloud);
Eigen::Vector4f c;
pcl::compute3DCentroid<pcl::PointXYZRGBA> (*temp_cloud, c);
int segment_index = 0;
double segment_distance = c[0] * c[0] + c[1] * c[1];
//choose most near cloud to z axis.
for (size_t i = 1; i < cluster_indices.size (); i++)
{
temp_cloud.reset (new pcl::PointCloud<pcl::PointXYZRGBA>);
extractSegmentCluster (cloud, cluster_indices, int (i), *temp_cloud);
pcl::compute3DCentroid<pcl::PointXYZRGBA> (*temp_cloud, c);
double distance = c[0] * c[0] + c[1] * c[1];
if (distance < segment_distance)
{
segment_index = int (i);
segment_distance = distance;
}
}
extractSegmentCluster (cloud, cluster_indices, segment_index, *segmented_cloud);
}
CloudPtr
get ()
{
//lock while we swap our cloud and reset it.
boost::mutex::scoped_lock lock (mtx_);
CloudPtr temp_cloud (new Cloud);
CloudPtr temp_cloud2 (new Cloud);
grid_.setInputCloud (cloud_);
grid_.filter (*temp_cloud);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
seg_.setInputCloud (temp_cloud);
seg_.segment (*inliers, *coefficients);
// Project the model inliers
proj_.setInputCloud (temp_cloud);
proj_.setModelCoefficients (coefficients);
proj_.filter (*temp_cloud2);
// Create a Convex Hull representation of the projected inliers
chull_.setInputCloud (temp_cloud2);
chull_.reconstruct (*temp_cloud);
return (temp_cloud);
}
CloudPtr
get ()
{
//lock while we swap our cloud and reset it.
boost::mutex::scoped_lock lock (mtx_);
CloudPtr temp_cloud (new Cloud);
grid_.setInputCloud (cloud_);
grid_.filter (*temp_cloud);
return (temp_cloud);
}
void pcdCloud::updateReservedIndices()
{
if(!_removedIndices->empty()){
CloudPtr temp_cloud(new Cloud);
extract(temp_cloud, _removedIndices, _reservedIndices);
if(VERBOSE) std::cout << "update: # _reservedIndices = " << _reservedIndices->size() << std::endl;
}else{
std::cerr << "cannot update _reservedIndices since _removedIndices is empty\n";
exit(1);
}
}
void DenseReconstruction::normalsEstimation(const pcl::PointCloud<pcl::PointXYZLRegionF>::Ptr &cloud,pcl::PointCloud<pcl::Normal>::Ptr &normals){
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::copyPointCloud(*cloud,*temp_cloud);
// Create a KD-Tree
tree_.reset(new pcl::search::KdTree<pcl::PointXYZRGBA>);
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
ne.setInputCloud(temp_cloud);
ne.setSearchMethod(tree_);
ne.setRadiusSearch(0.03);
// ne.compute(*cloud_normals_);
ne.compute(*normals);
}
extern "C" void parse(const rosbag::MessageInstance* m,
madara::knowledge::KnowledgeBase * kb,
std::string container_name)
{
// THIS METHOD IS HARDCODED TO RUN WITH PCL BASED SCHEMAS
boost::shared_ptr<sensor_msgs::PointCloud2> pointcloud =
m->instantiate<sensor_msgs::PointCloud2>();
// Load PCL schema
std::string pointcloud_schema_name = "PointCloudXYZ";
capnp::MallocMessageBuilder buffer;
gams::types::PointCloudXYZ::Builder pointcloud_builder =
buffer.initRoot<gams::types::PointCloudXYZ>();
// Convert from ROS PointCloud2 to a PCL PointCloud
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*pointcloud, pcl_pc2);
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(pcl_pc2, *temp_cloud);
//Fill the list of points with data
auto point_list = pointcloud_builder.initPoints((*temp_cloud).size());
int point_counter = 0;
for (pcl::PointXYZ point : *temp_cloud)
{
point_list[point_counter].setX(point.x);
point_list[point_counter].setY(point.y);
point_list[point_counter].setZ(point.z);
point_counter++;
}
// Now fill the other fields
pointcloud_builder.setTov(
((unsigned long)pointcloud->header.stamp.sec * 1e9) +
(unsigned long)pointcloud->header.stamp.nsec);
pointcloud_builder.setFrameId(pointcloud->header.frame_id.c_str());
pointcloud_builder.setWidth(pointcloud->width);
pointcloud_builder.setHeight(pointcloud->height);
pointcloud_builder.setIsDense((bool)pointcloud->is_dense);
// Store in the knowledgebase
madara::knowledge::GenericCapnObject any(pointcloud_schema_name.c_str(),
buffer);
kb->set_any(container_name, any);
}
int sac_ia_alignment(pcl::PointCloud<pcl::PointXYZ>::Ptr prev_cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>(*prev_cloud));
std::cout << "Performing Sample Consensus Initial Alignment.. "
<< std::endl;
FeatureCloud targetCloud;
FeatureCloud templateCloud;
targetCloud.setInputCloud(temp_cloud);
templateCloud.setInputCloud(cloud_in);
TemplateAlignment templateAlign;
templateAlign.addTemplateCloud(templateCloud);
templateAlign.setTargetCloud(targetCloud);
Result bestAlignment;
std::cout << "entering alignment" << std::endl;
templateAlign.findBestAlignment(bestAlignment);
std::cout << "exiting alignment" << std::endl;
printf("Fitness Score: %f \n", bestAlignment.fitness_score);
Eigen::Matrix3f rotation = bestAlignment.final_transformation.block<3,3>(0, 0);
Eigen::Vector3f translation =
bestAlignment.final_transformation.block<3,1>(0, 3);
printf("\n");
printf(" | %6.3f %6.3f %6.3f | \n", rotation(0, 0), rotation(0, 1),
rotation(0, 2));
printf("R = | %6.3f %6.3f %6.3f | \n", rotation(1, 0), rotation(1, 1),
rotation(1, 2));
printf(" | %6.3f %6.3f %6.3f | \n", rotation(2, 0), rotation(2, 1),
rotation(2, 2));
printf("\n");
printf("t = < %0.3f, %0.3f, %0.3f >\n", translation(0), translation(1),
translation(2));
pcl::transformPointCloud(*templateCloud.getPointCloud(), *cloud_in,
bestAlignment.final_transformation);
std::cout << "***Initial alignment complete***" << std::endl;
}
void display::mapPublisher(const boost::shared_ptr<const sensor_msgs::PointCloud2>& input)
{
pcl::PCLPointCloud2 pcl_pc2;
pcl_conversions::toPCL(*input,pcl_pc2);
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(pcl_pc2,*temp_cloud);
sensor_msgs::PointCloud2 pcl_msg;
pcl::toROSMsg(*temp_cloud,pcl_msg);
const char* MAP_FRAME_ID = "Quad";
pcl_msg.header.frame_id = MAP_FRAME_ID;
pcl_msg.header.stamp = ros::Time::now();
pcl_pub2.publish(input);
}
CloudPtr
get ()
{
//lock while we swap our cloud and reset it.
boost::mutex::scoped_lock lock (mtx_);
CloudPtr temp_cloud (new Cloud);
CloudPtr temp_cloud2 (new Cloud);
grid_.setInputCloud (cloud_);
grid_.filter (*temp_cloud);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
seg_.setInputCloud (temp_cloud);
seg_.segment (*inliers, *coefficients);
extract_.setInputCloud (temp_cloud);
extract_.setIndices (inliers);
extract_.filter (*temp_cloud2);
return (temp_cloud2);
}
int pcDenoise::pcDecompose()
{
if( cloud->empty())
{
std::cout<< "The input point cloud seems to be empty, try invoke setCloud or read it from pcd file.";
return -1;
}
pcType temp_cloud(new pcl::PointCloud<PT>);
int K = 100;
pcl::KdTreeFLANN<PT> kdtree;
kdtree.setInputCloud(cloud);
std::vector<int> Idx(K);
std::vector<float> Dist(K);
double mx=0;
double my=0;
double mz=0;
for( int i=0; i<cloud->width;i++)
{
mx += (cloud->points.at(i).x);
my += (cloud->points.at(i).y);
mz += (cloud->points.at(i).z);
}
mx/=cloud->width;
my/=cloud->width;
mz/=cloud->width;
for( int i=0; i<cloud->width;i++)
{
kdtree.nearestKSearch (i, K, Idx, Dist);
int neighbourIdx = 0;
double xx=0;
double yy=0;
double zz=0;
for( int j=0; j<K; j++)
{
neighbourIdx = Idx[j];
xx += (cloud->points.at(neighbourIdx).x);
yy += (cloud->points.at(neighbourIdx).y);
zz += (cloud->points.at(neighbourIdx).z);
}
xx/=K;
yy/=K;
zz/=K;
//xx-=cloud->points.at(i).x;
//yy-=cloud->points.at(i).y;
//zz-=cloud->points.at(i).z;
xx-=mx;
yy-=my;
zz-=mz;
PT* temp = new PT();
temp->x = xx;
temp->y = yy;
temp->z = zz;
temp->r = (cloud->points.at(i).r);
temp->g = (cloud->points.at(i).g);
temp->b = (cloud->points.at(i).b);
temp_cloud->push_back( *temp );
}
cloud->clear();
pcl::copyPointCloud(*temp_cloud,*cloud);
return 0;
}
int
main (int argc, char ** argv)
{
if (argc < 2)
{
pcl::console::print_info ("Syntax is: %s input.pcd <options>\n", argv[0]);
pcl::console::print_info (" where options are:\n");
pcl::console::print_info (" -p dist_threshold,max_iters ..... Subtract the dominant plane\n");
pcl::console::print_info (" -c tolerance,min_size,max_size ... Cluster points\n");
pcl::console::print_info (" -s output.pcd .................... Save the largest cluster\n");
return (1);
}
// Load the input file
PointCloudPtr cloud (new PointCloud);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::console::print_info ("Loaded %s (%zu points)\n", argv[1], cloud->size ());
// Subtract the dominant plane
double dist_threshold, max_iters;
bool subtract_plane = pcl::console::parse_2x_arguments (argc, argv, "-p", dist_threshold, max_iters) > 0;
if (subtract_plane)
{
size_t n = cloud->size ();
cloud = findAndSubtractPlane (cloud, dist_threshold, (int)max_iters);
pcl::console::print_info ("Subtracted %zu points along the detected plane\n", n - cloud->size ());
}
// Cluster points
double tolerance, min_size, max_size;
std::vector<pcl::PointIndices> cluster_indices;
bool cluster_points = pcl::console::parse_3x_arguments (argc, argv, "-c", tolerance, min_size, max_size) > 0;
if (cluster_points)
{
clusterObjects (cloud, tolerance, (int)min_size, (int)max_size, cluster_indices);
pcl::console::print_info ("Found %zu clusters\n", cluster_indices.size ());
}
// Save output
std::string output_filename;
bool save_cloud = pcl::console::parse_argument (argc, argv, "-s", output_filename) > 0;
if (save_cloud)
{
// If clustering was performed, save only the first (i.e., largest) cluster
if (cluster_points)
{
PointCloudPtr temp_cloud (new PointCloud);
pcl::copyPointCloud (*cloud, cluster_indices[0], *temp_cloud);
cloud = temp_cloud;
}
pcl::console::print_info ("Saving result as %s...\n", output_filename.c_str ());
pcl::io::savePCDFile (output_filename, *cloud);
}
// Or visualize the result
else
{
pcl::console::print_info ("Starting visualizer... Close window to exit.\n");
pcl::visualization::PCLVisualizer vis;
// If clustering was performed, display each cluster with a random color
if (cluster_points)
{
for (size_t i = 0; i < cluster_indices.size (); ++i)
{
// Extract the i_th cluster into a new cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_i (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud (*cloud, cluster_indices[i], *cluster_i);
// Create a random color
pcl::visualization::PointCloudColorHandlerRandom<pcl::PointXYZ> random_color (cluster_i);
// Create a unique identifier
std::stringstream cluster_id ("cluster");
cluster_id << i;
// Add the i_th cluster to the visualizer with a random color and a unique identifier
vis.addPointCloud<pcl::PointXYZ> (cluster_i, random_color, cluster_id.str ());
}
}
else
{
// If clustering wasn't performed, just display the cloud
vis.addPointCloud (cloud);
}
vis.resetCamera ();
vis.spin ();
}
return (0);
}
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr& input)
{
sensor_msgs::PointCloud2 result_cloud_msg;
pcl::PointCloud<PointInT>::Ptr cloud (new pcl::PointCloud<PointInT>);
pcl::fromROSMsg(*input, *cloud);
// Perform downsamplign via VoxelGrid
pcl::VoxelGrid<PointInT> voxel_grid;
voxel_grid.setInputCloud(cloud);
voxel_grid.setLeafSize(grid_size, grid_size, grid_size);
PointInTPtr temp_cloud (new pcl::PointCloud<PointInT> ());
voxel_grid.filter(*temp_cloud);
cloud = temp_cloud;
// Filtering out noise
pcl::StatisticalOutlierRemoval<PointInT> sor;
sor.setInputCloud (cloud);
sor.setMeanK (sor_mean_k); // 50 - default
sor.setStddevMulThresh (sor_stddev_mul_th);
sor.filter (*cloud);
octree->setInputCloud (cloud);
octree->addPointsFromInputCloud ();
std::vector<int> newPointIdxVector;
octree->getPointIndicesFromNewVoxels (newPointIdxVector, noise_filter);
if(!newPointIdxVector.size())
{
ROS_INFO("Person wasn't detected\n");
return;
}
pcl::PointCloud<PointInT>::Ptr person_cloud (new pcl::PointCloud<PointInT>);
pcl::copyPointCloud(*cloud, newPointIdxVector, *person_cloud);
ROS_INFO("Person cloud has %d points", (int)person_cloud->points.size());
pcl::RadiusOutlierRemoval<PointInT> outrem;
outrem.setInputCloud(person_cloud);
outrem.setRadiusSearch(ror_rad);
outrem.setMinNeighborsInRadius (ror_min_heighbors);
outrem.filter (*person_cloud);
// Reject too small cloud
if(person_cloud->points.size() < min_size_thresh)
{
ROS_INFO("Person wasn't detected\n");
return;
}
pcl::toROSMsg(*person_cloud, result_cloud_msg);
result_cloud_msg.header.frame_id = input->header.frame_id;
result_pub.publish(result_cloud_msg);
// Get pose of person and publish results
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*person_cloud, centroid);
ROS_INFO("Person pose: %.3f, %.3f, %.3f", centroid[0], centroid[1], centroid[2]);
geometry_msgs::Pose pose;
pose.position.x = centroid[0];
pose.position.y = centroid[1];
pose.position.z = centroid[2];
pose.orientation.w = 0;
pose.orientation.x = 0;
pose.orientation.y = 0;
pose.orientation.z = 0;
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = input->header.frame_id;
pose_msg.pose = pose;
pose_pub.publish(pose_msg);
// Calculate bounding box
float x_len, y_len, z_len; // lenghts of the bounding box
Eigen::Vector4f min, max;
pcl::getMinMax3D(*person_cloud, min, max);
x_len = max[0] - min[0];
y_len = max[1] - min[1];
z_len = max[2] - min[2];
shape_msgs::SolidPrimitive bbox;
bbox.type = bbox.BOX;
bbox.dimensions.resize(3);
bbox.dimensions[0] = x_len;
bbox.dimensions[1] = y_len;
bbox.dimensions[2] = z_len;
velodyne_track_person::PersonInstance person_instance;
person_instance.pose = pose;
person_instance.bbox = bbox;
person_instance.header.frame_id = input->header.frame_id;
person_instance.header.stamp = ros::Time::now();
person_pub.publish(person_instance);
ROS_INFO("Person pose was published\n");
// Switch octree buffers: This resets octree but keeps previous tree structure in memory.
octree->switchBuffers ();
}
CloudPtr
get ()
{
//lock while we swap our cloud and reset it.
boost::mutex::scoped_lock lock (mtx_);
CloudPtr temp_cloud (new Cloud);
CloudPtr temp_cloud2 (new Cloud);
CloudPtr temp_cloud3 (new Cloud);
CloudPtr temp_cloud4 (new Cloud);
CloudPtr temp_cloud5 (new Cloud);
CloudConstPtr empty_cloud;
cout << "===============================\n"
"======Start of frame===========\n"
"===============================\n";
//cerr << "cloud size orig: " << cloud_->size() << endl;
voxel_grid.setInputCloud (cloud_);
voxel_grid.filter (*temp_cloud); // filter cloud for z depth
//cerr << "cloud size postzfilter: " << temp_cloud->size() << endl;
pcl::ModelCoefficients::Ptr planecoefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices ());
std::vector<pcl::ModelCoefficients> linecoefficients1;
pcl::ModelCoefficients model;
model.values.resize (6);
pcl::PointIndices::Ptr line_inliers (new pcl::PointIndices ());
std::vector<Eigen::Vector3f> corners;
if(temp_cloud->size() > MIN_CLOUD_POINTS) {
plane_seg.setInputCloud (temp_cloud);
plane_seg.segment (*plane_inliers, *planecoefficients); // find plane
}
//cerr << "plane inliers size: " << plane_inliers->indices.size() << endl;
cout << "planecoeffs: "
<< planecoefficients->values[0] << " "
<< planecoefficients->values[1] << " "
<< planecoefficients->values[2] << " "
<< planecoefficients->values[3] << " "
<< endl;
Eigen::Vector3f pn = Eigen::Vector3f(
planecoefficients->values[0],
planecoefficients->values[1],
planecoefficients->values[2]);
float planedeg = pcl::rad2deg(acos(pn.dot(Eigen::Vector3f::UnitZ())));
cout << "angle of camera to floor normal: " << planedeg << " degrees" << endl;
cout << "distance of camera to floor: " << planecoefficients->values[3]
<< " meters" << endl;
plane_extract.setNegative (true);
plane_extract.setInputCloud (temp_cloud);
plane_extract.setIndices (plane_inliers);
plane_extract.filter (*temp_cloud2); // remove plane
plane_extract.setNegative (false);
plane_extract.filter (*temp_cloud5); // only plane
for(size_t i = 0; i < temp_cloud5->size (); ++i)
{
temp_cloud5->points[i].r = 0;
temp_cloud5->points[i].g = 255;
temp_cloud5->points[i].b = 0;
// tint found plane green for ground
}
for(size_t j = 0 ; j < MAX_LINES && temp_cloud2->size() > MIN_CLOUD_POINTS; j++)
// look for x lines until cloud gets too small
{
// cerr << "cloud size: " << temp_cloud2->size() << endl;
line_seg.setInputCloud (temp_cloud2);
line_seg.segment (*line_inliers, model); // find line
// cerr << "line inliears size: " << line_inliers->indices.size() << endl;
if(line_inliers->indices.size() < MIN_CLOUD_POINTS)
break;
linecoefficients1.push_back (model); // store line coeffs
line_extract.setNegative (true);
line_extract.setInputCloud (temp_cloud2);
line_extract.setIndices (line_inliers);
line_extract.filter (*temp_cloud3); // remove plane
line_extract.setNegative (false);
line_extract.filter (*temp_cloud4); // only plane
for(size_t i = 0; i < temp_cloud4->size (); ++i) {
temp_cloud4->points[i].g = 0;
if(j%2) {
temp_cloud4->points[i].r = 255-j*int(255/MAX_LINES);
temp_cloud4->points[i].b = 0+j*int(255/MAX_LINES);
} else {
temp_cloud4->points[i].b = 255-j*int(255/MAX_LINES);
temp_cloud4->points[i].r = 0+j*int(255/MAX_LINES);
}
}
*temp_cloud5 += *temp_cloud4; // add line to ground
temp_cloud2.swap ( temp_cloud3); // remove line
}
cout << "found " << linecoefficients1.size() << " lines." << endl;
for(size_t i = 0; i < linecoefficients1.size(); i++)
{
//cerr << "linecoeffs: " << i << " "
// << linecoefficients1[i].values[0] << " "
// << linecoefficients1[i].values[1] << " "
// << linecoefficients1[i].values[2] << " "
// << linecoefficients1[i].values[3] << " "
// << linecoefficients1[i].values[4] << " "
// << linecoefficients1[i].values[5] << " "
// << endl;
Eigen::Vector3f lv = Eigen::Vector3f(
linecoefficients1[i].values[3],
linecoefficients1[i].values[4],
linecoefficients1[i].values[5]);
Eigen::Vector3f lp = Eigen::Vector3f(
linecoefficients1[i].values[0],
linecoefficients1[i].values[1],
linecoefficients1[i].values[2]);
float r = pn.dot(lv);
float deg = pcl::rad2deg(acos(r));
cout << "angle of line to floor normal: " << deg << " degrees" << endl;
if(abs(deg-30) < 5 || abs(deg-150) < 5)
{
cout << "found corner line" << endl;
float t = -(lp.dot(pn) + planecoefficients->values[3])/ r;
Eigen::Vector3f intersect = lp + lv*t;
cout << "corner intersects floor at: " << endl << intersect << endl;
cout << "straight line distance from camera to corner: " <<
intersect.norm() << " meters" << endl;
corners.push_back(intersect);
Eigen::Vector3f floor_distance = intersect + pn; // should be - ???
cout << "distance along floor to corner: " <<
floor_distance.norm() << " meters" << endl;
}
else if(abs(deg-90) < 5)
{
cout << "found horizontal line" << endl;
}
}
switch(corners.size())
{
case 2:
cout << "distance between corners " << (corners[0] - corners[1]).norm() << endl;
cout << "angle of pyramid to camera " <<
pcl::rad2deg(acos(((corners[0] - corners[1]).normalized()).dot(Eigen::Vector3f::UnitX())))
<< endl;
break;
case 3:
cout << "distance between corners " << (corners[0] - corners[1]).norm() << endl;
cout << "distance between corners " << (corners[0] - corners[2]).norm() << endl;
cout << "distance between corners " << (corners[1] - corners[2]).norm() << endl;
cout << "angle of corner on floor (should be 90) " <<
pcl::rad2deg(acos((corners[0] - corners[1]).dot(corners[0] - corners [2])))
<< endl;
cout << "angle of pyramid to camera " <<
pcl::rad2deg(acos(((corners[0] - corners[1]).normalized()).dot(Eigen::Vector3f::UnitX())))
<< endl;
break;
}
if (saveCloud)
{
std::stringstream stream, stream1;
std::string filename;
stream << "inputCloud" << filesSaved << ".pcd";
filename = stream.str();
if (pcl::io::savePCDFile(filename, *cloud_, true) == 0)
{
filesSaved++;
cout << "Saved " << filename << "." << endl;
}
else PCL_ERROR("Problem saving %s.\n", filename.c_str());
stream1 << "inputCloud" << filesSaved << ".pcd";
filename = stream1.str();
if (pcl::io::savePCDFile(filename, *temp_cloud5, true) == 0)
{
filesSaved++;
cout << "Saved " << filename << "." << endl;
}
else PCL_ERROR("Problem saving %s.\n", filename.c_str());
saveCloud = false;
}
empty_cloud.swap(cloud_); // set cloud_ to null
if(toggleView == 1)
return (temp_cloud); // return orig cloud
else
return (temp_cloud5); // return colored cloud
}
void
cloud_cb_white (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
// Container for original & filtered data
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_segment (new pcl::PointCloud<pcl::PointXYZ>);
// Convert to PCL data type
pcl_conversions::toPCL(*cloud_msg, *cloud);
// Perform downsampling
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloudPtr);
sor.setLeafSize (0.01, 0.01, 0.01);
sor.filter (cloud_filtered);
// convert PointCloud2 to pcl::PointCloud<pcl::PointXYZ>
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(cloud_filtered,*temp_cloud);
// segmenting area
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud (temp_cloud);
// left - right
pass.setFilterFieldName ("x");
pass.setFilterLimits (-3, 3);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
pass.setInputCloud (cloud_segment);
// up - down
pass.setFilterFieldName ("y");
pass.setFilterLimits (-3, 3);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
pass.setInputCloud (cloud_segment);
// backward - forward
pass.setFilterFieldName ("z");
pass.setFilterLimits (0.5, 4);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_segment);
// // Find centroid
float sum_x = 0;
float sum_y = 0;
float sum_z = 0;
if(cloud_segment->points.size() > 50) {
for(size_t i = 0; i < cloud_segment->points.size(); ++i) {
sum_x += cloud_segment->points[i].x;
sum_y += cloud_segment->points[i].y;
sum_z += cloud_segment->points[i].z;
}
sum_x = sum_x / cloud_segment->points.size();
sum_y = sum_y / cloud_segment->points.size();
sum_z = sum_z / cloud_segment->points.size();
geometry_msgs::PoseStamped sPose;
sPose.header.stamp = ros::Time::now();
sPose.pose.position.x = sum_z; // equals to z in pcl // backward - forward
sPose.pose.position.y = -(sum_x); // equals to -x in pcl // right - left
sPose.pose.position.z = -(sum_y); // equals to -y in pcl // down - up
sPose.pose.orientation.x = 0.0;
sPose.pose.orientation.y = 0.0;
sPose.pose.orientation.z = 0.0;
sPose.pose.orientation.w = 1.0;
cock_pose_white_pub.publish(sPose);
}
// Convert to ROS data type
sensor_msgs::PointCloud2 output;
//pcl_conversions::fromPCL(cloud_filtered, output);
pcl::toROSMsg(*cloud_segment, output);
// Publish the data
cloud_pub_white.publish (output);
}
