void PeopleDetector::addNewCloudToViewer(PointCloudT::Ptr cloud, pcl::visualization::PCLVisualizer::Ptr viewer_obj)
{
viewer_obj->removeAllPointClouds();
viewer_obj->removeAllShapes();
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer_obj->addPointCloud<PointT> (cloud, rgb, "input_cloud");
}
void
setCam( /* in: */ Eigen::Vector3d &pos, Eigen::Vector3d &up, Eigen::Vector3d &dir,
::pcl::visualization::PCLVisualizer::Ptr pViewerPtr )
{
vtkSmartPointer<vtkRendererCollection> rens =
pViewerPtr->getRendererCollection();
rens->InitTraversal ();
vtkRenderer* renderer = NULL;
int it = 0;
while ((renderer = rens->GetNextItem ()) != NULL && (it < 1) )
{
vtkCamera& camera = *renderer->GetActiveCamera();
camera.SetPosition ( pos[0], pos[1], pos[2] );
camera.SetViewUp ( up[0], up[1], up[2] );
camera.SetFocalPoint( dir[0], dir[1], dir[2] );
++it;
}
pViewerPtr->spinOnce();
}
void Visualizer::saveCloud(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud, pcl::visualization::PCLVisualizer::Ptr &visualizer)
{
lock.lock();
std::ostringstream oss, oss_cloud;
oss_cloud << savePath << std::setfill('0') << std::setw(5) << saveFrameCloud << "_" << names[index] << ".pcd";
oss << savePath << std::setfill('0') << std::setw(5) << saveFrameCloud << "_" << names[index] << ".png";
outInfo("saving cloud: " << oss_cloud.str());
// pcl::io::savePCDFileASCII(oss.str(), *cloud);
outInfo("saving screenshot: " << oss.str());
visualizer->saveScreenshot(oss.str());
++saveFrameCloud;
lock.unlock();
}
void showGT(const std::vector<ModelT> &model_set, const std::vector<poseT> &poses, pcl::visualization::PCLVisualizer::Ptr viewer)
{
std::vector<pcl::PointCloud<myPointXYZ>::Ptr> rec;
for( std::vector<ModelT>::const_iterator it = model_set.begin() ; it < model_set.end() ; it++ )
{
pcl::PointCloud<myPointXYZ>::Ptr cur_cloud(new pcl::PointCloud<myPointXYZ>());
pcl::fromPCLPointCloud2(it->model_mesh->cloud, *cur_cloud);
rec.push_back(cur_cloud);
}
int count = 0;
Eigen::Quaternionf calibrate_rot(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f (1, 0, 0)));
for(std::vector<poseT>::const_iterator it = poses.begin() ; it < poses.end() ; it++, count++ )
{
for( int i = 0 ; i < model_set.size() ; i++ )
{
if(model_set[i].model_label == it->model_name )
{
pcl::PointCloud<myPointXYZ>::Ptr cur_cloud(new pcl::PointCloud<myPointXYZ>());
//pcl::transformPointCloud(*rec[i], *cur_cloud, Eigen::Vector3f (0, 0, 0), calibrate_rot);
pcl::transformPointCloud(*rec[i], *cur_cloud, it->shift, it->rotation*calibrate_rot);
std::stringstream ss;
ss << count;
viewer->addPolygonMesh<myPointXYZ>(cur_cloud, model_set[i].model_mesh->polygons, it->model_name+"_"+ss.str());
if( it->model_name == "link" )
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.55, 0.05, it->model_name+"_"+ss.str());
else if( it->model_name == "node" )
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.05, 0.55, 1.0, it->model_name+"_"+ss.str());
break;
}
}
}
}
void visualize(pcl::PointCloud<pcl::PointXYZ>::Ptr pCloud, pcl::visualization::PCLVisualizer::Ptr pVisualizer)
{
//init visualizer
pVisualizer->setSize(640, 480);
pVisualizer->setPosition(640, 0);
pVisualizer->setBackgroundColor(0x00, 0x00, 0x00);
pVisualizer->initCameraParameters();
pVisualizer->addPointCloud(pCloud, "cloud");
//reload visualizer content
pVisualizer->spinOnce(1000000);
}
void fetchViewerZBuffer( /* out: */ cv::Mat & zBufMat,
/* in: */ ::pcl::visualization::PCLVisualizer::Ptr const& viewer, double zNear, double zFar )
{
std::cout << "saving vtkZBuffer...";
vtkSmartPointer<vtkWindowToImageFilter> filter = vtkSmartPointer<vtkWindowToImageFilter>::New();
vtkSmartPointer<vtkImageShiftScale> scale = vtkSmartPointer<vtkImageShiftScale>::New();
vtkSmartPointer<vtkWindow> renWin = viewer->getRenderWindow();
// Create Depth Map
filter->SetInput( renWin.GetPointer() );
filter->SetMagnification(1);
filter->SetInputBufferTypeToZBuffer();
// scale
scale->SetOutputScalarTypeToFloat();
scale->SetInputConnection(filter->GetOutputPort());
scale->SetShift(0.f);
scale->SetScale(1.f);
scale->Update();
// fetch data
vtkSmartPointer<vtkImageData> imageData = scale->GetOutput();
int* dims = imageData->GetDimensions();
if ( dims[2] > 1 )
{
std::cerr << "am::util::pcl::fetchZBuffer(): ZDim != 1 !!!!" << std::endl;
}
// output
zBufMat.create( dims[1], dims[0], CV_32FC1 );
for ( int y = 0; y < dims[1]; ++y )
{
for ( int x = 0; x < dims[0]; ++x )
{
float* pixel = static_cast<float*>( imageData->GetScalarPointer(x,y,0) );
float d = round(2.0 * zNear * zFar / (zFar + zNear - pixel[0] * (zFar - zNear)) * 1000.f);
zBufMat.at<float>( dims[1] - y - 1, x ) = (d > 10000.f) ? 0.f : d;
//data[ z * dims[1] * dims[0] + (dims[1] - y - 1) * dims[0] + x ] = pixel[0];//(pixel[0] == 10001) ? 0 : pixel[0];
}
//std::cout << std::endl;
}
//std::cout << std::endl;
}
virtual void processBufferElement(CloudConstPtr cloud) override {
cloud_viewer_->spinOnce();
if (!cloud_viewer_->updatePointCloud(cloud, "OpenNICloud")) {
cloud_viewer_->setPosition(0, 0);
cloud_viewer_->setSize(cloud->width, cloud->height);
cloud_viewer_->addPointCloud(cloud, "OpenNICloud");
cloud_viewer_->resetCameraViewpoint("OpenNICloud");
cloud_viewer_->setCameraPosition(
0, 0, 0, // Position
0, 0, 1, // Viewpoint
0, -1, 0); // Up
}
// TODO; Figure out how to achieve this
// if(cloud_viewer_->wasStopped()) {
// this->cloud_buffer_.reset(new Buffer<CloudConstPtr>());
// this->stop();
// cloud_viewer_->close();
// }
}
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.");
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"greedy_objrec_ransac_node");
ros::NodeHandle nh;
pcl::console::parse_argument(argc, argv, "--m", method_id);
pcl::console::parse_argument(argc, argv, "--p", path);
pcl::console::parse_argument(argc, argv, "--t", trial_id);
pcl::console::parse_argument(argc, argv, "--link", link_mesh_name);
pcl::console::parse_argument(argc, argv, "--node", node_mesh_name);
pcl::console::parse_argument(argc, argv, "--link-width", linkWidth);
pcl::console::parse_argument(argc, argv, "--node-width", nodeWidth);
pcl::console::parse_argument(argc, argv, "--voxel-size", voxelSize);
linkrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(linkWidth, voxelSize));
noderec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize));
objrec = std::shared_ptr<greedyObjRansac>(new greedyObjRansac(nodeWidth, voxelSize));
bool view_flag = false;
if( pcl::console::find_switch(argc, argv, "-v") == true )
view_flag = true;
if( view_flag == true )
{
viewer = pcl::visualization::PCLVisualizer::Ptr (new pcl::visualization::PCLVisualizer());
viewer->initCameraParameters();
viewer->addCoordinateSystem(0.1);
viewer->setSize(640, 480);
viewer->setCameraPosition(0, 0, 0.1, 0, 0, 1, 0, -1, 0);
}
std::stringstream mm;
mm << method_id;
if( method_id >= 3 )
{
ROS_INFO("Loading link mesh from \"%s.obj\"...",link_mesh_name.c_str());
link_mesh = LoadMesh(link_mesh_name+".obj", "link");
ROS_INFO("Loading node mesh from \"%s.obj\"...",node_mesh_name.c_str());
node_mesh = LoadMesh(node_mesh_name+".obj", "node");
mesh_set.push_back(link_mesh);
mesh_set.push_back(node_mesh);
}
switch(method_id)
{
case 0:
case 1:
case 2:
objrec->AddModel(link_mesh_name, "link");
objrec->AddModel(node_mesh_name, "node");
break;
case 3:
case 4:
case 5:
linkrec->AddModel(link_mesh_name, "link");
noderec->AddModel(node_mesh_name, "node");
break;
default:return 0;
}
if (method_id < 3) {
pub_link = nh.advertise<geometry_msgs::PoseArray>("objects",1000);
} else {
pub_link = nh.advertise<geometry_msgs::PoseArray>("links",1000);
pub_node = nh.advertise<geometry_msgs::PoseArray>("nodes",1000);
}
sub = nh.subscribe("points_in",1,callback);
// send and recieve messages
while (ros::ok()) {
ros::spin();
}
return 0;
}
int
main (int argc, char** argv)
{
srand (time (0));
print_info ("The viewer window provides interactive commands; for help, press 'h' or 'H' from within the window.\n");
if (argc < 2)
{
printHelp (argc, argv);
return (-1);
}
// Command line parsing
double bcolor[3] = {0, 0, 0};
pcl::console::parse_3x_arguments (argc, argv, "-bc", bcolor[0], bcolor[1], bcolor[2]);
std::vector<double> fcolor_r, fcolor_b, fcolor_g;
bool fcolorparam = pcl::console::parse_multiple_3x_arguments (argc, argv, "-fc", fcolor_r, fcolor_g, fcolor_b);
std::vector<int> psize;
pcl::console::parse_multiple_arguments (argc, argv, "-ps", psize);
std::vector<double> opaque;
pcl::console::parse_multiple_arguments (argc, argv, "-opaque", opaque);
int mview = 0;
pcl::console::parse_argument (argc, argv, "-multiview", mview);
int normals = 0;
pcl::console::parse_argument (argc, argv, "-normals", normals);
double normals_scale = NORMALS_SCALE;
pcl::console::parse_argument (argc, argv, "-normals_scale", normals_scale);
int pc = 0;
pcl::console::parse_argument (argc, argv, "-pc", pc);
double pc_scale = PC_SCALE;
pcl::console::parse_argument (argc, argv, "-pc_scale", pc_scale);
// Parse the command line arguments for .pcd files
std::vector<int> p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
std::vector<int> vtk_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".vtk");
if (p_file_indices.size () == 0 && vtk_file_indices.size () == 0)
{
print_error ("No .PCD or .VTK file given. Nothing to visualize.\n");
return (-1);
}
// Multiview enabled?
int y_s = 0, x_s = 0;
double x_step = 0, y_step = 0;
if (mview)
{
print_highlight ("Multi-viewport rendering enabled.\n");
if (p_file_indices.size () != 0)
{
y_s = static_cast<int>(floor (sqrt (static_cast<float>(p_file_indices.size ()))));
x_s = y_s + static_cast<int>(ceil ((p_file_indices.size () / static_cast<double>(y_s)) - y_s));
print_highlight ("Preparing to load "); print_value ("%d", p_file_indices.size ());
}
else if (vtk_file_indices.size () != 0)
{
y_s = static_cast<int>(floor (sqrt (static_cast<float>(vtk_file_indices.size ()))));
x_s = y_s + static_cast<int>(ceil ((vtk_file_indices.size () / static_cast<double>(y_s)) - y_s));
print_highlight ("Preparing to load "); print_value ("%d", vtk_file_indices.size ());
}
x_step = static_cast<double>(1.0 / static_cast<double>(x_s));
y_step = static_cast<double>(1.0 / static_cast<double>(y_s));
print_info (" files ("); print_value ("%d", x_s); print_info ("x"); print_value ("%d", y_s);
print_info (" / "); print_value ("%f", x_step); print_info ("x"); print_value ("%f", y_step);
print_info (")\n");
}
// Fix invalid multiple arguments
if (psize.size () != p_file_indices.size () && psize.size () > 0)
for (size_t i = psize.size (); i < p_file_indices.size (); ++i)
psize.push_back (1);
if (opaque.size () != p_file_indices.size () && opaque.size () > 0)
for (size_t i = opaque.size (); i < p_file_indices.size (); ++i)
opaque.push_back (1.0);
// Create the PCLHistogramVisualizer object
pcl::visualization::PCLHistogramVisualizer::Ptr ph;
// Using min_p, max_p to set the global Y min/max range for the histogram
float min_p = FLT_MAX; float max_p = -FLT_MAX;
int k = 0, l = 0, viewport = 0;
// Load the data files
pcl::PCDReader pcd;
pcl::console::TicToc tt;
ColorHandlerPtr color_handler;
GeometryHandlerPtr geometry_handler;
// Go through VTK files
for (size_t i = 0; i < vtk_file_indices.size (); ++i)
{
// Load file
tt.tic ();
print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[vtk_file_indices.at (i)]);
vtkPolyDataReader* reader = vtkPolyDataReader::New ();
reader->SetFileName (argv[vtk_file_indices.at (i)]);
reader->Update ();
vtkSmartPointer<vtkPolyData> polydata = reader->GetOutput ();
if (!polydata)
return (-1);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", polydata->GetNumberOfPoints ()); print_info (" points]\n");
// Create the PCLVisualizer object here on the first encountered XYZ file
if (!p)
p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
// Multiview enabled?
if (mview)
{
p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
k++;
if (k >= x_s)
{
k = 0;
l++;
}
}
// Add as actor
std::stringstream cloud_name ("vtk-");
cloud_name << argv[vtk_file_indices.at (i)] << "-" << i;
p->addModelFromPolyData (polydata, cloud_name.str (), viewport);
// Change the shape rendered color
if (fcolorparam && fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, fcolor_r[i], fcolor_g[i], fcolor_b[i], cloud_name.str ());
// Change the shape rendered point size
if (psize.size () > 0)
p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());
// Change the shape rendered opacity
if (opaque.size () > 0)
p->setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
}
pcl::PCLPointCloud2::Ptr cloud;
// Go through PCD files
for (size_t i = 0; i < p_file_indices.size (); ++i)
{
cloud.reset (new pcl::PCLPointCloud2);
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
int version;
print_highlight (stderr, "Loading "); print_value (stderr, "%s ", argv[p_file_indices.at (i)]);
tt.tic ();
if (pcd.read (argv[p_file_indices.at (i)], *cloud, origin, orientation, version) < 0)
return (-1);
std::stringstream cloud_name;
// ---[ Special check for 1-point multi-dimension histograms
if (cloud->fields.size () == 1 && isMultiDimensionalFeatureField (cloud->fields[0]))
{
cloud_name << argv[p_file_indices.at (i)];
if (!ph)
ph.reset (new pcl::visualization::PCLHistogramVisualizer);
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud->fields[0].count); print_info (" points]\n");
pcl::getMinMax (*cloud, 0, cloud->fields[0].name, min_p, max_p);
ph->addFeatureHistogram (*cloud, cloud->fields[0].name, cloud_name.str ());
continue;
}
cloud_name << argv[p_file_indices.at (i)] << "-" << i;
// Create the PCLVisualizer object here on the first encountered XYZ file
if (!p)
{
p.reset (new pcl::visualization::PCLVisualizer (argc, argv, "PCD viewer"));
p->registerPointPickingCallback (&pp_callback, (void*)&cloud);
Eigen::Matrix3f rotation;
rotation = orientation;
p->setCameraPosition (origin [0] , origin [1] , origin [2],
origin [0] + rotation (0, 2), origin [1] + rotation (1, 2), origin [2] + rotation (2, 2),
rotation (0, 1), rotation (1, 1), rotation (2, 1));
}
// Multiview enabled?
if (mview)
{
p->createViewPort (k * x_step, l * y_step, (k + 1) * x_step, (l + 1) * y_step, viewport);
k++;
if (k >= x_s)
{
k = 0;
l++;
}
}
if (cloud->width * cloud->height == 0)
{
print_error ("[error: no points found!]\n");
return (-1);
}
print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", (int)cloud->width * cloud->height); print_info (" points]\n");
print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (*cloud).c_str ());
// If no color was given, get random colors
if (fcolorparam)
{
if (fcolor_r.size () > i && fcolor_g.size () > i && fcolor_b.size () > i)
color_handler.reset (new pcl::visualization::PointCloudColorHandlerCustom<pcl::PCLPointCloud2> (cloud, fcolor_r[i], fcolor_g[i], fcolor_b[i]));
else
color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud));
}
else
color_handler.reset (new pcl::visualization::PointCloudColorHandlerRandom<pcl::PCLPointCloud2> (cloud));
// Add the dataset with a XYZ and a random handler
geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerXYZ<pcl::PCLPointCloud2> (cloud));
// Add the cloud to the renderer
//p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, color_handler, cloud_name.str (), viewport);
p->addPointCloud (cloud, geometry_handler, color_handler, origin, orientation, cloud_name.str (), viewport);
// If normal lines are enabled
if (normals != 0)
{
int normal_idx = pcl::getFieldIndex (*cloud, "normal_x");
if (normal_idx == -1)
{
print_error ("Normal information requested but not available.\n");
continue;
//return (-1);
}
//
// Convert from blob to pcl::PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (*cloud, *cloud_xyz);
cloud_xyz->sensor_origin_ = origin;
cloud_xyz->sensor_orientation_ = orientation;
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
std::stringstream cloud_name_normals;
cloud_name_normals << argv[p_file_indices.at (i)] << "-" << i << "-normals";
p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, normals, normals_scale, cloud_name_normals.str (), viewport);
}
// If principal curvature lines are enabled
if (pc != 0)
{
if (normals == 0)
normals = pc;
int normal_idx = pcl::getFieldIndex (*cloud, "normal_x");
if (normal_idx == -1)
{
print_error ("Normal information requested but not available.\n");
continue;
//return (-1);
}
int pc_idx = pcl::getFieldIndex (*cloud, "principal_curvature_x");
if (pc_idx == -1)
{
print_error ("Principal Curvature information requested but not available.\n");
continue;
//return (-1);
}
//
// Convert from blob to pcl::PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (*cloud, *cloud_xyz);
cloud_xyz->sensor_origin_ = origin;
cloud_xyz->sensor_orientation_ = orientation;
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::fromPCLPointCloud2 (*cloud, *cloud_normals);
pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_pc (new pcl::PointCloud<pcl::PrincipalCurvatures>);
pcl::fromPCLPointCloud2 (*cloud, *cloud_pc);
std::stringstream cloud_name_normals_pc;
cloud_name_normals_pc << argv[p_file_indices.at (i)] << "-" << i << "-normals";
int factor = (std::min)(normals, pc);
p->addPointCloudNormals<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, factor, normals_scale, cloud_name_normals_pc.str (), viewport);
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, cloud_name_normals_pc.str ());
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
cloud_name_normals_pc << "-pc";
p->addPointCloudPrincipalCurvatures<pcl::PointXYZ, pcl::Normal> (cloud_xyz, cloud_normals, cloud_pc, factor, pc_scale, cloud_name_normals_pc.str (), viewport);
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 3, cloud_name_normals_pc.str ());
}
// Add every dimension as a possible color
if (!fcolorparam)
{
for (size_t f = 0; f < cloud->fields.size (); ++f)
{
if (cloud->fields[f].name == "rgb" || cloud->fields[f].name == "rgba")
color_handler.reset (new pcl::visualization::PointCloudColorHandlerRGBField<pcl::PCLPointCloud2> (cloud));
else
{
if (!isValidFieldName (cloud->fields[f].name))
continue;
color_handler.reset (new pcl::visualization::PointCloudColorHandlerGenericField<pcl::PCLPointCloud2> (cloud, cloud->fields[f].name));
}
// Add the cloud to the renderer
//p->addPointCloud<pcl::PointXYZ> (cloud_xyz, color_handler, cloud_name.str (), viewport);
p->addPointCloud (cloud, color_handler, origin, orientation, cloud_name.str (), viewport);
}
}
// Additionally, add normals as a handler
geometry_handler.reset (new pcl::visualization::PointCloudGeometryHandlerSurfaceNormal<pcl::PCLPointCloud2> (cloud));
if (geometry_handler->isCapable ())
//p->addPointCloud<pcl::PointXYZ> (cloud_xyz, geometry_handler, cloud_name.str (), viewport);
p->addPointCloud (cloud, geometry_handler, origin, orientation, cloud_name.str (), viewport);
// Set immediate mode rendering ON
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_IMMEDIATE_RENDERING, 1.0, cloud_name.str ());
// Change the cloud rendered point size
if (psize.size () > 0)
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize.at (i), cloud_name.str ());
// Change the cloud rendered opacity
if (opaque.size () > 0)
p->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opaque.at (i), cloud_name.str ());
// Reset camera viewpoint to center of cloud if camera parameters were not passed manually and this is the first loaded cloud
//if (i == 0 && !p->cameraParamsSet ())
// p->resetCameraViewpoint (cloud_name.str ());
}
////////////////////////////////////////////////////////////////
// Key binding for saving simulated point cloud:
if (p)
p->registerKeyboardCallback(simulate_callback, (void*)&p);
int width = 640;
int height = 480;
window_width_ = width * 2;
window_height_ = height * 2;
print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
for (int i=0; i<2048; i++)
{
float v = i/2048.0;
v = powf(v, 3)* 6;
t_gamma[i] = v*6*256;
}
GLenum err = glewInit ();
if (GLEW_OK != err)
{
std::cerr << "Error: " << glewGetErrorString (err) << std::endl;
exit (-1);
}
std::cout << "Status: Using GLEW " << glewGetString (GLEW_VERSION) << std::endl;
if (glewIsSupported ("GL_VERSION_2_0"))
std::cout << "OpenGL 2.0 supported" << std::endl;
else
{
std::cerr << "Error: OpenGL 2.0 not supported" << std::endl;
exit(1);
}
camera_ = Camera::Ptr (new Camera ());
scene_ = Scene::Ptr (new Scene ());
range_likelihood_ = RangeLikelihood::Ptr (new RangeLikelihood(1, 1, height, width, scene_));
// range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(10, 10, 96, 96, scene_));
// range_likelihood_ = RangeLikelihood::Ptr(new RangeLikelihood(1, 1, 480, 640, scene_));
// Actually corresponds to default parameters:
range_likelihood_->setCameraIntrinsicsParameters (640,480, 576.09757860,
576.09757860, 321.06398107, 242.97676897);
range_likelihood_->setComputeOnCPU (false);
range_likelihood_->setSumOnCPU (true);
range_likelihood_->setUseColor (true);
initialize (argc, argv);
if (p)
p->setBackgroundColor (bcolor[0], bcolor[1], bcolor[2]);
// Read axes settings
double axes = 0.0;
pcl::console::parse_argument (argc, argv, "-ax", axes);
if (axes != 0.0 && p)
{
double ax_x = 0.0, ax_y = 0.0, ax_z = 0.0;
pcl::console::parse_3x_arguments (argc, argv, "-ax_pos", ax_x, ax_y, ax_z, false);
// Draw XYZ axes if command-line enabled
p->addCoordinateSystem (axes, ax_x, ax_y, ax_z, "reference");
}
// Clean up the memory used by the binary blob
// Note: avoid resetting the cloud, otherwise the PointPicking callback will fail
//cloud.reset ();
if (ph)
{
print_highlight ("Setting the global Y range for all histograms to: "); print_value ("%f -> %f\n", min_p, max_p);
ph->setGlobalYRange (min_p, max_p);
ph->updateWindowPositions ();
if (p)
p->spin ();
else
ph->spin ();
}
else if (p)
p->spin ();
}
virtual void initializeThreadResources() override {
cloud_viewer_.reset(new pcl::visualization::PCLVisualizer("PCL OpenNI2 Cloud"));
cloud_viewer_->registerMouseCallback(&CloudViewer::mouseCallback, *this);
cloud_viewer_->registerKeyboardCallback(&CloudViewer::keyboardCallback, *this);
}