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);
911 p.x += (*normals)(x, y).normal[0] * scale;
912 p.y += (*normals)(x, y).normal[1] * scale;
913 p.z += (*normals)(x, y).normal[2] * scale;
915 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
916 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
917 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
918 pts[2 * cell_count * 3 + 3] = p.x;
919 pts[2 * cell_count * 3 + 4] = p.y;
920 pts[2 * cell_count * 3 + 5] = p.z;
922 lines->InsertNextCell (2);
923 lines->InsertCellPoint (2 * cell_count);
924 lines->InsertCellPoint (2 * cell_count + 1);
930 nr_normals = (cloud->
size () - 1) / level + 1 ;
931 pts =
new float[2 * nr_normals * 3];
933 for (vtkIdType i = 0, j = 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);
956 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
957 points->SetData (data);
960 polyData->SetPoints (points);
961 polyData->SetLines (lines);
964 mapper->SetInputData (polyData);
965 mapper->SetColorModeToMapScalars();
966 mapper->SetScalarModeToUsePointData();
970 actor->SetMapper (mapper);
975 actor->SetUserMatrix (transformation);
978 addActorToRenderer (actor, viewport);
981 (*cloud_actor_map_)[id].actor = actor;
986template <
typename Po
intNT>
bool
990 int level,
float scale,
991 const std::string &
id,
int viewport)
993 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
997template <
typename Po
intT,
typename Po
intNT>
bool
1002 int level,
float scale,
1003 const std::string &
id,
int viewport)
1007 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
1013 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1021 unsigned char green[3] = {0, 255, 0};
1022 unsigned char blue[3] = {0, 0, 255};
1026 line_1_colors->SetNumberOfComponents (3);
1027 line_1_colors->SetName (
"Colors");
1029 line_2_colors->SetNumberOfComponents (3);
1030 line_2_colors->SetName (
"Colors");
1033 for (std::size_t i = 0; i < cloud->
size (); i+=level)
1036 p.x += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[0]) * scale;
1037 p.y += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[1]) * scale;
1038 p.z += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[2]) * scale;
1041 line_1->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1042 line_1->SetPoint2 (p.x, p.y, p.z);
1044 polydata_1->AddInputData (line_1->GetOutput ());
1045 line_1_colors->InsertNextTupleValue (green);
1047 polydata_1->Update ();
1049 line_1_data->GetCellData ()->SetScalars (line_1_colors);
1052 for (std::size_t i = 0; i < cloud->
size (); i += level)
1054 Eigen::Vector3f pc ((*pcs)[i].principal_curvature[0],
1055 (*pcs)[i].principal_curvature[1],
1056 (*pcs)[i].principal_curvature[2]);
1057 Eigen::Vector3f normal ((*normals)[i].normal[0],
1058 (*normals)[i].normal[1],
1059 (*normals)[i].normal[2]);
1060 Eigen::Vector3f pc_c = pc.cross (normal);
1063 p.x += ((*pcs)[i].pc2 * pc_c[0]) * scale;
1064 p.y += ((*pcs)[i].pc2 * pc_c[1]) * scale;
1065 p.z += ((*pcs)[i].pc2 * pc_c[2]) * scale;
1068 line_2->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1069 line_2->SetPoint2 (p.x, p.y, p.z);
1071 polydata_2->AddInputData (line_2->GetOutput ());
1073 line_2_colors->InsertNextTupleValue (blue);
1075 polydata_2->Update ();
1077 line_2_data->GetCellData ()->SetScalars (line_2_colors);
1081 alldata->AddInputData (line_1_data);
1082 alldata->AddInputData (line_2_data);
1087 createActorFromVTKDataSet (alldata->GetOutput (), actor);
1088 actor->GetMapper ()->SetScalarModeToUseCellData ();
1091 addActorToRenderer (actor, viewport);
1096 (*cloud_actor_map_)[id] = act;
1101template <
typename Po
intT,
typename GradientT>
bool
1105 int level,
double scale,
1106 const std::string &
id,
int viewport)
1108 if (gradients->
size () != cloud->
size ())
1110 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1115 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1122 points->SetDataTypeToFloat ();
1124 data->SetNumberOfComponents (3);
1126 vtkIdType nr_gradients = (cloud->
size () - 1) / level + 1 ;
1127 float* pts =
new float[2 * nr_gradients * 3];
1129 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1132 p.x += (*gradients)[i].gradient[0] * scale;
1133 p.y += (*gradients)[i].gradient[1] * scale;
1134 p.z += (*gradients)[i].gradient[2] * scale;
1136 pts[2 * j * 3 + 0] = (*cloud)[i].x;
1137 pts[2 * j * 3 + 1] = (*cloud)[i].y;
1138 pts[2 * j * 3 + 2] = (*cloud)[i].z;
1139 pts[2 * j * 3 + 3] = p.x;
1140 pts[2 * j * 3 + 4] = p.y;
1141 pts[2 * j * 3 + 5] = p.z;
1143 lines->InsertNextCell(2);
1144 lines->InsertCellPoint(2*j);
1145 lines->InsertCellPoint(2*j+1);
1148 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1149 points->SetData (data);
1152 polyData->SetPoints(points);
1153 polyData->SetLines(lines);
1156 mapper->SetInputData (polyData);
1157 mapper->SetColorModeToMapScalars();
1158 mapper->SetScalarModeToUsePointData();
1162 actor->SetMapper (mapper);
1165 addActorToRenderer (actor, viewport);
1168 (*cloud_actor_map_)[id].actor = actor;
1173template <
typename Po
intT>
bool
1177 const std::vector<int> &correspondences,
1178 const std::string &
id,
1182 corrs.resize (correspondences.size ());
1184 std::size_t index = 0;
1185 for (
auto &corr : corrs)
1187 corr.index_query = index;
1188 corr.index_match = correspondences[index];
1192 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1196template <
typename Po
intT>
bool
1202 const std::string &
id,
1206 if (correspondences.empty ())
1208 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1213 auto am_it = shape_actor_map_->find (
id);
1214 if (am_it != shape_actor_map_->end () && !overwrite)
1216 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1219 if (am_it == shape_actor_map_->end () && overwrite)
1224 int n_corr =
static_cast<int>(correspondences.size () / nth);
1229 line_colors->SetNumberOfComponents (3);
1230 line_colors->SetName (
"Colors");
1231 line_colors->SetNumberOfTuples (n_corr);
1235 line_points->SetNumberOfPoints (2 * n_corr);
1238 line_cells_id->SetNumberOfComponents (3);
1239 line_cells_id->SetNumberOfTuples (n_corr);
1240 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1244 line_tcoords->SetNumberOfComponents (1);
1245 line_tcoords->SetNumberOfTuples (n_corr * 2);
1246 line_tcoords->SetName (
"Texture Coordinates");
1248 double tc[3] = {0.0, 0.0, 0.0};
1250 Eigen::Affine3f source_transformation;
1252 source_transformation.translation () = source_points->
sensor_origin_.head (3);
1253 Eigen::Affine3f target_transformation;
1255 target_transformation.translation () = target_points->
sensor_origin_.head (3);
1259 for (std::size_t i = 0; i < correspondences.size (); i += nth, ++j)
1261 if (correspondences[i].index_match ==
UNAVAILABLE)
1263 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1267 PointT p_src ((*source_points)[correspondences[i].index_query]);
1268 PointT p_tgt ((*target_points)[correspondences[i].index_match]);
1270 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1271 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1273 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1275 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1276 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1278 *line_cell_id++ = 2;
1279 *line_cell_id++ = id1;
1280 *line_cell_id++ = id2;
1282 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1283 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1286 rgb[0] = vtkMath::Random (32, 255);
1287 rgb[1] = vtkMath::Random (32, 255);
1288 rgb[2] = vtkMath::Random (32, 255);
1289 line_colors->InsertTuple (i, rgb);
1291 line_colors->SetNumberOfTuples (j);
1292 line_cells_id->SetNumberOfTuples (j);
1293 line_cells->SetCells (n_corr, line_cells_id);
1294 line_points->SetNumberOfPoints (j*2);
1295 line_tcoords->SetNumberOfTuples (j*2);
1298 line_data->SetPoints (line_points);
1299 line_data->SetLines (line_cells);
1300 line_data->GetPointData ()->SetTCoords (line_tcoords);
1301 line_data->GetCellData ()->SetScalars (line_colors);
1307 createActorFromVTKDataSet (line_data, actor);
1308 actor->GetProperty ()->SetRepresentationToWireframe ();
1309 actor->GetProperty ()->SetOpacity (0.5);
1310 addActorToRenderer (actor, viewport);
1313 (*shape_actor_map_)[id] = actor;
1321 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1328template <
typename Po
intT>
bool
1334 const std::string &
id,
1337 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1341template <
typename Po
intT>
bool
1342pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1345 const std::string &
id,
1347 const Eigen::Vector4f& sensor_origin,
1348 const Eigen::Quaternion<float>& sensor_orientation)
1352 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1358 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1365 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1368 bool has_colors =
false;
1370 if (
auto scalars = color_handler.
getColor ())
1372 polydata->GetPointData ()->SetScalars (scalars);
1373 scalars->GetRange (minmax);
1379 createActorFromVTKDataSet (polydata, actor);
1381 actor->GetMapper ()->SetScalarRange (minmax);
1384 addActorToRenderer (actor, viewport);
1387 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1388 cloud_actor.actor = actor;
1389 cloud_actor.cells = initcells;
1393 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1394 cloud_actor.viewpoint_transformation_ = transformation;
1395 cloud_actor.actor->SetUserMatrix (transformation);
1396 cloud_actor.actor->Modified ();
1402template <
typename Po
intT>
bool
1403pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1404 const PointCloudGeometryHandler<PointT> &geometry_handler,
1405 const ColorHandlerConstPtr &color_handler,
1406 const std::string &
id,
1408 const Eigen::Vector4f& sensor_origin,
1409 const Eigen::Quaternion<float>& sensor_orientation)
1411 if (!geometry_handler.isCapable ())
1413 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.getName ().c_str ());
1417 if (!color_handler->isCapable ())
1419 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->getName ().c_str ());
1426 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1430 bool has_colors =
false;
1432 if (
auto scalars = color_handler->getColor ())
1434 polydata->GetPointData ()->SetScalars (scalars);
1435 scalars->GetRange (minmax);
1441 createActorFromVTKDataSet (polydata, actor);
1443 actor->GetMapper ()->SetScalarRange (minmax);
1446 addActorToRenderer (actor, viewport);
1449 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1450 cloud_actor.actor = actor;
1451 cloud_actor.cells = initcells;
1452 cloud_actor.color_handlers.push_back (color_handler);
1456 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1457 cloud_actor.viewpoint_transformation_ = transformation;
1458 cloud_actor.actor->SetUserMatrix (transformation);
1459 cloud_actor.actor->Modified ();
1465template <
typename Po
intT>
bool
1466pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1467 const GeometryHandlerConstPtr &geometry_handler,
1468 const PointCloudColorHandler<PointT> &color_handler,
1469 const std::string &
id,
1471 const Eigen::Vector4f& sensor_origin,
1472 const Eigen::Quaternion<float>& sensor_orientation)
1474 if (!geometry_handler->isCapable ())
1476 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->getName ().c_str ());
1480 if (!color_handler.isCapable ())
1482 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.getName ().c_str ());
1489 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1493 bool has_colors =
false;
1495 if (
auto scalars = color_handler.getColor ())
1497 polydata->GetPointData ()->SetScalars (scalars);
1498 scalars->GetRange (minmax);
1504 createActorFromVTKDataSet (polydata, actor);
1506 actor->GetMapper ()->SetScalarRange (minmax);
1509 addActorToRenderer (actor, viewport);
1512 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1513 cloud_actor.actor = actor;
1514 cloud_actor.cells = initcells;
1515 cloud_actor.geometry_handlers.push_back (geometry_handler);
1519 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1520 cloud_actor.viewpoint_transformation_ = transformation;
1521 cloud_actor.actor->SetUserMatrix (transformation);
1522 cloud_actor.actor->Modified ();
1528template <
typename Po
intT>
bool
1530 const std::string &
id)
1533 auto am_it = cloud_actor_map_->find (
id);
1535 if (am_it == cloud_actor_map_->end ())
1542 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1546 polydata->GetPointData ()->SetScalars (scalars);
1548 minmax[0] = std::numeric_limits<double>::min ();
1549 minmax[1] = std::numeric_limits<double>::max ();
1550 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1553 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1558template <
typename Po
intT>
bool
1561 const std::string &
id)
1564 auto am_it = cloud_actor_map_->find (
id);
1566 if (am_it == cloud_actor_map_->end ())
1573 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1577 polydata->GetPointData ()->SetScalars (scalars);
1579 minmax[0] = std::numeric_limits<double>::min ();
1580 minmax[1] = std::numeric_limits<double>::max ();
1581 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1584 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1590template <
typename Po
intT>
bool
1593 const std::string &
id)
1596 auto am_it = cloud_actor_map_->find (
id);
1598 if (am_it == cloud_actor_map_->end ())
1606 convertPointCloudToVTKPolyData<PointT>(cloud, polydata, am_it->second.cells);
1609 bool has_colors =
false;
1611 if (
auto scalars = color_handler.
getColor ())
1614 polydata->GetPointData ()->SetScalars (scalars);
1615 scalars->GetRange (minmax);
1620 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1623 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1628template <
typename Po
intT>
bool
1631 const std::vector<pcl::Vertices> &vertices,
1632 const std::string &
id,
1635 if (vertices.empty () || cloud->
points.empty ())
1640 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1645 std::vector<pcl::PCLPointField> fields;
1647 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1649 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1653 colors->SetNumberOfComponents (3);
1654 colors->SetName (
"Colors");
1655 std::uint32_t offset = fields[rgb_idx].offset;
1656 for (std::size_t i = 0; i < cloud->
size (); ++i)
1660 const auto*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1661 unsigned char color[3];
1662 color[0] = rgb_data->r;
1663 color[1] = rgb_data->g;
1664 color[2] = rgb_data->b;
1665 colors->InsertNextTupleValue (color);
1671 vtkIdType nr_points = cloud->
size ();
1672 points->SetNumberOfPoints (nr_points);
1676 float *data =
dynamic_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1679 std::vector<int> lookup;
1683 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3) {
1684 std::copy(&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1689 lookup.resize (nr_points);
1691 for (vtkIdType i = 0; i < nr_points; ++i)
1697 lookup[i] =
static_cast<int> (j);
1698 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1703 points->SetNumberOfPoints (nr_points);
1707 int max_size_of_polygon = -1;
1708 for (
const auto &vertex : vertices)
1709 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1710 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1712 if (vertices.size () > 1)
1717 const auto idx =
details::fillCells(lookup,vertices,cell_array, max_size_of_polygon);
1720 allocVtkPolyData (polydata);
1721 cell_array->GetData ()->SetNumberOfValues (idx);
1722 cell_array->Squeeze ();
1723 polydata->SetPolys (cell_array);
1724 polydata->SetPoints (points);
1727 polydata->GetPointData ()->SetScalars (colors);
1729 createActorFromVTKDataSet (polydata, actor,
false);
1734 std::size_t n_points = vertices[0].vertices.size ();
1735 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1737 if (!lookup.empty ())
1739 for (std::size_t j = 0; j < (n_points - 1); ++j)
1740 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1744 for (std::size_t j = 0; j < (n_points - 1); ++j)
1745 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1749 poly_grid->Allocate (1, 1);
1750 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1751 poly_grid->SetPoints (points);
1753 poly_grid->GetPointData ()->SetScalars (colors);
1755 createActorFromVTKDataSet (poly_grid, actor,
false);
1757 addActorToRenderer (actor, viewport);
1758 actor->GetProperty ()->SetRepresentationToSurface ();
1760 actor->GetProperty ()->BackfaceCullingOff ();
1761 actor->GetProperty ()->SetInterpolationToFlat ();
1762 actor->GetProperty ()->EdgeVisibilityOff ();
1763 actor->GetProperty ()->ShadingOff ();
1766 (*cloud_actor_map_)[id].actor = actor;
1771 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1777template <
typename Po
intT>
bool
1780 const std::vector<pcl::Vertices> &verts,
1781 const std::string &
id)
1790 auto am_it = cloud_actor_map_->find (
id);
1791 if (am_it == cloud_actor_map_->end ())
1803 vtkIdType nr_points = cloud->
size ();
1804 points->SetNumberOfPoints (nr_points);
1807 float *data = (
dynamic_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1810 std::vector<int> lookup;
1814 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1815 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1819 lookup.resize (nr_points);
1821 for (vtkIdType i = 0; i < nr_points; ++i)
1827 lookup [i] =
static_cast<int> (j);
1828 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1833 points->SetNumberOfPoints (nr_points);
1837 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1841 std::vector<pcl::PCLPointField> fields;
1842 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1844 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1845 if (rgb_idx != -1 && colors)
1848 std::uint32_t offset = fields[rgb_idx].offset;
1849 for (std::size_t i = 0; i < cloud->
size (); ++i)
1853 const auto*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1854 unsigned char color[3];
1855 color[0] = rgb_data->r;
1856 color[1] = rgb_data->g;
1857 color[2] = rgb_data->b;
1858 colors->SetTupleValue (j++, color);
1863 int max_size_of_polygon = -1;
1864 for (
const auto &vertex : verts)
1865 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1866 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1873 cells->GetData ()->SetNumberOfValues (idx);
1876 polydata->SetPolys (cells);
1881#ifdef vtkGenericDataArray_h
1883#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
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.