/* \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 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);
}
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
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;
}
}
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 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);
}
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
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;
}
}
//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 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();
}
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 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");
}
}
void viz_cb (pcl::visualization::PCLVisualizer& viz)
{
// cout << "PbMapMaker::viz_cb(...)\n";
if (cloud1->empty())
{
boost::this_thread::sleep (boost::posix_time::milliseconds (10));
return;
}
{
// boost::mutex::scoped_lock lock (viz_mutex);
// viz.removeAllShapes();
viz.removeAllPointClouds();
{ //mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);
boost::mutex::scoped_lock updateLock(visualizationMutex);
// if (!viz.updatePointCloud (cloud, "sphereCloud"))
// viz.addPointCloud (sphereCloud, "sphereCloud");
if (!viz.updatePointCloud (cloud1, "cloud1"))
viz.addPointCloud (cloud1, "cloud1");
if (!viz.updatePointCloud (cloud2, "cloud2"))
viz.addPointCloud (cloud2, "cloud2");
if (!viz.updatePointCloud (cloud3, "cloud3"))
viz.addPointCloud (cloud3, "cloud3");
if (!viz.updatePointCloud (cloud4, "cloud4"))
viz.addPointCloud (cloud4, "cloud4");
if (!viz.updatePointCloud (cloud5, "cloud5"))
viz.addPointCloud (cloud5, "cloud5");
if (!viz.updatePointCloud (cloud6, "cloud6"))
viz.addPointCloud (cloud6, "cloud6");
if (!viz.updatePointCloud (cloud7, "cloud7"))
viz.addPointCloud (cloud7, "cloud7");
if (!viz.updatePointCloud (cloud8, "cloud8"))
viz.addPointCloud (cloud8, "cloud8");
updateLock.unlock();
}
}
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
{
double pointSize = 1.0;
if(firstRun)
{
visualizer.addPointCloud(dispCloudPtr_, std::string("stuff"));
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::string("stuff"));
}
else
{
visualizer.updatePointCloud(dispCloudPtr_, std::string("stuff"));
visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, std::string("stuff"));
}
}
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);
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
{
const std::string cloudname = this->name;
if(firstRun)
{
visualizer.addPointCloud(dispCloud, cloudname);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
}
else
{
visualizer.updatePointCloud(dispCloud, cloudname);
visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
}
}
//Draw model reference point cloud
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
//move close to camera a little for better visualization
transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
CloudPtr result_cloud (new Cloud ());
pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
//Draw blue model reference point cloud
{
pcl::visualization::PointCloudColorHandlerCustom<RefPointType> blue_color (result_cloud, 0, 0, 255);
if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud"))
viz.addPointCloud (result_cloud, blue_color, "resultcloud");
}
}
void viewerUpdate(pcl::visualization::PCLVisualizer& viewer)
{
std::stringstream ss;
ss << "Rawlog entry: " << rawlogEntry;
viewer.removeShape ("text", 0);
viewer.addText (ss.str(), 10,50, "text", 0);
static mrpt::system::TTimeStamp last_time = INVALID_TIMESTAMP;
{ // Mutex protected
std::lock_guard<std::mutex> lock(td_cs);
if (td.new_timestamp!=last_time)
{
last_time = td.new_timestamp;
viewer.removePointCloud("cloud", 0);
viewer.addPointCloud (td.new_cloud,"cloud",0);
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3.0);
const size_t N = td.new_cloud->size();
std::cout << "Showing new point cloud of size=" << N << std::endl;
static bool first = true;
if (N && first)
{
first = false;
//viewer.resetCameraViewpoint("cloud");
}
#if 0
std::cout << mrpt::format(
"camera: clip = %f %f\n"
" focal = %f %f %f\n"
" pos = %f %f %f\n"
" view = %f %f %f\n",
viewer.camera_.clip[0],viewer.camera_.clip[1],
viewer.camera_.focal[0],viewer.camera_.focal[1],viewer.camera_.focal[2],
viewer.camera_.pos[0],viewer.camera_.pos[1],viewer.camera_.pos[2],
viewer.camera_.view[0],viewer.camera_.view[1],viewer.camera_.view[2]);
#endif
}
}
}
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
// move a little bit for better visualization
transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
RefCloudPtr result_cloud (new RefCloud ());
if (!visualize_non_downsample_)
pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
else
pcl::transformPointCloud<RefPointType> (*reference_, *result_cloud, transformation);
{
pcl::visualization::PointCloudColorHandlerCustom<RefPointType> red_color (result_cloud, 0, 0, 255);
if (!viz.updatePointCloud (result_cloud, red_color, "resultcloud"))
viz.addPointCloud (result_cloud, red_color, "resultcloud");
}
}
void
visualizeCurve (ON_NurbsCurve &curve, ON_NurbsSurface &surface, pcl::visualization::PCLVisualizer &viewer)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::on_nurbs::Triangulation::convertCurve2PointCloud (curve, surface, curve_cloud, 4);
for (std::size_t i = 0; i < curve_cloud->size () - 1; i++)
{
pcl::PointXYZRGB &p1 = curve_cloud->at (i);
pcl::PointXYZRGB &p2 = curve_cloud->at (i + 1);
std::ostringstream os;
os << "line" << i;
viewer.removeShape (os.str ());
viewer.addLine<pcl::PointXYZRGB> (p1, p2, 1.0, 0.0, 0.0, os.str ());
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr curve_cps (new pcl::PointCloud<pcl::PointXYZRGB>);
for (int i = 0; i < curve.CVCount (); i++)
{
ON_3dPoint p1;
curve.GetCV (i, p1);
double pnt[3];
surface.Evaluate (p1.x, p1.y, 0, 3, pnt);
pcl::PointXYZRGB p2;
p2.x = float (pnt[0]);
p2.y = float (pnt[1]);
p2.z = float (pnt[2]);
p2.r = 255;
p2.g = 0;
p2.b = 0;
curve_cps->push_back (p2);
}
viewer.removePointCloud ("cloud_cps");
viewer.addPointCloud (curve_cps, "cloud_cps");
}
void fillVisualizerWithLock(pcl::visualization::PCLVisualizer &visualizer, const bool firstRun)
{
const std::string &cloudname = this->name;
if(firstRun)
{
visualizer.addPointCloud(cloud, cloudname);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
}
else
{
visualizer.updatePointCloud(cloud, cloudname);
visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname);
visualizer.removeAllShapes();
}
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0.2, 0, 0), 1, 0, 0, "X");
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0.2, 0), 0, 1, 0, "Y");
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pcl::PointXYZ(0, 0, 0.2), 0, 0, 1, "Z");
tf::Vector3 origin = worldToCam * tf::Vector3(0, 0, 0);
tf::Vector3 lineX = worldToCam * tf::Vector3(0.2, 0, 0);
tf::Vector3 lineY = worldToCam * tf::Vector3(0, 0.2, 0);
tf::Vector3 lineZ = worldToCam * tf::Vector3(0, 0, 0.2);
pcl::PointXYZ pclOrigin(origin.x(), origin.y(), origin.z());
pcl::PointXYZ pclLineX(lineX.x(), lineX.y(), lineX.z());
pcl::PointXYZ pclLineY(lineY.x(), lineY.y(), lineY.z());
pcl::PointXYZ pclLineZ(lineZ.x(), lineZ.y(), lineZ.z());
visualizer.addLine(pcl::PointXYZ(0, 0, 0), pclOrigin, 1, 1, 1, "line");
visualizer.addLine(pclOrigin, pclLineX, 1, 0, 0, "lineX");
visualizer.addLine(pclOrigin, pclLineY, 0, 1, 0, "lineY");
visualizer.addLine(pclOrigin, pclLineZ, 0, 0, 1, "lineZ");
for(int i = 0; i < regions.size(); ++i)
{
const Region ®ion = regions[i];
tf::Transform transform = worldToCam * region.transform;
std::ostringstream oss;
oss << "region_" << i;
tf::Vector3 originB = transform * tf::Vector3(0, 0, 0);
tf::Vector3 lineXB = transform * tf::Vector3(0.2, 0, 0);
tf::Vector3 lineYB = transform * tf::Vector3(0, 0.2, 0);
tf::Vector3 lineZB = transform * tf::Vector3(0, 0, 0.2);
pcl::PointXYZ pclOriginB(originB.x(), originB.y(), originB.z());
pcl::PointXYZ pclLineXB(lineXB.x(), lineXB.y(), lineXB.z());
pcl::PointXYZ pclLineYB(lineYB.x(), lineYB.y(), lineYB.z());
pcl::PointXYZ pclLineZB(lineZB.x(), lineZB.y(), lineZB.z());
visualizer.addLine(pclOrigin, pclOriginB, 1, 1, 1, "line_" + oss.str());
visualizer.addLine(pclOriginB, pclLineXB, 1, 0, 0, "lineX_" + oss.str());
visualizer.addLine(pclOriginB, pclLineYB, 0, 1, 0, "lineY_" + oss.str());
visualizer.addLine(pclOriginB, pclLineZB, 0, 0, 1, "lineZ_" + oss.str());
Eigen::Vector3d translation;
Eigen::Quaterniond rotation;
tf::vectorTFToEigen(transform.getOrigin(), translation);
tf::quaternionTFToEigen(transform.getRotation(), rotation);
visualizer.addCube(translation.cast<float>(), rotation.cast<float>(), region.width, region.height, region.depth, oss.str());
}
}
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);
}
int main(int argc,char** argv){
if (argc < 2){
std::cout<<"Please enter <input.pcd> file"<<std::endl;
return 0;
}
if (pcl::io::loadPCDFile (argv[1], *model) < 0)
{
std::cout << "Error loading model cloud." << std::endl;
return (-1);
}
model_name = argv[1];
model_name = model_name.substr(0,model_name.size()-4);
if(pcl::console::find_switch(argc,argv,"-vfh")){
vfh = true;
}
else if(pcl::console::find_switch(argc,argv,"-rv")){
std::cout<<"performing Resultant vector feature calculation"<<std::endl;
rv = true;
}else{
std::cout<<"no algorithm specified using default algorithm vfh"<<std::endl;
vfh = true;
}
if (pcl::console::find_switch (argc, argv, "-ds"))
{
_downsample = true;
std::cout<<"performing downsampling on the input file"<<std::endl;
}
if (pcl::console::find_switch (argc, argv, "-sn"))
{
show_normals = true;
std::cout<<"showing calclated normals"<<std::endl;
}
if(_downsample){
rec.setInputCloud(model,_leaf_size);
std::cout<<"saving downsampled file to model_down.pcd"<<std::endl;
pcl::io::savePCDFileASCII ("model_down.pcd", *(rec.getCloud()));
}
else{
rec.setInputCloud(model);
std::cout<<"setting input model without further downsampling"<<std::endl;
}
if(pcl::console::find_switch(argc,argv,"--showaxis")){
_show_axis = true;
}
if(vfh){
std::cout<<"estimating VFH features"<<std::endl;
rec.Estimate_VFH_Features();
}
else if(rv){
std::cout<<"estimating features using the resultant vector"<<std::endl;
rec.Estimate_RV_Features();
PointNormalCloudT::Ptr cloud (new PointNormalCloudT);
pcl::PointCloud<pcl::Normal>::Ptr norm_cloud (new pcl::PointCloud<pcl::Normal>);
cloud = rec.getPointNormalCloud();
norm_cloud = rec.getNormalCloud();
pcl::PointCloud<pcl::PointXYZ>::Ptr plaincloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud,*plaincloud);
//pcl::PointXYZ mass_center(rec.rv_centroid.x,rec.rv_centroid.y,rec.rv_centroid.z);
pcl::PointXYZ mass_center(0,0,0);
pcl::PointXYZ kinectZ(0,0,-1);
pcl::PointXYZ res_vec (rec.rv_resultant.normal_x,rec.rv_resultant.normal_y,rec.rv_resultant.normal_z);
//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(plaincloud);
//viewer.addPointCloud<pcl::PointXYZ> (plaincloud, rgb, "model_cloud");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> single_color(cloud, double(0), double(255), double(0));
viewer.addPointCloud(cloud,single_color,"sample cloud");
if(_show_axis){
viewer.addLine(mass_center,res_vec,1.0f,0.0f,0.0f,"resultantvector");
viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,"KinectZ");
}
std::cout<<"resultant vector :"<<res_vec.x<<" i"<<" + "<<res_vec.y<<" j"<<" + "<<res_vec.z<<" k"<<std::endl;
if(show_normals){
std::cout<<"showing 1 in "<<normalsratio<<" normals"<<std::endl;
viewer.addPointCloudNormals<pcl::PointNormal,pcl::Normal>(cloud, norm_cloud,normalsratio, 0.05, "normalscloud");
}
while(!viewer.wasStopped())
viewer.spinOnce();
}
std::cout<<"feature calculation complete"<<std::endl;
ofstream myfile;
if (vfh){
std::stringstream ss;
ss<<"vfh_"<<model_name<<".txt";
myfile.open(ss.str().c_str());
for(size_t k =0 ;k<308;k++){
if(k != 307)
myfile << rec._vfh_features->points[0].histogram[k]<<",";
else
myfile << rec._vfh_features->points[0].histogram[k];
}
std::cout<<"wrote the histogram to file :" <<ss.str()<<std::endl;
myfile.close();
}else if(rv){
std::stringstream ss;
ss<<"rv_"<<model_name<<".txt";
myfile.open(ss.str().c_str());
for(int k = 0;k<181;k++){
if(k != 180)
myfile << rec._rv_features_181[k]<<",";
else
myfile << rec._rv_features_181[k];
}
std::cout<<"wrote the histogram to file: "<< ss.str()<<std::endl;
//myfile.open(ss.c_str());
}
}
void PbMapVisualizer::viz_cb (pcl::visualization::PCLVisualizer& viz)
{
if (pbmap.globalMapPtr->empty())
{
mrpt::system::sleep(10);
return;
}
// Render the data
{
viz.removeAllShapes();
viz.removeAllPointClouds();
char name[1024];
if(graphRepresentation)
{
// cout << "show graphRepresentation\n";
for(size_t i=0; i<pbmap.vPlanes.size(); i++)
{
pcl::PointXYZ center(2*pbmap.vPlanes[i].v3center[0], 2*pbmap.vPlanes[i].v3center[1], 2*pbmap.vPlanes[i].v3center[2]);
double radius = 0.1 * sqrt(pbmap.vPlanes[i].areaVoxels);
// cout << "radius " << radius << endl;
sprintf (name, "sphere%u", static_cast<unsigned>(i));
viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name);
if( !pbmap.vPlanes[i].label.empty() )
viz.addText3D (pbmap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], pbmap.vPlanes[i].label);
else
{
sprintf (name, "P%u", static_cast<unsigned>(i));
viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
// Draw edges
// for(set<unsigned>::iterator it = pbmap.vPlanes[i].nearbyPlanes.begin(); it != pbmap.vPlanes[i].nearbyPlanes.end(); it++)
// {
// if(*it > pbmap.vPlanes[i].id)
// break;
//
// sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it));
// pcl::PointXYZ center_it(2*pbmap.vPlanes[*it].v3center[0], 2*pbmap.vPlanes[*it].v3center[1], 2*pbmap.vPlanes[*it].v3center[2]);
// viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
// }
for(map<unsigned,unsigned>::iterator it = pbmap.vPlanes[i].neighborPlanes.begin(); it != pbmap.vPlanes[i].neighborPlanes.end(); it++)
{
if(it->first > pbmap.vPlanes[i].id)
break;
sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
pcl::PointXYZ center_it(2*pbmap.vPlanes[it->first].v3center[0], 2*pbmap.vPlanes[it->first].v3center[1], 2*pbmap.vPlanes[it->first].v3center[2]);
viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
char commonObs[8];
sprintf (commonObs, "%u", it->second);
pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 );
viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name);
}
}
}
else
{ // Regular representation
if (!viz.updatePointCloud (pbmap.globalMapPtr, "cloud"))
viz.addPointCloud (pbmap.globalMapPtr, "cloud");
sprintf (name, "PointCloud size %u", static_cast<unsigned>( pbmap.globalMapPtr->size() ) );
viz.addText(name, 10, 20);
for(size_t i=0; i<pbmap.vPlanes.size(); i++)
{
Plane &plane_i = pbmap.vPlanes[i];
// sprintf (name, "normal_%u", static_cast<unsigned>(i));
name[0] = *(mrpt::format("normal_%u", static_cast<unsigned>(i)).c_str());
pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization
pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]);
pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
if( !plane_i.label.empty() )
viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label);
else
{
sprintf (name, "n%u", static_cast<unsigned>(i));
viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
sprintf (name, "approx_plane_%02d", int (i));
viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name);
}
}
}
}
void
process ()
{
std::cout << "threshold: " << threshold_ << std::endl;
std::cout << "depth dependent: " << (depth_dependent_ ? "true\n" : "false\n");
unsigned char red [6] = {255, 0, 0, 255, 255, 0};
unsigned char grn [6] = { 0, 255, 0, 255, 0, 255};
unsigned char blu [6] = { 0, 0, 255, 0, 255, 255};
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr refinement_compare (new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
refinement_compare->setDistanceThreshold (threshold_, depth_dependent_);
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (5000);
mps.setAngularThreshold (0.017453 * 3.0); //3 degrees
mps.setDistanceThreshold (0.03); //2cm
mps.setRefinementComparator (refinement_compare);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
typename pcl::PointCloud<PointT>::Ptr contour (new pcl::PointCloud<PointT>);
typename pcl::PointCloud<PointT>::Ptr approx_contour (new pcl::PointCloud<PointT>);
char name[1024];
typename pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
double normal_start = pcl::getTime ();
ne.setInputCloud (cloud);
ne.compute (*normal_cloud);
double normal_end = pcl::getTime ();
std::cout << "Normal Estimation took " << double (normal_end - normal_start) << std::endl;
double plane_extract_start = pcl::getTime ();
mps.setInputNormals (normal_cloud);
mps.setInputCloud (cloud);
if (refine_)
mps.segmentAndRefine (regions);
else
mps.segment (regions);
double plane_extract_end = pcl::getTime ();
std::cout << "Plane extraction took " << double (plane_extract_end - plane_extract_start) << " with planar regions found: " << regions.size () << std::endl;
std::cout << "Frame took " << double (plane_extract_end - normal_start) << std::endl;
typename pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
viewer.removeAllPointClouds (0);
viewer.removeAllShapes (0);
pcl::visualization::PointCloudColorHandlerCustom<PointT> single_color (cloud, 0, 255, 0);
viewer.addPointCloud<PointT> (cloud, single_color, "cloud");
pcl::PlanarPolygon<PointT> approx_polygon;
//Draw Visualization
for (size_t i = 0; i < regions.size (); i++)
{
Eigen::Vector3f centroid = regions[i].getCentroid ();
Eigen::Vector4f model = regions[i].getCoefficients ();
pcl::PointXYZ pt1 = pcl::PointXYZ (centroid[0], centroid[1], centroid[2]);
pcl::PointXYZ pt2 = pcl::PointXYZ (centroid[0] + (0.5f * model[0]),
centroid[1] + (0.5f * model[1]),
centroid[2] + (0.5f * model[2]));
sprintf (name, "normal_%d", unsigned (i));
viewer.addArrow (pt2, pt1, 1.0, 0, 0, std::string (name));
contour->points = regions[i].getContour ();
sprintf (name, "plane_%02d", int (i));
pcl::visualization::PointCloudColorHandlerCustom <PointT> color (contour, red[i], grn[i], blu[i]);
viewer.addPointCloud (contour, color, name);
pcl::approximatePolygon (regions[i], approx_polygon, threshold_, polygon_refinement_);
approx_contour->points = approx_polygon.getContour ();
std::cout << "polygon: " << contour->size () << " -> " << approx_contour->size () << std::endl;
typename pcl::PointCloud<PointT>::ConstPtr approx_contour_const = approx_contour;
// sprintf (name, "approx_plane_%02d", int (i));
// viewer.addPolygon<PointT> (approx_contour_const, 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
for (unsigned idx = 0; idx < approx_contour->points.size (); ++idx)
{
sprintf (name, "approx_plane_%02d_%03d", int (i), int(idx));
viewer.addLine (approx_contour->points [idx], approx_contour->points [(idx+1)%approx_contour->points.size ()], 0.5 * red[i], 0.5 * grn[i], 0.5 * blu[i], name);
}
}
}
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 ();
}
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_pass_)
{
boost::this_thread::sleep (boost::posix_time::seconds (1));
return;
}
if (new_cloud_ && cloud_pass_downsampled_)
{
CloudPtr cloud_pass;
if (!visualize_non_downsample_)
cloud_pass = cloud_pass_downsampled_;
else
cloud_pass = cloud_pass_;
if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
{
viz.addPointCloud (cloud_pass, "cloudpass");
viz.resetCameraViewpoint ("cloudpass");
}
}
if (new_cloud_ && reference_)
{
bool ret = drawParticles (viz);
if (ret)
{
drawResult (viz);
// draw some texts
viz.removeShape ("N");
viz.addText ((boost::format ("number of Reference PointClouds: %d") % tracker_->getReferenceCloud ()->points.size ()).str (),
10, 20, 20, 1.0, 1.0, 1.0, "N");
viz.removeShape ("M");
viz.addText ((boost::format ("number of Measured PointClouds: %d") % cloud_pass_downsampled_->points.size ()).str (),
10, 40, 20, 1.0, 1.0, 1.0, "M");
viz.removeShape ("tracking");
viz.addText ((boost::format ("tracking: %f fps") % (1.0 / tracking_time_)).str (),
10, 60, 20, 1.0, 1.0, 1.0, "tracking");
viz.removeShape ("downsampling");
viz.addText ((boost::format ("downsampling: %f fps") % (1.0 / downsampling_time_)).str (),
10, 80, 20, 1.0, 1.0, 1.0, "downsampling");
viz.removeShape ("computation");
viz.addText ((boost::format ("computation: %f fps") % (1.0 / computation_time_)).str (),
10, 100, 20, 1.0, 1.0, 1.0, "computation");
viz.removeShape ("particles");
viz.addText ((boost::format ("particles: %d") % tracker_->getParticles ()->points.size ()).str (),
10, 120, 20, 1.0, 1.0, 1.0, "particles");
}
}
new_cloud_ = false;
}
int main (int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile (argv[1], *cloud);
pcl::copyPointCloud( *cloud,*cloud_filtered);
float i ;
float j;
float k;
cv::namedWindow( "picture");
cvCreateTrackbar("X_limit", "picture", &a, 30, NULL);
cvCreateTrackbar("Y_limit", "picture", &b, 30, NULL);
cvCreateTrackbar("Z_limit", "picture", &c, 30, NULL);
char last_c = 0;
// pcl::visualization::PCLVisualizer viewer ("picture");
viewer.setBackgroundColor (0.0, 0.0, 0.5);//set backgroung according to the color of points
pcl::PassThrough<pcl::PointXYZ> pass;
while (!viewer.wasStopped ())
{
pcl::copyPointCloud(*cloud_filtered, *cloud);
i = 0.1*((float)a);
j = 0.1*((float)b);
k = 0.1*((float)c);
// cout << "i = " << i << " j = " << j << " k = " << k << endl;
pass.setInputCloud (cloud);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-j, j);
pass.filter (*cloud);
pass.setInputCloud (cloud);
pass.setFilterFieldName ("x");
pass.setFilterLimits (-i, i);
pass.filter (*cloud);
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (-k,k);
pass.filter (*cloud);
viewer.addPointCloud (cloud, "scene_cloud");
viewer.spinOnce ();
viewer.removePointCloud("scene_cloud");
waitKey(10);
}
return 0;
}
void PbMapMaker::viz_cb (pcl::visualization::PCLVisualizer& viz)
{
if (mPbMap.globalMapPtr->empty())
{
mrpt::system::sleep(10);
return;
}
{ mrpt::synch::CCriticalSectionLocker csl(&CS_visualize);
// Render the data
{
viz.removeAllShapes();
viz.removeAllPointClouds();
char name[1024];
if(graphRepresentation)
{
for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
{
pcl::PointXYZ center(2*mPbMap.vPlanes[i].v3center[0], 2*mPbMap.vPlanes[i].v3center[1], 2*mPbMap.vPlanes[i].v3center[2]);
double radius = 0.1 * sqrt(mPbMap.vPlanes[i].areaVoxels);
sprintf (name, "sphere%u", static_cast<unsigned>(i));
viz.addSphere (center, radius, ared[i%10], agrn[i%10], ablu[i%10], name);
if( !mPbMap.vPlanes[i].label.empty() )
viz.addText3D (mPbMap.vPlanes[i].label, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], mPbMap.vPlanes[i].label);
else
{
sprintf (name, "P%u", static_cast<unsigned>(i));
viz.addText3D (name, center, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
// Draw edges
if(!configPbMap.graph_mode) // Nearby neighbors
for(set<unsigned>::iterator it = mPbMap.vPlanes[i].nearbyPlanes.begin(); it != mPbMap.vPlanes[i].nearbyPlanes.end(); it++)
{
if(*it > mPbMap.vPlanes[i].id)
break;
sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(*it));
pcl::PointXYZ center_it(2*mPbMap.vPlanes[*it].v3center[0], 2*mPbMap.vPlanes[*it].v3center[1], 2*mPbMap.vPlanes[*it].v3center[2]);
viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
}
else
for(map<unsigned,unsigned>::iterator it = mPbMap.vPlanes[i].neighborPlanes.begin(); it != mPbMap.vPlanes[i].neighborPlanes.end(); it++)
{
if(it->first > mPbMap.vPlanes[i].id)
break;
sprintf (name, "commonObs%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
pcl::PointXYZ center_it(2*mPbMap.vPlanes[it->first].v3center[0], 2*mPbMap.vPlanes[it->first].v3center[1], 2*mPbMap.vPlanes[it->first].v3center[2]);
viz.addLine (center, center_it, ared[i%10], agrn[i%10], ablu[i%10], name);
sprintf (name, "edge%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(it->first));
char commonObs[8];
sprintf (commonObs, "%u", it->second);
pcl::PointXYZ half_edge( (center_it.x+center.x)/2, (center_it.y+center.y)/2, (center_it.z+center.z)/2 );
viz.addText3D (commonObs, half_edge, 0.05, 1.0, 1.0, 1.0, name);
}
}
}
else
{ // Regular representation
if (!viz.updatePointCloud (mPbMap.globalMapPtr, "cloud"))
viz.addPointCloud (mPbMap.globalMapPtr, "cloud");
if(mpPbMapLocaliser != NULL)
if(mpPbMapLocaliser->alignedModelPtr){
if (!viz.updatePointCloud (mpPbMapLocaliser->alignedModelPtr, "model"))
viz.addPointCloud (mpPbMapLocaliser->alignedModelPtr, "model");}
sprintf (name, "PointCloud size %u", static_cast<unsigned>( mPbMap.globalMapPtr->size() ) );
viz.addText(name, 10, 20);
for(size_t i=0; i<mPbMap.vPlanes.size(); i++)
{
Plane &plane_i = mPbMap.vPlanes[i];
sprintf (name, "normal_%u", static_cast<unsigned>(i));
pcl::PointXYZ pt1, pt2; // Begin and end points of normal's arrow for visualization
pt1 = pcl::PointXYZ(plane_i.v3center[0], plane_i.v3center[1], plane_i.v3center[2]);
pt2 = pcl::PointXYZ(plane_i.v3center[0] + (0.5f * plane_i.v3normal[0]),
plane_i.v3center[1] + (0.5f * plane_i.v3normal[1]),
plane_i.v3center[2] + (0.5f * plane_i.v3normal[2]));
viz.addArrow (pt2, pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
// Draw Ppal diretion
// if( plane_i.elongation > 1.3 )
// {
// sprintf (name, "ppalComp_%u", static_cast<unsigned>(i));
// pcl::PointXYZ pt3 = pcl::PointXYZ ( plane_i.v3center[0] + (0.2f * plane_i.v3PpalDir[0]),
// plane_i.v3center[1] + (0.2f * plane_i.v3PpalDir[1]),
// plane_i.v3center[2] + (0.2f * plane_i.v3PpalDir[2]));
// viz.addArrow (pt3, plane_i.pt1, ared[i%10], agrn[i%10], ablu[i%10], false, name);
// }
// if( !plane_i.label.empty() )
// viz.addText3D (plane_i.label, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], plane_i.label);
// else
{
sprintf (name, "n%u", static_cast<unsigned>(i));
// sprintf (name, "n%u_%u", static_cast<unsigned>(i), static_cast<unsigned>(plane_i.semanticGroup));
viz.addText3D (name, pt2, 0.1, ared[i%10], agrn[i%10], ablu[i%10], name);
}
// sprintf (name, "planeRaw_%02u", static_cast<unsigned>(i));
// viz.addPointCloud (plane_i.planeRawPointCloudPtr, name);// contourPtr, planePointCloudPtr, polygonContourPtr
// if(!configPbMap.makeClusters)
// {
// sprintf (name, "plane_%02u", static_cast<unsigned>(i));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[i%10], grn[i%10], blu[i%10]);
//// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
// viz.addPointCloud (plane_i.planePointCloudPtr, color, name);
// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, name);
// }
// else
// {
// sprintf (name, "plane_%02u", static_cast<unsigned>(i));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
// viz.addPointCloud (plane_i.planePointCloudPtr, color, name);
// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 4, name);
// }
// sprintf (name, "planeBorder_%02u", static_cast<unsigned>(i));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color2 (plane_i.contourPtr, 255, 255, 255);
// viz.addPointCloud (plane_i.contourPtr, color2, name);// contourPtr, planePointCloudPtr, polygonContourPtr
// //Edges
// if(mPbMap.edgeCloudPtr->size() > 0)
// {
// sprintf (name, "planeEdge_%02u", static_cast<unsigned>(i));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color4 (mPbMap.edgeCloudPtr, 255, 255, 0);
// viz.addPointCloud (mPbMap.edgeCloudPtr, color4, name);// contourPtr, planePointCloudPtr, polygonContourPtr
// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, name);
//
// sprintf (name, "edge%u", static_cast<unsigned>(i));
// viz.addLine (mPbMap.edgeCloudPtr->points.front(), mPbMap.edgeCloudPtr->points.back(), ared[3], agrn[3], ablu[3], name);
// }
sprintf (name, "approx_plane_%02d", int (i));
viz.addPolygon<PointT> (plane_i.polygonContourPtr, 0.5 * red[i%10], 0.5 * grn[i%10], 0.5 * blu[i%10], name);
}
//if(configPbMap.makeClusters)
// for(map<unsigned, std::vector<unsigned> >::iterator it=clusterize->groups.begin(); it != clusterize->groups.end(); it++)
// for(size_t i=0; i < it->second.size(); i++)
// {
// unsigned planeID = it->second[i];
// Plane &plane_i = mPbMap.vPlanes[planeID];
// sprintf (name, "plane_%02u", static_cast<unsigned>(planeID));
// pcl::visualization::PointCloudColorHandlerCustom <PointT> color (plane_i.planePointCloudPtr, red[planeID%10], grn[planeID%10], blu[planeID%10]);
// viz.addPointCloud (plane_i.planePointCloudPtr, color, name);// contourPtr, planePointCloudPtr, polygonContourPtr
// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, name);
// }
// Draw recognized plane labels
if(mpPbMapLocaliser != NULL)
for(map<string, pcl::PointXYZ>::iterator it = mpPbMapLocaliser->foundPlaces.begin(); it != mpPbMapLocaliser->foundPlaces.end(); it++)
viz.addText3D (it->first, it->second, 0.3, 0.9, 0.9, 0.9, it->first);
}
}
}
}
