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>
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/common/utils.h>
69#ifdef vtkGenericDataArray_h
70#define SetTupleValue SetTypedTuple
71#define InsertNextTupleValue InsertNextTypedTuple
72#define GetTupleValue GetTypedTuple
76template <
typename Po
intT>
bool
79 const std::string &
id,
int viewport)
83 return (addPointCloud<PointT> (cloud, geometry_handler,
id, viewport));
87template <
typename Po
intT>
bool
91 const std::string &
id,
int viewport)
95 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
99 if (pcl::traits::has_color<PointT>())
109template <
typename Po
intT>
bool
113 const std::string &
id,
int viewport)
119 auto am_it = cloud_actor_map_->find (
id);
120 am_it->second.geometry_handlers.push_back (geometry_handler);
130template <
typename Po
intT>
bool
134 const std::string &
id,
int viewport)
138 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
152template <
typename Po
intT>
bool
156 const std::string &
id,
int viewport)
159 auto am_it = cloud_actor_map_->find (
id);
160 if (am_it != cloud_actor_map_->end ())
164 am_it->second.color_handlers.push_back (color_handler);
173template <
typename Po
intT>
bool
178 const std::string &
id,
int viewport)
181 auto am_it = cloud_actor_map_->find (
id);
182 if (am_it != cloud_actor_map_->end ())
186 am_it->second.geometry_handlers.push_back (geometry_handler);
187 am_it->second.color_handlers.push_back (color_handler);
194template <
typename Po
intT>
bool
199 const std::string &
id,
int viewport)
203 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
215template <
typename Po
intT>
void
216pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
224 allocVtkPolyData (polydata);
226 polydata->SetVerts (vertices);
230 vertices = polydata->GetVerts ();
234 vtkIdType nr_points = cloud->
size ();
240 points->SetDataTypeToFloat ();
241 polydata->SetPoints (points);
243 points->SetNumberOfPoints (nr_points);
246 float *data = (
dynamic_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
252 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3) {
253 std::copy(&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
259 for (vtkIdType i = 0; i < nr_points; ++i)
265 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
270 points->SetNumberOfPoints (nr_points);
273#ifdef VTK_CELL_ARRAY_V2
277 auto numOfCells = vertices->GetNumberOfCells();
280 if (numOfCells < nr_points)
282 for (
int i = numOfCells; i < nr_points; i++)
284 vertices->InsertNextCell(1);
285 vertices->InsertCellPoint(i);
289 else if (numOfCells > nr_points)
291 vertices->ResizeExact(nr_points, nr_points);
294 polydata->SetPoints(points);
295 polydata->SetVerts(vertices);
299 updateCells (cells, initcells, nr_points);
302 vertices->SetCells (nr_points, cells);
305 vertices->SetNumberOfCells(nr_points);
310template <
typename Po
intT>
void
311pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
319 allocVtkPolyData (polydata);
321 polydata->SetVerts (vertices);
327 polydata->SetPoints (points);
329 vtkIdType nr_points = points->GetNumberOfPoints ();
332 vertices = polydata->GetVerts ();
336#ifdef VTK_CELL_ARRAY_V2
340 auto numOfCells = vertices->GetNumberOfCells();
343 if (numOfCells < nr_points)
345 for (
int i = numOfCells; i < nr_points; i++)
347 vertices->InsertNextCell(1);
348 vertices->InsertCellPoint(i);
352 else if (numOfCells > nr_points)
354 vertices->ResizeExact(nr_points, nr_points);
357 polydata->SetPoints(points);
358 polydata->SetVerts(vertices);
362 updateCells (cells, initcells, nr_points);
364 vertices->SetCells (nr_points, cells);
369template <
typename Po
intT>
bool
372 double r,
double g,
double b,
const std::string &
id,
int viewport)
379 auto am_it = shape_actor_map_->find (
id);
380 if (am_it != shape_actor_map_->end ())
385 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
389 surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
391 all_data->AddInputData (poly_data);
395 createActorFromVTKDataSet (all_data->GetOutput (), actor);
396 actor->GetProperty ()->SetRepresentationToWireframe ();
397 actor->GetProperty ()->SetColor (r, g, b);
398 actor->GetMapper ()->ScalarVisibilityOff ();
399 removeActorFromRenderer (am_it->second, viewport);
400 addActorToRenderer (actor, viewport);
403 (*shape_actor_map_)[id] = actor;
409 createActorFromVTKDataSet (data, actor);
410 actor->GetProperty ()->SetRepresentationToWireframe ();
411 actor->GetProperty ()->SetColor (r, g, b);
412 actor->GetMapper ()->ScalarVisibilityOff ();
413 addActorToRenderer (actor, viewport);
416 (*shape_actor_map_)[id] = actor;
423template <
typename Po
intT>
bool
426 double r,
double g,
double b,
const std::string &
id,
int viewport)
433 auto am_it = shape_actor_map_->find (
id);
434 if (am_it != shape_actor_map_->end ())
439 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
443 surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
445 all_data->AddInputData (poly_data);
449 createActorFromVTKDataSet (all_data->GetOutput (), actor);
450 actor->GetProperty ()->SetRepresentationToWireframe ();
451 actor->GetProperty ()->SetColor (r, g, b);
452 actor->GetMapper ()->ScalarVisibilityOn ();
453 actor->GetProperty ()->BackfaceCullingOff ();
454 removeActorFromRenderer (am_it->second, viewport);
455 addActorToRenderer (actor, viewport);
458 (*shape_actor_map_)[id] = actor;
464 createActorFromVTKDataSet (data, actor);
465 actor->GetProperty ()->SetRepresentationToWireframe ();
466 actor->GetProperty ()->SetColor (r, g, b);
467 actor->GetMapper ()->ScalarVisibilityOn ();
468 actor->GetProperty ()->BackfaceCullingOff ();
469 addActorToRenderer (actor, viewport);
472 (*shape_actor_map_)[id] = actor;
478template <
typename Po
intT>
bool
481 const std::string &
id,
int viewport)
483 return (addPolygon<PointT> (cloud, 0.5, 0.5, 0.5,
id, viewport));
487template <
typename P1,
typename P2>
bool
492 PCL_WARN (
"[addLine] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
500 createActorFromVTKDataSet (data, actor);
501 actor->GetProperty ()->SetRepresentationToWireframe ();
502 actor->GetProperty ()->SetColor (r, g, b);
503 actor->GetMapper ()->ScalarVisibilityOff ();
504 addActorToRenderer (actor, viewport);
507 (*shape_actor_map_)[id] = actor;
512template <
typename P1,
typename P2>
bool
517 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
523 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
524 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
525 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
526 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
527 leader->SetArrowStyleToFilled ();
528 leader->AutoLabelOn ();
530 leader->GetProperty ()->SetColor (r, g, b);
531 addActorToRenderer (leader, viewport);
534 (*shape_actor_map_)[id] = leader;
539template <
typename P1,
typename P2>
bool
544 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
550 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
551 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
552 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
553 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
554 leader->SetArrowStyleToFilled ();
555 leader->SetArrowPlacementToPoint1 ();
557 leader->AutoLabelOn ();
559 leader->AutoLabelOff ();
561 leader->GetProperty ()->SetColor (r, g, b);
562 addActorToRenderer (leader, viewport);
565 (*shape_actor_map_)[id] = leader;
569template <
typename P1,
typename P2>
bool
571 double r_line,
double g_line,
double b_line,
572 double r_text,
double g_text,
double b_text,
573 const std::string &
id,
int viewport)
577 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
583 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
584 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
585 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
586 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
587 leader->SetArrowStyleToFilled ();
588 leader->AutoLabelOn ();
590 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
592 leader->GetProperty ()->SetColor (r_line, g_line, b_line);
593 addActorToRenderer (leader, viewport);
596 (*shape_actor_map_)[id] = leader;
601template <
typename P1,
typename P2>
bool
604 return (addLine (pt1, pt2, 0.5, 0.5, 0.5,
id, viewport));
608template <
typename Po
intT>
bool
613 PCL_WARN (
"[addSphere] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
618 data->SetRadius (radius);
619 data->SetCenter (
static_cast<double>(center.x),
static_cast<double>(center.y),
static_cast<double>(center.z));
620 data->SetPhiResolution (10);
621 data->SetThetaResolution (10);
622 data->LatLongTessellationOff ();
627 mapper->SetInputConnection (data->GetOutputPort ());
631 actor->SetMapper (mapper);
633 actor->GetProperty ()->SetRepresentationToSurface ();
634 actor->GetProperty ()->SetInterpolationToFlat ();
635 actor->GetProperty ()->SetColor (r, g, b);
636 actor->GetMapper ()->StaticOn ();
637 actor->GetMapper ()->ScalarVisibilityOff ();
638 actor->GetMapper ()->Update ();
639 addActorToRenderer (actor, viewport);
642 (*shape_actor_map_)[id] = actor;
647template <
typename Po
intT>
bool
650 return (addSphere (center, radius, 0.5, 0.5, 0.5,
id, viewport));
654template<
typename Po
intT>
bool
664 auto am_it = shape_actor_map_->find (
id);
665 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
668 vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
669 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
673 src->SetCenter (
double (center.x), double (center.y), double (center.z));
674 src->SetRadius (radius);
676 actor->GetProperty ()->SetColor (r, g, b);
683template <
typename Po
intT>
bool
685 const std::string &text,
691 const std::string &
id,
704 if (rens_->GetNumberOfItems () <= viewport)
706 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)! \n",
713 rens_->InitTraversal ();
714 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
716 const std::string uid = tid + std::string (i,
'*');
719 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! \n"
720 "Please choose a different id and retry.\n",
731 textSource->SetText (text.c_str());
732 textSource->Update ();
735 textMapper->SetInputConnection (textSource->GetOutputPort ());
738 rens_->InitTraversal ();
739 vtkRenderer* renderer;
741 while ((renderer = rens_->GetNextItem ()))
744 if (viewport == 0 || viewport == i)
747 textActor->SetMapper (textMapper);
748 textActor->SetPosition (position.x, position.y, position.z);
749 textActor->SetScale (textScale);
750 textActor->GetProperty ()->SetColor (r, g, b);
751 textActor->SetCamera (renderer->GetActiveCamera ());
753 renderer->AddActor (textActor);
757 const std::string uid = tid + std::string (i,
'*');
758 (*shape_actor_map_)[uid] = textActor;
768template <
typename Po
intT>
bool
770 const std::string &text,
772 double orientation[3],
777 const std::string &
id,
790 if (rens_->GetNumberOfItems () <= viewport)
792 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)!\n",
799 rens_->InitTraversal ();
800 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
802 const std::string uid = tid + std::string (i,
'*');
805 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! "
806 "Please choose a different id and retry.\n",
817 textSource->SetText (text.c_str());
818 textSource->Update ();
821 textMapper->SetInputConnection (textSource->GetOutputPort ());
824 textActor->SetMapper (textMapper);
825 textActor->SetPosition (position.x, position.y, position.z);
826 textActor->SetScale (textScale);
827 textActor->GetProperty ()->SetColor (r, g, b);
828 textActor->SetOrientation (orientation);
831 rens_->InitTraversal ();
833 for ( vtkRenderer* renderer = rens_->GetNextItem ();
835 renderer = rens_->GetNextItem (), ++i)
837 if (viewport == 0 || viewport == i)
839 renderer->AddActor (textActor);
840 const std::string uid = tid + std::string (i,
'*');
841 (*shape_actor_map_)[uid] = textActor;
849template <
typename Po
intNT>
bool
852 int level,
float scale,
const std::string &
id,
int viewport)
854 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale,
id, viewport));
858template <
typename Po
intT,
typename Po
intNT>
bool
862 int level,
float scale,
863 const std::string &
id,
int viewport)
865 if (normals->
size () != cloud->
size ())
867 PCL_ERROR (
"[addPointCloudNormals] The number of points differs from the number of normals!\n");
871 if (normals->
empty ())
873 PCL_WARN (
"[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
879 PCL_WARN (
"[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
886 points->SetDataTypeToFloat ();
888 data->SetNumberOfComponents (3);
891 vtkIdType nr_normals = 0;
892 float* pts =
nullptr;
897 auto point_step =
static_cast<vtkIdType
> (sqrt (
static_cast<double>(level)));
898 nr_normals = (
static_cast<vtkIdType
> ((cloud->
width - 1)/ point_step) + 1) *
899 (
static_cast<vtkIdType
> ((cloud->
height - 1) / point_step) + 1);
900 pts =
new float[2 * nr_normals * 3];
902 vtkIdType cell_count = 0;
903 for (vtkIdType y = 0; y < normals->
height; y += point_step)
904 for (vtkIdType x = 0; x < normals->
width; x += point_step)
906 PointT p = (*cloud)(x, y);
909 p.x += (*normals)(x, y).normal[0] * scale;
910 p.y += (*normals)(x, y).normal[1] * scale;
911 p.z += (*normals)(x, y).normal[2] * scale;
913 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
914 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
915 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
916 pts[2 * cell_count * 3 + 3] = p.x;
917 pts[2 * cell_count * 3 + 4] = p.y;
918 pts[2 * cell_count * 3 + 5] = p.z;
920 lines->InsertNextCell (2);
921 lines->InsertCellPoint (2 * cell_count);
922 lines->InsertCellPoint (2 * cell_count + 1);
925 nr_normals = cell_count;
929 nr_normals = (cloud->
size () - 1) / level + 1 ;
930 pts =
new float[2 * nr_normals * 3];
933 for (vtkIdType i = 0; (j < nr_normals) && (i < static_cast<vtkIdType>(cloud->
size())); i += level)
938 p.x += (*normals)[i].normal[0] * scale;
939 p.y += (*normals)[i].normal[1] * scale;
940 p.z += (*normals)[i].normal[2] * scale;
942 pts[2 * j * 3 + 0] = (*cloud)[i].x;
943 pts[2 * j * 3 + 1] = (*cloud)[i].y;
944 pts[2 * j * 3 + 2] = (*cloud)[i].z;
945 pts[2 * j * 3 + 3] = p.x;
946 pts[2 * j * 3 + 4] = p.y;
947 pts[2 * j * 3 + 5] = p.z;
949 lines->InsertNextCell (2);
950 lines->InsertCellPoint (2 * j);
951 lines->InsertCellPoint (2 * j + 1);
957 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
958 points->SetData (data);
961 polyData->SetPoints (points);
962 polyData->SetLines (lines);
965 mapper->SetInputData (polyData);
966 mapper->SetColorModeToMapScalars();
967 mapper->SetScalarModeToUsePointData();
971 actor->SetMapper (mapper);
976 actor->SetUserMatrix (transformation);
979 addActorToRenderer (actor, viewport);
982 (*cloud_actor_map_)[id].actor = actor;
987template <
typename Po
intNT>
bool
991 int level,
float scale,
992 const std::string &
id,
int viewport)
994 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
998template <
typename Po
intT,
typename Po
intNT>
bool
1003 int level,
float scale,
1004 const std::string &
id,
int viewport)
1008 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
1014 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1022 unsigned char green[3] = {0, 255, 0};
1023 unsigned char blue[3] = {0, 0, 255};
1027 line_1_colors->SetNumberOfComponents (3);
1028 line_1_colors->SetName (
"Colors");
1030 line_2_colors->SetNumberOfComponents (3);
1031 line_2_colors->SetName (
"Colors");
1034 for (std::size_t i = 0; i < cloud->
size (); i+=level)
1037 p.x += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[0]) * scale;
1038 p.y += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[1]) * scale;
1039 p.z += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[2]) * scale;
1042 line_1->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1043 line_1->SetPoint2 (p.x, p.y, p.z);
1045 polydata_1->AddInputData (line_1->GetOutput ());
1046 line_1_colors->InsertNextTupleValue (green);
1048 polydata_1->Update ();
1050 line_1_data->GetCellData ()->SetScalars (line_1_colors);
1053 for (std::size_t i = 0; i < cloud->
size (); i += level)
1055 Eigen::Vector3f pc ((*pcs)[i].principal_curvature[0],
1056 (*pcs)[i].principal_curvature[1],
1057 (*pcs)[i].principal_curvature[2]);
1058 Eigen::Vector3f normal ((*normals)[i].normal[0],
1059 (*normals)[i].normal[1],
1060 (*normals)[i].normal[2]);
1061 Eigen::Vector3f pc_c = pc.cross (normal);
1064 p.x += ((*pcs)[i].pc2 * pc_c[0]) * scale;
1065 p.y += ((*pcs)[i].pc2 * pc_c[1]) * scale;
1066 p.z += ((*pcs)[i].pc2 * pc_c[2]) * scale;
1069 line_2->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1070 line_2->SetPoint2 (p.x, p.y, p.z);
1072 polydata_2->AddInputData (line_2->GetOutput ());
1074 line_2_colors->InsertNextTupleValue (blue);
1076 polydata_2->Update ();
1078 line_2_data->GetCellData ()->SetScalars (line_2_colors);
1082 alldata->AddInputData (line_1_data);
1083 alldata->AddInputData (line_2_data);
1088 createActorFromVTKDataSet (alldata->GetOutput (), actor);
1089 actor->GetMapper ()->SetScalarModeToUseCellData ();
1092 addActorToRenderer (actor, viewport);
1097 (*cloud_actor_map_)[id] = act;
1102template <
typename Po
intT,
typename GradientT>
bool
1106 int level,
double scale,
1107 const std::string &
id,
int viewport)
1109 if (gradients->
size () != cloud->
size ())
1111 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1116 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1123 points->SetDataTypeToFloat ();
1125 data->SetNumberOfComponents (3);
1127 vtkIdType nr_gradients = (cloud->
size () - 1) / level + 1 ;
1128 float* pts =
new float[2 * nr_gradients * 3];
1130 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1133 p.x += (*gradients)[i].gradient[0] * scale;
1134 p.y += (*gradients)[i].gradient[1] * scale;
1135 p.z += (*gradients)[i].gradient[2] * scale;
1137 pts[2 * j * 3 + 0] = (*cloud)[i].x;
1138 pts[2 * j * 3 + 1] = (*cloud)[i].y;
1139 pts[2 * j * 3 + 2] = (*cloud)[i].z;
1140 pts[2 * j * 3 + 3] = p.x;
1141 pts[2 * j * 3 + 4] = p.y;
1142 pts[2 * j * 3 + 5] = p.z;
1144 lines->InsertNextCell(2);
1145 lines->InsertCellPoint(2*j);
1146 lines->InsertCellPoint(2*j+1);
1149 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1150 points->SetData (data);
1153 polyData->SetPoints(points);
1154 polyData->SetLines(lines);
1157 mapper->SetInputData (polyData);
1158 mapper->SetColorModeToMapScalars();
1159 mapper->SetScalarModeToUsePointData();
1163 actor->SetMapper (mapper);
1166 addActorToRenderer (actor, viewport);
1169 (*cloud_actor_map_)[id].actor = actor;
1174template <
typename Po
intT>
bool
1178 const std::vector<int> &correspondences,
1179 const std::string &
id,
1183 corrs.resize (correspondences.size ());
1185 std::size_t index = 0;
1186 for (
auto &corr : corrs)
1188 corr.index_query = index;
1189 corr.index_match = correspondences[index];
1193 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1197template <
typename Po
intT>
bool
1203 const std::string &
id,
1207 if (correspondences.empty ())
1209 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1214 auto am_it = shape_actor_map_->find (
id);
1215 if (am_it != shape_actor_map_->end () && !overwrite)
1217 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1220 if (am_it == shape_actor_map_->end () && overwrite)
1225 int n_corr =
static_cast<int>(correspondences.size () / nth);
1230 line_colors->SetNumberOfComponents (3);
1231 line_colors->SetName (
"Colors");
1232 line_colors->SetNumberOfTuples (n_corr);
1236 line_points->SetNumberOfPoints (2 * n_corr);
1239 line_cells_id->SetNumberOfComponents (3);
1240 line_cells_id->SetNumberOfTuples (n_corr);
1241 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1245 line_tcoords->SetNumberOfComponents (1);
1246 line_tcoords->SetNumberOfTuples (n_corr * 2);
1247 line_tcoords->SetName (
"Texture Coordinates");
1249 double tc[3] = {0.0, 0.0, 0.0};
1251 Eigen::Affine3f source_transformation;
1253 source_transformation.translation () = source_points->
sensor_origin_.template head<3> ();
1254 Eigen::Affine3f target_transformation;
1256 target_transformation.translation () = target_points->
sensor_origin_.template head<3> ();
1260 for (std::size_t i = 0; i < correspondences.size (); i += nth, ++j)
1262 if (correspondences[i].index_match ==
UNAVAILABLE)
1264 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1268 PointT p_src ((*source_points)[correspondences[i].index_query]);
1269 PointT p_tgt ((*target_points)[correspondences[i].index_match]);
1271 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1272 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1274 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1276 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1277 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1279 *line_cell_id++ = 2;
1280 *line_cell_id++ = id1;
1281 *line_cell_id++ = id2;
1283 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1284 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1287 rgb[0] = vtkMath::Random (32, 255);
1288 rgb[1] = vtkMath::Random (32, 255);
1289 rgb[2] = vtkMath::Random (32, 255);
1290 line_colors->InsertTuple (i, rgb);
1292 line_colors->SetNumberOfTuples (j);
1293 line_cells_id->SetNumberOfTuples (j);
1294 line_cells->SetCells (n_corr, line_cells_id);
1295 line_points->SetNumberOfPoints (j*2);
1296 line_tcoords->SetNumberOfTuples (j*2);
1299 line_data->SetPoints (line_points);
1300 line_data->SetLines (line_cells);
1301 line_data->GetPointData ()->SetTCoords (line_tcoords);
1302 line_data->GetCellData ()->SetScalars (line_colors);
1308 createActorFromVTKDataSet (line_data, actor);
1309 actor->GetProperty ()->SetRepresentationToWireframe ();
1310 actor->GetProperty ()->SetOpacity (0.5);
1311 addActorToRenderer (actor, viewport);
1314 (*shape_actor_map_)[id] = actor;
1322 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1329template <
typename Po
intT>
bool
1335 const std::string &
id,
1338 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1342template <
typename Po
intT>
bool
1343pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1346 const std::string &
id,
1348 const Eigen::Vector4f& sensor_origin,
1349 const Eigen::Quaternion<float>& sensor_orientation)
1353 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1359 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1366 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1369 bool has_colors =
false;
1371 if (
auto scalars = color_handler.
getColor ())
1373 polydata->GetPointData ()->SetScalars (scalars);
1374 scalars->GetRange (minmax);
1380 createActorFromVTKDataSet (polydata, actor);
1382 actor->GetMapper ()->SetScalarRange (minmax);
1385 addActorToRenderer (actor, viewport);
1388 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1389 cloud_actor.actor = actor;
1390 cloud_actor.cells = initcells;
1394 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1395 cloud_actor.viewpoint_transformation_ = transformation;
1396 cloud_actor.actor->SetUserMatrix (transformation);
1397 cloud_actor.actor->Modified ();
1403template <
typename Po
intT>
bool
1404pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1405 const PointCloudGeometryHandler<PointT> &geometry_handler,
1406 const ColorHandlerConstPtr &color_handler,
1407 const std::string &
id,
1409 const Eigen::Vector4f& sensor_origin,
1410 const Eigen::Quaternion<float>& sensor_orientation)
1412 if (!geometry_handler.isCapable ())
1414 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.getName ().c_str ());
1418 if (!color_handler->isCapable ())
1420 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->getName ().c_str ());
1427 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1431 bool has_colors =
false;
1433 if (
auto scalars = color_handler->getColor ())
1435 polydata->GetPointData ()->SetScalars (scalars);
1436 scalars->GetRange (minmax);
1442 createActorFromVTKDataSet (polydata, actor);
1444 actor->GetMapper ()->SetScalarRange (minmax);
1447 addActorToRenderer (actor, viewport);
1450 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1451 cloud_actor.actor = actor;
1452 cloud_actor.cells = initcells;
1453 cloud_actor.color_handlers.push_back (color_handler);
1457 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1458 cloud_actor.viewpoint_transformation_ = transformation;
1459 cloud_actor.actor->SetUserMatrix (transformation);
1460 cloud_actor.actor->Modified ();
1466template <
typename Po
intT>
bool
1467pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1468 const GeometryHandlerConstPtr &geometry_handler,
1469 const PointCloudColorHandler<PointT> &color_handler,
1470 const std::string &
id,
1472 const Eigen::Vector4f& sensor_origin,
1473 const Eigen::Quaternion<float>& sensor_orientation)
1475 if (!geometry_handler->isCapable ())
1477 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->getName ().c_str ());
1481 if (!color_handler.isCapable ())
1483 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.getName ().c_str ());
1490 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1494 bool has_colors =
false;
1496 if (
auto scalars = color_handler.getColor ())
1498 polydata->GetPointData ()->SetScalars (scalars);
1499 scalars->GetRange (minmax);
1505 createActorFromVTKDataSet (polydata, actor);
1507 actor->GetMapper ()->SetScalarRange (minmax);
1510 addActorToRenderer (actor, viewport);
1513 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1514 cloud_actor.actor = actor;
1515 cloud_actor.cells = initcells;
1516 cloud_actor.geometry_handlers.push_back (geometry_handler);
1520 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1521 cloud_actor.viewpoint_transformation_ = transformation;
1522 cloud_actor.actor->SetUserMatrix (transformation);
1523 cloud_actor.actor->Modified ();
1529template <
typename Po
intT>
bool
1531 const std::string &
id)
1534 auto am_it = cloud_actor_map_->find (
id);
1536 if (am_it == cloud_actor_map_->end ())
1543 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1547 polydata->GetPointData ()->SetScalars (scalars);
1549 minmax[0] = std::numeric_limits<double>::min ();
1550 minmax[1] = std::numeric_limits<double>::max ();
1551 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1554 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1559template <
typename Po
intT>
bool
1562 const std::string &
id)
1565 auto am_it = cloud_actor_map_->find (
id);
1567 if (am_it == cloud_actor_map_->end ())
1574 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1578 polydata->GetPointData ()->SetScalars (scalars);
1580 minmax[0] = std::numeric_limits<double>::min ();
1581 minmax[1] = std::numeric_limits<double>::max ();
1582 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1585 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1591template <
typename Po
intT>
bool
1594 const std::string &
id)
1597 auto am_it = cloud_actor_map_->find (
id);
1599 if (am_it == cloud_actor_map_->end ())
1607 convertPointCloudToVTKPolyData<PointT>(cloud, polydata, am_it->second.cells);
1610 bool has_colors =
false;
1612 if (
auto scalars = color_handler.
getColor ())
1615 polydata->GetPointData ()->SetScalars (scalars);
1616 scalars->GetRange (minmax);
1621 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1624 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1629template <
typename Po
intT>
bool
1632 const std::vector<pcl::Vertices> &vertices,
1633 const std::string &
id,
1636 if (vertices.empty () || cloud->
points.empty ())
1641 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1646 std::vector<pcl::PCLPointField> fields;
1648 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1650 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1654 colors->SetNumberOfComponents (3);
1655 colors->SetName (
"Colors");
1656 std::uint32_t offset = fields[rgb_idx].offset;
1657 for (std::size_t i = 0; i < cloud->
size (); ++i)
1661 const auto*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1662 unsigned char color[3];
1663 color[0] = rgb_data->r;
1664 color[1] = rgb_data->g;
1665 color[2] = rgb_data->b;
1666 colors->InsertNextTupleValue (color);
1672 vtkIdType nr_points = cloud->
size ();
1673 points->SetNumberOfPoints (nr_points);
1677 float *data =
dynamic_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1680 std::vector<int> lookup;
1684 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3) {
1685 std::copy(&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1690 lookup.resize (nr_points);
1692 for (vtkIdType i = 0; i < nr_points; ++i)
1698 lookup[i] =
static_cast<int> (j);
1699 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1704 points->SetNumberOfPoints (nr_points);
1708 int max_size_of_polygon = -1;
1709 for (
const auto &vertex : vertices)
1710 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1711 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1713 if (vertices.size () > 1)
1718 const auto idx =
details::fillCells(lookup,vertices,cell_array, max_size_of_polygon);
1721 allocVtkPolyData (polydata);
1722 cell_array->GetData ()->SetNumberOfValues (idx);
1723 cell_array->Squeeze ();
1724 polydata->SetPolys (cell_array);
1725 polydata->SetPoints (points);
1728 polydata->GetPointData ()->SetScalars (colors);
1730 createActorFromVTKDataSet (polydata, actor,
false);
1735 std::size_t n_points = vertices[0].vertices.size ();
1736 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1738 if (!lookup.empty ())
1740 for (std::size_t j = 0; j < (n_points - 1); ++j)
1741 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1745 for (std::size_t j = 0; j < (n_points - 1); ++j)
1746 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1750 poly_grid->Allocate (1, 1);
1751 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1752 poly_grid->SetPoints (points);
1754 poly_grid->GetPointData ()->SetScalars (colors);
1756 createActorFromVTKDataSet (poly_grid, actor,
false);
1758 addActorToRenderer (actor, viewport);
1759 actor->GetProperty ()->SetRepresentationToSurface ();
1761 actor->GetProperty ()->BackfaceCullingOff ();
1762 actor->GetProperty ()->SetInterpolationToFlat ();
1763 actor->GetProperty ()->EdgeVisibilityOff ();
1764 actor->GetProperty ()->ShadingOff ();
1767 (*cloud_actor_map_)[id].actor = actor;
1772 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1778template <
typename Po
intT>
bool
1781 const std::vector<pcl::Vertices> &verts,
1782 const std::string &
id)
1791 auto am_it = cloud_actor_map_->find (
id);
1792 if (am_it == cloud_actor_map_->end ())
1804 vtkIdType nr_points = cloud->
size ();
1805 points->SetNumberOfPoints (nr_points);
1808 float *data = (
dynamic_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1811 std::vector<int> lookup;
1815 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1816 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1820 lookup.resize (nr_points);
1822 for (vtkIdType i = 0; i < nr_points; ++i)
1828 lookup [i] =
static_cast<int> (j);
1829 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1834 points->SetNumberOfPoints (nr_points);
1838 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1842 std::vector<pcl::PCLPointField> fields;
1843 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1845 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1846 if (rgb_idx != -1 && colors)
1849 std::uint32_t offset = fields[rgb_idx].offset;
1850 for (std::size_t i = 0; i < cloud->
size (); ++i)
1854 const auto*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1855 unsigned char color[3];
1856 color[0] = rgb_data->r;
1857 color[1] = rgb_data->g;
1858 color[2] = rgb_data->b;
1859 colors->SetTupleValue (j++, color);
1864 int max_size_of_polygon = -1;
1865 for (
const auto &vertex : verts)
1866 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1867 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1874 cells->GetData ()->SetNumberOfValues (idx);
1877 polydata->SetPolys (cells);
1882#ifdef vtkGenericDataArray_h
1884#undef InsertNextTupleValue
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
std::uint32_t width
The point cloud width (if organized as an image-structure).
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
std::uint32_t height
The point cloud height (if organized as an image-structure).
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
shared_ptr< const PointCloud< PointT > > ConstPtr
vtkSmartPointer< vtkLODActor > actor
The actor holding the data to render.
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 addPolygonMesh(const pcl::PolygonMesh &polymesh, const std::string &id="polygon", int viewport=0)
Add a PolygonMesh object to screen.
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 addPointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a Point Cloud (templated) to screen.
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.
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)
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.
GeometryHandler::ConstPtr GeometryHandlerConstPtr
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.
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.
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.
ColorHandler::ConstPtr ColorHandlerConstPtr
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 addLine(const P1 &pt1, const P2 &pt2, const std::string &id="line", int viewport=0)
Add a line segment from two points.
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.
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.
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.
Handler for predefined user colors.
Base Handler class for PointCloud colors.
virtual vtkSmartPointer< vtkDataArray > getColor() const =0
Obtain the actual color for the input dataset as a VTK data array.
bool isCapable() const
Check if this handler is capable of handling the input data or not.
virtual std::string getName() const =0
Abstract getName method.
RGB handler class for colors.
Base handler class for PointCloud geometry.
virtual std::string getName() const =0
Abstract getName method.
bool isCapable() const
Check if this handler is capable of handling the input data or not.
virtual void getGeometry(vtkSmartPointer< vtkPoints > &points) const =0
Obtain the actual point geometry for the input dataset in VTK format.
XYZ handler class for PointCloud geometry.
PCL_EXPORTS vtkSmartPointer< vtkDataSet > createLine(const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
Create a line shape from two points.
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
PCL_EXPORTS vtkIdType fillCells(std::vector< int > &lookup, const std::vector< pcl::Vertices > &vertices, vtkSmartPointer< vtkCellArray > cell_array, int max_size_of_polygon)
PCL_EXPORTS void allocVtkUnstructuredGrid(vtkSmartPointer< vtkUnstructuredGrid > &polydata)
Allocate a new unstructured grid smartpointer.
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...
static constexpr index_t UNAVAILABLE
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
constexpr bool isNormalFinite(const PointT &) noexcept
constexpr bool isXYZFinite(const PointT &) noexcept
Define methods or creating 3D shapes from parametric models.
A point structure representing Euclidean xyz coordinates, and the RGB color.
A structure representing RGB color information.