38 #ifndef PCL_PCL_VISUALIZER_IMPL_H_
39 #define PCL_PCL_VISUALIZER_IMPL_H_
41 #include <vtkVersion.h>
42 #include <vtkSmartPointer.h>
43 #include <vtkCellArray.h>
44 #include <vtkLeaderActor2D.h>
45 #include <vtkVectorText.h>
46 #include <vtkAlgorithmOutput.h>
47 #include <vtkFollower.h>
49 #include <vtkSphereSource.h>
50 #include <vtkProperty2D.h>
51 #include <vtkDataSetSurfaceFilter.h>
52 #include <vtkPointData.h>
53 #include <vtkPolyDataMapper.h>
54 #include <vtkProperty.h>
55 #include <vtkMapper.h>
56 #include <vtkCellData.h>
57 #include <vtkDataSetMapper.h>
58 #include <vtkRenderer.h>
59 #include <vtkRendererCollection.h>
60 #include <vtkAppendPolyData.h>
61 #include <vtkTextProperty.h>
62 #include <vtkLODActor.h>
63 #include <vtkLineSource.h>
65 #include <pcl/common/utils.h>
69 #ifdef vtkGenericDataArray_h
70 #define SetTupleValue SetTypedTuple
71 #define InsertNextTupleValue InsertNextTypedTuple
72 #define GetTupleValue GetTypedTuple
76 template <
typename Po
intT>
bool
79 const std::string &
id,
int viewport)
83 return (addPointCloud<PointT> (cloud, geometry_handler,
id, viewport));
87 template <
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>())
109 template <
typename Po
intT>
bool
113 const std::string &
id,
int viewport)
119 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
120 am_it->second.geometry_handlers.push_back (geometry_handler);
130 template <
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 ());
152 template <
typename Po
intT>
bool
156 const std::string &
id,
int viewport)
159 CloudActorMap::iterator 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);
173 template <
typename Po
intT>
bool
178 const std::string &
id,
int viewport)
181 CloudActorMap::iterator 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);
194 template <
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 ());
215 template <
typename Po
intT>
void
216 pcl::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 = (
static_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]);
258 for (vtkIdType i = 0; i < nr_points; ++i)
261 if (!std::isfinite ((*cloud)[i].x) ||
262 !std::isfinite ((*cloud)[i].y) ||
263 !std::isfinite ((*cloud)[i].z))
266 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
271 points->SetNumberOfPoints (nr_points);
274 #ifdef VTK_CELL_ARRAY_V2
278 auto numOfCells = vertices->GetNumberOfCells();
281 if (numOfCells < nr_points)
283 for (
int i = numOfCells; i < nr_points; i++)
285 vertices->InsertNextCell(1);
286 vertices->InsertCellPoint(i);
290 else if (numOfCells > nr_points)
292 vertices->ResizeExact(nr_points, nr_points);
295 polydata->SetPoints(points);
296 polydata->SetVerts(vertices);
300 updateCells (cells, initcells, nr_points);
303 vertices->SetCells (nr_points, cells);
306 vertices->SetNumberOfCells(nr_points);
311 template <
typename Po
intT>
void
312 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
320 allocVtkPolyData (polydata);
322 polydata->SetVerts (vertices);
328 polydata->SetPoints (points);
330 vtkIdType nr_points = points->GetNumberOfPoints ();
333 vertices = polydata->GetVerts ();
337 #ifdef VTK_CELL_ARRAY_V2
341 auto numOfCells = vertices->GetNumberOfCells();
344 if (numOfCells < nr_points)
346 for (
int i = numOfCells; i < nr_points; i++)
348 vertices->InsertNextCell(1);
349 vertices->InsertCellPoint(i);
353 else if (numOfCells > nr_points)
355 vertices->ResizeExact(nr_points, nr_points);
358 polydata->SetPoints(points);
359 polydata->SetVerts(vertices);
363 updateCells (cells, initcells, nr_points);
365 vertices->SetCells (nr_points, cells);
370 template <
typename Po
intT>
bool
373 double r,
double g,
double b,
const std::string &
id,
int viewport)
380 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
381 if (am_it != shape_actor_map_->end ())
386 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
390 surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
392 all_data->AddInputData (poly_data);
396 createActorFromVTKDataSet (all_data->GetOutput (), actor);
397 actor->GetProperty ()->SetRepresentationToWireframe ();
398 actor->GetProperty ()->SetColor (r, g, b);
399 actor->GetMapper ()->ScalarVisibilityOff ();
400 removeActorFromRenderer (am_it->second, viewport);
401 addActorToRenderer (actor, viewport);
404 (*shape_actor_map_)[id] = actor;
410 createActorFromVTKDataSet (data, actor);
411 actor->GetProperty ()->SetRepresentationToWireframe ();
412 actor->GetProperty ()->SetColor (r, g, b);
413 actor->GetMapper ()->ScalarVisibilityOff ();
414 addActorToRenderer (actor, viewport);
417 (*shape_actor_map_)[id] = actor;
424 template <
typename Po
intT>
bool
427 double r,
double g,
double b,
const std::string &
id,
int viewport)
434 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
435 if (am_it != shape_actor_map_->end ())
440 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
444 surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
446 all_data->AddInputData (poly_data);
450 createActorFromVTKDataSet (all_data->GetOutput (), actor);
451 actor->GetProperty ()->SetRepresentationToWireframe ();
452 actor->GetProperty ()->SetColor (r, g, b);
453 actor->GetMapper ()->ScalarVisibilityOn ();
454 actor->GetProperty ()->BackfaceCullingOff ();
455 removeActorFromRenderer (am_it->second, viewport);
456 addActorToRenderer (actor, viewport);
459 (*shape_actor_map_)[id] = actor;
465 createActorFromVTKDataSet (data, actor);
466 actor->GetProperty ()->SetRepresentationToWireframe ();
467 actor->GetProperty ()->SetColor (r, g, b);
468 actor->GetMapper ()->ScalarVisibilityOn ();
469 actor->GetProperty ()->BackfaceCullingOff ();
470 addActorToRenderer (actor, viewport);
473 (*shape_actor_map_)[id] = actor;
479 template <
typename Po
intT>
bool
482 const std::string &
id,
int viewport)
484 return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5,
id, viewport));
488 template <
typename P1,
typename P2>
bool
493 PCL_WARN (
"[addLine] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
501 createActorFromVTKDataSet (data, actor);
502 actor->GetProperty ()->SetRepresentationToWireframe ();
503 actor->GetProperty ()->SetColor (r, g, b);
504 actor->GetMapper ()->ScalarVisibilityOff ();
505 addActorToRenderer (actor, viewport);
508 (*shape_actor_map_)[id] = actor;
513 template <
typename P1,
typename P2>
bool
518 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
524 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
525 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
526 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
527 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
528 leader->SetArrowStyleToFilled ();
529 leader->AutoLabelOn ();
531 leader->GetProperty ()->SetColor (r, g, b);
532 addActorToRenderer (leader, viewport);
535 (*shape_actor_map_)[id] = leader;
540 template <
typename P1,
typename P2>
bool
545 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
551 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
552 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
553 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
554 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
555 leader->SetArrowStyleToFilled ();
556 leader->SetArrowPlacementToPoint1 ();
558 leader->AutoLabelOn ();
560 leader->AutoLabelOff ();
562 leader->GetProperty ()->SetColor (r, g, b);
563 addActorToRenderer (leader, viewport);
566 (*shape_actor_map_)[id] = leader;
570 template <
typename P1,
typename P2>
bool
572 double r_line,
double g_line,
double b_line,
573 double r_text,
double g_text,
double b_text,
574 const std::string &
id,
int viewport)
578 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
584 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
585 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
586 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
587 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
588 leader->SetArrowStyleToFilled ();
589 leader->AutoLabelOn ();
591 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
593 leader->GetProperty ()->SetColor (r_line, g_line, b_line);
594 addActorToRenderer (leader, viewport);
597 (*shape_actor_map_)[id] = leader;
602 template <
typename P1,
typename P2>
bool
605 return (!addLine (pt1, pt2, 0.5, 0.5, 0.5,
id, viewport));
609 template <
typename Po
intT>
bool
614 PCL_WARN (
"[addSphere] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
619 data->SetRadius (radius);
620 data->SetCenter (
double (center.x), double (center.y), double (center.z));
621 data->SetPhiResolution (10);
622 data->SetThetaResolution (10);
623 data->LatLongTessellationOff ();
628 mapper->SetInputConnection (data->GetOutputPort ());
632 actor->SetMapper (mapper);
634 actor->GetProperty ()->SetRepresentationToSurface ();
635 actor->GetProperty ()->SetInterpolationToFlat ();
636 actor->GetProperty ()->SetColor (r, g, b);
637 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
638 actor->GetMapper ()->ImmediateModeRenderingOn ();
640 actor->GetMapper ()->StaticOn ();
641 actor->GetMapper ()->ScalarVisibilityOff ();
642 actor->GetMapper ()->Update ();
643 addActorToRenderer (actor, viewport);
646 (*shape_actor_map_)[id] = actor;
651 template <
typename Po
intT>
bool
654 return (addSphere (center, radius, 0.5, 0.5, 0.5,
id, viewport));
658 template<
typename Po
intT>
bool
668 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
669 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
672 vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
673 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
677 src->SetCenter (
double (center.x), double (center.y), double (center.z));
678 src->SetRadius (radius);
680 actor->GetProperty ()->SetColor (r, g, b);
687 template <
typename Po
intT>
bool
689 const std::string &text,
695 const std::string &
id,
708 if (rens_->GetNumberOfItems () <= viewport)
710 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)! \n",
717 rens_->InitTraversal ();
718 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
720 const std::string uid = tid + std::string (i,
'*');
723 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! \n"
724 "Please choose a different id and retry.\n",
735 textSource->SetText (text.c_str());
736 textSource->Update ();
739 textMapper->SetInputConnection (textSource->GetOutputPort ());
742 rens_->InitTraversal ();
743 vtkRenderer* renderer;
745 while ((renderer = rens_->GetNextItem ()))
748 if (viewport == 0 || viewport == i)
751 textActor->SetMapper (textMapper);
752 textActor->SetPosition (position.x, position.y, position.z);
753 textActor->SetScale (textScale);
754 textActor->GetProperty ()->SetColor (r, g, b);
755 textActor->SetCamera (renderer->GetActiveCamera ());
757 renderer->AddActor (textActor);
762 const std::string uid = tid + std::string (i,
'*');
763 (*shape_actor_map_)[uid] = textActor;
773 template <
typename Po
intT>
bool
775 const std::string &text,
777 double orientation[3],
782 const std::string &
id,
795 if (rens_->GetNumberOfItems () <= viewport)
797 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)!\n",
804 rens_->InitTraversal ();
805 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
807 const std::string uid = tid + std::string (i,
'*');
810 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! "
811 "Please choose a different id and retry.\n",
822 textSource->SetText (text.c_str());
823 textSource->Update ();
826 textMapper->SetInputConnection (textSource->GetOutputPort ());
829 textActor->SetMapper (textMapper);
830 textActor->SetPosition (position.x, position.y, position.z);
831 textActor->SetScale (textScale);
832 textActor->GetProperty ()->SetColor (r, g, b);
833 textActor->SetOrientation (orientation);
836 rens_->InitTraversal ();
838 for ( vtkRenderer* renderer = rens_->GetNextItem ();
840 renderer = rens_->GetNextItem (), ++i)
842 if (viewport == 0 || viewport == i)
844 renderer->AddActor (textActor);
845 const std::string uid = tid + std::string (i,
'*');
846 (*shape_actor_map_)[uid] = textActor;
854 template <
typename Po
intNT>
bool
857 int level,
float scale,
const std::string &
id,
int viewport)
859 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale,
id, viewport));
863 template <
typename Po
intT,
typename Po
intNT>
bool
867 int level,
float scale,
868 const std::string &
id,
int viewport)
870 if (normals->
size () != cloud->
size ())
872 PCL_ERROR (
"[addPointCloudNormals] The number of points differs from the number of normals!\n");
876 if (normals->
empty ())
878 PCL_WARN (
"[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
884 PCL_WARN (
"[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
891 points->SetDataTypeToFloat ();
893 data->SetNumberOfComponents (3);
896 vtkIdType nr_normals = 0;
897 float* pts =
nullptr;
902 vtkIdType point_step =
static_cast<vtkIdType
> (sqrt (
double (level)));
903 nr_normals = (
static_cast<vtkIdType
> ((cloud->
width - 1)/ point_step) + 1) *
904 (
static_cast<vtkIdType
> ((cloud->
height - 1) / point_step) + 1);
905 pts =
new float[2 * nr_normals * 3];
907 vtkIdType cell_count = 0;
908 for (vtkIdType y = 0; y < normals->
height; y += point_step)
909 for (vtkIdType x = 0; x < normals->
width; x += point_step)
911 PointT p = (*cloud)(x, y);
912 p.x += (*normals)(x, y).normal[0] * scale;
913 p.y += (*normals)(x, y).normal[1] * scale;
914 p.z += (*normals)(x, y).normal[2] * scale;
916 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
917 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
918 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
919 pts[2 * cell_count * 3 + 3] = p.x;
920 pts[2 * cell_count * 3 + 4] = p.y;
921 pts[2 * cell_count * 3 + 5] = p.z;
923 lines->InsertNextCell (2);
924 lines->InsertCellPoint (2 * cell_count);
925 lines->InsertCellPoint (2 * cell_count + 1);
931 nr_normals = (cloud->
size () - 1) / level + 1 ;
932 pts =
new float[2 * nr_normals * 3];
934 for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
937 p.x += (*normals)[i].normal[0] * scale;
938 p.y += (*normals)[i].normal[1] * scale;
939 p.z += (*normals)[i].normal[2] * scale;
941 pts[2 * j * 3 + 0] = (*cloud)[i].x;
942 pts[2 * j * 3 + 1] = (*cloud)[i].y;
943 pts[2 * j * 3 + 2] = (*cloud)[i].z;
944 pts[2 * j * 3 + 3] = p.x;
945 pts[2 * j * 3 + 4] = p.y;
946 pts[2 * j * 3 + 5] = p.z;
948 lines->InsertNextCell (2);
949 lines->InsertCellPoint (2 * j);
950 lines->InsertCellPoint (2 * j + 1);
954 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
955 points->SetData (data);
958 polyData->SetPoints (points);
959 polyData->SetLines (lines);
962 mapper->SetInputData (polyData);
963 mapper->SetColorModeToMapScalars();
964 mapper->SetScalarModeToUsePointData();
968 actor->SetMapper (mapper);
973 actor->SetUserMatrix (transformation);
976 addActorToRenderer (actor, viewport);
979 (*cloud_actor_map_)[id].actor = actor;
984 template <
typename Po
intNT>
bool
988 int level,
float scale,
989 const std::string &
id,
int viewport)
991 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
995 template <
typename Po
intT,
typename Po
intNT>
bool
1000 int level,
float scale,
1001 const std::string &
id,
int viewport)
1005 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
1011 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1019 unsigned char green[3] = {0, 255, 0};
1020 unsigned char blue[3] = {0, 0, 255};
1024 line_1_colors->SetNumberOfComponents (3);
1025 line_1_colors->SetName (
"Colors");
1027 line_2_colors->SetNumberOfComponents (3);
1028 line_2_colors->SetName (
"Colors");
1031 for (std::size_t i = 0; i < cloud->
size (); i+=level)
1034 p.x += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[0]) * scale;
1035 p.y += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[1]) * scale;
1036 p.z += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[2]) * scale;
1039 line_1->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1040 line_1->SetPoint2 (p.x, p.y, p.z);
1042 polydata_1->AddInputData (line_1->GetOutput ());
1043 line_1_colors->InsertNextTupleValue (green);
1045 polydata_1->Update ();
1047 line_1_data->GetCellData ()->SetScalars (line_1_colors);
1050 for (std::size_t i = 0; i < cloud->
size (); i += level)
1052 Eigen::Vector3f pc ((*pcs)[i].principal_curvature[0],
1053 (*pcs)[i].principal_curvature[1],
1054 (*pcs)[i].principal_curvature[2]);
1055 Eigen::Vector3f normal ((*normals)[i].normal[0],
1056 (*normals)[i].normal[1],
1057 (*normals)[i].normal[2]);
1058 Eigen::Vector3f pc_c = pc.cross (normal);
1061 p.x += ((*pcs)[i].pc2 * pc_c[0]) * scale;
1062 p.y += ((*pcs)[i].pc2 * pc_c[1]) * scale;
1063 p.z += ((*pcs)[i].pc2 * pc_c[2]) * scale;
1066 line_2->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1067 line_2->SetPoint2 (p.x, p.y, p.z);
1069 polydata_2->AddInputData (line_2->GetOutput ());
1071 line_2_colors->InsertNextTupleValue (blue);
1073 polydata_2->Update ();
1075 line_2_data->GetCellData ()->SetScalars (line_2_colors);
1079 alldata->AddInputData (line_1_data);
1080 alldata->AddInputData (line_2_data);
1084 createActorFromVTKDataSet (alldata->GetOutput (), actor);
1085 actor->GetMapper ()->SetScalarModeToUseCellData ();
1088 addActorToRenderer (actor, viewport);
1093 (*cloud_actor_map_)[id] = act;
1098 template <
typename Po
intT,
typename GradientT>
bool
1102 int level,
double scale,
1103 const std::string &
id,
int viewport)
1105 if (gradients->
size () != cloud->
size ())
1107 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1112 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1119 points->SetDataTypeToFloat ();
1121 data->SetNumberOfComponents (3);
1123 vtkIdType nr_gradients = (cloud->
size () - 1) / level + 1 ;
1124 float* pts =
new float[2 * nr_gradients * 3];
1126 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1129 p.x += (*gradients)[i].gradient[0] * scale;
1130 p.y += (*gradients)[i].gradient[1] * scale;
1131 p.z += (*gradients)[i].gradient[2] * scale;
1133 pts[2 * j * 3 + 0] = (*cloud)[i].x;
1134 pts[2 * j * 3 + 1] = (*cloud)[i].y;
1135 pts[2 * j * 3 + 2] = (*cloud)[i].z;
1136 pts[2 * j * 3 + 3] = p.x;
1137 pts[2 * j * 3 + 4] = p.y;
1138 pts[2 * j * 3 + 5] = p.z;
1140 lines->InsertNextCell(2);
1141 lines->InsertCellPoint(2*j);
1142 lines->InsertCellPoint(2*j+1);
1145 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1146 points->SetData (data);
1149 polyData->SetPoints(points);
1150 polyData->SetLines(lines);
1153 mapper->SetInputData (polyData);
1154 mapper->SetColorModeToMapScalars();
1155 mapper->SetScalarModeToUsePointData();
1159 actor->SetMapper (mapper);
1162 addActorToRenderer (actor, viewport);
1165 (*cloud_actor_map_)[id].actor = actor;
1170 template <
typename Po
intT>
bool
1174 const std::vector<int> &correspondences,
1175 const std::string &
id,
1179 corrs.resize (correspondences.size ());
1181 std::size_t index = 0;
1182 for (
auto &corr : corrs)
1184 corr.index_query = index;
1185 corr.index_match = correspondences[index];
1189 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1193 template <
typename Po
intT>
bool
1199 const std::string &
id,
1203 if (correspondences.empty ())
1205 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1210 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
1211 if (am_it != shape_actor_map_->end () && !overwrite)
1213 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1216 if (am_it == shape_actor_map_->end () && overwrite)
1221 int n_corr = int (correspondences.size () / nth);
1226 line_colors->SetNumberOfComponents (3);
1227 line_colors->SetName (
"Colors");
1228 line_colors->SetNumberOfTuples (n_corr);
1232 line_points->SetNumberOfPoints (2 * n_corr);
1235 line_cells_id->SetNumberOfComponents (3);
1236 line_cells_id->SetNumberOfTuples (n_corr);
1237 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1241 line_tcoords->SetNumberOfComponents (1);
1242 line_tcoords->SetNumberOfTuples (n_corr * 2);
1243 line_tcoords->SetName (
"Texture Coordinates");
1245 double tc[3] = {0.0, 0.0, 0.0};
1247 Eigen::Affine3f source_transformation;
1249 source_transformation.translation () = source_points->
sensor_origin_.head (3);
1250 Eigen::Affine3f target_transformation;
1252 target_transformation.translation () = target_points->
sensor_origin_.head (3);
1256 for (std::size_t i = 0; i < correspondences.size (); i += nth, ++j)
1258 if (correspondences[i].index_match == -1)
1260 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1264 PointT p_src ((*source_points)[correspondences[i].index_query]);
1265 PointT p_tgt ((*target_points)[correspondences[i].index_match]);
1267 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1268 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1270 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1272 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1273 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1275 *line_cell_id++ = 2;
1276 *line_cell_id++ = id1;
1277 *line_cell_id++ = id2;
1279 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1280 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1283 rgb[0] = vtkMath::Random (32, 255);
1284 rgb[1] = vtkMath::Random (32, 255);
1285 rgb[2] = vtkMath::Random (32, 255);
1286 line_colors->InsertTuple (i, rgb);
1288 line_colors->SetNumberOfTuples (j);
1289 line_cells_id->SetNumberOfTuples (j);
1290 line_cells->SetCells (n_corr, line_cells_id);
1291 line_points->SetNumberOfPoints (j*2);
1292 line_tcoords->SetNumberOfTuples (j*2);
1295 line_data->SetPoints (line_points);
1296 line_data->SetLines (line_cells);
1297 line_data->GetPointData ()->SetTCoords (line_tcoords);
1298 line_data->GetCellData ()->SetScalars (line_colors);
1304 createActorFromVTKDataSet (line_data, actor);
1305 actor->GetProperty ()->SetRepresentationToWireframe ();
1306 actor->GetProperty ()->SetOpacity (0.5);
1307 addActorToRenderer (actor, viewport);
1310 (*shape_actor_map_)[id] = actor;
1318 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1325 template <
typename Po
intT>
bool
1331 const std::string &
id,
1334 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1338 template <
typename Po
intT>
bool
1339 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1342 const std::string &
id,
1344 const Eigen::Vector4f& sensor_origin,
1345 const Eigen::Quaternion<float>& sensor_orientation)
1349 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1355 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1362 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1365 bool has_colors =
false;
1367 if (
auto scalars = color_handler.
getColor ())
1369 polydata->GetPointData ()->SetScalars (scalars);
1370 scalars->GetRange (minmax);
1376 createActorFromVTKDataSet (polydata, actor);
1378 actor->GetMapper ()->SetScalarRange (minmax);
1381 addActorToRenderer (actor, viewport);
1384 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1385 cloud_actor.actor = actor;
1386 cloud_actor.cells = initcells;
1390 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1391 cloud_actor.viewpoint_transformation_ = transformation;
1392 cloud_actor.actor->SetUserMatrix (transformation);
1393 cloud_actor.actor->Modified ();
1399 template <
typename Po
intT>
bool
1400 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1401 const PointCloudGeometryHandler<PointT> &geometry_handler,
1402 const ColorHandlerConstPtr &color_handler,
1403 const std::string &
id,
1405 const Eigen::Vector4f& sensor_origin,
1406 const Eigen::Quaternion<float>& sensor_orientation)
1408 if (!geometry_handler.isCapable ())
1410 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.getName ().c_str ());
1414 if (!color_handler->isCapable ())
1416 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->getName ().c_str ());
1423 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1427 bool has_colors =
false;
1429 if (
auto scalars = color_handler->getColor ())
1431 polydata->GetPointData ()->SetScalars (scalars);
1432 scalars->GetRange (minmax);
1438 createActorFromVTKDataSet (polydata, actor);
1440 actor->GetMapper ()->SetScalarRange (minmax);
1443 addActorToRenderer (actor, viewport);
1446 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1447 cloud_actor.actor = actor;
1448 cloud_actor.cells = initcells;
1449 cloud_actor.color_handlers.push_back (color_handler);
1453 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1454 cloud_actor.viewpoint_transformation_ = transformation;
1455 cloud_actor.actor->SetUserMatrix (transformation);
1456 cloud_actor.actor->Modified ();
1462 template <
typename Po
intT>
bool
1463 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1464 const GeometryHandlerConstPtr &geometry_handler,
1465 const PointCloudColorHandler<PointT> &color_handler,
1466 const std::string &
id,
1468 const Eigen::Vector4f& sensor_origin,
1469 const Eigen::Quaternion<float>& sensor_orientation)
1471 if (!geometry_handler->isCapable ())
1473 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->getName ().c_str ());
1477 if (!color_handler.isCapable ())
1479 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.getName ().c_str ());
1486 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1490 bool has_colors =
false;
1492 if (
auto scalars = color_handler.getColor ())
1494 polydata->GetPointData ()->SetScalars (scalars);
1495 scalars->GetRange (minmax);
1501 createActorFromVTKDataSet (polydata, actor);
1503 actor->GetMapper ()->SetScalarRange (minmax);
1506 addActorToRenderer (actor, viewport);
1509 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1510 cloud_actor.actor = actor;
1511 cloud_actor.cells = initcells;
1512 cloud_actor.geometry_handlers.push_back (geometry_handler);
1516 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1517 cloud_actor.viewpoint_transformation_ = transformation;
1518 cloud_actor.actor->SetUserMatrix (transformation);
1519 cloud_actor.actor->Modified ();
1525 template <
typename Po
intT>
bool
1527 const std::string &
id)
1530 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1532 if (am_it == cloud_actor_map_->end ())
1539 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1543 polydata->GetPointData ()->SetScalars (scalars);
1545 minmax[0] = std::numeric_limits<double>::min ();
1546 minmax[1] = std::numeric_limits<double>::max ();
1547 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1548 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1550 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1553 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1558 template <
typename Po
intT>
bool
1561 const std::string &
id)
1564 CloudActorMap::iterator 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 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1582 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1584 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1587 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1593 template <
typename Po
intT>
bool
1596 const std::string &
id)
1599 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1601 if (am_it == cloud_actor_map_->end ())
1609 convertPointCloudToVTKPolyData<PointT>(cloud, polydata, am_it->second.cells);
1612 bool has_colors =
false;
1614 if (
auto scalars = color_handler.
getColor ())
1617 polydata->GetPointData ()->SetScalars (scalars);
1618 scalars->GetRange (minmax);
1622 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1623 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1627 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1630 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1635 template <
typename Po
intT>
bool
1638 const std::vector<pcl::Vertices> &vertices,
1639 const std::string &
id,
1642 if (vertices.empty () || cloud->
points.empty ())
1647 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1652 std::vector<pcl::PCLPointField> fields;
1654 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1656 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1660 colors->SetNumberOfComponents (3);
1661 colors->SetName (
"Colors");
1662 std::uint32_t offset = fields[rgb_idx].offset;
1663 for (std::size_t i = 0; i < cloud->
size (); ++i)
1667 const pcl::RGB*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1668 unsigned char color[3];
1669 color[0] = rgb_data->r;
1670 color[1] = rgb_data->g;
1671 color[2] = rgb_data->b;
1672 colors->InsertNextTupleValue (color);
1678 vtkIdType nr_points = cloud->
size ();
1679 points->SetNumberOfPoints (nr_points);
1683 float *data =
static_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1686 std::vector<int> lookup;
1690 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1691 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1695 lookup.resize (nr_points);
1697 for (vtkIdType i = 0; i < nr_points; ++i)
1703 lookup[i] =
static_cast<int> (j);
1704 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1709 points->SetNumberOfPoints (nr_points);
1713 int max_size_of_polygon = -1;
1714 for (
const auto &vertex : vertices)
1715 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1716 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1718 if (vertices.size () > 1)
1723 const auto idx =
details::fillCells(lookup,vertices,cell_array, max_size_of_polygon);
1726 allocVtkPolyData (polydata);
1727 cell_array->GetData ()->SetNumberOfValues (idx);
1728 cell_array->Squeeze ();
1729 polydata->SetPolys (cell_array);
1730 polydata->SetPoints (points);
1733 polydata->GetPointData ()->SetScalars (colors);
1735 createActorFromVTKDataSet (polydata, actor,
false);
1740 std::size_t n_points = vertices[0].vertices.size ();
1741 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1743 if (!lookup.empty ())
1745 for (std::size_t j = 0; j < (n_points - 1); ++j)
1746 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1750 for (std::size_t j = 0; j < (n_points - 1); ++j)
1751 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1755 poly_grid->Allocate (1, 1);
1756 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1757 poly_grid->SetPoints (points);
1759 poly_grid->GetPointData ()->SetScalars (colors);
1761 createActorFromVTKDataSet (poly_grid, actor,
false);
1763 addActorToRenderer (actor, viewport);
1764 actor->GetProperty ()->SetRepresentationToSurface ();
1766 actor->GetProperty ()->BackfaceCullingOff ();
1767 actor->GetProperty ()->SetInterpolationToFlat ();
1768 actor->GetProperty ()->EdgeVisibilityOff ();
1769 actor->GetProperty ()->ShadingOff ();
1772 (*cloud_actor_map_)[id].actor = actor;
1777 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1783 template <
typename Po
intT>
bool
1786 const std::vector<pcl::Vertices> &verts,
1787 const std::string &
id)
1796 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1797 if (am_it == cloud_actor_map_->end ())
1809 vtkIdType nr_points = cloud->
size ();
1810 points->SetNumberOfPoints (nr_points);
1813 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1816 std::vector<int> lookup;
1820 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1821 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1825 lookup.resize (nr_points);
1827 for (vtkIdType i = 0; i < nr_points; ++i)
1833 lookup [i] =
static_cast<int> (j);
1834 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1839 points->SetNumberOfPoints (nr_points);
1843 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1847 std::vector<pcl::PCLPointField> fields;
1848 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1850 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1851 if (rgb_idx != -1 && colors)
1854 std::uint32_t offset = fields[rgb_idx].offset;
1855 for (std::size_t i = 0; i < cloud->
size (); ++i)
1859 const pcl::RGB*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1860 unsigned char color[3];
1861 color[0] = rgb_data->r;
1862 color[1] = rgb_data->g;
1863 color[2] = rgb_data->b;
1864 colors->SetTupleValue (j++, color);
1869 int max_size_of_polygon = -1;
1870 for (
const auto &vertex : verts)
1871 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1872 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1879 cells->GetData ()->SetNumberOfValues (idx);
1882 polydata->SetPolys (cells);
1887 #ifdef vtkGenericDataArray_h
1888 #undef SetTupleValue
1889 #undef InsertNextTupleValue
1890 #undef GetTupleValue