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 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++; viewer.removeShape ("text", 0); viewer.addText (ss.str(), 200, 300, "text", 0); //FIXME: possible race condition here: user_data++; }
/* \brief Helper function that draw info for the user on the viewer * */ void showLegend(bool showCubes) { char dataDisplay[256]; sprintf(dataDisplay, "Displaying data as %s", (showCubes) ? ("CUBES") : ("POINTS")); viz.removeShape("disp_t"); viz.addText(dataDisplay, 0, 60, 1.0, 0.0, 0.0, "disp_t"); char level[256]; sprintf(level, "Displayed depth is %d on %d", displayedDepth, octree.getTreeDepth()); viz.removeShape("level_t1"); viz.addText(level, 0, 45, 1.0, 0.0, 0.0, "level_t1"); viz.removeShape("level_t2"); sprintf(level, "Voxel size: %.4fm [%lu voxels]", sqrt(octree.getVoxelSquaredSideLen(displayedDepth)), displayCloud->points.size()); viz.addText(level, 0, 30, 1.0, 0.0, 0.0, "level_t2"); viz.removeShape("org_t"); if (showPointsWithCubes) viz.addText("Displaying original cloud", 0, 15, 1.0, 0.0, 0.0, "org_t"); }
void viewer_initial_cloud(pcl::visualization::PCLVisualizer& viewer) { //int text_id(0); long cloud_size = initial_cloud->width * initial_cloud->height; std::stringstream info; info << "Points in initial cloud: " << cloud_size; std::cout << "Initial cloud rendered with " << cloud_size << " points" << std::endl; pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(initial_cloud, 255, 0, 0); viewer.addPointCloud<pcl::PointXYZ> (initial_cloud, single_color, "initial cloud"); viewer.addText(info.str(), 10, 20, "points", 0); }
void Inspector::enableModel() { use_intrinsics_ = true; if(use_intrinsics_ && !dddm_) { cout << "Cannot apply non-existent distortion model. Supply one at the command line." << endl; use_intrinsics_ = false; return; } pcd_vis_.removeShape("text"); int x = 10; int y = 10; int fontsize = 16; pcd_vis_.addText("undistorted", x, y, fontsize, 0, 1, 0, "text"); }
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 initVisualizer (pcl::visualization::PCLVisualizer &viewer) { // Setting the initial viewer parameters viewer.initCameraParameters (); viewer.setBackgroundColor (0, 0, 0); viewer.addCoordinateSystem (1000); viewer.camera_.view[0] = 0; viewer.camera_.view[1] = 0; viewer.camera_.view[2] = 1; viewer.camera_.pos[0] = 8000; viewer.camera_.pos[1] = 20000; viewer.camera_.pos[2] = 2500; viewer.updateCamera (); viewer.addText ("Shift + click to select noisy objects. \nPress 0 to confirm the removal.", 50, 300, "user"); }
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 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 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; }
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); } } } }