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));
}
}