TEST_F(test_map, map_image_corners) { SKIP_IF_FAST vector<Point> corners; // select points close to corners, avoid NaNs int margin = 160; corners.push_back(Point(margin, margin)); corners.push_back(Point(img.cols - margin, margin)); corners.push_back(Point(img.cols - margin, img.rows - margin)); corners.push_back(Point(margin, img.rows - margin)); PointCloudT corners3d; mapToCloud(corners3d, corners, img, cloud); EXPECT_EQ(corners.size(), corners3d.size()); EXPECT_EQ(4, corners.size()); EXPECT_EQ(4, corners3d.size()); for (PointCloudT::iterator it = corners3d.begin(), end = corners3d.end(); it != end; it++) { cout << boost::format("%d: %8f,%8f,%8f") % (it - corners3d.begin()) % (*it).x % (*it).y % (*it).z << endl; } // show the whole stuff PCLVisualizer vis; addPose(vis, PoseRT()); addMarkerPolygon3d(vis, corners3d); vis.addPointCloud(cloud); vis.spin(); }
int main() { PolygonMesh human_1_mesh; loadPolygonFilePLY("../data/human_1.ply", human_1_mesh); PCLVisualizer* viewer = new PCLVisualizer("Mesh viewer"); viewer->addPolygonMesh(human_1_mesh); while (!viewer->wasStopped()) { viewer->spinOnce(100); } viewer->close(); delete viewer; }
void run (float pair_width, float voxel_size, float max_coplanarity_angle, int num_hypotheses_to_show) { PointCloud<PointXYZ>::Ptr scene_points (new PointCloud<PointXYZ> ()), model_points (new PointCloud<PointXYZ> ()); PointCloud<Normal>::Ptr scene_normals (new PointCloud<Normal> ()), model_normals (new PointCloud<Normal> ()); // Get the points and normals from the scene if ( !vtk_to_pointcloud ("../../test/tum_table_scene.vtk", *scene_points, *scene_normals) ) return; vtkPolyData *vtk_model = vtkPolyData::New (); // Get the points and normals from the scene if ( !vtk_to_pointcloud ("../../test/tum_amicelli_box.vtk", *model_points, *model_normals, vtk_model) ) return; // The recognition object ObjRecRANSAC objrec (pair_width, voxel_size); objrec.setMaxCoplanarityAngleDegrees (max_coplanarity_angle); objrec.addModel (*model_points, *model_normals, "amicelli", vtk_model); // Switch to the test mode in which only oriented point pairs from the scene are sampled objrec.enterTestModeTestHypotheses (); // The visualizer PCLVisualizer viz; CallbackParameters params(objrec, viz, *scene_points, *scene_normals, num_hypotheses_to_show); viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (¶ms)); // Run the recognition and update the viewer update (¶ms); #ifdef _SHOW_SCENE_POINTS_ viz.addPointCloud (scene_points, "cloud in"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in"); #endif #ifdef _SHOW_OCTREE_POINTS_ PointCloud<PointXYZ>::Ptr octree_points (new PointCloud<PointXYZ> ()); objrec.getSceneOctree ().getFullLeavesPoints (*octree_points); viz.addPointCloud (octree_points, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points"); #endif #if defined _SHOW_OCTREE_NORMALS_ && defined _SHOW_OCTREE_POINTS_ PointCloud<Normal>::Ptr octree_normals (new PointCloud<Normal> ()); objrec.getSceneOctree ().getNormalsOfFullLeaves (*octree_normals); viz.addPointCloudNormals<PointXYZ,Normal> (octree_points, octree_normals, 1, 6.0f, "normals out"); #endif // Enter the main loop while (!viz.wasStopped ()) { //main loop of the visualizer viz.spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } vtk_model->Delete (); }
void show_octree (ORROctree* octree, PCLVisualizer& viz) { vtkSmartPointer<vtkPolyData> vtk_octree = vtkSmartPointer<vtkPolyData>::New (); vtkSmartPointer<vtkAppendPolyData> append = vtkSmartPointer<vtkAppendPolyData>::New (); cout << "There are " << octree->getFullLeaves ().size () << " full leaves.\n"; std::vector<ORROctree::Node*>& full_leaves = octree->getFullLeaves (); for (const auto &full_leaf : full_leaves) // Add it to the other cubes node_to_cube (full_leaf, append); // Save the result append->Update(); vtk_octree->DeepCopy (append->GetOutput ()); // Add to the visualizer vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer (); vtkSmartPointer<vtkActor> octree_actor = vtkSmartPointer<vtkActor>::New(); vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New (); mapper->SetInputData (vtk_octree); octree_actor->SetMapper(mapper); // Set the appearance & add to the renderer octree_actor->GetProperty ()->SetColor (1.0, 1.0, 1.0); renderer->AddActor(octree_actor); }
void ObjectDetectionViewer::onUpdate(PCLVisualizer& visualizer) { Table::Collection tables; Object::Collection objects; { mutex::scoped_lock(resultMutex); tables = this->detectedTables; objects = this->detectedObjects; } visualizer.removeAllShapes(); char strbuf[20]; int tableId = 0; for (std::vector<Table::Ptr>::iterator table = tables->begin(); table != tables->end(); ++table) { sprintf(strbuf, "table%d", tableId++); visualizer.addPolygon<Point>((*table)->getConvexHull(), 255, 0, 0, strbuf); } int objectId = 0; for (std::vector<Object::Ptr>::iterator object = objects->begin(); object != objects->end(); ++object) { sprintf(strbuf, "object%d", objectId++); visualizer.addPolygon<Point>((*object)->getBaseConvexHull(), 0, 255, 0, strbuf); } // TODO: print processing time in lower left corner (together with FPS) }
void updateViewer (ORROctree& octree, PCLVisualizer& viz, std::vector<ORROctree::Node*>::iterator leaf) { viz.removeAllShapes(); const float *b = (*leaf)->getBounds (), *center = (*leaf)->getData ()->getPoint (); float radius = 0.1f*octree.getRoot ()->getRadius (); // Add the main leaf as a cube viz.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 0.0, 0.0, 1.0, "main cube"); // Get all full leaves intersecting a sphere with certain radius std::list<ORROctree::Node*> intersected_leaves; octree.getFullLeavesIntersectedBySphere(center, radius, intersected_leaves); char cube_id[128]; int i = 0; // Show the cubes for ( std::list<ORROctree::Node*>::iterator it = intersected_leaves.begin () ; it != intersected_leaves.end () ; ++it ) { sprintf(cube_id, "cube %i", ++i); b = (*it)->getBounds (); viz.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 1.0, 1.0, 0.0, cube_id); } // Get a random full leaf on the sphere defined by 'center' and 'radius' ORROctree::Node *rand_leaf = octree.getRandomFullLeafOnSphere (center, radius); if ( rand_leaf ) { pcl::ModelCoefficients sphere_coeffs; sphere_coeffs.values.resize (4); sphere_coeffs.values[0] = rand_leaf->getCenter ()[0]; sphere_coeffs.values[1] = rand_leaf->getCenter ()[1]; sphere_coeffs.values[2] = rand_leaf->getCenter ()[2]; sphere_coeffs.values[3] = 0.5f*(b[1] - b[0]); viz.addSphere (sphere_coeffs, "random_full_leaf"); } }
void run (const char* file_name, float voxel_size) { PointCloud<PointXYZ>::Ptr points_in (new PointCloud<PointXYZ> ()); PointCloud<PointXYZ>::Ptr points_out (new PointCloud<PointXYZ> ()); // Get the points and normals from the input vtk file if ( !vtk_to_pointcloud (file_name, *points_in) ) return; // Build the octree with the desired resolution ORROctree octree; octree.build (*points_in, voxel_size); // Now build the octree z-projection ORROctreeZProjection zproj; zproj.build (octree, 0.15f*voxel_size, 0.15f*voxel_size); // The visualizer PCLVisualizer viz; show_octree(&octree, viz); show_octree_zproj(&zproj, viz); #ifdef _SHOW_POINTS_ // Get the point of every full octree leaf octree.getFullLeafPoints (*points_out); // Add the point clouds viz.addPointCloud (points_in, "cloud in"); viz.addPointCloud (points_out, "cloud out"); // Change the appearance viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud out"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud out"); #endif // Enter the main loop while (!viz.wasStopped ()) { //main loop of the visualizer viz.spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } }
void show_octree_zproj (ORROctreeZProjection* zproj, PCLVisualizer& viz) { cout << "There is (are) " << zproj->getFullPixels ().size () << " full pixel(s).\n"; vtkSmartPointer<vtkAppendPolyData> upper_bound = vtkSmartPointer<vtkAppendPolyData>::New (), lower_bound = vtkSmartPointer<vtkAppendPolyData>::New (); const ORROctreeZProjection::Pixel *pixel; const float *b = zproj->getBounds (); float x, y, psize = zproj->getPixelSize (); int i, j, width, height; zproj->getNumberOfPixels (width, height); for ( i = 0, x = b[0] ; i < width ; ++i, x += psize ) { for ( j = 0, y = b[2] ; j < height ; ++j, y += psize ) { pixel = zproj->getPixel (i, j); if ( !pixel ) continue; rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z1 (), lower_bound); rectangle_to_vtk (x, x + psize, y, y + psize, pixel->z2 (), upper_bound); } } // Save the result upper_bound->Update(); lower_bound->Update(); // Add to the visualizer vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer (); vtkSmartPointer<vtkActor> upper_actor = vtkSmartPointer<vtkActor>::New(), lower_actor = vtkSmartPointer<vtkActor>::New(); vtkSmartPointer<vtkDataSetMapper> upper_mapper = vtkSmartPointer<vtkDataSetMapper>::New (), lower_mapper = vtkSmartPointer<vtkDataSetMapper>::New (); upper_mapper->SetInputData (upper_bound->GetOutput ()); upper_actor->SetMapper(upper_mapper); lower_mapper->SetInputData (lower_bound->GetOutput ()); lower_actor->SetMapper(lower_mapper); // Set the appearance & add to the renderer upper_actor->GetProperty ()->SetColor (1.0, 0.0, 0.0); renderer->AddActor(upper_actor); lower_actor->GetProperty ()->SetColor (1.0, 1.0, 0.0); renderer->AddActor(lower_actor); }
int main (int argc, char *argv[]) { if (argc < 3) { cout << "Enter the two files for registration ..\n"; return -1; } pcl::console::setVerbosityLevel (pcl::console::L_DEBUG); string sourcefile = argv[1]; string targetfile = argv[2]; CloudPtr cloud1 ( new Cloud ); CloudPtr cloud2 ( new Cloud ); readPCDBinaryFile (sourcefile.c_str (), cloud1); readPCDBinaryFile (targetfile.c_str (), cloud2); //pcl::IterativeClosestPointNonLinear <Point, Point> icp; pcl::IterativeClosestPoint <Point, Point> icp; icp.setInputSource (cloud1); icp.setInputTarget (cloud2); icp.setMaximumIterations (2500); icp.setTransformationEpsilon (0.0000001); Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ()); Eigen::Translation3f init_translation (1.79387, 0.720047, 0); //Eigen::Matrix4f init_guess = Eigen::Matrix4f::Identity (); Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix (); CloudPtr output (new Cloud); icp.align (*output, init_guess); //pcl::transformPointCloud (*cloud1, *output, icp.getFinalTransformation ()); std::cout << "ICP NL has converged:" << icp.hasConverged () << " score: " << icp.getFitnessScore () << std::endl; cout << "--------- Final transformation ---------------\n"; cout << icp.getFinalTransformation () << "\n\n"; CloudPtr cloud1_ig (new Cloud); pcl::transformPointCloud (*cloud1, *cloud1_ig, init_guess); PCLVisualizer* p = new PCLVisualizer (argc, argv, "Registration"); int vp1 = 1; p->createViewPort (0.0, 0.0, 0.5, 1.0, vp1); int vp2 = 2; p->createViewPort (0.5, 0.0, 1.0, 1.0, vp2); p->setBackgroundColor (113.0/255, 113.0/255, 154.0/255); int color[3] = { 255, 0, 0}; displayPointCloud (p, cloud1_ig, color, (char *) "opcloud1", vp1); color[0] = 0; color[1] = 255; color[2] = 0; displayPointCloud (p, cloud2, color, (char *) "opcloud2", vp1); color[0] = 0; color[1] = 255; color[2] = 0; displayPointCloud (p, output, color, (char *) "opcloud11", vp2); color[0] = 255; color[1] = 0; color[2] = 0; displayPointCloud (p, cloud2, color, (char *) "opcloud12", vp2); p->spin (); return 0; }
void show_octree (ORROctree* octree, PCLVisualizer& viz, bool show_full_leaves_only) { vtkSmartPointer<vtkPolyData> vtk_octree = vtkSmartPointer<vtkPolyData>::New (); vtkSmartPointer<vtkAppendPolyData> append = vtkSmartPointer<vtkAppendPolyData>::New (); cout << "There are " << octree->getFullLeaves ().size () << " full leaves.\n"; if ( show_full_leaves_only ) { std::vector<ORROctree::Node*>& full_leaves = octree->getFullLeaves (); for ( std::vector<ORROctree::Node*>::iterator it = full_leaves.begin () ; it != full_leaves.end () ; ++it ) // Add it to the other cubes node_to_cube (*it, append); } else { ORROctree::Node* node; std::list<ORROctree::Node*> nodes; nodes.push_back (octree->getRoot ()); while ( !nodes.empty () ) { node = nodes.front (); nodes.pop_front (); // Visualize the node if it has children if ( node->getChildren () ) { // Add it to the other cubes node_to_cube (node, append); // Add all the children to the working list for ( int i = 0 ; i < 8 ; ++i ) nodes.push_back (node->getChild (i)); } // If we arrived at a leaf -> check if it's full and visualize it else if ( node->getData () ) node_to_cube (node, append); } } // Just print the leaf size std::vector<ORROctree::Node*>::iterator first_leaf = octree->getFullLeaves ().begin (); if ( first_leaf != octree->getFullLeaves ().end () ) printf("leaf size = %f\n", (*first_leaf)->getBounds ()[1] - (*first_leaf)->getBounds ()[0]); // Save the result append->Update(); vtk_octree->DeepCopy (append->GetOutput ()); // Add to the visualizer vtkRenderer *renderer = viz.getRenderWindow ()->GetRenderers ()->GetFirstRenderer (); vtkSmartPointer<vtkActor> octree_actor = vtkSmartPointer<vtkActor>::New(); vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New (); mapper->SetInput(vtk_octree); octree_actor->SetMapper(mapper); // Set the appearance & add to the renderer octree_actor->GetProperty ()->SetColor (1.0, 1.0, 1.0); octree_actor->GetProperty ()->SetLineWidth (1); octree_actor->GetProperty ()->SetRepresentationToWireframe (); renderer->AddActor(octree_actor); }
TEST_F(test_map, map_markers) { SKIP_IF_FAST vector<Point> red2d; vector<Point> green2d; vector<Point> yellow2d; vector<Point> blue2d; red2d.push_back(Point(727, 269)); red2d.push_back(Point(716, 448)); red2d.push_back(Point(806, 449)); red2d.push_back(Point(834, 268)); green2d.push_back(Point(336, 195)); green2d.push_back(Point(1157, 198)); yellow2d.push_back(Point(338, 183)); yellow2d.push_back(Point(1154, 189)); blue2d.push_back(Point(697, 86)); blue2d.push_back(Point(371, 92)); PointCloudT red3d; PointCloudT green3d; PointCloudT yellow3d; PointCloudT blue3d; mapToCloud(red3d, red2d, markerImg, cloud); mapToCloud(green3d, green2d, markerImg, cloud); mapToCloud(yellow3d, yellow2d, markerImg, cloud); mapToCloud(blue3d, blue2d, markerImg, cloud); /* PointCloudT red3d_hscaled; PointCloudT green3d_hscaled; PointCloudT yellow3d_hscaled; PointCloudT blue3d_hscaled; mapToCloud(red3d_hscaled, red2d, markerImg, cloud, false); mapToCloud(green3d_hscaled, green2d, markerImg, cloud, false); mapToCloud(yellow3d_hscaled, yellow2d, markerImg, cloud, false); mapToCloud(blue3d_hscaled, blue2d, markerImg, cloud, false); EXPECT_EQ(red2d.size(), red3d_hscaled.size()); EXPECT_EQ(green2d.size(), green3d_hscaled.size()); EXPECT_EQ(yellow2d.size(), yellow3d_hscaled.size()); EXPECT_EQ(blue2d.size(), blue3d_hscaled.size()); */ EXPECT_EQ(640, cloud.width); EXPECT_EQ(480, cloud.height); imshow("MapMarkers", markerImg); waitKey(-1); // TODO: PCLVisualizer segfaults for no reason... #ifdef ENABLE_PCLVIS // show the whole stuff PCLVisualizer vis; addPose(vis, PoseRT()); addMarkerPolygon3d(vis, red3d, 255, 0, 0, "red"); addMarkerPolygon3d(vis, green3d, 0, 216, 0, "green"); addMarkerPolygon3d(vis, yellow3d, 255, 216, 0, "yellow"); addMarkerPolygon3d(vis, blue3d, 0, 66, 255, "blue"); /* addMarkerPolygon3d(vis, red3d_hscaled, 204, 153, 153, "red_hscaled"); addMarkerPolygon3d(vis, green3d_hscaled, 153, 204, 153, "green_hscaled"); addMarkerPolygon3d(vis, yellow3d_hscaled, 204, 204, 153, "yellow_hscaled"); addMarkerPolygon3d(vis, blue3d_hscaled, 153, 153, 204, "blue_hscaled");*/ vis.addPointCloud(cloud); vis.spin(); #endif }
void visualize (const ModelLibrary::HashTable* hash_table) { const ModelLibrary::HashTableCell* cells = hash_table->getVoxels (); size_t max_num_entries = 0; int id3[3], num_cells = hash_table->getNumberOfVoxels (); double cell_center[3]; vtkPoints* sphere_centers = vtkPoints::New (VTK_DOUBLE); vtkDoubleArray* scale = vtkDoubleArray::New (); scale->SetNumberOfComponents(1); // Get the positions of the spheres (one sphere per full cell) for (int i = 0 ; i < num_cells ; ++i, ++cells) { // Does the cell have any entries? if (cells->size ()) { // Insert the center of the current voxel in the point set hash_table->compute3dId (i, id3); hash_table->computeVoxelCenter (id3, cell_center); sphere_centers->InsertNextPoint (cell_center); // Save the number of entries scale->InsertNextValue (static_cast<double> (cells->size ())); // Get the max if (cells->size () > max_num_entries) max_num_entries = cells->size (); } } PCLVisualizer vis; vis.setBackgroundColor (0.1, 0.1, 0.1); // Is there something to visualize? if (max_num_entries) { // Compute the factor which maps all the scale values in (0, 1] double factor = 1.0/static_cast<double> (max_num_entries); // Set the true scale for (vtkIdType i = 0 ; i < scale->GetNumberOfTuples () ; ++i) scale->SetValue(i, factor*scale->GetValue (i)); // Input for the glyph object: the centers + scale vtkPolyData *positions = vtkPolyData::New (); positions->SetPoints (sphere_centers); positions->GetPointData ()->SetScalars (scale); // The spheres vtkSphereSource* sphere_src = vtkSphereSource::New (); sphere_src->SetPhiResolution(8); sphere_src->SetThetaResolution(8); sphere_src->SetRadius(0.5*hash_table->getVoxelSpacing ()[0]); // Now that we have the points and the corresponding scalars, build the glyph object vtkGlyph3D *glyph = vtkGlyph3D::New (); glyph->SetScaleModeToScaleByScalar (); glyph->SetColorModeToColorByScalar (); glyph->SetInput (positions); glyph->SetSource (sphere_src->GetOutput ()); glyph->Update (); vtkSmartPointer<vtkPolyData> glyph_output (glyph->GetOutput ()); vis.addModelFromPolyData(glyph_output); // Cleanup glyph->Delete (); positions->Delete (); sphere_src->Delete (); } vis.spin(); // Cleanup sphere_centers->Delete(); scale->Delete(); }
int main (int argc, char** argv) { // Read command line arguments. for (char c; (c = getopt (argc, argv, "s:hc:r:")) != -1; ) { switch (c) { case 'c': coordinate_frame = RangeImage::CoordinateFrame (strtol (optarg, NULL, 0)); break; case 'r': { angular_resolution = strtod (optarg, NULL); cout << PVARN(angular_resolution); break; } case 's': { source = strtol (optarg, NULL, 0); if (source < 0 || source > 2) { cout << "Source "<<source<<" is unknown.\n"; exit (0); } cout << "Receiving "<<(source==0 ? "point clouds" : (source==1 ? "depth images" : "disparity images"))<<".\n"; break; } case 'h': default: printUsage (argv[0]); exit (0); } } angular_resolution = deg2rad (angular_resolution); ros::init (argc, argv, "tutorial_range_image_live_viewer"); ros::NodeHandle node_handle; ros::Subscriber subscriber, subscriber2; if (node_handle.resolveName("input")=="/input") std::cerr << "Did you forget input:=<your topic>?\n"; if (source == 0) subscriber = node_handle.subscribe ("input", 1, cloud_msg_cb); else if (source == 1) { if (node_handle.resolveName("input2")=="/input2") std::cerr << "Did you forget input2:=<your camera_info_topic>?\n"; subscriber = node_handle.subscribe ("input", 1, depth_image_msg_cb); subscriber2 = node_handle.subscribe ("input2", 1, camera_info_msg_cb); } else if (source == 2) subscriber = node_handle.subscribe ("input", 1, disparity_image_msg_cb); PCLVisualizer viewer (argc, argv, "Live viewer - point cloud"); RangeImageVisualizer range_image_widget("Live viewer - range image"); RangeImagePlanar* range_image_planar; RangeImage* range_image; if (source==0) range_image = new RangeImage; else { range_image_planar = new RangeImagePlanar; range_image = range_image_planar; } while (node_handle.ok ()) { usleep (10000); viewer.spinOnce (10); RangeImageVisualizer::spinOnce (); ros::spinOnce (); if (source==0) { // If no new message received: continue if (!cloud_msg || cloud_msg == old_cloud_msg) continue; old_cloud_msg = cloud_msg; Eigen::Affine3f sensor_pose(Eigen::Affine3f::Identity()); PointCloud<PointWithViewpoint> far_ranges; RangeImage::extractFarRanges(*cloud_msg, far_ranges); if (pcl::getFieldIndex(*cloud_msg, "vp_x")>=0) { PointCloud<PointWithViewpoint> tmp_pc; fromROSMsg(*cloud_msg, tmp_pc); Eigen::Vector3f average_viewpoint = RangeImage::getAverageViewPoint(tmp_pc); sensor_pose = Eigen::Translation3f(average_viewpoint) * sensor_pose; } PointCloud<PointType> point_cloud; fromROSMsg(*cloud_msg, point_cloud); range_image->createFromPointCloud(point_cloud, angular_resolution, deg2rad(360.0f), deg2rad(180.0f), sensor_pose, coordinate_frame, noise_level, min_range, border_size); } else if (source==1) { // If no new message received: continue if (!depth_image_msg || depth_image_msg == old_depth_image_msg || !camera_info_msg) continue; old_depth_image_msg = depth_image_msg; range_image_planar->setDepthImage(reinterpret_cast<const float*> (&depth_image_msg->data[0]), depth_image_msg->width, depth_image_msg->height, camera_info_msg->P[2], camera_info_msg->P[6], camera_info_msg->P[0], camera_info_msg->P[5], angular_resolution); } else if (source==2) { // If no new message received: continue if (!disparity_image_msg || disparity_image_msg == old_disparity_image_msg) continue; old_disparity_image_msg = disparity_image_msg; range_image_planar->setDisparityImage(reinterpret_cast<const float*> (&disparity_image_msg->image.data[0]), disparity_image_msg->image.width, disparity_image_msg->image.height, disparity_image_msg->f, disparity_image_msg->T, angular_resolution); } range_image_widget.setRangeImage (*range_image); viewer.removePointCloud ("range image cloud"); pcl_visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> color_handler_cloud(*range_image, 200, 200, 200); viewer.addPointCloud (*range_image, color_handler_cloud, "range image cloud"); } }
void PointCloudViewer::onInit(PCLVisualizer& visualizer) { // position viewport 3m behind kinect, but look around the point 2m in front of it visualizer.setCameraPosition(0., 0., -3., 0., 0., 2., 0., -1., 0.); visualizer.setCameraClipDistances(1.0, 10.0); visualizer.setBackgroundColor(0.3, 0.3, 0.8); }
void run (const char* file_name, float voxel_size) { PointCloud<PointXYZ>::Ptr points_in (new PointCloud<PointXYZ> ()); PointCloud<PointXYZ>::Ptr points_out (new PointCloud<PointXYZ> ()); PointCloud<Normal>::Ptr normals_in (new PointCloud<Normal> ()); PointCloud<Normal>::Ptr normals_out (new PointCloud<Normal> ()); // Get the points and normals from the input vtk file #ifdef _SHOW_OCTREE_NORMALS_ if ( !vtk_to_pointcloud (file_name, *points_in, &(*normals_in)) ) return; #else if ( !vtk_to_pointcloud (file_name, *points_in, NULL) ) return; #endif // Build the octree with the desired resolution ORROctree octree; if ( normals_in->size () ) octree.build (*points_in, voxel_size, &*normals_in); else octree.build (*points_in, voxel_size); // Get the first full leaf in the octree (arbitrary order) std::vector<ORROctree::Node*>::iterator leaf = octree.getFullLeaves ().begin (); // Get the average points in every full octree leaf octree.getFullLeavesPoints (*points_out); // Get the average normal at the points in each leaf if ( normals_in->size () ) octree.getNormalsOfFullLeaves (*normals_out); // The visualizer PCLVisualizer viz; // Register a keyboard callback CallbackParameters params(octree, viz, leaf); viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (¶ms)); // Add the point clouds viz.addPointCloud (points_in, "cloud in"); viz.addPointCloud (points_out, "cloud out"); if ( normals_in->size () ) viz.addPointCloudNormals<PointXYZ,Normal> (points_out, normals_out, 1, 6.0f, "normals out"); // Change the appearance viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "cloud out"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cloud out"); // Convert the octree to a VTK poly-data object // show_octree(&octree, viz, true/*show full leaves only*/); updateViewer (octree, viz, leaf); // Enter the main loop while (!viz.wasStopped ()) { //main loop of the visualizer viz.spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } }
void visualize (const ModelLibrary::HashTable& hash_table) { PCLVisualizer vis; vis.setBackgroundColor (0.1, 0.1, 0.1); const ModelLibrary::HashTableCell* cells = hash_table.getVoxels (); size_t max_num_entries = 0; int i, id3[3], num_cells = hash_table.getNumberOfVoxels (); float half_side, b[6], cell_center[3], spacing = hash_table.getVoxelSpacing ()[0]; char cube_id[128]; // Just get the maximal number of entries in the cells for ( i = 0 ; i < num_cells ; ++i, ++cells ) { if (cells->size ()) // That's the number of models in the cell (it's maximum one, since we loaded only one model) { size_t num_entries = (*cells->begin ()).second.size(); // That's the number of entries in the current cell for the model we loaded // Get the max number of entries if ( num_entries > max_num_entries ) max_num_entries = num_entries; } } // Now, that we have the max. number of entries, we can compute the // right scale factor for the spheres float s = (0.5f*spacing)/static_cast<float> (max_num_entries); cout << "s = " << s << ", max_num_entries = " << max_num_entries << endl; // Now, render a sphere with the right radius at the right place for ( i = 0, cells = hash_table.getVoxels () ; i < num_cells ; ++i, ++cells ) { // Does the cell have any entries? if (cells->size ()) { hash_table.compute3dId (i, id3); hash_table.computeVoxelCenter (id3, cell_center); // That's half of the cube's side length half_side = s*static_cast<float> ((*cells->begin ()).second.size ()); // Adjust the bounds of the cube b[0] = cell_center[0] - half_side; b[1] = cell_center[0] + half_side; b[2] = cell_center[1] - half_side; b[3] = cell_center[1] + half_side; b[4] = cell_center[2] - half_side; b[5] = cell_center[2] + half_side; // Set the id sprintf (cube_id, "cube %i", i); // Add to the visualizer vis.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 1.0, 1.0, 0.0, cube_id); } } vis.addCoordinateSystem(1.5, "global"); vis.resetCamera (); // Enter the main loop while (!vis.wasStopped ()) { vis.spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } }
void run (float pair_width, float voxel_size, float max_coplanarity_angle) { // The object recognizer ObjRecRANSAC objrec (pair_width, voxel_size); objrec.setMaxCoplanarityAngleDegrees (max_coplanarity_angle); // The models to be loaded list<string> model_names; model_names.emplace_back("tum_amicelli_box"); model_names.emplace_back("tum_rusk_box"); model_names.emplace_back("tum_soda_bottle"); list<PointCloud<PointXYZ>::Ptr> model_points_list; list<PointCloud<Normal>::Ptr> model_normals_list; list<vtkSmartPointer<vtkPolyData> > vtk_models_list; // Load the models and add them to the recognizer for (const auto &model_name : model_names) { PointCloud<PointXYZ>::Ptr model_points (new PointCloud<PointXYZ> ()); model_points_list.push_back (model_points); PointCloud<Normal>::Ptr model_normals (new PointCloud<Normal> ()); model_normals_list.push_back (model_normals); vtkSmartPointer<vtkPolyData> vtk_model = vtkSmartPointer<vtkPolyData>::New (); vtk_models_list.push_back (vtk_model); // Compose the file string file_name = string("../../test/") + model_name + string (".vtk"); // Get the points and normals from the input model if ( !vtk2PointCloud (file_name.c_str (), *model_points, *model_normals, vtk_model) ) continue; // Add the model objrec.addModel (*model_points, *model_normals, model_name, vtk_model); } // The scene in which the models are supposed to be recognized PointCloud<PointXYZ>::Ptr non_plane_points (new PointCloud<PointXYZ> ()), plane_points (new PointCloud<PointXYZ> ()); PointCloud<Normal>::Ptr non_plane_normals (new PointCloud<Normal> ()); // Detect the largest plane in the dataset if ( !loadScene ("../../test/tum_table_scene.vtk", *non_plane_points, *non_plane_normals, *plane_points) ) return; // The parameters for the callback function and the visualizer PCLVisualizer viz; CallbackParameters params(objrec, viz, *non_plane_points, *non_plane_normals); viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (¶ms)); // Run the recognition and update the viewer. Have a look at this method, to see how to start the recognition and use the result! update (¶ms); // From this line on: visualization stuff only! #ifdef _SHOW_OCTREE_ show_octree(objrec.getSceneOctree (), viz); #endif #ifdef _SHOW_SCENE_POINTS_ viz.addPointCloud (scene_points, "scene points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "scene points"); #endif #ifdef _SHOW_OCTREE_POINTS_ PointCloud<PointXYZ>::Ptr octree_points (new PointCloud<PointXYZ> ()); objrec.getSceneOctree ().getFullLeavesPoints (*octree_points); viz.addPointCloud (octree_points, "octree points"); // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "octree points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points"); viz.addPointCloud (plane_points, "plane points"); viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.9, 0.9, 0.9, "plane points"); #endif #if defined _SHOW_OCTREE_NORMALS_ && defined _SHOW_OCTREE_POINTS_ PointCloud<Normal>::Ptr normals_octree (new PointCloud<Normal> ()); objrec.getSceneOctree ().getNormalsOfFullLeaves (*normals_octree); viz.addPointCloudNormals<PointXYZ,Normal> (points_octree, normals_octree, 1, 6.0f, "normals out"); #endif // Enter the main loop while (!viz.wasStopped ()) { //main loop of the visualizer viz.spinOnce (100); boost::this_thread::sleep (boost::posix_time::microseconds (100000)); } }