// 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);
}
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;
}
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
viewer_matched_cloud(pcl::visualization::PCLVisualizer& viewer)
{
int text_id(0);
long cloud_size = matched_cloud->width * matched_cloud->height;
std::stringstream info;
info << "Points in matched cloud: " << cloud_size;
std::cout << "Matched cloud rendered with " << cloud_size << " points" << std::endl;
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(initial_cloud, 0, 255, 0);
viewer.addPointCloud<pcl::PointXYZ> (matched_cloud, single_color, "matched cloud");
viewer.addText(info.str(), 10, 20, "points", text_id);
if (r > 0)
{
viewer.addSphere (o, r, "sphere", 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);
}
}
}
}
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 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 ();
}
