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)
262 if (!std::isfinite ((*cloud)[i].x) ||
263 !std::isfinite ((*cloud)[i].y) ||
264 !std::isfinite ((*cloud)[i].z))
267 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
272 points->SetNumberOfPoints (nr_points);
275#ifdef VTK_CELL_ARRAY_V2
279 auto numOfCells = vertices->GetNumberOfCells();
282 if (numOfCells < nr_points)
284 for (
int i = numOfCells; i < nr_points; i++)
286 vertices->InsertNextCell(1);
287 vertices->InsertCellPoint(i);
291 else if (numOfCells > nr_points)
293 vertices->ResizeExact(nr_points, nr_points);
296 polydata->SetPoints(points);
297 polydata->SetVerts(vertices);
301 updateCells (cells, initcells, nr_points);
304 vertices->SetCells (nr_points, cells);
307 vertices->SetNumberOfCells(nr_points);
312template <
typename Po
intT>
void
313pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
321 allocVtkPolyData (polydata);
323 polydata->SetVerts (vertices);
329 polydata->SetPoints (points);
331 vtkIdType nr_points = points->GetNumberOfPoints ();
334 vertices = polydata->GetVerts ();
338#ifdef VTK_CELL_ARRAY_V2
342 auto numOfCells = vertices->GetNumberOfCells();
345 if (numOfCells < nr_points)
347 for (
int i = numOfCells; i < nr_points; i++)
349 vertices->InsertNextCell(1);
350 vertices->InsertCellPoint(i);
354 else if (numOfCells > nr_points)
356 vertices->ResizeExact(nr_points, nr_points);
359 polydata->SetPoints(points);
360 polydata->SetVerts(vertices);
364 updateCells (cells, initcells, nr_points);
366 vertices->SetCells (nr_points, cells);
371template <
typename Po
intT>
bool
374 double r,
double g,
double b,
const std::string &
id,
int viewport)
381 auto am_it = shape_actor_map_->find (
id);
382 if (am_it != shape_actor_map_->end ())
387 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
391 surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
393 all_data->AddInputData (poly_data);
397 createActorFromVTKDataSet (all_data->GetOutput (), actor);
398 actor->GetProperty ()->SetRepresentationToWireframe ();
399 actor->GetProperty ()->SetColor (r, g, b);
400 actor->GetMapper ()->ScalarVisibilityOff ();
401 removeActorFromRenderer (am_it->second, viewport);
402 addActorToRenderer (actor, viewport);
405 (*shape_actor_map_)[id] = actor;
411 createActorFromVTKDataSet (data, actor);
412 actor->GetProperty ()->SetRepresentationToWireframe ();
413 actor->GetProperty ()->SetColor (r, g, b);
414 actor->GetMapper ()->ScalarVisibilityOff ();
415 addActorToRenderer (actor, viewport);
418 (*shape_actor_map_)[id] = actor;
425template <
typename Po
intT>
bool
428 double r,
double g,
double b,
const std::string &
id,
int viewport)
435 auto am_it = shape_actor_map_->find (
id);
436 if (am_it != shape_actor_map_->end ())
441 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
445 surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
447 all_data->AddInputData (poly_data);
451 createActorFromVTKDataSet (all_data->GetOutput (), actor);
452 actor->GetProperty ()->SetRepresentationToWireframe ();
453 actor->GetProperty ()->SetColor (r, g, b);
454 actor->GetMapper ()->ScalarVisibilityOn ();
455 actor->GetProperty ()->BackfaceCullingOff ();
456 removeActorFromRenderer (am_it->second, viewport);
457 addActorToRenderer (actor, viewport);
460 (*shape_actor_map_)[id] = actor;
466 createActorFromVTKDataSet (data, actor);
467 actor->GetProperty ()->SetRepresentationToWireframe ();
468 actor->GetProperty ()->SetColor (r, g, b);
469 actor->GetMapper ()->ScalarVisibilityOn ();
470 actor->GetProperty ()->BackfaceCullingOff ();
471 addActorToRenderer (actor, viewport);
474 (*shape_actor_map_)[id] = actor;
480template <
typename Po
intT>
bool
483 const std::string &
id,
int viewport)
485 return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5,
id, viewport));
489template <
typename P1,
typename P2>
bool
494 PCL_WARN (
"[addLine] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
502 createActorFromVTKDataSet (data, actor);
503 actor->GetProperty ()->SetRepresentationToWireframe ();
504 actor->GetProperty ()->SetColor (r, g, b);
505 actor->GetMapper ()->ScalarVisibilityOff ();
506 addActorToRenderer (actor, viewport);
509 (*shape_actor_map_)[id] = actor;
514template <
typename P1,
typename P2>
bool
519 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
525 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
526 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
527 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
528 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
529 leader->SetArrowStyleToFilled ();
530 leader->AutoLabelOn ();
532 leader->GetProperty ()->SetColor (r, g, b);
533 addActorToRenderer (leader, viewport);
536 (*shape_actor_map_)[id] = leader;
541template <
typename P1,
typename P2>
bool
546 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
552 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
553 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
554 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
555 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
556 leader->SetArrowStyleToFilled ();
557 leader->SetArrowPlacementToPoint1 ();
559 leader->AutoLabelOn ();
561 leader->AutoLabelOff ();
563 leader->GetProperty ()->SetColor (r, g, b);
564 addActorToRenderer (leader, viewport);
567 (*shape_actor_map_)[id] = leader;
571template <
typename P1,
typename P2>
bool
573 double r_line,
double g_line,
double b_line,
574 double r_text,
double g_text,
double b_text,
575 const std::string &
id,
int viewport)
579 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
585 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
586 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
587 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
588 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
589 leader->SetArrowStyleToFilled ();
590 leader->AutoLabelOn ();
592 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
594 leader->GetProperty ()->SetColor (r_line, g_line, b_line);
595 addActorToRenderer (leader, viewport);
598 (*shape_actor_map_)[id] = leader;
603template <
typename P1,
typename P2>
bool
606 return (!addLine (pt1, pt2, 0.5, 0.5, 0.5,
id, viewport));
610template <
typename Po
intT>
bool
615 PCL_WARN (
"[addSphere] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
620 data->SetRadius (radius);
621 data->SetCenter (
static_cast<double>(center.x),
static_cast<double>(center.y),
static_cast<double>(center.z));
622 data->SetPhiResolution (10);
623 data->SetThetaResolution (10);
624 data->LatLongTessellationOff ();
629 mapper->SetInputConnection (data->GetOutputPort ());
633 actor->SetMapper (mapper);
635 actor->GetProperty ()->SetRepresentationToSurface ();
636 actor->GetProperty ()->SetInterpolationToFlat ();
637 actor->GetProperty ()->SetColor (r, g, b);
638 actor->GetMapper ()->StaticOn ();
639 actor->GetMapper ()->ScalarVisibilityOff ();
640 actor->GetMapper ()->Update ();
641 addActorToRenderer (actor, viewport);
644 (*shape_actor_map_)[id] = actor;
649template <
typename Po
intT>
bool
652 return (addSphere (center, radius, 0.5, 0.5, 0.5,
id, viewport));
656template<
typename Po
intT>
bool
666 auto am_it = shape_actor_map_->find (
id);
667 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
670 vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
671 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
675 src->SetCenter (
double (center.x), double (center.y), double (center.z));
676 src->SetRadius (radius);
678 actor->GetProperty ()->SetColor (r, g, b);
685template <
typename Po
intT>
bool
687 const std::string &text,
693 const std::string &
id,
706 if (rens_->GetNumberOfItems () <= viewport)
708 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)! \n",
715 rens_->InitTraversal ();
716 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
718 const std::string uid = tid + std::string (i,
'*');
721 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! \n"
722 "Please choose a different id and retry.\n",
733 textSource->SetText (text.c_str());
734 textSource->Update ();
737 textMapper->SetInputConnection (textSource->GetOutputPort ());
740 rens_->InitTraversal ();
741 vtkRenderer* renderer;
743 while ((renderer = rens_->GetNextItem ()))
746 if (viewport == 0 || viewport == i)
749 textActor->SetMapper (textMapper);
750 textActor->SetPosition (position.x, position.y, position.z);
751 textActor->SetScale (textScale);
752 textActor->GetProperty ()->SetColor (r, g, b);
753 textActor->SetCamera (renderer->GetActiveCamera ());
755 renderer->AddActor (textActor);
759 const std::string uid = tid + std::string (i,
'*');
760 (*shape_actor_map_)[uid] = textActor;
770template <
typename Po
intT>
bool
772 const std::string &text,
774 double orientation[3],
779 const std::string &
id,
792 if (rens_->GetNumberOfItems () <= viewport)
794 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)!\n",
801 rens_->InitTraversal ();
802 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
804 const std::string uid = tid + std::string (i,
'*');
807 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! "
808 "Please choose a different id and retry.\n",
819 textSource->SetText (text.c_str());
820 textSource->Update ();
823 textMapper->SetInputConnection (textSource->GetOutputPort ());
826 textActor->SetMapper (textMapper);
827 textActor->SetPosition (position.x, position.y, position.z);
828 textActor->SetScale (textScale);
829 textActor->GetProperty ()->SetColor (r, g, b);
830 textActor->SetOrientation (orientation);
833 rens_->InitTraversal ();
835 for ( vtkRenderer* renderer = rens_->GetNextItem ();
837 renderer = rens_->GetNextItem (), ++i)
839 if (viewport == 0 || viewport == i)
841 renderer->AddActor (textActor);
842 const std::string uid = tid + std::string (i,
'*');
843 (*shape_actor_map_)[uid] = textActor;
851template <
typename Po
intNT>
bool
854 int level,
float scale,
const std::string &
id,
int viewport)
856 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale,
id, viewport));
860template <
typename Po
intT,
typename Po
intNT>
bool
864 int level,
float scale,
865 const std::string &
id,
int viewport)
867 if (normals->
size () != cloud->
size ())
869 PCL_ERROR (
"[addPointCloudNormals] The number of points differs from the number of normals!\n");
873 if (normals->
empty ())
875 PCL_WARN (
"[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
881 PCL_WARN (
"[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
888 points->SetDataTypeToFloat ();
890 data->SetNumberOfComponents (3);
893 vtkIdType nr_normals = 0;
894 float* pts =
nullptr;
899 auto point_step =
static_cast<vtkIdType
> (sqrt (
static_cast<double>(level)));
900 nr_normals = (
static_cast<vtkIdType
> ((cloud->
width - 1)/ point_step) + 1) *
901 (
static_cast<vtkIdType
> ((cloud->
height - 1) / point_step) + 1);
902 pts =
new float[2 * nr_normals * 3];
904 vtkIdType cell_count = 0;
905 for (vtkIdType y = 0; y < normals->
height; y += point_step)
906 for (vtkIdType x = 0; x < normals->
width; x += point_step)
908 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);
928 nr_normals = (cloud->
size () - 1) / level + 1 ;
929 pts =
new float[2 * nr_normals * 3];
931 for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
934 p.x += (*normals)[i].normal[0] * scale;
935 p.y += (*normals)[i].normal[1] * scale;
936 p.z += (*normals)[i].normal[2] * scale;
938 pts[2 * j * 3 + 0] = (*cloud)[i].x;
939 pts[2 * j * 3 + 1] = (*cloud)[i].y;
940 pts[2 * j * 3 + 2] = (*cloud)[i].z;
941 pts[2 * j * 3 + 3] = p.x;
942 pts[2 * j * 3 + 4] = p.y;
943 pts[2 * j * 3 + 5] = p.z;
945 lines->InsertNextCell (2);
946 lines->InsertCellPoint (2 * j);
947 lines->InsertCellPoint (2 * j + 1);
951 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
952 points->SetData (data);
955 polyData->SetPoints (points);
956 polyData->SetLines (lines);
959 mapper->SetInputData (polyData);
960 mapper->SetColorModeToMapScalars();
961 mapper->SetScalarModeToUsePointData();
965 actor->SetMapper (mapper);
970 actor->SetUserMatrix (transformation);
973 addActorToRenderer (actor, viewport);
976 (*cloud_actor_map_)[id].actor = actor;
981template <
typename Po
intNT>
bool
985 int level,
float scale,
986 const std::string &
id,
int viewport)
988 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
992template <
typename Po
intT,
typename Po
intNT>
bool
997 int level,
float scale,
998 const std::string &
id,
int viewport)
1002 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
1008 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1016 unsigned char green[3] = {0, 255, 0};
1017 unsigned char blue[3] = {0, 0, 255};
1021 line_1_colors->SetNumberOfComponents (3);
1022 line_1_colors->SetName (
"Colors");
1024 line_2_colors->SetNumberOfComponents (3);
1025 line_2_colors->SetName (
"Colors");
1028 for (std::size_t i = 0; i < cloud->
size (); i+=level)
1031 p.x += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[0]) * scale;
1032 p.y += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[1]) * scale;
1033 p.z += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[2]) * scale;
1036 line_1->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1037 line_1->SetPoint2 (p.x, p.y, p.z);
1039 polydata_1->AddInputData (line_1->GetOutput ());
1040 line_1_colors->InsertNextTupleValue (green);
1042 polydata_1->Update ();
1044 line_1_data->GetCellData ()->SetScalars (line_1_colors);
1047 for (std::size_t i = 0; i < cloud->
size (); i += level)
1049 Eigen::Vector3f pc ((*pcs)[i].principal_curvature[0],
1050 (*pcs)[i].principal_curvature[1],
1051 (*pcs)[i].principal_curvature[2]);
1052 Eigen::Vector3f normal ((*normals)[i].normal[0],
1053 (*normals)[i].normal[1],
1054 (*normals)[i].normal[2]);
1055 Eigen::Vector3f pc_c = pc.cross (normal);
1058 p.x += ((*pcs)[i].pc2 * pc_c[0]) * scale;
1059 p.y += ((*pcs)[i].pc2 * pc_c[1]) * scale;
1060 p.z += ((*pcs)[i].pc2 * pc_c[2]) * scale;
1063 line_2->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1064 line_2->SetPoint2 (p.x, p.y, p.z);
1066 polydata_2->AddInputData (line_2->GetOutput ());
1068 line_2_colors->InsertNextTupleValue (blue);
1070 polydata_2->Update ();
1072 line_2_data->GetCellData ()->SetScalars (line_2_colors);
1076 alldata->AddInputData (line_1_data);
1077 alldata->AddInputData (line_2_data);
1082 createActorFromVTKDataSet (alldata->GetOutput (), actor);
1083 actor->GetMapper ()->SetScalarModeToUseCellData ();
1086 addActorToRenderer (actor, viewport);
1091 (*cloud_actor_map_)[id] = act;
1096template <
typename Po
intT,
typename GradientT>
bool
1100 int level,
double scale,
1101 const std::string &
id,
int viewport)
1103 if (gradients->
size () != cloud->
size ())
1105 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1110 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1117 points->SetDataTypeToFloat ();
1119 data->SetNumberOfComponents (3);
1121 vtkIdType nr_gradients = (cloud->
size () - 1) / level + 1 ;
1122 float* pts =
new float[2 * nr_gradients * 3];
1124 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1127 p.x += (*gradients)[i].gradient[0] * scale;
1128 p.y += (*gradients)[i].gradient[1] * scale;
1129 p.z += (*gradients)[i].gradient[2] * scale;
1131 pts[2 * j * 3 + 0] = (*cloud)[i].x;
1132 pts[2 * j * 3 + 1] = (*cloud)[i].y;
1133 pts[2 * j * 3 + 2] = (*cloud)[i].z;
1134 pts[2 * j * 3 + 3] = p.x;
1135 pts[2 * j * 3 + 4] = p.y;
1136 pts[2 * j * 3 + 5] = p.z;
1138 lines->InsertNextCell(2);
1139 lines->InsertCellPoint(2*j);
1140 lines->InsertCellPoint(2*j+1);
1143 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1144 points->SetData (data);
1147 polyData->SetPoints(points);
1148 polyData->SetLines(lines);
1151 mapper->SetInputData (polyData);
1152 mapper->SetColorModeToMapScalars();
1153 mapper->SetScalarModeToUsePointData();
1157 actor->SetMapper (mapper);
1160 addActorToRenderer (actor, viewport);
1163 (*cloud_actor_map_)[id].actor = actor;
1168template <
typename Po
intT>
bool
1172 const std::vector<int> &correspondences,
1173 const std::string &
id,
1177 corrs.resize (correspondences.size ());
1179 std::size_t index = 0;
1180 for (
auto &corr : corrs)
1182 corr.index_query = index;
1183 corr.index_match = correspondences[index];
1187 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1191template <
typename Po
intT>
bool
1197 const std::string &
id,
1201 if (correspondences.empty ())
1203 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1208 auto am_it = shape_actor_map_->find (
id);
1209 if (am_it != shape_actor_map_->end () && !overwrite)
1211 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1214 if (am_it == shape_actor_map_->end () && overwrite)
1219 int n_corr =
static_cast<int>(correspondences.size () / nth);
1224 line_colors->SetNumberOfComponents (3);
1225 line_colors->SetName (
"Colors");
1226 line_colors->SetNumberOfTuples (n_corr);
1230 line_points->SetNumberOfPoints (2 * n_corr);
1233 line_cells_id->SetNumberOfComponents (3);
1234 line_cells_id->SetNumberOfTuples (n_corr);
1235 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1239 line_tcoords->SetNumberOfComponents (1);
1240 line_tcoords->SetNumberOfTuples (n_corr * 2);
1241 line_tcoords->SetName (
"Texture Coordinates");
1243 double tc[3] = {0.0, 0.0, 0.0};
1245 Eigen::Affine3f source_transformation;
1247 source_transformation.translation () = source_points->
sensor_origin_.head (3);
1248 Eigen::Affine3f target_transformation;
1250 target_transformation.translation () = target_points->
sensor_origin_.head (3);
1254 for (std::size_t i = 0; i < correspondences.size (); i += nth, ++j)
1256 if (correspondences[i].index_match ==
UNAVAILABLE)
1258 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1262 PointT p_src ((*source_points)[correspondences[i].index_query]);
1263 PointT p_tgt ((*target_points)[correspondences[i].index_match]);
1265 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1266 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1268 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1270 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1271 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1273 *line_cell_id++ = 2;
1274 *line_cell_id++ = id1;
1275 *line_cell_id++ = id2;
1277 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1278 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1281 rgb[0] = vtkMath::Random (32, 255);
1282 rgb[1] = vtkMath::Random (32, 255);
1283 rgb[2] = vtkMath::Random (32, 255);
1284 line_colors->InsertTuple (i, rgb);
1286 line_colors->SetNumberOfTuples (j);
1287 line_cells_id->SetNumberOfTuples (j);
1288 line_cells->SetCells (n_corr, line_cells_id);
1289 line_points->SetNumberOfPoints (j*2);
1290 line_tcoords->SetNumberOfTuples (j*2);
1293 line_data->SetPoints (line_points);
1294 line_data->SetLines (line_cells);
1295 line_data->GetPointData ()->SetTCoords (line_tcoords);
1296 line_data->GetCellData ()->SetScalars (line_colors);
1302 createActorFromVTKDataSet (line_data, actor);
1303 actor->GetProperty ()->SetRepresentationToWireframe ();
1304 actor->GetProperty ()->SetOpacity (0.5);
1305 addActorToRenderer (actor, viewport);
1308 (*shape_actor_map_)[id] = actor;
1316 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1323template <
typename Po
intT>
bool
1329 const std::string &
id,
1332 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1336template <
typename Po
intT>
bool
1337pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1340 const std::string &
id,
1342 const Eigen::Vector4f& sensor_origin,
1343 const Eigen::Quaternion<float>& sensor_orientation)
1347 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1353 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1360 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1363 bool has_colors =
false;
1365 if (
auto scalars = color_handler.
getColor ())
1367 polydata->GetPointData ()->SetScalars (scalars);
1368 scalars->GetRange (minmax);
1374 createActorFromVTKDataSet (polydata, actor);
1376 actor->GetMapper ()->SetScalarRange (minmax);
1379 addActorToRenderer (actor, viewport);
1382 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1383 cloud_actor.actor = actor;
1384 cloud_actor.cells = initcells;
1388 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1389 cloud_actor.viewpoint_transformation_ = transformation;
1390 cloud_actor.actor->SetUserMatrix (transformation);
1391 cloud_actor.actor->Modified ();
1397template <
typename Po
intT>
bool
1398pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1399 const PointCloudGeometryHandler<PointT> &geometry_handler,
1400 const ColorHandlerConstPtr &color_handler,
1401 const std::string &
id,
1403 const Eigen::Vector4f& sensor_origin,
1404 const Eigen::Quaternion<float>& sensor_orientation)
1406 if (!geometry_handler.isCapable ())
1408 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.getName ().c_str ());
1412 if (!color_handler->isCapable ())
1414 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->getName ().c_str ());
1421 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1425 bool has_colors =
false;
1427 if (
auto scalars = color_handler->getColor ())
1429 polydata->GetPointData ()->SetScalars (scalars);
1430 scalars->GetRange (minmax);
1436 createActorFromVTKDataSet (polydata, actor);
1438 actor->GetMapper ()->SetScalarRange (minmax);
1441 addActorToRenderer (actor, viewport);
1444 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1445 cloud_actor.actor = actor;
1446 cloud_actor.cells = initcells;
1447 cloud_actor.color_handlers.push_back (color_handler);
1451 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1452 cloud_actor.viewpoint_transformation_ = transformation;
1453 cloud_actor.actor->SetUserMatrix (transformation);
1454 cloud_actor.actor->Modified ();
1460template <
typename Po
intT>
bool
1461pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1462 const GeometryHandlerConstPtr &geometry_handler,
1463 const PointCloudColorHandler<PointT> &color_handler,
1464 const std::string &
id,
1466 const Eigen::Vector4f& sensor_origin,
1467 const Eigen::Quaternion<float>& sensor_orientation)
1469 if (!geometry_handler->isCapable ())
1471 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->getName ().c_str ());
1475 if (!color_handler.isCapable ())
1477 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.getName ().c_str ());
1484 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1488 bool has_colors =
false;
1490 if (
auto scalars = color_handler.getColor ())
1492 polydata->GetPointData ()->SetScalars (scalars);
1493 scalars->GetRange (minmax);
1499 createActorFromVTKDataSet (polydata, actor);
1501 actor->GetMapper ()->SetScalarRange (minmax);
1504 addActorToRenderer (actor, viewport);
1507 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1508 cloud_actor.actor = actor;
1509 cloud_actor.cells = initcells;
1510 cloud_actor.geometry_handlers.push_back (geometry_handler);
1514 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1515 cloud_actor.viewpoint_transformation_ = transformation;
1516 cloud_actor.actor->SetUserMatrix (transformation);
1517 cloud_actor.actor->Modified ();
1523template <
typename Po
intT>
bool
1525 const std::string &
id)
1528 auto am_it = cloud_actor_map_->find (
id);
1530 if (am_it == cloud_actor_map_->end ())
1537 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1541 polydata->GetPointData ()->SetScalars (scalars);
1543 minmax[0] = std::numeric_limits<double>::min ();
1544 minmax[1] = std::numeric_limits<double>::max ();
1545 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1548 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1553template <
typename Po
intT>
bool
1556 const std::string &
id)
1559 auto am_it = cloud_actor_map_->find (
id);
1561 if (am_it == cloud_actor_map_->end ())
1568 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1572 polydata->GetPointData ()->SetScalars (scalars);
1574 minmax[0] = std::numeric_limits<double>::min ();
1575 minmax[1] = std::numeric_limits<double>::max ();
1576 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1579 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1585template <
typename Po
intT>
bool
1588 const std::string &
id)
1591 auto am_it = cloud_actor_map_->find (
id);
1593 if (am_it == cloud_actor_map_->end ())
1601 convertPointCloudToVTKPolyData<PointT>(cloud, polydata, am_it->second.cells);
1604 bool has_colors =
false;
1606 if (
auto scalars = color_handler.
getColor ())
1609 polydata->GetPointData ()->SetScalars (scalars);
1610 scalars->GetRange (minmax);
1615 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1618 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1623template <
typename Po
intT>
bool
1626 const std::vector<pcl::Vertices> &vertices,
1627 const std::string &
id,
1630 if (vertices.empty () || cloud->
points.empty ())
1635 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1640 std::vector<pcl::PCLPointField> fields;
1642 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1644 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1648 colors->SetNumberOfComponents (3);
1649 colors->SetName (
"Colors");
1650 std::uint32_t offset = fields[rgb_idx].offset;
1651 for (std::size_t i = 0; i < cloud->
size (); ++i)
1655 const auto*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1656 unsigned char color[3];
1657 color[0] = rgb_data->r;
1658 color[1] = rgb_data->g;
1659 color[2] = rgb_data->b;
1660 colors->InsertNextTupleValue (color);
1666 vtkIdType nr_points = cloud->
size ();
1667 points->SetNumberOfPoints (nr_points);
1671 float *data =
dynamic_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1674 std::vector<int> lookup;
1678 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3) {
1679 std::copy(&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1684 lookup.resize (nr_points);
1686 for (vtkIdType i = 0; i < nr_points; ++i)
1692 lookup[i] =
static_cast<int> (j);
1693 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1698 points->SetNumberOfPoints (nr_points);
1702 int max_size_of_polygon = -1;
1703 for (
const auto &vertex : vertices)
1704 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1705 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1707 if (vertices.size () > 1)
1712 const auto idx =
details::fillCells(lookup,vertices,cell_array, max_size_of_polygon);
1715 allocVtkPolyData (polydata);
1716 cell_array->GetData ()->SetNumberOfValues (idx);
1717 cell_array->Squeeze ();
1718 polydata->SetPolys (cell_array);
1719 polydata->SetPoints (points);
1722 polydata->GetPointData ()->SetScalars (colors);
1724 createActorFromVTKDataSet (polydata, actor,
false);
1729 std::size_t n_points = vertices[0].vertices.size ();
1730 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1732 if (!lookup.empty ())
1734 for (std::size_t j = 0; j < (n_points - 1); ++j)
1735 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1739 for (std::size_t j = 0; j < (n_points - 1); ++j)
1740 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1744 poly_grid->Allocate (1, 1);
1745 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1746 poly_grid->SetPoints (points);
1748 poly_grid->GetPointData ()->SetScalars (colors);
1750 createActorFromVTKDataSet (poly_grid, actor,
false);
1752 addActorToRenderer (actor, viewport);
1753 actor->GetProperty ()->SetRepresentationToSurface ();
1755 actor->GetProperty ()->BackfaceCullingOff ();
1756 actor->GetProperty ()->SetInterpolationToFlat ();
1757 actor->GetProperty ()->EdgeVisibilityOff ();
1758 actor->GetProperty ()->ShadingOff ();
1761 (*cloud_actor_map_)[id].actor = actor;
1766 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1772template <
typename Po
intT>
bool
1775 const std::vector<pcl::Vertices> &verts,
1776 const std::string &
id)
1785 auto am_it = cloud_actor_map_->find (
id);
1786 if (am_it == cloud_actor_map_->end ())
1798 vtkIdType nr_points = cloud->
size ();
1799 points->SetNumberOfPoints (nr_points);
1802 float *data = (
dynamic_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1805 std::vector<int> lookup;
1809 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1810 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1814 lookup.resize (nr_points);
1816 for (vtkIdType i = 0; i < nr_points; ++i)
1822 lookup [i] =
static_cast<int> (j);
1823 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1828 points->SetNumberOfPoints (nr_points);
1832 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1836 std::vector<pcl::PCLPointField> fields;
1837 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1839 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1840 if (rgb_idx != -1 && colors)
1843 std::uint32_t offset = fields[rgb_idx].offset;
1844 for (std::size_t i = 0; i < cloud->
size (); ++i)
1848 const auto*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1849 unsigned char color[3];
1850 color[0] = rgb_data->r;
1851 color[1] = rgb_data->g;
1852 color[2] = rgb_data->b;
1853 colors->SetTupleValue (j++, color);
1858 int max_size_of_polygon = -1;
1859 for (
const auto &vertex : verts)
1860 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1861 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1868 cells->GetData ()->SetNumberOfValues (idx);
1871 polydata->SetPolys (cells);
1876#ifdef vtkGenericDataArray_h
1878#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
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.