void Planar_filter::Downsample_cloud(std::string input)
{
if(input == "Normals only"){
_vgn.setInputCloud (_cloud_with_normals);
_vgn.setLeafSize (0.01f,0.01f,0.01f);
PointNormalCloudT::Ptr filtered_cloud(new PointNormalCloudT);
_vgn.filter (*filtered_cloud);
*_cloud_with_normals = *filtered_cloud;
}else{
_vgcn.setInputCloud (_cloud_color_normals);
_vgcn.setLeafSize (0.01f,0.01f,0.01f);
PointColorNormalCloudT::Ptr filtered_cloud(new PointColorNormalCloudT);
_vgcn.filter (*filtered_cloud);
*_cloud_color_normals = *filtered_cloud;
}
//std::cout<<"cloud with normals has been downsampled " <<std::endl;
}
// Create the filtering object: downsample the dataset using a leaf size of 1cm
void Planar_filter::Downsample_cloud()
{
_vg.setInputCloud (_cloud);
_vg.setLeafSize (0.01f,0.01f,0.01f);
PointCloudT::Ptr filtered_cloud(new PointCloudT);
_vg.filter (*filtered_cloud);
*_cloud = *filtered_cloud;
//std::cout << "PointCloud after filtering has: " << _cloud->points.size () << " data points." << std::endl; //*
}
void Recognizer::setInputCloud(PointNormalCloudT::Ptr cloud,double leaf_size){
_vgn.setInputCloud (cloud);
_vgn.setLeafSize (leaf_size,leaf_size,leaf_size);
PointNormalCloudT::Ptr filtered_cloud(new PointNormalCloudT);
_vgn.filter (*filtered_cloud);
*cloud = *filtered_cloud;
pcl::copyPointCloud(*cloud,*_cloud);
pcl::copyPointCloud(*cloud,*_normals);
pcl::copyPointCloud(*cloud,*_cloud_with_normals);
}
template<typename PointInT> void
pcl::TextureMapping<PointInT>::removeOccludedPoints (const pcl::TextureMesh &tex_mesh, pcl::TextureMesh &cleaned_mesh, const double octree_voxel_size)
{
// copy mesh
cleaned_mesh = tex_mesh;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// load points into a PCL format
pcl::fromPCLPointCloud2 (tex_mesh.cloud, *cloud);
std::vector<int> visible, occluded;
removeOccludedPoints (cloud, filtered_cloud, octree_voxel_size, visible, occluded);
// Now that we know which points are visible, let's iterate over each face.
// if the face has one invisible point => out!
for (size_t polygons = 0; polygons < cleaned_mesh.tex_polygons.size (); ++polygons)
{
// remove all faces from cleaned mesh
cleaned_mesh.tex_polygons[polygons].clear ();
// iterate over faces
for (size_t faces = 0; faces < tex_mesh.tex_polygons[polygons].size (); ++faces)
{
// check if all the face's points are visible
bool faceIsVisible = true;
std::vector<int>::iterator it;
// iterate over face's vertex
for (size_t points = 0; points < tex_mesh.tex_polygons[polygons][faces].vertices.size (); ++points)
{
it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[polygons][faces].vertices[points]);
if (it == occluded.end ())
{
// point is not in the occluded vector
// PCL_INFO (" VISIBLE!\n");
}
else
{
// point was occluded
// PCL_INFO(" OCCLUDED!\n");
faceIsVisible = false;
}
}
if (faceIsVisible)
{
cleaned_mesh.tex_polygons[polygons].push_back (tex_mesh.tex_polygons[polygons][faces]);
}
}
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr
zFilter (pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud, float min_ratio, float max_ratio)
{
std::vector<float> z_range = findZRange(input_cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(input_cloud);
pass.setFilterFieldName("z");
pass.setFilterLimits(min_ratio * z_range[0], max_ratio * z_range[1]);
pass.filter(*filtered_cloud);
return filtered_cloud;
}
void recons(std::string infile, std::string outfile) {
sensor_msgs::PointCloud2 cloud_blob;
// run this from BodyScanner/build/surface_reconstructor/
pcl::io::loadPCDFile(infile, cloud_blob);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal> ());
pcl::fromROSMsg (cloud_blob, *cloud);
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointNormal>());
int l = 0;
for(int j = 0; j < 480; j += 1) {
for(int i = j%2; i < 640; i += 2) {
int k = i + j*640;
pcl::PointNormal& p = (*cloud)[k];
if((i&1 && j&1) // keep 1/4 of the points
&& mag(p) < 3) { // keep only points within a close distance to the origin
filtered_cloud->push_back(p);
}
}
}
// Create search tree*
pcl::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::KdTreeFLANN<pcl::PointNormal>);
tree2->setInputCloud(filtered_cloud);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (0.025);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (100);
setMaximumSurfaceAngle(gp3, 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(filtered_cloud);
gp3.setSearchMethod(tree2);
gp3.reconstruct(triangles);
//pcl::io::savePLYFile(outfile, triangles);
pcl::io::saveVTKFile(outfile, triangles);
}
/* ========================================
* Fill Code: SERVICE
* ========================================*/
bool filterCallback(lesson_perception::FilterCloud::Request& request,
lesson_perception::FilterCloud::Response& response)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (request.pcdfilename.empty())
{
pcl::fromROSMsg(request.input_cloud, *cloud);
ROS_INFO_STREAM("cloud size: " <<cloud->size());
if (cloud->empty())
{
ROS_ERROR("input cloud empty");
response.success = false;
return false;
}
}
else
{
pcl::io::loadPCDFile(request.pcdfilename, *cloud);
}
switch (request.operation)
{
case lesson_perception::FilterCloud::Request::VOXELGRID :
{
filtered_cloud = voxelGrid(cloud, 0.01);
break;
}
default :
{
ROS_ERROR("no point cloud found");
return false;
}
}
/*
* SETUP RESPONSE
*/
pcl::toROSMsg(*filtered_cloud, response.output_cloud);
response.output_cloud.header=request.input_cloud.header;
response.output_cloud.header.frame_id="kinect_link";
response.success = true;
return true;
}
void processPointCloud(const cloud_t::Ptr &cloud, std::vector<point_t> ¢roids)
{
if (cloud->points.size() < 10) {
return;
}
// Downsample using Voxel Grid
cloud_t::Ptr filtered_cloud(new cloud_t);
downsample(cloud, filtered_cloud);
// Calculate normals
pcl::search::Search<point_t>::Ptr tree = boost::shared_ptr<pcl::search::Search<point_t> > (new pcl::search::KdTree<point_t>);
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::NormalEstimation<point_t, pcl::Normal> normal_estimator;
normal_estimator.setSearchMethod (tree);
normal_estimator.setInputCloud (filtered_cloud);
normal_estimator.setKSearch (20);
normal_estimator.compute (*normals);
// Region growing
pcl::RegionGrowing<point_t, pcl::Normal> reg;
//Maximum and minimum number of points to classify as a cluster
// First check: to see if object is large (or takes up a large portion of the laser's view)
reg.setMinClusterSize(40);
reg.setMaxClusterSize(1000000);
reg.setSearchMethod(tree);
// Number of neighbors to search
reg.setNumberOfNeighbours(35);
reg.setInputCloud(filtered_cloud);
reg.setInputNormals(normals);
reg.setSmoothnessThreshold(6.0 / 180.0 * M_PI);
reg.setCurvatureThreshold(.15);
std::vector<pcl::PointIndices> clusters;
reg.extract(clusters);
// Update colored cloud
colored_cloud = reg.getColoredCloud();
// Use clusters to find door(s)
findDoorCentroids(filtered_cloud, clusters, centroids);
}
void viz(std::string fname) {
sensor_msgs::PointCloud2 cloud_blob;
// run this from BodyScanner/build/surface_reconstructor/
pcl::io::loadPCDFile(fname, cloud_blob);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal> ());
pcl::fromROSMsg (cloud_blob, *cloud);
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointNormal>());
int l = 0;
for(int j = 0; j < 480; j += 1) {
for(int i = j%2; i < 640; i += 2) {
int k = i + j*640;
pcl::PointNormal& p = (*cloud)[k];
if((i&1 && j&1) // keep 1/4 of the points
&& mag(p) < 3) { // keep only points within a close distance to the origin
filtered_cloud->push_back(p);
}
}
}
// potentially fancier downsampling
/*
pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
sor.setInputCloud (cloud_blob);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered_blob);
*/
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = normalsVis(filtered_cloud);
viewer->registerKeyboardCallback(&handleKeyEvent, NULL);
pcl::PointXYZ o;
o.x = 1.0;
o.y = 0;
o.z = 0;
viewer->addSphere (o, 0.25, "sphere", 0);
while (!viewer->wasStopped ()) {
viewer->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
}
void ObjectRecognition::pipeline(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud) {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr table_hull(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr objects_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
boost::shared_ptr<std::vector<Object>> objects;
filtered_cloud = this->object_pipeline->passthroughPointCloud(input_cloud);
if (nullptr == filtered_cloud) {
return;
}
table_hull = this->object_pipeline->findTableHull(filtered_cloud);
if (nullptr == table_hull) {
return;
}
objects_cloud = this->object_pipeline->createObjectCloud(filtered_cloud, table_hull);
if (nullptr == objects_cloud) {
return;
}
objects = this->object_pipeline->createObjectClusters(objects_cloud);
this->object_pipeline->keyPointExtraction(objects);
this->object_pipeline->createObjectDescriptors(objects);
this->object_pipeline->objectMatching(objects);
this->publish(objects);
}
void FacadeGeography::ComputeFeature(pcl::PointCloud<pcl::PointXYZ>::Ptr facade_cloud) {
// First down sample the dense cloud
std::cout << "down sampling ... \n";
pcl::VoxelGrid<pcl::PointXYZ> filter;
filter.setInputCloud(facade_cloud);
filter.setLeafSize(0.1f, 0.1f, 0.1f);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
filter.filter(*filtered_cloud);
std::cout << "down sampling done. After filter, " << filtered_cloud->width * filtered_cloud->height <<
" points left.\n";
//
pcl::search::KdTree<pcl::PointXYZ> search;
pcl::PointXYZ cloud_min, cloud_max;
pcl::getMinMax3D(*filtered_cloud, cloud_min, cloud_max);
boost::progress_display display(height_ + width_);
Eigen::Vector4f min_pt, max_pt;
for (int i = 0; i < width_; ++i) // for each width, compute the average z value
{
min_pt(0) = extents_min_.x + i * resolution_;
min_pt(1) = extents_min_.y;
min_pt(2) = cloud_min.x;
min_pt(3) = 1.0;
max_pt(0) = min_pt(0) + resolution_;
max_pt(1) = min_pt(1) + resolution_ * height_;
max_pt(2) = cloud_max.x;
max_pt(3) = 1.0;
std::vector<int> indices;
pcl::getPointsInBox(*filtered_cloud, min_pt, max_pt, indices);
assert(indices.size() >= 0);
double sum = 0.0;
for (int k = 0; k < indices.size(); ++k)
{
sum += filtered_cloud->points[indices[k]].z;
}
sum /= indices.size();
horizonal_feature.push_back(sum);
++display;
}
for (int i = 0; i < height_; ++i) // for each height, compute the average z value
{
min_pt(0) = extents_min_.x;
min_pt(1) = extents_min_.y + i * resolution_;
min_pt(2) = cloud_min.x;
min_pt(3) = 1.0;
max_pt(0) = min_pt(0) + resolution_ * width_;
max_pt(1) = min_pt(1) + resolution_;
max_pt(2) = cloud_max.x;
max_pt(3) = 1.0;
std::vector<int> indices;
pcl::getPointsInBox(*filtered_cloud, min_pt, max_pt, indices);
assert(indices.size() >= 0);
double sum = 0.0;
for (int k = 0; k < indices.size(); ++k)
{
sum += filtered_cloud->points[indices[k]].z;
}
sum /= indices.size();
vertical_feature.push_back(sum);
++display;
}
}
template<typename PointInT> int
pcl::TextureMapping<PointInT>::sortFacesByCamera (pcl::TextureMesh &tex_mesh, pcl::TextureMesh &sorted_mesh,
const pcl::texture_mapping::CameraVector &cameras, const double octree_voxel_size,
PointCloud &visible_pts)
{
if (tex_mesh.tex_polygons.size () != 1)
{
PCL_ERROR ("The mesh must contain only 1 sub-mesh!\n");
return (-1);
}
if (cameras.size () == 0)
{
PCL_ERROR ("Must provide at least one camera info!\n");
return (-1);
}
// copy mesh
sorted_mesh = tex_mesh;
// clear polygons from cleaned_mesh
sorted_mesh.tex_polygons.clear ();
pcl::PointCloud<pcl::PointXYZ>::Ptr original_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
// load points into a PCL format
pcl::fromPCLPointCloud2 (tex_mesh.cloud, *original_cloud);
// for each camera
for (size_t cam = 0; cam < cameras.size (); ++cam)
{
// get camera pose as transform
Eigen::Affine3f cam_trans = cameras[cam].pose;
// transform original cloud in camera coordinates
pcl::transformPointCloud (*original_cloud, *transformed_cloud, cam_trans.inverse ());
// find occlusions on transformed cloud
std::vector<int> visible, occluded;
removeOccludedPoints (transformed_cloud, filtered_cloud, octree_voxel_size, visible, occluded);
visible_pts = *filtered_cloud;
// find visible faces => add them to polygon N for camera N
// add polygon group for current camera in clean
std::vector<pcl::Vertices> visibleFaces_currentCam;
// iterate over the faces of the current mesh
for (size_t faces = 0; faces < tex_mesh.tex_polygons[0].size (); ++faces)
{
// check if all the face's points are visible
bool faceIsVisible = true;
std::vector<int>::iterator it;
// iterate over face's vertex
for (size_t current_pt_indice = 0; faceIsVisible && current_pt_indice < tex_mesh.tex_polygons[0][faces].vertices.size (); ++current_pt_indice)
{
// TODO this is far too long! Better create an helper function that raycasts here.
it = find (occluded.begin (), occluded.end (), tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]);
if (it == occluded.end ())
{
// point is not occluded
// does it land on the camera's image plane?
pcl::PointXYZ pt = transformed_cloud->points[tex_mesh.tex_polygons[0][faces].vertices[current_pt_indice]];
Eigen::Vector2f dummy_UV;
if (!getPointUVCoordinates (pt, cameras[cam], dummy_UV))
{
// point is not visible by the camera
faceIsVisible = false;
}
}
else
{
faceIsVisible = false;
}
}
if (faceIsVisible)
{
// push current visible face into the sorted mesh
visibleFaces_currentCam.push_back (tex_mesh.tex_polygons[0][faces]);
// remove it from the unsorted mesh
tex_mesh.tex_polygons[0].erase (tex_mesh.tex_polygons[0].begin () + faces);
faces--;
}
}
sorted_mesh.tex_polygons.push_back (visibleFaces_currentCam);
}
// we should only have occluded and non-visible faces left in tex_mesh.tex_polygons[0]
// we need to add them as an extra polygon in the sorted mesh
sorted_mesh.tex_polygons.push_back (tex_mesh.tex_polygons[0]);
return (0);
}
void callback(const sensor_msgs::PointCloud2 &pc) {
double t1, t2, avg_t;
ROS_INFO("Received point cloud! Applying method %d",method_id);
pcl::PointCloud<PointT>::Ptr scene_pc(new pcl::PointCloud<PointT>());
pcl::PCLPointCloud2 pcl_pc;
pcl_conversions::toPCL(pc, pcl_pc);
pcl::fromPCLPointCloud2(pcl_pc, *scene_pc);
if( scene_pc->empty() == true ) {
ROS_ERROR("Recieved empty point cloud message!");
return;
}
pcl::PointCloud<myPointXYZ>::Ptr scene_xyz(new pcl::PointCloud<myPointXYZ>());
pcl::copyPointCloud(*scene_pc, *scene_xyz);
if( view_flag == true )
{
viewer->removeAllPointClouds();
viewer->addPointCloud(scene_xyz, "whole_scene");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.5, 0.5, 0.5, "whole_scene");
}
std::vector<poseT> all_poses;
std::vector<poseT> link_poses, node_poses;
std::cout << " ... processing:" << std::endl;
switch(method_id)
{
case 0:
t1 = get_wall_time();
objrec->StandardRecognize(scene_xyz, all_poses);
t2 = get_wall_time();
break;
case 1:
{
t1 = get_wall_time();
int pose_num = 0;
int iter = 0;
pcl::PointCloud<myPointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<myPointXYZ>());
filtered_cloud = scene_xyz;
while(true)
{
//std::cerr<< "Recognizing Attempt --- " << iter << std::endl;
objrec->StandardRecognize(filtered_cloud, all_poses);
if( all_poses.size() <= pose_num )
break;
else
pose_num = all_poses.size();
pcl::PointCloud<myPointXYZ>::Ptr model_cloud = objrec->FillModelCloud(all_poses);
filtered_cloud = FilterCloud(filtered_cloud, model_cloud);
iter++;
}
t2 = get_wall_time();
//std::cerr<< "Recognizing Done!!!" << std::endl;
break;
}
case 2:
{
//std::vector<poseT> tmp_poses;
t1 = get_wall_time();
objrec->GreedyRecognize(scene_xyz, all_poses);
t2 = get_wall_time();
//all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses);
break;
}
case 3:
{
pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>());
pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>());
splitCloud(scene_pc, link_cloud, node_cloud);
t1 = get_wall_time();
std::cout << " ... starting at " << t1 << std::endl;
//std::vector<poseT> link_poses, node_poses;
linkrec->StandardRecognize(link_cloud, link_poses);
noderec->StandardRecognize(node_cloud, node_poses);
t2 = get_wall_time();
std::cout << " ... done at " << t2 << std::endl;
all_poses.insert(all_poses.end(), link_poses.begin(), link_poses.end());
all_poses.insert(all_poses.end(), node_poses.begin(), node_poses.end());
break;
}
case 4:
{
pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>());
pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>());
splitCloud(scene_pc, link_cloud, node_cloud);
t1 = get_wall_time();
std::cout << " ... starting at " << t1 << std::endl;
int pose_num = 0;
std::vector<poseT> tmp_poses;
int iter = 0;
while(true)
{
//std::cerr<< "Recognizing Attempt --- " << iter << std::endl;
list<AcceptedHypothesis> acc_hypotheses;
std::cout << " ... generating" << std::endl;
linkrec->genHypotheses(link_cloud, acc_hypotheses);
noderec->genHypotheses(node_cloud, acc_hypotheses);
std::cout << " ... merging" << std::endl;
linkrec->mergeHypotheses(scene_xyz, acc_hypotheses, tmp_poses);
if( tmp_poses.size() <= pose_num ) {
break;
} else {
pose_num = tmp_poses.size();
std::cout << "Number of merged hypotheses: " << tmp_poses.size() << std::endl;
}
pcl::PointCloud<myPointXYZ>::Ptr link_model = linkrec->FillModelCloud(tmp_poses);
pcl::PointCloud<myPointXYZ>::Ptr node_model = noderec->FillModelCloud(tmp_poses);
std::cout << " ... filtering" << std::endl;
if( link_model->empty() == false ) {
link_cloud = FilterCloud(link_cloud, link_model);
}
if( node_model->empty() == false) {
node_cloud = FilterCloud(node_cloud, node_model);
}
iter++;
}
t2 = get_wall_time();
std::cout << " ... done at " << t2 << std::endl;
//std::cerr<< "Recognizing Done!!!" << std::endl;
all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses);
break;
}
case 5:
{
pcl::PointCloud<myPointXYZ>::Ptr link_cloud(new pcl::PointCloud<myPointXYZ>());
pcl::PointCloud<myPointXYZ>::Ptr node_cloud(new pcl::PointCloud<myPointXYZ>());
splitCloud(scene_pc, link_cloud, node_cloud);
t1 = get_wall_time();
linkrec->GreedyRecognize(link_cloud, link_poses);
noderec->GreedyRecognize(node_cloud, node_poses);
t2 = get_wall_time();
std::vector<poseT> tmp_poses;
tmp_poses.insert(tmp_poses.end(), link_poses.begin(), link_poses.end());
tmp_poses.insert(tmp_poses.end(), node_poses.begin(), node_poses.end());
//all_poses = tmp_poses;
all_poses = RefinePoses(scene_xyz, mesh_set, tmp_poses);
break;
}
default:
ROS_ERROR("Code %d not recognized!",method_id);
return;
}
if( view_flag == true )
{
switch(method_id)
{
case 0:
case 1:
case 2:
objrec->visualize(viewer, all_poses);
break;
case 3:
case 4:
case 5:
linkrec->visualize(viewer, all_poses);
noderec->visualize(viewer, all_poses);
break;
}
viewer->spin();
}
geometry_msgs::PoseArray links;
geometry_msgs::PoseArray nodes;
links.header.frame_id = pc.header.frame_id;
nodes.header.frame_id = pc.header.frame_id;
// publish poses as TF
if (method_id < 3) {
for (poseT &p: all_poses) {
geometry_msgs::Pose pose;
pose.position.x = p.shift[0];
pose.position.y = p.shift[1];
pose.position.z = p.shift[2];
pose.orientation.x = p.rotation.x();
pose.orientation.y = p.rotation.y();
pose.orientation.z = p.rotation.z();
pose.orientation.w = p.rotation.w();
links.poses.push_back(pose);
}
pub_link.publish(links);
} else {
for (poseT &p: all_poses) {
geometry_msgs::Pose pose;
pose.position.x = p.shift[0];
pose.position.y = p.shift[1];
pose.position.z = p.shift[2];
pose.orientation.x = p.rotation.x();
pose.orientation.y = p.rotation.y();
pose.orientation.z = p.rotation.z();
pose.orientation.w = p.rotation.w();
links.poses.push_back(pose);
}
for (poseT &p: all_poses) {
geometry_msgs::Pose pose;
pose.position.x = p.shift[0];
pose.position.y = p.shift[1];
pose.position.z = p.shift[2];
pose.orientation.x = p.rotation.x();
pose.orientation.y = p.rotation.y();
pose.orientation.z = p.rotation.z();
pose.orientation.w = p.rotation.w();
nodes.poses.push_back(pose);
}
pub_link.publish(links);
pub_link.publish(nodes);
}
std::cout << "Time 1 = " << t1 << std::endl;
std::cout << "Time 2 = " << t2 << std::endl;
ROS_INFO("... done.");
}
void colorRecons(std::string infile, std::string outfile) {
sensor_msgs::PointCloud2 cloud_blob;
// run this from BodyScanner/build/surface_reconstructor/
pcl::io::loadPCDFile(infile, cloud_blob);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB> ());
pcl::fromROSMsg(cloud_blob, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
int l = 0;
for(int j = 0; j < 480; j += 1) {
for(int i = j%2; i < 640; i += 2) {
int k = i + j*640;
pcl::PointXYZRGB& p = (*cloud)[k];
pcl::PointXYZ pp;
pcl::PointXYZRGB pc;
pc.x = pp.x = p.x;
pc.y = pp.y = p.y;
pc.z = pp.z = p.z;
pc.r = p.r;
pc.g = p.g;
pc.b = p.b;
//pp.nx = 0; pp.ny = 0; pp.nz = 0;
//if((i&1 && j&1) // keep 1/4 of the points
if(mag(p) < 3) { // keep only points within a close distance to the origin
filtered_cloud->push_back(pp);
filtered_cloud_color->push_back(pc);
}
}
}
// Normal estimation*
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
pcl::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::KdTreeFLANN<pcl::PointXYZ>);
tree->setInputCloud(filtered_cloud);
n.setInputCloud(filtered_cloud);
n.setSearchMethod(tree);
n.setKSearch(20);
n.compute(*normals);
// Concatenate the XYZ and normal fields*
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*filtered_cloud, *normals, *filtered_cloud_with_normals);
// Create search tree*
pcl::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::KdTreeFLANN<pcl::PointNormal>);
tree2->setInputCloud(filtered_cloud_with_normals);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius(0.1);
// Set typical values for the parameters
gp3.setMu(2.5);
gp3.setMaximumNearestNeighbors(50); // reducing this didn't fix flips
setMaximumSurfaceAngle(gp3, M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(true); // changing this didn't fix flips
// Get result
gp3.setInputCloud(filtered_cloud_with_normals);
gp3.setSearchMethod(tree2);
gp3.reconstruct(triangles);
saveObj(outfile, triangles, filtered_cloud_color);
}
void PointCloudLocalization::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
{
vital_checker_->poke();
boost::mutex::scoped_lock lock(mutex_);
//JSK_NODELET_INFO("cloudCallback");
latest_cloud_ = cloud_msg;
if (localize_requested_){
JSK_NODELET_INFO("localization is requested");
try {
pcl::PointCloud<pcl::PointNormal>::Ptr
local_cloud (new pcl::PointCloud<pcl::PointNormal>);
pcl::fromROSMsg(*latest_cloud_, *local_cloud);
JSK_NODELET_INFO("waiting for tf transformation from %s tp %s",
latest_cloud_->header.frame_id.c_str(),
global_frame_.c_str());
if (tf_listener_->waitForTransform(
latest_cloud_->header.frame_id,
global_frame_,
latest_cloud_->header.stamp,
ros::Duration(1.0))) {
pcl::PointCloud<pcl::PointNormal>::Ptr
input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl_ros::transformPointCloudWithNormals(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
else {
pcl_ros::transformPointCloud(global_frame_,
*local_cloud,
*input_cloud,
*tf_listener_);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
input_downsampled_cloud (new pcl::PointCloud<pcl::PointNormal>);
applyDownsampling(input_cloud, *input_downsampled_cloud);
if (isFirstTime()) {
all_cloud_ = input_downsampled_cloud;
first_time_ = false;
}
else {
// run ICP
ros::ServiceClient client
= pnh_->serviceClient<jsk_pcl_ros::ICPAlign>("icp_align");
jsk_pcl_ros::ICPAlign icp_srv;
if (clip_unseen_pointcloud_) {
// Before running ICP, remove pointcloud where we cannot see
// First, transform reference pointcloud, that is all_cloud_, into
// sensor frame.
// And after that, remove points which are x < 0.
tf::StampedTransform global_sensor_tf_transform
= lookupTransformWithDuration(
tf_listener_,
global_frame_,
sensor_frame_,
cloud_msg->header.stamp,
ros::Duration(1.0));
Eigen::Affine3f global_sensor_transform;
tf::transformTFToEigen(global_sensor_tf_transform,
global_sensor_transform);
pcl::PointCloud<pcl::PointNormal>::Ptr sensor_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*all_cloud_,
*sensor_cloud,
global_sensor_transform.inverse());
// Remove negative-x points
pcl::PassThrough<pcl::PointNormal> pass;
pass.setInputCloud(sensor_cloud);
pass.setFilterFieldName("x");
pass.setFilterLimits(0.0, 100.0);
pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pass.filter(*filtered_cloud);
JSK_NODELET_INFO("clipping: %lu -> %lu", sensor_cloud->points.size(), filtered_cloud->points.size());
// Convert the pointcloud to global frame again
pcl::PointCloud<pcl::PointNormal>::Ptr global_filtered_cloud
(new pcl::PointCloud<pcl::PointNormal>);
pcl::transformPointCloudWithNormals(
*filtered_cloud,
*global_filtered_cloud,
global_sensor_transform);
pcl::toROSMsg(*global_filtered_cloud,
icp_srv.request.target_cloud);
}
else {
pcl::toROSMsg(*all_cloud_,
icp_srv.request.target_cloud);
}
pcl::toROSMsg(*input_downsampled_cloud,
icp_srv.request.reference_cloud);
if (client.call(icp_srv)) {
Eigen::Affine3f transform;
tf::poseMsgToEigen(icp_srv.response.result.pose, transform);
Eigen::Vector3f transform_pos(transform.translation());
float roll, pitch, yaw;
pcl::getEulerAngles(transform, roll, pitch, yaw);
JSK_NODELET_INFO("aligned parameter --");
JSK_NODELET_INFO(" - pos: [%f, %f, %f]",
transform_pos[0],
transform_pos[1],
transform_pos[2]);
JSK_NODELET_INFO(" - rot: [%f, %f, %f]", roll, pitch, yaw);
pcl::PointCloud<pcl::PointNormal>::Ptr
transformed_input_cloud (new pcl::PointCloud<pcl::PointNormal>);
if (use_normal_) {
pcl::transformPointCloudWithNormals(*input_cloud,
*transformed_input_cloud,
transform);
}
else {
pcl::transformPointCloud(*input_cloud,
*transformed_input_cloud,
transform);
}
pcl::PointCloud<pcl::PointNormal>::Ptr
concatenated_cloud (new pcl::PointCloud<pcl::PointNormal>);
*concatenated_cloud = *all_cloud_ + *transformed_input_cloud;
// update *all_cloud
applyDownsampling(concatenated_cloud, *all_cloud_);
// update localize_transform_
tf::Transform icp_transform;
tf::transformEigenToTF(transform, icp_transform);
{
boost::mutex::scoped_lock tf_lock(tf_mutex_);
localize_transform_ = localize_transform_ * icp_transform;
}
}
else {
JSK_NODELET_ERROR("Failed to call ~icp_align");
return;
}
}
localize_requested_ = false;
}
else {
JSK_NODELET_WARN("No tf transformation is available");
}
}
catch (tf2::ConnectivityException &e)
{
JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
return;
}
catch (tf2::InvalidArgumentException &e)
{
JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
return;
}
}
}
int
main (int argc, char** argv)
{
// Loading first scan of room.
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
return (-1);
}
std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
// Loading second scan of room from new perspective.
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
return (-1);
}
std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
// Filtering input scan to roughly 10% of original size to increase speed of registration.
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
approximate_voxel_filter.setInputCloud (input_cloud);
approximate_voxel_filter.filter (*filtered_cloud);
std::cout << "Filtered cloud contains " << filtered_cloud->size ()
<< " data points from room_scan2.pcd" << std::endl;
// Initializing Normal Distributions Transform (NDT).
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
// Setting scale dependent NDT parameters
// Setting minimum transformation difference for termination condition.
ndt.setTransformationEpsilon (0.01);
// Setting maximum step size for More-Thuente line search.
ndt.setStepSize (0.1);
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt.setResolution (1.0);
// Setting max number of registration iterations.
ndt.setMaximumIterations (35);
// Setting point cloud to be aligned.
ndt.setInputCloud (filtered_cloud);
// Setting point cloud to be aligned to.
ndt.setInputTarget (target_cloud);
// Set initial alignment estimate found using robot odometry.
Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
// Calculating required rigid transform to align the input cloud to the target cloud.
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
ndt.align (*output_cloud, init_guess);
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
<< " score: " << ndt.getFitnessScore () << std::endl;
// Transforming unfiltered, input cloud using found transform.
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());
// Saving transformed input cloud.
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
// Initializing point cloud visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_final->setBackgroundColor (0, 0, 0);
// Coloring and visualizing target cloud (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color (target_cloud, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
// Coloring and visualizing transformed input cloud (green).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color (output_cloud, 0, 255, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
// Starting visualizer
viewer_final->addCoordinateSystem (1.0);
viewer_final->initCameraParameters ();
// Wait until visualizer window is closed.
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
