int main (int argc, char const* argv[])
{
if (argc != 2) {
cout << "Usage : obb_test filename.pcd" << endl;
return 1;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud) == -1) {
PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
return 1;
}
cloud_viewer.addPointCloud (cloud, "single_cloud");
OrientedBoundingBox obb;
Eigen::Quaternionf q;
Eigen::Vector3f t, dims;
obb.compute_obb_pca (cloud, q, t, dims);
cloud_viewer.addCube(t, q, dims.x(), dims.y(), dims.z());
cout << dims.x() << " " << dims.y() << " " << dims.z() << endl;
while (!cloud_viewer.wasStopped()) {
cloud_viewer.spinOnce(1);
}
return 0;
}
void Inspector::updateDepth(const openni_wrapper::Image& image,
const openni_wrapper::DepthImage& depth)
{
frame_.depth_->setZero();
ushort data[depth.getHeight() * depth.getWidth()];
depth.fillDepthImageRaw(depth.getWidth(), depth.getHeight(), data);
int i = 0;
for(size_t y = 0; y < depth.getHeight(); ++y) {
for(size_t x = 0; x < depth.getWidth(); ++x, ++i) {
if(data[i] == depth.getNoSampleValue() || data[i] == depth.getShadowValue())
continue;
frame_.depth_->coeffRef(y, x) = data[i];
}
}
if(dddm_ && use_intrinsics_)
dddm_->undistort(frame_.depth_.get());
frame_.img_ = oniToCV(image);
Cloud::Ptr cloud(new Cloud);
FrameProjector proj;
proj.width_ = 640;
proj.height_ = 480;
proj.cx_ = proj.width_ / 2;
proj.cy_ = proj.height_ / 2;
proj.fx_ = 525;
proj.fy_ = 525;
proj.frameToCloud(frame_, cloud.get());
pcd_vis_.updatePointCloud(cloud, "cloud");
pcd_vis_.spinOnce(5);
}
void addVoxels(pcl::visualization::PCLVisualizer &visualizer, const std::string &name)
{
pcl::PointCloud< pcl::PointXYZRGBA>::Ptr colored_voxel_cloud = supervoxels->getColoredVoxelCloud();
visualizer.addPointCloud(colored_voxel_cloud, name);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, name);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.8, name);
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
mtx_.lock ();
if (!cloud_ || !normals_)
{
mtx_.unlock ();
return;
}
CloudConstPtr temp_cloud;
pcl::PointCloud<pcl::Normal>::Ptr temp_normals;
temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
temp_normals.swap (normals_);
mtx_.unlock ();
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_)
{
viz.removePointCloud ("normalcloud");
viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, temp_normals, 100, 0.05f, "normalcloud");
new_cloud_ = false;
}
}
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
if (particles)
{
if (visualize_particles_)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
for (size_t i = 0; i < particles->points.size (); i++)
{
pcl::PointXYZ point;
point.x = particles->points[i].x;
point.y = particles->points[i].y;
point.z = particles->points[i].z;
particle_cloud->points.push_back (point);
}
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color (particle_cloud, 250, 99, 71);
if (!viz.updatePointCloud (particle_cloud, blue_color, "particle cloud"))
viz.addPointCloud (particle_cloud, blue_color, "particle cloud");
}
}
return true;
}
else
{
PCL_WARN ("no particles\n");
return false;
}
}
void
keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
{
double opacity;
if (event.keyUp())
{
switch (event.getKeyCode())
{
case '1':
viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "nan boundary edges");
break;
case '2':
viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluding edges");
break;
case '3':
viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "occluded edges");
break;
case '4':
viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "high curvature edges");
break;
case '5':
viewer.getPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-opacity, "rgb edges");
break;
}
}
}
void addCentroids(pcl::visualization::PCLVisualizer &visualizer, const std::string &name)
{
// pcl::PointCloud< pcl::PointXYZRGBA>::Ptr voxel_centroid_cloud = supervoxels->getVoxelCentroidCloud();
visualizer.addPointCloud(supervoxelcloud, name);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, name);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.95, name);
}
int
main (int argc, char *argv[])
{
std::string pcd_file;
if (argc > 1)
{
pcd_file = argv[1];
}
else
{
printf ("\nUsage: pcl_example_nurbs_fitting_curve pcd-file \n\n");
printf (" pcd-file point-cloud file\n");
exit (0);
}
// #################### LOAD FILE #########################
printf (" loading %s\n", pcd_file.c_str ());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 cloud2;
if (pcl::io::loadPCDFile (pcd_file, cloud2) == -1)
throw std::runtime_error (" PCD file not found.");
fromPCLPointCloud2 (cloud2, *cloud);
// convert to NURBS data structure
pcl::on_nurbs::NurbsDataCurve2d data;
PointCloud2Vector2d (cloud, data.interior);
viewer.setSize (800, 600);
viewer.addPointCloud<pcl::PointXYZ> (cloud, "cloud");
// #################### CURVE PARAMETERS #########################
unsigned order (3);
unsigned n_control_points (10);
pcl::on_nurbs::FittingCurve2d::Parameter curve_params;
curve_params.smoothness = 0.000001;
curve_params.rScale = 1.0;
// #################### CURVE FITTING #########################
ON_NurbsCurve curve = pcl::on_nurbs::FittingCurve2d::initNurbsPCA (order, &data, n_control_points);
pcl::on_nurbs::FittingCurve2d fit (&data, curve);
fit.assemble (curve_params);
Eigen::Vector2d fix1 (0.1, 0.1);
Eigen::Vector2d fix2 (1.0, 0.0);
// fit.addControlPointConstraint (0, fix1, 100.0);
// fit.addControlPointConstraint (curve.CVCount () - 1, fix2, 100.0);
fit.solve ();
// visualize
VisualizeCurve (fit.m_nurbs, 1.0, 0.0, 0.0, true);
viewer.spin ();
return 0;
}
//Draw the current particles
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
if (particles && new_cloud_)
{
//Set pointCloud with particle's points
pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
for (size_t i = 0; i < particles->points.size (); i++)
{
pcl::PointXYZ point;
point.x = particles->points[i].x;
point.y = particles->points[i].y;
point.z = particles->points[i].z;
particle_cloud->points.push_back (point);
}
//Draw red particles
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color (particle_cloud, 250, 99, 71);
if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud"))
viz.addPointCloud (particle_cloud, red_color, "particle cloud");
}
return true;
}
else
{
return false;
}
}
//visualization's callback function
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_pass_)
{
std::this_thread::sleep_for(1s);
return;
}
//Draw downsampled point cloud from sensor
if (new_cloud_ && cloud_pass_downsampled_)
{
CloudPtr cloud_pass;
cloud_pass = cloud_pass_downsampled_;
if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
{
viz.addPointCloud (cloud_pass, "cloudpass");
viz.resetCameraViewpoint ("cloudpass");
}
bool ret = drawParticles (viz);
if (ret)
drawResult (viz);
}
new_cloud_ = false;
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_)
{
boost::this_thread::sleep(boost::posix_time::seconds(1));
return;
}
CloudConstPtr temp_cloud;
temp_cloud.swap (cloud_); //here we set cloud_ to null, so that
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_ && normals_)
{
viz.removePointCloud ("normalcloud");
viz.addPointCloudNormals<PointType, pcl::Normal> (temp_cloud, normals_, 200, 0.05, "normalcloud");
new_cloud_ = false;
}
}
/* \brief Visual update. Create visualizations and add them to the viewer
*
*/
void update()
{
//remove existing shapes from visualizer
clearView();
//prevent the display of too many cubes
bool displayCubeLegend = displayCubes && static_cast<int> (displayCloud->points.size ()) <= MAX_DISPLAYED_CUBES;
showLegend(displayCubeLegend);
if (displayCubeLegend)
{
//show octree as cubes
showCubes(sqrt(octree.getVoxelSquaredSideLen(displayedDepth)));
if (showPointsWithCubes)
{
//add original cloud in visualizer
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(cloud, "z");
viz.addPointCloud(cloud, color_handler, "cloud");
}
}
else
{
//add current cloud in visualizer
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> color_handler(displayCloud,"z");
viz.addPointCloud(displayCloud, color_handler, "cloud");
}
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
if (!cloud_ || !new_cloud_)
{
boost::this_thread::sleep (boost::posix_time::milliseconds (1));
return;
}
{
boost::mutex::scoped_lock lock (mtx_);
FPS_CALC ("visualization");
CloudPtr temp_cloud;
temp_cloud.swap (cloud_pass_);
if (!viz.updatePointCloud (temp_cloud, "OpenNICloud"))
{
viz.addPointCloud (temp_cloud, "OpenNICloud");
viz.resetCameraViewpoint ("OpenNICloud");
}
// Render the data
if (new_cloud_ && cloud_hull_)
{
viz.removePointCloud ("hull");
viz.addPolygonMesh<PointType> (cloud_hull_, vertices_, "hull");
}
new_cloud_ = false;
}
}
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 OnNewMapFrame(pcl::PointCloud< pcl::PointXYZ > mapFrame)
{
_viewer.removeAllPointClouds(0);
_viewer.addPointCloud(mapFrame.makeShared(), "map");
_viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "map");
_viewer.spinOnce();
}
void
run ()
{
// initial processing
process ();
while (!viewer.wasStopped ())
viewer.spinOnce (100);
}
/* \brief Graphic loop for the viewer
*
*/
void run()
{
while (!viz.wasStopped())
{
//main loop of the visualizer
viz.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
/* \brief remove dynamic objects from the viewer
*
*/
void clearView()
{
//remove cubes if any
vtkRenderer *renderer = viz.getRenderWindow()->GetRenderers()->GetFirstRenderer();
while (renderer->GetActors()->GetNumberOfItems() > 0)
renderer->RemoveActor(renderer->GetActors()->GetLastActor());
//remove point clouds if any
viz.removePointCloud("cloud");
}
int main(int argc, char* argv[]) {
if(argc < 3) {
PCL_ERROR("run as ./project /path/to/cloud1/ /path/to/cloud2/ [pcd format]");
return -1;
}
string fCloud1 = "";
string fCloud2 = "";
string models = "";
string training = "";
int NN;
pcl::console::parse_argument(argc, argv, "-cloud1", fCloud1);
pcl::console::parse_argument(argc, argv, "-cloud2", fCloud2);
pcl::console::parse_argument(argc, argv, "-models", models);
pcl::console::parse_argument(argc, argv, "-training", training);
pcl::console::parse_argument(argc, argv, "-nn", NN);
moi.setVerbose(true);
classificator.setModelsDir(models);
classificator.setTrainingDir(training);
classificator.setNN(NN);
classificator.setup();
viewer.registerKeyboardCallback(&keyboard_cb, NULL);
viewer.setBackgroundColor(0, 0, 0);
viewer.initCameraParameters();
if(fCloud1 != "" && fCloud2 != "") {
if(pcl::io::loadPCDFile<pcl::PointXYZ>(fCloud1, *cloud1) == -1) {
PCL_ERROR("Cloud1 reading failed\n");
return(-1);
}
if(pcl::io::loadPCDFile<pcl::PointXYZ>(fCloud2, *cloud2) == -1) {
PCL_ERROR("Cloud2 reading failed\n");
return(-1);
}
findObjectsAndClassify();
} else {
keyboardCbLock = false;
printInstructions();
}
while(!viewer.wasStopped()) {
viewer.spinOnce(100);
boost::this_thread::sleep (boost::posix_time::milliseconds(100));
}
return 0;
}
void viewerOneOff(pcl::visualization::PCLVisualizer& viewer)
{
viewer.setBackgroundColor(1.0, 0.5, 1.0);
pcl::PointXYZ pt;
pt.x = 1.0;
pt.y = 0;
pt.z = 0;
viewer.addSphere(pt, 0.25, "sphere", 0);
std::cout << "I only run once..." << std::endl;
}
void Inspector::disableModel()
{
use_intrinsics_ = false;
pcd_vis_.removeShape("text");
int x = 10;
int y = 10;
int fontsize = 16;
pcd_vis_.addText("raw", x, y, fontsize, 1, 0, 0, "text");
}
void viewerPsycho(pcl::visualization::PCLVisualizer& viewer)
{
static unsigned count = 0;
std::stringstream ss;
ss << "Once per viewer loop: " << count++ << std::endl;
viewer.removeShape("text", 0);
viewer.addText(ss.str(), 200, 200, "text", 0);
user_data++;
}
void viewerOneOff (pcl::visualization::PCLVisualizer& viewer)
{
viewer.setBackgroundColor (1.0, 0.5, 1.0);
pcl::PointXYZ o;
o.x = 0;
o.y = 0;
o.z = 0;
viewer.addSphere (o, 0.25, "sphere", 0);
std::cout << "i only run once" << std::endl;
}
// The corners MUST be in the order which is defined in computeCuboidCornersWithMinMax3D!
// Otherwise this method will not work
// The viewport is the related to your open viewports in the PCLVisualizer instance
// If you have only one viewport, you can pass 0 there or leave it empty
void drawBoundingBoxLines(pcl::visualization::PCLVisualizer &visualizer, pcl::PointCloud<pcl::PointXYZRGB>::Ptr corner_points, int viewport=0)
{
if( corner_points->points.size() != 8)
{
std::cerr << "The corner pointcloud should contain 8 elements. Actual size: " << corner_points->points.size() << std::endl;
return;
}
// Front face after the transformation
visualizer.addLine(corner_points->points.at(0), corner_points->points.at(2), "bb_line_1",viewport);
visualizer.addLine(corner_points->points.at(0), corner_points->points.at(3), "bb_line_2",viewport);
visualizer.addLine(corner_points->points.at(6), corner_points->points.at(2), "bb_line_3",viewport);
visualizer.addLine(corner_points->points.at(6), corner_points->points.at(3), "bb_line_4",viewport);
// Back face after the transformation
visualizer.addLine(corner_points->points.at(4), corner_points->points.at(7), "bb_line_5",viewport);
visualizer.addLine(corner_points->points.at(4), corner_points->points.at(5), "bb_line_6",viewport);
visualizer.addLine(corner_points->points.at(1), corner_points->points.at(7), "bb_line_7",viewport);
visualizer.addLine(corner_points->points.at(1), corner_points->points.at(5), "bb_line_8",viewport);
// Connect both faces with each other
visualizer.addLine(corner_points->points.at(0), corner_points->points.at(4), "bb_line_9",viewport);
visualizer.addLine(corner_points->points.at(2), corner_points->points.at(7), "bb_line_10",viewport);
visualizer.addLine(corner_points->points.at(3), corner_points->points.at(5), "bb_line_11",viewport);
visualizer.addLine(corner_points->points.at(6), corner_points->points.at(1), "bb_line_12",viewport);
// Draw two diagonal lines to see where the center should be
visualizer.addLine(corner_points->points.at(0), corner_points->points.at(1), 255,255,0, "bb_diag_1",viewport);
visualizer.addLine(corner_points->points.at(2), corner_points->points.at(5), 255,255,0, "bb_diag_2",viewport);
// Draw the centroid of the object
Eigen::Vector4f centroid;
CuboidMatcher::computeCentroid(corner_points, centroid);
visualizer.addSphere(getPointXYZFromVector4f(centroid), 0.01, "centroid_bb", viewport);
}
int
main (int argc, char** argv)
{
//pcl::PointCloud<pcl::PointXYZ> cloud;
// Read Kinect live stream:
PointCloudT cloud_obj;
PointCloudT::Ptr cloud (new PointCloudT);
bool new_cloud_available_flag = false;
pcl::Grabber* interface = new pcl::OpenNIGrabber();
boost::function<void (const PointCloudT::ConstPtr&)> f =
boost::bind (&cloud_cb_, _1, &cloud_obj, &new_cloud_available_flag);
interface->registerCallback (f);
interface->start ();
// Wait for the first frame:
while(!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
pcl::copyPointCloud<PointT, PointT>(cloud_obj, *cloud);
new_cloud_available_flag = false;
// Display pointcloud:
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
viewer.setCameraPosition(0,0,-2,0,-1,0,0);
/*
// Add point picking callback to viewer:
struct callback_args cb_args;
PointCloudT::Ptr clicked_points_3d (new PointCloudT);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;
*/
// Spin until 'Q' is pressed (to allow ground manual initialization):
viewer.spin();
std::cout << "done." << std::endl;
pcl::io::savePCDFileASCII ("/home/igor/pcds/pcd_grabber_out_1.pcd", *cloud);
std::cerr << "Saved " << (*cloud).points.size () << " data points to pcd_grabber_out_1.pcd." << std::endl;
/*
for (size_t i = 0; i < cloud.points.size (); ++i)
std::cerr << " " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;
*/
while (!viewer.wasStopped ())
{
boost::this_thread::sleep (boost::posix_time::microseconds (100));
}
return (0);
}
void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
static unsigned count = 0;
std::stringstream ss;
ss << "Once per viewer loop: " << count++;
viewer.removeShape ("text", 0);
viewer.addText (ss.str(), 200, 300, "text", 0);
//FIXME: possible race condition here:
user_data++;
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
{
const std::string centroidname = this->name + "_centroids";
const std::string coloredvoxelname = this->name + "_voxels_colored";
const std::string normalsname = this->name + "_supervoxel_normals";
if(!firstRun)
{
visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, centroidname);
}
visualizer.removeAllPointClouds();
visualizer.removeAllShapes();
addCentroids(visualizer, centroidname);
switch(dispMode)
{
case ALL:
addVoxels(visualizer, coloredvoxelname);
addAdjacency(visualizer);
addNormals(visualizer, normalsname);
break;
case W_VOXELS:
addVoxels(visualizer, coloredvoxelname);
break;
case W_VA:
addVoxels(visualizer, coloredvoxelname);
addAdjacency(visualizer);
break;
case W_VN:
addVoxels(visualizer, coloredvoxelname);
addNormals(visualizer, normalsname);
break;
case W_AN:
addAdjacency(visualizer);
addNormals(visualizer, normalsname);
break;
case W_ADJACENCY:
addAdjacency(visualizer);
break;
case W_NORMALS:
addNormals(visualizer, normalsname);
break;
case ADJACENCY:
visualizer.removeAllPointClouds();
addAdjacency(visualizer);
break;
case NONE:
break;
case TEST:
filterAdjacency(visualizer);
break;
}
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, bool firstRun)
{
if(firstRun)
{
visualizer.addPointCloud(cloud, "cloud");
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud");
}
else
{
visualizer.updatePointCloud(cloud, "cloud");
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1.0, "cloud");
}
}
template <typename PointT> void
tviewer::PointCloudObject<PointT>::addDataToVisualizer (pcl::visualization::PCLVisualizer& v)
{
v.addPointCloud (data_, name_);
v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size_, name_);
v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, visibility_, name_);
if (use_fixed_color_ != 0)
{
float r, g, b;
std::tie (r, g, b) = getRGBFromColor (color_);
v.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, r, g, b, name_);
}
}
void filterAdjacency(pcl::visualization::PCLVisualizer &visualizer)
{
visualizer.removeAllPointClouds();
visualizer.removeAllShapes();
std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
supervoxels->getSupervoxelAdjacency(supervoxel_adjacency);
pcl::PointCloud< pcl::PointXYZRGBA>::Ptr filtered_supervoxel(new pcl::PointCloud< pcl::PointXYZRGBA>);
for(std::multimap<uint32_t, uint32_t>::iterator label_itr = supervoxel_adjacency.begin(); label_itr != supervoxel_adjacency.end();)
{
//First get the label
uint32_t supervoxel_label = label_itr->first;
//Now get the supervoxel corresponding to the label
pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr supervoxel = supervoxel_clusters.at(supervoxel_label);
int threshold = 7 - supervoxel->centroid_.z * 0.375;
int count = supervoxel_adjacency.count(supervoxel_label);
if(count >= threshold)
{
//Now we need to iterate through the adjacent supervoxels and make a point cloud of them
pcl::PointCloud< pcl::PointXYZRGBA>::Ptr adjacent_supervoxel_centers(new pcl::PointCloud< pcl::PointXYZRGBA>);
std::multimap<uint32_t, uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range(supervoxel_label).first;
for(; adjacent_itr != supervoxel_adjacency.equal_range(supervoxel_label).second; ++adjacent_itr)
{
uint32_t snd_label = adjacent_itr->second;
int snd_count = supervoxel_adjacency.count(snd_label);
pcl::Supervoxel<pcl::PointXYZRGBA>::Ptr neighbor_supervoxel = supervoxel_clusters.at(snd_label);
int snd_threshold = 7 - neighbor_supervoxel->centroid_.z * 0.375;
if(snd_count >= snd_threshold)
{
adjacent_supervoxel_centers->push_back(neighbor_supervoxel->centroid_);
filtered_supervoxel->push_back(neighbor_supervoxel->centroid_);
}
}
//Now we make a name for this polygon
std::stringstream ss;
ss << "supervoxel_" << supervoxel_label;
//This function generates a "star" polygon mesh from the points given
addSupervoxelConnectionsToViewer(supervoxel->centroid_, *adjacent_supervoxel_centers, ss.str(), visualizer);
}
label_itr = supervoxel_adjacency.upper_bound(supervoxel_label);
}
const std::string foo = "fassdfasdf";
visualizer.addPointCloud(filtered_supervoxel, foo);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, foo);
}
