void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
viewer.setBackgroundColor (0.3, 0.3, 0.3);
viewer.addCoordinateSystem(1.0, 0);
viewer.initCameraParameters();
viewer.camera_.pos[2] = 30;
viewer.updateCamera();
}
void initViewer(pcl::visualization::PCLVisualizer &viewer) {
viewer.setBackgroundColor(0, 0, 0);
viewer.addCoordinateSystem(1.0, "reference");
viewer.initCameraParameters();
viewer.setRepresentationToPointsForAllActors();
viewer.setCameraPosition(0, 0, -1, 0, 0, 0, 0, -1, 0);
viewer.registerKeyboardCallback(keyboardCallback);
}
PCDOrganizedMultiPlaneSegmentation (typename pcl::PointCloud<PointT>::ConstPtr cloud_, bool refine)
: viewer ("Viewer")
, cloud (cloud_)
, refine_ (refine)
, threshold_ (0.02f)
, depth_dependent_ (true)
, polygon_refinement_ (false)
{
viewer.setBackgroundColor (0, 0, 0);
//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
//viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.15, "cloud");
viewer.addCoordinateSystem (1.0, "global");
viewer.initCameraParameters ();
viewer.registerKeyboardCallback(&PCDOrganizedMultiPlaneSegmentation::keyboard_callback, *this, 0);
}
void
initVisualizer (pcl::visualization::PCLVisualizer &viewer)
{
// Setting the initial viewer parameters
viewer.initCameraParameters ();
viewer.setBackgroundColor (0, 0, 0);
viewer.addCoordinateSystem (1000);
viewer.camera_.view[0] = 0;
viewer.camera_.view[1] = 0;
viewer.camera_.view[2] = 1;
viewer.camera_.pos[0] = 8000;
viewer.camera_.pos[1] = 20000;
viewer.camera_.pos[2] = 2500;
viewer.updateCamera ();
viewer.addText ("Shift + click to select noisy objects. \nPress 0 to confirm the removal.", 50, 300, "user");
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
float th_dd, int max_search)
{
CloudPtr cloud (new Cloud);
fromROSMsg (*input, *cloud);
pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setNormalSmoothingSize (10.0f);
ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
ne.setInputCloud (cloud);
ne.compute (*normal);
TicToc tt;
tt.tic ();
//OrganizedEdgeBase<PointXYZRGBA, Label> oed;
//OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
//OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
oed.setInputNormals (normal);
oed.setInputCloud (cloud);
oed.setDepthDisconThreshold (th_dd);
oed.setMaxSearchNeighbors (max_search);
oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
PointCloud<Label> labels;
std::vector<PointIndices> label_indices;
oed.compute (labels, label_indices);
print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
// Make gray point clouds
for (int idx = 0; idx < (int)cloud->points.size (); idx++)
{
uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3;
cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray;
}
// Display edges in PCLVisualizer
viewer.setSize (640, 480);
viewer.addCoordinateSystem (0.2f);
viewer.addPointCloud (cloud, "original point cloud");
viewer.registerKeyboardCallback(&keyboard_callback);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges);
pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);
const int point_size = 2;
viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
pcl_sleep(0.1);
}
// Combine point clouds and edge labels
sensor_msgs::PointCloud2 output_edges;
toROSMsg (labels, output_edges);
concatenateFields (*input, output_edges, output);
}
void run(pcl::RFFaceDetectorTrainer & fdrf, typename pcl::PointCloud<PointInT>::Ptr & scene_vis, pcl::visualization::PCLVisualizer & vis, bool heat_map,
bool show_votes, const std::string & filename)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr scene (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud (*scene_vis, *scene);
fdrf.setInputCloud (scene);
if (heat_map)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
fdrf.setFaceHeatMapCloud (intensity_cloud);
}
fdrf.detectFaces ();
typedef typename pcl::traits::fieldList<PointInT>::type FieldListM;
double rgb_m;
bool exists_m;
pcl::for_each_type < FieldListM > (pcl::CopyIfFieldExists<PointInT, double> (scene_vis->points[0], "rgb", exists_m, rgb_m));
std::cout << "Color exists:" << static_cast<int> (exists_m) << std::endl;
if (exists_m)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr to_visualize (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud (*scene_vis, *to_visualize);
pcl::visualization::PointCloudColorHandlerRGBField < pcl::PointXYZRGB > handler_keypoints (to_visualize);
vis.addPointCloud < pcl::PointXYZRGB > (to_visualize, handler_keypoints, "scene_cloud");
} else
{
vis.addPointCloud (scene_vis, "scene_cloud");
}
if (heat_map)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
fdrf.getFaceHeatMap (intensity_cloud);
pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity");
vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map");
}
if (show_votes)
{
//display votes_
/*pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud(new pcl::PointCloud<pcl::PointXYZ>());
fdrf.getVotes(votes_cloud);
pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes(votes_cloud, 255, 0, 0);
vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud");
vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud");
vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");*/
pcl::PointCloud<pcl::PointXYZI>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
fdrf.getVotes2 (votes_cloud);
pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_votes (votes_cloud, "intensity");
vis.addPointCloud < pcl::PointXYZI > (votes_cloud, handler_votes, "votes_cloud");
vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
}
vis.addCoordinateSystem (0.1, "global");
std::vector<Eigen::VectorXd> heads;
fdrf.getDetectedFaces (heads);
face_detection_apps_utils::displayHeads (heads, vis);
if (SHOW_GT)
{
//check if there is ground truth data
std::string pose_file (filename);
boost::replace_all (pose_file, ".pcd", "_pose.txt");
Eigen::Matrix4d pose_mat;
pose_mat.setIdentity (4, 4);
bool result = face_detection_apps_utils::readMatrixFromFile (pose_file, pose_mat);
if (result)
{
Eigen::Vector3d ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
Eigen::Vector3d trans_vector = Eigen::Vector3d (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3));
std::cout << ea << std::endl;
std::cout << trans_vector << std::endl;
pcl::PointXYZ center_point;
center_point.x = trans_vector[0];
center_point.y = trans_vector[1];
center_point.z = trans_vector[2];
vis.addSphere (center_point, 0.05, 255, 0, 0, "sphere");
pcl::ModelCoefficients cylinder_coeff;
cylinder_coeff.values.resize (7); // We need 7 values
cylinder_coeff.values[0] = center_point.x;
cylinder_coeff.values[1] = center_point.y;
cylinder_coeff.values[2] = center_point.z;
cylinder_coeff.values[3] = ea[0];
cylinder_coeff.values[4] = ea[1];
cylinder_coeff.values[5] = ea[2];
Eigen::Vector3d vec = Eigen::Vector3d::UnitZ () * -1.;
Eigen::Matrix3d matrixxx;
matrixxx = Eigen::AngleAxisd (ea[0], Eigen::Vector3d::UnitX ()) * Eigen::AngleAxisd (ea[1], Eigen::Vector3d::UnitY ())
* Eigen::AngleAxisd (ea[2], Eigen::Vector3d::UnitZ ());
//matrixxx = pose_mat.block<3,3>(0,0);
vec = matrixxx * vec;
cylinder_coeff.values[3] = vec[0];
cylinder_coeff.values[4] = vec[1];
cylinder_coeff.values[5] = vec[2];
cylinder_coeff.values[6] = 0.01;
vis.addCylinder (cylinder_coeff, "cylinder");
}
}
vis.setRepresentationToSurfaceForAllActors ();
if (VIDEO)
{
vis.spinOnce (50, true);
} else
{
vis.spin ();
}
vis.removeAllPointClouds ();
vis.removeAllShapes ();
}
