38 #ifndef PCL_PCL_VISUALIZER_IMPL_H_ 39 #define PCL_PCL_VISUALIZER_IMPL_H_ 41 #include <vtkVersion.h> 42 #include <vtkSmartPointer.h> 43 #include <vtkCellArray.h> 44 #include <vtkLeaderActor2D.h> 45 #include <vtkVectorText.h> 46 #include <vtkAlgorithmOutput.h> 47 #include <vtkFollower.h> 49 #include <vtkSphereSource.h> 50 #include <vtkProperty2D.h> 51 #include <vtkDataSetSurfaceFilter.h> 52 #include <vtkPointData.h> 53 #include <vtkPolyDataMapper.h> 54 #include <vtkProperty.h> 55 #include <vtkMapper.h> 56 #include <vtkCellData.h> 57 #include <vtkDataSetMapper.h> 58 #include <vtkRenderer.h> 59 #include <vtkRendererCollection.h> 60 #include <vtkAppendPolyData.h> 61 #include <vtkTextProperty.h> 62 #include <vtkLODActor.h> 63 #include <vtkLineSource.h> 65 #include <pcl/visualization/common/shapes.h> 68 template <
typename Po
intT>
bool 71 const std::string &
id,
int viewport)
75 return (addPointCloud<PointT> (cloud, geometry_handler,
id, viewport));
79 template <
typename Po
intT>
bool 83 const std::string &
id,
int viewport)
87 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
91 if (pcl::traits::has_color<PointT>())
101 template <
typename Po
intT>
bool 105 const std::string &
id,
int viewport)
111 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
112 am_it->second.geometry_handlers.push_back (geometry_handler);
122 template <
typename Po
intT>
bool 126 const std::string &
id,
int viewport)
130 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
144 template <
typename Po
intT>
bool 148 const std::string &
id,
int viewport)
151 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
152 if (am_it != cloud_actor_map_->end ())
156 am_it->second.color_handlers.push_back (color_handler);
165 template <
typename Po
intT>
bool 170 const std::string &
id,
int viewport)
173 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
174 if (am_it != cloud_actor_map_->end ())
178 am_it->second.geometry_handlers.push_back (geometry_handler);
179 am_it->second.color_handlers.push_back (color_handler);
186 template <
typename Po
intT>
bool 191 const std::string &
id,
int viewport)
195 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
207 template <
typename Po
intT>
void 208 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
216 allocVtkPolyData (polydata);
218 polydata->SetVerts (vertices);
222 vertices = polydata->GetVerts ();
226 vtkIdType nr_points = cloud->
points.size ();
232 points->SetDataTypeToFloat ();
233 polydata->SetPoints (points);
235 points->SetNumberOfPoints (nr_points);
238 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
243 for (vtkIdType i = 0; i < nr_points; ++i)
244 memcpy (&data[i * 3], &cloud->
points[i].x, 12);
249 for (vtkIdType i = 0; i < nr_points; ++i)
252 if (!pcl_isfinite (cloud->
points[i].x) ||
253 !pcl_isfinite (cloud->
points[i].y) ||
254 !pcl_isfinite (cloud->
points[i].z))
257 memcpy (&data[j * 3], &cloud->
points[i].x, 12);
261 points->SetNumberOfPoints (nr_points);
265 updateCells (cells, initcells, nr_points);
268 vertices->SetCells (nr_points, cells);
272 template <
typename Po
intT>
void 273 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
281 allocVtkPolyData (polydata);
283 polydata->SetVerts (vertices);
289 polydata->SetPoints (points);
291 vtkIdType nr_points = points->GetNumberOfPoints ();
294 vertices = polydata->GetVerts ();
299 updateCells (cells, initcells, nr_points);
301 vertices->SetCells (nr_points, cells);
305 template <
typename Po
intT>
bool 308 double r,
double g,
double b,
const std::string &
id,
int viewport)
315 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
316 if (am_it != shape_actor_map_->end ())
321 #if VTK_MAJOR_VERSION < 6 322 all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
324 all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
329 #if VTK_MAJOR_VERSION < 6 330 surface_filter->AddInput (vtkUnstructuredGrid::SafeDownCast (data));
332 surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
335 #if VTK_MAJOR_VERSION < 6 336 all_data->AddInput (poly_data);
338 all_data->AddInputData (poly_data);
343 createActorFromVTKDataSet (all_data->GetOutput (), actor);
344 actor->GetProperty ()->SetRepresentationToWireframe ();
345 actor->GetProperty ()->SetColor (r, g, b);
346 actor->GetMapper ()->ScalarVisibilityOff ();
347 removeActorFromRenderer (am_it->second, viewport);
348 addActorToRenderer (actor, viewport);
351 (*shape_actor_map_)[id] = actor;
357 createActorFromVTKDataSet (data, actor);
358 actor->GetProperty ()->SetRepresentationToWireframe ();
359 actor->GetProperty ()->SetColor (r, g, b);
360 actor->GetMapper ()->ScalarVisibilityOff ();
361 addActorToRenderer (actor, viewport);
364 (*shape_actor_map_)[id] = actor;
371 template <
typename Po
intT>
bool 374 double r,
double g,
double b,
const std::string &
id,
int viewport)
381 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
382 if (am_it != shape_actor_map_->end ())
387 #if VTK_MAJOR_VERSION < 6 388 all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
390 all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
395 #if VTK_MAJOR_VERSION < 6 396 surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
398 surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
401 #if VTK_MAJOR_VERSION < 6 402 all_data->AddInput (poly_data);
404 all_data->AddInputData (poly_data);
409 createActorFromVTKDataSet (all_data->GetOutput (), actor);
410 actor->GetProperty ()->SetRepresentationToWireframe ();
411 actor->GetProperty ()->SetColor (r, g, b);
412 actor->GetMapper ()->ScalarVisibilityOn ();
413 actor->GetProperty ()->BackfaceCullingOff ();
414 removeActorFromRenderer (am_it->second, viewport);
415 addActorToRenderer (actor, viewport);
418 (*shape_actor_map_)[id] = actor;
424 createActorFromVTKDataSet (data, actor);
425 actor->GetProperty ()->SetRepresentationToWireframe ();
426 actor->GetProperty ()->SetColor (r, g, b);
427 actor->GetMapper ()->ScalarVisibilityOn ();
428 actor->GetProperty ()->BackfaceCullingOff ();
429 addActorToRenderer (actor, viewport);
432 (*shape_actor_map_)[id] = actor;
438 template <
typename Po
intT>
bool 441 const std::string &
id,
int viewport)
443 return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5,
id, viewport));
447 template <
typename P1,
typename P2>
bool 452 PCL_WARN (
"[addLine] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
460 createActorFromVTKDataSet (data, actor);
461 actor->GetProperty ()->SetRepresentationToWireframe ();
462 actor->GetProperty ()->SetColor (r, g, b);
463 actor->GetMapper ()->ScalarVisibilityOff ();
464 addActorToRenderer (actor, viewport);
467 (*shape_actor_map_)[id] = actor;
472 template <
typename P1,
typename P2>
bool 477 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
483 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
484 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
485 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
486 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
487 leader->SetArrowStyleToFilled ();
488 leader->AutoLabelOn ();
490 leader->GetProperty ()->SetColor (r, g, b);
491 addActorToRenderer (leader, viewport);
494 (*shape_actor_map_)[id] = leader;
499 template <
typename P1,
typename P2>
bool 504 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
510 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
511 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
512 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
513 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
514 leader->SetArrowStyleToFilled ();
515 leader->SetArrowPlacementToPoint1 ();
517 leader->AutoLabelOn ();
519 leader->AutoLabelOff ();
521 leader->GetProperty ()->SetColor (r, g, b);
522 addActorToRenderer (leader, viewport);
525 (*shape_actor_map_)[id] = leader;
529 template <
typename P1,
typename P2>
bool 531 double r_line,
double g_line,
double b_line,
532 double r_text,
double g_text,
double b_text,
533 const std::string &
id,
int viewport)
537 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
543 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
544 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
545 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
546 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
547 leader->SetArrowStyleToFilled ();
548 leader->AutoLabelOn ();
550 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
552 leader->GetProperty ()->SetColor (r_line, g_line, b_line);
553 addActorToRenderer (leader, viewport);
556 (*shape_actor_map_)[id] = leader;
561 template <
typename P1,
typename P2>
bool 564 return (!
addLine (pt1, pt2, 0.5, 0.5, 0.5,
id, viewport));
568 template <
typename Po
intT>
bool 573 PCL_WARN (
"[addSphere] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
578 data->SetRadius (radius);
579 data->SetCenter (
double (center.x), double (center.y), double (center.z));
580 data->SetPhiResolution (10);
581 data->SetThetaResolution (10);
582 data->LatLongTessellationOff ();
587 mapper->SetInputConnection (data->GetOutputPort ());
591 actor->SetMapper (mapper);
593 actor->GetProperty ()->SetRepresentationToSurface ();
594 actor->GetProperty ()->SetInterpolationToFlat ();
595 actor->GetProperty ()->SetColor (r, g, b);
596 actor->GetMapper ()->ImmediateModeRenderingOn ();
597 actor->GetMapper ()->StaticOn ();
598 actor->GetMapper ()->ScalarVisibilityOff ();
599 actor->GetMapper ()->Update ();
600 addActorToRenderer (actor, viewport);
603 (*shape_actor_map_)[id] = actor;
608 template <
typename Po
intT>
bool 611 return (
addSphere (center, radius, 0.5, 0.5, 0.5,
id, viewport));
615 template<
typename Po
intT>
bool 625 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
626 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
629 #if VTK_MAJOR_VERSION < 6 630 vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
632 vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
634 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
638 src->SetCenter (
double (center.x), double (center.y), double (center.z));
639 src->SetRadius (radius);
641 actor->GetProperty ()->SetColor (r, g, b);
648 template <
typename Po
intT>
bool 650 const std::string &text,
656 const std::string &
id,
667 PCL_WARN (
"[addText3D] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
672 textSource->SetText (text.c_str());
673 textSource->Update ();
676 textMapper->SetInputConnection (textSource->GetOutputPort ());
679 rens_->InitTraversal ();
680 vtkRenderer* renderer = NULL;
682 while ((renderer = rens_->GetNextItem ()) != NULL)
685 if (viewport == 0 || viewport == i)
688 textActor->SetMapper (textMapper);
689 textActor->SetPosition (position.x, position.y, position.z);
690 textActor->SetScale (textScale);
691 textActor->GetProperty ()->SetColor (r, g, b);
692 textActor->SetCamera (renderer->GetActiveCamera ());
694 renderer->AddActor (textActor);
699 std::string alternate_tid = tid;
700 alternate_tid.append(i,
'*');
702 (*shape_actor_map_)[(viewport == 0) ? tid : alternate_tid] = textActor;
712 template <
typename Po
intNT>
bool 715 int level,
float scale,
const std::string &
id,
int viewport)
717 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale,
id, viewport));
721 template <
typename Po
intT,
typename Po
intNT>
bool 725 int level,
float scale,
726 const std::string &
id,
int viewport)
730 PCL_ERROR (
"[addPointCloudNormals] The number of points differs from the number of normals!\n");
735 PCL_WARN (
"[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
742 points->SetDataTypeToFloat ();
744 data->SetNumberOfComponents (3);
747 vtkIdType nr_normals = 0;
753 vtkIdType point_step =
static_cast<vtkIdType
> (sqrt (
double (level)));
754 nr_normals = (
static_cast<vtkIdType
> ((cloud->
width - 1)/ point_step) + 1) *
755 (static_cast<vtkIdType> ((cloud->
height - 1) / point_step) + 1);
756 pts =
new float[2 * nr_normals * 3];
758 vtkIdType cell_count = 0;
759 for (vtkIdType y = 0; y < normals->
height; y += point_step)
760 for (vtkIdType x = 0; x < normals->
width; x += point_step)
762 PointT p = (*cloud)(x, y);
763 p.x += (*normals)(x, y).normal[0] * scale;
764 p.y += (*normals)(x, y).normal[1] * scale;
765 p.z += (*normals)(x, y).normal[2] * scale;
767 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
768 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
769 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
770 pts[2 * cell_count * 3 + 3] = p.x;
771 pts[2 * cell_count * 3 + 4] = p.y;
772 pts[2 * cell_count * 3 + 5] = p.z;
774 lines->InsertNextCell (2);
775 lines->InsertCellPoint (2 * cell_count);
776 lines->InsertCellPoint (2 * cell_count + 1);
782 nr_normals = (cloud->
points.size () - 1) / level + 1 ;
783 pts =
new float[2 * nr_normals * 3];
785 for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
788 p.x += normals->
points[i].normal[0] * scale;
789 p.y += normals->
points[i].normal[1] * scale;
790 p.z += normals->
points[i].normal[2] * scale;
792 pts[2 * j * 3 + 0] = cloud->
points[i].x;
793 pts[2 * j * 3 + 1] = cloud->
points[i].y;
794 pts[2 * j * 3 + 2] = cloud->
points[i].z;
795 pts[2 * j * 3 + 3] = p.x;
796 pts[2 * j * 3 + 4] = p.y;
797 pts[2 * j * 3 + 5] = p.z;
799 lines->InsertNextCell (2);
800 lines->InsertCellPoint (2 * j);
801 lines->InsertCellPoint (2 * j + 1);
805 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
806 points->SetData (data);
809 polyData->SetPoints (points);
810 polyData->SetLines (lines);
813 #if VTK_MAJOR_VERSION < 6 814 mapper->SetInput (polyData);
816 mapper->SetInputData (polyData);
818 mapper->SetColorModeToMapScalars();
819 mapper->SetScalarModeToUsePointData();
823 actor->SetMapper (mapper);
826 addActorToRenderer (actor, viewport);
829 (*cloud_actor_map_)[id].actor = actor;
834 template <
typename Po
intNT>
bool 838 int level,
float scale,
839 const std::string &
id,
int viewport)
841 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
845 template <
typename Po
intT,
typename Po
intNT>
bool 850 int level,
float scale,
851 const std::string &
id,
int viewport)
855 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
861 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
869 unsigned char green[3] = {0, 255, 0};
870 unsigned char blue[3] = {0, 0, 255};
874 line_1_colors->SetNumberOfComponents (3);
875 line_1_colors->SetName (
"Colors");
877 line_2_colors->SetNumberOfComponents (3);
878 line_2_colors->SetName (
"Colors");
881 for (
size_t i = 0; i < cloud->
points.size (); i+=level)
884 p.x += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[0]) * scale;
885 p.y += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[1]) * scale;
886 p.z += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[2]) * scale;
890 line_1->SetPoint2 (p.x, p.y, p.z);
892 #if VTK_MAJOR_VERSION < 6 893 polydata_1->AddInput (line_1->GetOutput ());
895 polydata_1->AddInputData (line_1->GetOutput ());
897 line_1_colors->InsertNextTupleValue (green);
899 polydata_1->Update ();
901 line_1_data->GetCellData ()->SetScalars (line_1_colors);
904 for (
size_t i = 0; i < cloud->
points.size (); i += level)
906 Eigen::Vector3f pc (pcs->
points[i].principal_curvature[0],
907 pcs->
points[i].principal_curvature[1],
908 pcs->
points[i].principal_curvature[2]);
909 Eigen::Vector3f normal (normals->
points[i].normal[0],
910 normals->
points[i].normal[1],
911 normals->
points[i].normal[2]);
912 Eigen::Vector3f pc_c = pc.cross (normal);
915 p.x += (pcs->
points[i].pc2 * pc_c[0]) * scale;
916 p.y += (pcs->
points[i].pc2 * pc_c[1]) * scale;
917 p.z += (pcs->
points[i].pc2 * pc_c[2]) * scale;
921 line_2->SetPoint2 (p.x, p.y, p.z);
923 #if VTK_MAJOR_VERSION < 6 924 polydata_2->AddInput (line_2->GetOutput ());
926 polydata_2->AddInputData (line_2->GetOutput ());
929 line_2_colors->InsertNextTupleValue (blue);
931 polydata_2->Update ();
933 line_2_data->GetCellData ()->SetScalars (line_2_colors);
937 #if VTK_MAJOR_VERSION < 6 938 alldata->AddInput (line_1_data);
939 alldata->AddInput (line_2_data);
941 alldata->AddInputData (line_1_data);
942 alldata->AddInputData (line_2_data);
947 createActorFromVTKDataSet (alldata->GetOutput (), actor);
948 actor->GetMapper ()->SetScalarModeToUseCellData ();
951 addActorToRenderer (actor, viewport);
956 (*cloud_actor_map_)[id] = act;
961 template <
typename Po
intT,
typename GradientT>
bool 965 int level,
double scale,
966 const std::string &
id,
int viewport)
970 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
975 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
982 points->SetDataTypeToFloat ();
984 data->SetNumberOfComponents (3);
986 vtkIdType nr_gradients = (cloud->
points.size () - 1) / level + 1 ;
987 float* pts =
new float[2 * nr_gradients * 3];
989 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
992 p.x += gradients->
points[i].gradient[0] * scale;
993 p.y += gradients->
points[i].gradient[1] * scale;
994 p.z += gradients->
points[i].gradient[2] * scale;
996 pts[2 * j * 3 + 0] = cloud->
points[i].x;
997 pts[2 * j * 3 + 1] = cloud->
points[i].y;
998 pts[2 * j * 3 + 2] = cloud->
points[i].z;
999 pts[2 * j * 3 + 3] = p.x;
1000 pts[2 * j * 3 + 4] = p.y;
1001 pts[2 * j * 3 + 5] = p.z;
1003 lines->InsertNextCell(2);
1004 lines->InsertCellPoint(2*j);
1005 lines->InsertCellPoint(2*j+1);
1008 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1009 points->SetData (data);
1012 polyData->SetPoints(points);
1013 polyData->SetLines(lines);
1016 #if VTK_MAJOR_VERSION < 6 1017 mapper->SetInput (polyData);
1019 mapper->SetInputData (polyData);
1021 mapper->SetColorModeToMapScalars();
1022 mapper->SetScalarModeToUsePointData();
1026 actor->SetMapper (mapper);
1029 addActorToRenderer (actor, viewport);
1032 (*cloud_actor_map_)[id].actor = actor;
1037 template <
typename Po
intT>
bool 1041 const std::vector<int> &correspondences,
1042 const std::string &
id,
1046 corrs.resize (correspondences.size ());
1049 for (pcl::Correspondences::iterator corrs_it (corrs.begin ()); corrs_it != corrs.end (); ++corrs_it)
1051 corrs_it->index_query = index;
1052 corrs_it->index_match = correspondences[index];
1056 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1060 template <
typename Po
intT>
bool 1066 const std::string &
id,
1070 if (correspondences.empty ())
1072 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1077 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
1078 if (am_it != shape_actor_map_->end () && overwrite ==
false)
1080 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1082 }
else if (am_it == shape_actor_map_->end () && overwrite ==
true)
1087 int n_corr = int (correspondences.size () / nth);
1092 line_colors->SetNumberOfComponents (3);
1093 line_colors->SetName (
"Colors");
1094 line_colors->SetNumberOfTuples (n_corr);
1098 line_points->SetNumberOfPoints (2 * n_corr);
1101 line_cells_id->SetNumberOfComponents (3);
1102 line_cells_id->SetNumberOfTuples (n_corr);
1103 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1107 line_tcoords->SetNumberOfComponents (1);
1108 line_tcoords->SetNumberOfTuples (n_corr * 2);
1109 line_tcoords->SetName (
"Texture Coordinates");
1111 double tc[3] = {0.0, 0.0, 0.0};
1113 Eigen::Affine3f source_transformation;
1115 source_transformation.translation () = source_points->
sensor_origin_.head (3);
1116 Eigen::Affine3f target_transformation;
1118 target_transformation.translation () = target_points->
sensor_origin_.head (3);
1122 for (
size_t i = 0; i < correspondences.size (); i += nth, ++j)
1124 if (correspondences[i].index_match == -1)
1126 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1130 PointT p_src (source_points->
points[correspondences[i].index_query]);
1131 PointT p_tgt (target_points->
points[correspondences[i].index_match]);
1133 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1134 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1136 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1138 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1139 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1141 *line_cell_id++ = 2;
1142 *line_cell_id++ = id1;
1143 *line_cell_id++ = id2;
1145 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1146 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1149 rgb[0] = vtkMath::Random (32, 255);
1150 rgb[1] = vtkMath::Random (32, 255);
1151 rgb[2] = vtkMath::Random (32, 255);
1152 line_colors->InsertTuple (i, rgb);
1154 line_colors->SetNumberOfTuples (j);
1155 line_cells_id->SetNumberOfTuples (j);
1156 line_cells->SetCells (n_corr, line_cells_id);
1157 line_points->SetNumberOfPoints (j*2);
1158 line_tcoords->SetNumberOfTuples (j*2);
1161 line_data->SetPoints (line_points);
1162 line_data->SetLines (line_cells);
1163 line_data->GetPointData ()->SetTCoords (line_tcoords);
1164 line_data->GetCellData ()->SetScalars (line_colors);
1170 createActorFromVTKDataSet (line_data, actor);
1171 actor->GetProperty ()->SetRepresentationToWireframe ();
1172 actor->GetProperty ()->SetOpacity (0.5);
1173 addActorToRenderer (actor, viewport);
1176 (*shape_actor_map_)[id] = actor;
1184 #if VTK_MAJOR_VERSION < 6 1185 reinterpret_cast<vtkPolyDataMapper*
>(actor->GetMapper ())->SetInput (line_data);
1187 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1195 template <
typename Po
intT>
bool 1201 const std::string &
id,
1204 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1208 template <
typename Po
intT>
bool 1209 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1212 const std::string &
id,
1214 const Eigen::Vector4f& sensor_origin,
1215 const Eigen::Quaternion<float>& sensor_orientation)
1219 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1225 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1232 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1236 bool has_colors =
false;
1239 if (color_handler.
getColor (scalars))
1241 polydata->GetPointData ()->SetScalars (scalars);
1242 scalars->GetRange (minmax);
1248 createActorFromVTKDataSet (polydata, actor);
1250 actor->GetMapper ()->SetScalarRange (minmax);
1253 addActorToRenderer (actor, viewport);
1256 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1257 cloud_actor.
actor = actor;
1258 cloud_actor.
cells = initcells;
1264 cloud_actor.
actor->SetUserMatrix (transformation);
1265 cloud_actor.
actor->Modified ();
1271 template <
typename Po
intT>
bool 1272 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1275 const std::string &
id,
1277 const Eigen::Vector4f& sensor_origin,
1278 const Eigen::Quaternion<float>& sensor_orientation)
1282 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1288 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->
getName ().c_str ());
1295 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1299 bool has_colors =
false;
1302 if (color_handler->
getColor (scalars))
1304 polydata->GetPointData ()->SetScalars (scalars);
1305 scalars->GetRange (minmax);
1311 createActorFromVTKDataSet (polydata, actor);
1313 actor->GetMapper ()->SetScalarRange (minmax);
1316 addActorToRenderer (actor, viewport);
1319 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1320 cloud_actor.
actor = actor;
1321 cloud_actor.
cells = initcells;
1328 cloud_actor.
actor->SetUserMatrix (transformation);
1329 cloud_actor.
actor->Modified ();
1335 template <
typename Po
intT>
bool 1336 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1339 const std::string &
id,
1341 const Eigen::Vector4f& sensor_origin,
1342 const Eigen::Quaternion<float>& sensor_orientation)
1346 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->
getName ().c_str ());
1352 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1359 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1363 bool has_colors =
false;
1366 if (color_handler.
getColor (scalars))
1368 polydata->GetPointData ()->SetScalars (scalars);
1369 scalars->GetRange (minmax);
1375 createActorFromVTKDataSet (polydata, actor);
1377 actor->GetMapper ()->SetScalarRange (minmax);
1380 addActorToRenderer (actor, viewport);
1383 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1384 cloud_actor.
actor = actor;
1385 cloud_actor.
cells = initcells;
1392 cloud_actor.
actor->SetUserMatrix (transformation);
1393 cloud_actor.
actor->Modified ();
1399 template <
typename Po
intT>
bool 1401 const std::string &
id)
1404 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1406 if (am_it == cloud_actor_map_->end ())
1411 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1415 polydata->GetPointData ()->SetScalars (scalars);
1417 minmax[0] = std::numeric_limits<double>::min ();
1418 minmax[1] = std::numeric_limits<double>::max ();
1419 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1420 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1423 #if VTK_MAJOR_VERSION < 6 1424 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1426 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1432 template <
typename Po
intT>
bool 1435 const std::string &
id)
1438 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1440 if (am_it == cloud_actor_map_->end ())
1447 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1451 polydata->GetPointData ()->SetScalars (scalars);
1453 minmax[0] = std::numeric_limits<double>::min ();
1454 minmax[1] = std::numeric_limits<double>::max ();
1455 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1456 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1459 #if VTK_MAJOR_VERSION < 6 1460 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1462 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1469 template <
typename Po
intT>
bool 1472 const std::string &
id)
1475 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1477 if (am_it == cloud_actor_map_->end ())
1487 vtkIdType nr_points = cloud->
points.size ();
1488 points->SetNumberOfPoints (nr_points);
1491 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1497 for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1498 memcpy (&data[pts], &cloud->
points[i].x, 12);
1503 for (vtkIdType i = 0; i < nr_points; ++i)
1509 memcpy (&data[pts], &cloud->
points[i].x, 12);
1514 points->SetNumberOfPoints (nr_points);
1518 updateCells (cells, am_it->second.cells, nr_points);
1521 vertices->SetCells (nr_points, cells);
1527 scalars->GetRange (minmax);
1529 polydata->GetPointData ()->SetScalars (scalars);
1531 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1532 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1535 #if VTK_MAJOR_VERSION < 6 1536 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1538 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1544 template <
typename Po
intT>
bool 1547 const std::vector<pcl::Vertices> &vertices,
1548 const std::string &
id,
1551 if (vertices.empty () || cloud->
points.empty ())
1556 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1561 std::vector<pcl::PCLPointField> fields;
1569 colors->SetNumberOfComponents (3);
1570 colors->SetName (
"Colors");
1573 for (
size_t i = 0; i < cloud->
size (); ++i)
1577 memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->
points[i]) + fields[rgb_idx].offset, sizeof (
pcl::RGB));
1578 unsigned char color[3];
1579 color[0] = rgb_data.r;
1580 color[1] = rgb_data.g;
1581 color[2] = rgb_data.b;
1582 colors->InsertNextTupleValue (color);
1588 vtkIdType nr_points = cloud->
points.size ();
1589 points->SetNumberOfPoints (nr_points);
1593 float *data =
static_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1596 std::vector<int> lookup;
1600 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1601 memcpy (&data[ptr], &cloud->
points[i].x, sizeof (
float) * 3);
1605 lookup.resize (nr_points);
1607 for (vtkIdType i = 0; i < nr_points; ++i)
1613 lookup[i] =
static_cast<int> (j);
1614 memcpy (&data[ptr], &cloud->
points[i].x, sizeof (
float) * 3);
1619 points->SetNumberOfPoints (nr_points);
1623 int max_size_of_polygon = -1;
1624 for (
size_t i = 0; i < vertices.size (); ++i)
1625 if (max_size_of_polygon < static_cast<int> (vertices[i].vertices.size ()))
1626 max_size_of_polygon =
static_cast<int> (vertices[i].vertices.size ());
1628 if (vertices.size () > 1)
1632 vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1634 if (lookup.size () > 0)
1636 for (
size_t i = 0; i < vertices.size (); ++i, ++idx)
1638 size_t n_points = vertices[i].vertices.size ();
1641 for (
size_t j = 0; j < n_points; j++, ++idx)
1642 *cell++ = lookup[vertices[i].vertices[j]];
1648 for (
size_t i = 0; i < vertices.size (); ++i, ++idx)
1650 size_t n_points = vertices[i].vertices.size ();
1653 for (
size_t j = 0; j < n_points; j++, ++idx)
1654 *cell++ = vertices[i].vertices[j];
1659 allocVtkPolyData (polydata);
1660 cell_array->GetData ()->SetNumberOfValues (idx);
1661 cell_array->Squeeze ();
1662 polydata->SetPolys (cell_array);
1663 polydata->SetPoints (points);
1666 polydata->GetPointData ()->SetScalars (colors);
1668 createActorFromVTKDataSet (polydata, actor,
false);
1673 size_t n_points = vertices[0].vertices.size ();
1674 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1676 if (lookup.size () > 0)
1678 for (
size_t j = 0; j < (n_points - 1); ++j)
1679 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1683 for (
size_t j = 0; j < (n_points - 1); ++j)
1684 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1687 allocVtkUnstructuredGrid (poly_grid);
1688 poly_grid->Allocate (1, 1);
1689 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1690 poly_grid->SetPoints (points);
1692 poly_grid->GetPointData ()->SetScalars (colors);
1694 createActorFromVTKDataSet (poly_grid, actor,
false);
1696 addActorToRenderer (actor, viewport);
1697 actor->GetProperty ()->SetRepresentationToSurface ();
1699 actor->GetProperty ()->BackfaceCullingOff ();
1700 actor->GetProperty ()->SetInterpolationToFlat ();
1701 actor->GetProperty ()->EdgeVisibilityOff ();
1702 actor->GetProperty ()->ShadingOff ();
1705 (*cloud_actor_map_)[id].actor = actor;
1710 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1716 template <
typename Po
intT>
bool 1719 const std::vector<pcl::Vertices> &verts,
1720 const std::string &
id)
1729 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1730 if (am_it == cloud_actor_map_->end ())
1742 vtkIdType nr_points = cloud->
points.size ();
1743 points->SetNumberOfPoints (nr_points);
1746 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1749 std::vector<int> lookup;
1753 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1754 memcpy (&data[ptr], &cloud->
points[i].x, sizeof (
float) * 3);
1758 lookup.resize (nr_points);
1760 for (vtkIdType i = 0; i < nr_points; ++i)
1766 lookup [i] =
static_cast<int> (j);
1767 memcpy (&data[ptr], &cloud->
points[i].x, sizeof (
float) * 3);
1772 points->SetNumberOfPoints (nr_points);
1776 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1780 std::vector<pcl::PCLPointField> fields;
1784 if (rgb_idx != -1 && colors)
1788 for (
size_t i = 0; i < cloud->
size (); ++i)
1793 reinterpret_cast<const char*> (&cloud->
points[i]) + fields[rgb_idx].offset,
1795 unsigned char color[3];
1796 color[0] = rgb_data.r;
1797 color[1] = rgb_data.g;
1798 color[2] = rgb_data.b;
1799 colors->SetTupleValue (j++, color);
1804 int max_size_of_polygon = -1;
1805 for (
size_t i = 0; i < verts.size (); ++i)
1806 if (max_size_of_polygon < static_cast<int> (verts[i].vertices.size ()))
1807 max_size_of_polygon =
static_cast<int> (verts[i].vertices.size ());
1811 vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1813 if (lookup.size () > 0)
1815 for (
size_t i = 0; i < verts.size (); ++i, ++idx)
1817 size_t n_points = verts[i].vertices.size ();
1819 for (
size_t j = 0; j < n_points; j++, cell++, ++idx)
1820 *cell = lookup[verts[i].vertices[j]];
1825 for (
size_t i = 0; i < verts.size (); ++i, ++idx)
1827 size_t n_points = verts[i].vertices.size ();
1829 for (
size_t j = 0; j < n_points; j++, cell++, ++idx)
1830 *cell = verts[i].vertices[j];
1833 cells->GetData ()->SetNumberOfValues (idx);
1836 polydata->SetPolys (cells);
bool isCapable() const
Check if this handler is capable of handling the input data or not.
XYZ handler class for PointCloud geometry.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
ColorHandler::ConstPtr ColorHandlerConstPtr
virtual std::string getName() const =0
Abstract getName method.
bool addPointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a Point Cloud (templated) to screen.
static void convertToVtkMatrix(const Eigen::Matrix4f &m, vtkSmartPointer< vtkMatrix4x4 > &vtk_matrix)
Convert Eigen::Matrix4f to vtkMatrix4x4.
std::vector< ColorHandlerConstPtr > color_handlers
A vector of color handlers that can be used for rendering the data.
bool addPointCloudIntensityGradients(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const typename pcl::PointCloud< GradientT >::ConstPtr &gradients, int level=100, double scale=1e-6, const std::string &id="cloud", int viewport=0)
Add the estimated surface intensity gradients of a Point Cloud to screen.
bool contains(const std::string &id) const
Check if the cloud, shape, or coordinate with the given id was already added to this vizualizer...
bool addPolygon(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, double r, double g, double b, const std::string &id="polygon", int viewport=0)
Add a polygon (polyline) that represents the input point cloud (connects all points in order) ...
virtual std::string getName() const =0
Abstract getName method.
bool addPointCloudPrincipalCurvatures(const typename pcl::PointCloud< PointNT >::ConstPtr &cloud, const typename pcl::PointCloud< pcl::PrincipalCurvatures >::ConstPtr &pcs, int level=100, float scale=1.0f, const std::string &id="cloud", int viewport=0)
Add the estimated principal curvatures of a Point Cloud to screen.
Base Handler class for PointCloud colors.
uint32_t height
The point cloud height (if organized as an image-structure).
bool addSphere(const PointT ¢er, double radius, const std::string &id="sphere", int viewport=0)
Add a sphere shape from a point and a radius.
bool addCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const std::vector< int > &correspondences, const std::string &id="correspondences", int viewport=0)
Add the specified correspondences to the display.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
bool updateCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const pcl::Correspondences &correspondences, int nth, const std::string &id="correspondences", int viewport=0)
Update the specified correspondences to the display.
vtkSmartPointer< vtkIdTypeArray > cells
Internal cell array.
Handler for predefined user colors.
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
RGB handler class for colors.
bool addPolygonMesh(const pcl::PolygonMesh &polymesh, const std::string &id="polygon", int viewport=0)
Add a PolygonMesh object to screen.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
vtkSmartPointer< vtkLODActor > actor
The actor holding the data to render.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
bool addArrow(const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id="arrow", int viewport=0)
Add a line arrow segment between two points, and display the distance between them.
bool updatePointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud")
Updates the XYZ data for an existing cloud object id on screen.
bool isCapable() const
Checl if this handler is capable of handling the input data or not.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
virtual void getGeometry(vtkSmartPointer< vtkPoints > &points) const =0
Obtain the actual point geometry for the input dataset in VTK format.
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
Base handler class for PointCloud geometry.
std::vector< GeometryHandlerConstPtr > geometry_handlers
A vector of geometry handlers that can be used for rendering the data.
GeometryHandler::ConstPtr GeometryHandlerConstPtr
bool addLine(const P1 &pt1, const P2 &pt2, const std::string &id="line", int viewport=0)
Add a line segment from two points.
bool addText3D(const std::string &text, const PointT &position, double textScale=1.0, double r=1.0, double g=1.0, double b=1.0, const std::string &id="", int viewport=0)
Add a 3d text to the scene.
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual bool getColor(vtkSmartPointer< vtkDataArray > &scalars) const =0
Obtain the actual color for the input dataset as vtk scalars.
bool updateSphere(const PointT ¢er, double radius, double r, double g, double b, const std::string &id="sphere")
Update an existing sphere shape from a point and a radius.
vtkSmartPointer< vtkMatrix4x4 > viewpoint_transformation_
The viewpoint transformation matrix.
bool addPointCloudNormals(const typename pcl::PointCloud< PointNT >::ConstPtr &cloud, int level=100, float scale=0.02f, const std::string &id="cloud", int viewport=0)
Add the estimated surface normals of a Point Cloud to screen.
PCL_EXPORTS vtkSmartPointer< vtkDataSet > createLine(const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
Create a line shape from two points.
bool updatePolygonMesh(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::vector< pcl::Vertices > &vertices, const std::string &id="polygon")
Update a PolygonMesh object on screen.
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.