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
keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
{
if (event.keyUp())
{
double opacity;
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 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)
{
static bool first_time = true;
double psize = 1.0,opacity = 1.0,linesize =1.0;
std::string cloud_name ("cloud");
boost::mutex::scoped_lock l(m_mutex);
if (new_cloud)
{
//typedef pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBNormal> ColorHandler;
typedef pcl::visualization::PointCloudColorHandlerGenericField <pcl::PointXYZRGBNormal> ColorHandler;
//ColorHandler Color_handler (normal_cloud);
ColorHandler Color_handler (normal_cloud,"curvature");
if (!first_time)
{
viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
viz.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
//viz.removePointCloud ("normalcloud");
viz.removePointCloud ("cloud");
}
else
first_time = false;
//viz.addPointCloudNormals<pcl::PointXYZRGBNormal> (normal_cloud, 139, 0.1, "normalcloud");
viz.addPointCloud<pcl::PointXYZRGBNormal> (normal_cloud, Color_handler, std::string("cloud"), 0);
viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, linesize, cloud_name);
viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, cloud_name);
viz.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, psize, cloud_name);
new_cloud = false;
}
}
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 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 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)
{
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 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);
}
}
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
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!keypoints_ && !cloud_)
{
boost::this_thread::sleep(boost::posix_time::seconds(1));
return;
}
FPS_CALC ("visualization");
viz.removePointCloud ("raw");
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> color_handler (cloud_);
viz.addPointCloud<pcl::PointXYZRGBA> (cloud_, color_handler, "raw");
if (!viz.updatePointCloud<pcl::PointXYZ> (keypoints_, "keypoints"))
{
viz.addPointCloud<pcl::PointXYZ> (keypoints_, "keypoints");
viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5.0, "keypoints");
viz.resetCameraViewpoint ("keypoints");
}
}
void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
float th_dd, int max_search)
{
CloudPtr cloud (new Cloud);
fromROSMsg (*input, *cloud);
pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setNormalSmoothingSize (10.0f);
ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
ne.setInputCloud (cloud);
ne.compute (*normal);
TicToc tt;
tt.tic ();
//OrganizedEdgeBase<PointXYZRGBA, Label> oed;
//OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
//OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
oed.setInputNormals (normal);
oed.setInputCloud (cloud);
oed.setDepthDisconThreshold (th_dd);
oed.setMaxSearchNeighbors (max_search);
oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
PointCloud<Label> labels;
std::vector<PointIndices> label_indices;
oed.compute (labels, label_indices);
print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");
// Make gray point clouds
for (int idx = 0; idx < (int)cloud->points.size (); idx++)
{
uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3;
cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray;
}
// Display edges in PCLVisualizer
viewer.setSize (640, 480);
viewer.addCoordinateSystem (0.2f);
viewer.addPointCloud (cloud, "original point cloud");
viewer.registerKeyboardCallback(&keyboard_callback);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges);
pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);
const int point_size = 2;
viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");
viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
pcl_sleep(0.1);
}
// Combine point clouds and edge labels
sensor_msgs::PointCloud2 output_edges;
toROSMsg (labels, output_edges);
concatenateFields (*input, output_edges, output);
}
void addNormals(pcl::visualization::PCLVisualizer &visualizer, const std::string &name)
{
pcl::PointCloud< pcl::PointNormal>::Ptr sv_normal_cloud = supervoxels->makeSupervoxelNormalCloud(supervoxel_clusters);
visualizer.addPointCloudNormals<pcl::PointNormal> (sv_normal_cloud, 1, 0.05f, name);
visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, normalsColor[0], normalsColor[1], normalsColor[2], 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 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());
}
}
