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>
68 #ifdef vtkGenericDataArray_h
69 #define SetTupleValue SetTypedTuple
70 #define InsertNextTupleValue InsertNextTypedTuple
71 #define GetTupleValue GetTypedTuple
75 template <
typename Po
intT>
bool
78 const std::string &
id,
int viewport)
82 return (addPointCloud<PointT> (cloud, geometry_handler,
id, viewport));
86 template <
typename Po
intT>
bool
90 const std::string &
id,
int viewport)
94 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
98 if (pcl::traits::has_color<PointT>())
108 template <
typename Po
intT>
bool
112 const std::string &
id,
int viewport)
118 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
119 am_it->second.geometry_handlers.push_back (geometry_handler);
129 template <
typename Po
intT>
bool
133 const std::string &
id,
int viewport)
137 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
151 template <
typename Po
intT>
bool
155 const std::string &
id,
int viewport)
158 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
159 if (am_it != cloud_actor_map_->end ())
163 am_it->second.color_handlers.push_back (color_handler);
172 template <
typename Po
intT>
bool
177 const std::string &
id,
int viewport)
180 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
181 if (am_it != cloud_actor_map_->end ())
185 am_it->second.geometry_handlers.push_back (geometry_handler);
186 am_it->second.color_handlers.push_back (color_handler);
193 template <
typename Po
intT>
bool
198 const std::string &
id,
int viewport)
202 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
214 template <
typename Po
intT>
void
215 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
223 allocVtkPolyData (polydata);
225 polydata->SetVerts (vertices);
229 vertices = polydata->GetVerts ();
233 vtkIdType nr_points = cloud->
points.size ();
239 points->SetDataTypeToFloat ();
240 polydata->SetPoints (points);
242 points->SetNumberOfPoints (nr_points);
245 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
251 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
252 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[ptr]);
257 for (vtkIdType i = 0; i < nr_points; ++i)
260 if (!std::isfinite (cloud->
points[i].x) ||
261 !std::isfinite (cloud->
points[i].y) ||
262 !std::isfinite (cloud->
points[i].z))
265 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[ptr]);
269 points->SetNumberOfPoints (nr_points);
273 updateCells (cells, initcells, nr_points);
276 vertices->SetCells (nr_points, cells);
280 template <
typename Po
intT>
void
281 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
289 allocVtkPolyData (polydata);
291 polydata->SetVerts (vertices);
297 polydata->SetPoints (points);
299 vtkIdType nr_points = points->GetNumberOfPoints ();
302 vertices = polydata->GetVerts ();
307 updateCells (cells, initcells, nr_points);
309 vertices->SetCells (nr_points, cells);
313 template <
typename Po
intT>
bool
316 double r,
double g,
double b,
const std::string &
id,
int viewport)
323 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
324 if (am_it != shape_actor_map_->end ())
329 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
333 surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
335 all_data->AddInputData (poly_data);
339 createActorFromVTKDataSet (all_data->GetOutput (), actor);
340 actor->GetProperty ()->SetRepresentationToWireframe ();
341 actor->GetProperty ()->SetColor (r, g, b);
342 actor->GetMapper ()->ScalarVisibilityOff ();
343 removeActorFromRenderer (am_it->second, viewport);
344 addActorToRenderer (actor, viewport);
347 (*shape_actor_map_)[id] = actor;
353 createActorFromVTKDataSet (data, actor);
354 actor->GetProperty ()->SetRepresentationToWireframe ();
355 actor->GetProperty ()->SetColor (r, g, b);
356 actor->GetMapper ()->ScalarVisibilityOff ();
357 addActorToRenderer (actor, viewport);
360 (*shape_actor_map_)[id] = actor;
367 template <
typename Po
intT>
bool
370 double r,
double g,
double b,
const std::string &
id,
int viewport)
377 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
378 if (am_it != shape_actor_map_->end ())
383 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
387 surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
389 all_data->AddInputData (poly_data);
393 createActorFromVTKDataSet (all_data->GetOutput (), actor);
394 actor->GetProperty ()->SetRepresentationToWireframe ();
395 actor->GetProperty ()->SetColor (r, g, b);
396 actor->GetMapper ()->ScalarVisibilityOn ();
397 actor->GetProperty ()->BackfaceCullingOff ();
398 removeActorFromRenderer (am_it->second, viewport);
399 addActorToRenderer (actor, viewport);
402 (*shape_actor_map_)[id] = actor;
408 createActorFromVTKDataSet (data, actor);
409 actor->GetProperty ()->SetRepresentationToWireframe ();
410 actor->GetProperty ()->SetColor (r, g, b);
411 actor->GetMapper ()->ScalarVisibilityOn ();
412 actor->GetProperty ()->BackfaceCullingOff ();
413 addActorToRenderer (actor, viewport);
416 (*shape_actor_map_)[id] = actor;
422 template <
typename Po
intT>
bool
425 const std::string &
id,
int viewport)
427 return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5,
id, viewport));
431 template <
typename P1,
typename P2>
bool
436 PCL_WARN (
"[addLine] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
444 createActorFromVTKDataSet (data, actor);
445 actor->GetProperty ()->SetRepresentationToWireframe ();
446 actor->GetProperty ()->SetColor (r, g, b);
447 actor->GetMapper ()->ScalarVisibilityOff ();
448 addActorToRenderer (actor, viewport);
451 (*shape_actor_map_)[id] = actor;
456 template <
typename P1,
typename P2>
bool
461 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
467 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
468 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
469 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
470 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
471 leader->SetArrowStyleToFilled ();
472 leader->AutoLabelOn ();
474 leader->GetProperty ()->SetColor (r, g, b);
475 addActorToRenderer (leader, viewport);
478 (*shape_actor_map_)[id] = leader;
483 template <
typename P1,
typename P2>
bool
488 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
494 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
495 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
496 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
497 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
498 leader->SetArrowStyleToFilled ();
499 leader->SetArrowPlacementToPoint1 ();
501 leader->AutoLabelOn ();
503 leader->AutoLabelOff ();
505 leader->GetProperty ()->SetColor (r, g, b);
506 addActorToRenderer (leader, viewport);
509 (*shape_actor_map_)[id] = leader;
513 template <
typename P1,
typename P2>
bool
515 double r_line,
double g_line,
double b_line,
516 double r_text,
double g_text,
double b_text,
517 const std::string &
id,
int viewport)
521 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
527 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
528 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
529 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
530 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
531 leader->SetArrowStyleToFilled ();
532 leader->AutoLabelOn ();
534 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
536 leader->GetProperty ()->SetColor (r_line, g_line, b_line);
537 addActorToRenderer (leader, viewport);
540 (*shape_actor_map_)[id] = leader;
545 template <
typename P1,
typename P2>
bool
548 return (!addLine (pt1, pt2, 0.5, 0.5, 0.5,
id, viewport));
552 template <
typename Po
intT>
bool
557 PCL_WARN (
"[addSphere] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
562 data->SetRadius (radius);
563 data->SetCenter (
double (center.x), double (center.y), double (center.z));
564 data->SetPhiResolution (10);
565 data->SetThetaResolution (10);
566 data->LatLongTessellationOff ();
571 mapper->SetInputConnection (data->GetOutputPort ());
575 actor->SetMapper (mapper);
577 actor->GetProperty ()->SetRepresentationToSurface ();
578 actor->GetProperty ()->SetInterpolationToFlat ();
579 actor->GetProperty ()->SetColor (r, g, b);
580 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
581 actor->GetMapper ()->ImmediateModeRenderingOn ();
583 actor->GetMapper ()->StaticOn ();
584 actor->GetMapper ()->ScalarVisibilityOff ();
585 actor->GetMapper ()->Update ();
586 addActorToRenderer (actor, viewport);
589 (*shape_actor_map_)[id] = actor;
594 template <
typename Po
intT>
bool
597 return (addSphere (center, radius, 0.5, 0.5, 0.5,
id, viewport));
601 template<
typename Po
intT>
bool
611 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
612 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
615 vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
616 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
620 src->SetCenter (
double (center.x), double (center.y), double (center.z));
621 src->SetRadius (radius);
623 actor->GetProperty ()->SetColor (r, g, b);
630 template <
typename Po
intT>
bool
632 const std::string &text,
638 const std::string &
id,
651 if (rens_->GetNumberOfItems () <= viewport)
653 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
660 rens_->InitTraversal ();
661 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
663 const std::string uid = tid + std::string (i,
'*');
666 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! "
667 "Please choose a different id and retry.\n",
678 textSource->SetText (text.c_str());
679 textSource->Update ();
682 textMapper->SetInputConnection (textSource->GetOutputPort ());
685 rens_->InitTraversal ();
686 vtkRenderer* renderer;
688 while ((renderer = rens_->GetNextItem ()))
691 if (viewport == 0 || viewport == i)
694 textActor->SetMapper (textMapper);
695 textActor->SetPosition (position.x, position.y, position.z);
696 textActor->SetScale (textScale);
697 textActor->GetProperty ()->SetColor (r, g, b);
698 textActor->SetCamera (renderer->GetActiveCamera ());
700 renderer->AddActor (textActor);
705 const std::string uid = tid + std::string (i,
'*');
706 (*shape_actor_map_)[uid] = textActor;
716 template <
typename Po
intT>
bool
718 const std::string &text,
720 double orientation[3],
725 const std::string &
id,
738 if (rens_->GetNumberOfItems () <= viewport)
740 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
747 rens_->InitTraversal ();
748 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
750 const std::string uid = tid + std::string (i,
'*');
753 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! "
754 "Please choose a different id and retry.\n",
765 textSource->SetText (text.c_str());
766 textSource->Update ();
769 textMapper->SetInputConnection (textSource->GetOutputPort ());
772 textActor->SetMapper (textMapper);
773 textActor->SetPosition (position.x, position.y, position.z);
774 textActor->SetScale (textScale);
775 textActor->GetProperty ()->SetColor (r, g, b);
776 textActor->SetOrientation (orientation);
779 rens_->InitTraversal ();
781 for ( vtkRenderer* renderer = rens_->GetNextItem ();
783 renderer = rens_->GetNextItem (), ++i)
785 if (viewport == 0 || viewport == i)
787 renderer->AddActor (textActor);
788 const std::string uid = tid + std::string (i,
'*');
789 (*shape_actor_map_)[uid] = textActor;
797 template <
typename Po
intNT>
bool
800 int level,
float scale,
const std::string &
id,
int viewport)
802 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale,
id, viewport));
806 template <
typename Po
intT,
typename Po
intNT>
bool
810 int level,
float scale,
811 const std::string &
id,
int viewport)
815 PCL_ERROR (
"[addPointCloudNormals] The number of points differs from the number of normals!\n");
819 if (normals->
empty ())
821 PCL_WARN (
"[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
827 PCL_WARN (
"[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
834 points->SetDataTypeToFloat ();
836 data->SetNumberOfComponents (3);
839 vtkIdType nr_normals = 0;
840 float* pts =
nullptr;
845 vtkIdType point_step =
static_cast<vtkIdType
> (sqrt (
double (level)));
846 nr_normals = (
static_cast<vtkIdType
> ((cloud->
width - 1)/ point_step) + 1) *
847 (
static_cast<vtkIdType
> ((cloud->
height - 1) / point_step) + 1);
848 pts =
new float[2 * nr_normals * 3];
850 vtkIdType cell_count = 0;
851 for (vtkIdType y = 0; y < normals->
height; y += point_step)
852 for (vtkIdType x = 0; x < normals->
width; x += point_step)
854 PointT p = (*cloud)(x, y);
855 p.x += (*normals)(x, y).normal[0] * scale;
856 p.y += (*normals)(x, y).normal[1] * scale;
857 p.z += (*normals)(x, y).normal[2] * scale;
859 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
860 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
861 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
862 pts[2 * cell_count * 3 + 3] = p.x;
863 pts[2 * cell_count * 3 + 4] = p.y;
864 pts[2 * cell_count * 3 + 5] = p.z;
866 lines->InsertNextCell (2);
867 lines->InsertCellPoint (2 * cell_count);
868 lines->InsertCellPoint (2 * cell_count + 1);
874 nr_normals = (cloud->
points.size () - 1) / level + 1 ;
875 pts =
new float[2 * nr_normals * 3];
877 for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
880 p.x += normals->
points[i].normal[0] * scale;
881 p.y += normals->
points[i].normal[1] * scale;
882 p.z += normals->
points[i].normal[2] * scale;
884 pts[2 * j * 3 + 0] = cloud->
points[i].x;
885 pts[2 * j * 3 + 1] = cloud->
points[i].y;
886 pts[2 * j * 3 + 2] = cloud->
points[i].z;
887 pts[2 * j * 3 + 3] = p.x;
888 pts[2 * j * 3 + 4] = p.y;
889 pts[2 * j * 3 + 5] = p.z;
891 lines->InsertNextCell (2);
892 lines->InsertCellPoint (2 * j);
893 lines->InsertCellPoint (2 * j + 1);
897 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
898 points->SetData (data);
901 polyData->SetPoints (points);
902 polyData->SetLines (lines);
905 mapper->SetInputData (polyData);
906 mapper->SetColorModeToMapScalars();
907 mapper->SetScalarModeToUsePointData();
911 actor->SetMapper (mapper);
916 actor->SetUserMatrix (transformation);
919 addActorToRenderer (actor, viewport);
922 (*cloud_actor_map_)[id].actor = actor;
927 template <
typename Po
intNT>
bool
931 int level,
float scale,
932 const std::string &
id,
int viewport)
934 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
938 template <
typename Po
intT,
typename Po
intNT>
bool
943 int level,
float scale,
944 const std::string &
id,
int viewport)
948 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
954 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
962 unsigned char green[3] = {0, 255, 0};
963 unsigned char blue[3] = {0, 0, 255};
967 line_1_colors->SetNumberOfComponents (3);
968 line_1_colors->SetName (
"Colors");
970 line_2_colors->SetNumberOfComponents (3);
971 line_2_colors->SetName (
"Colors");
974 for (std::size_t i = 0; i < cloud->
points.size (); i+=level)
977 p.x += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[0]) * scale;
978 p.y += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[1]) * scale;
979 p.z += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[2]) * scale;
983 line_1->SetPoint2 (p.x, p.y, p.z);
985 polydata_1->AddInputData (line_1->GetOutput ());
986 line_1_colors->InsertNextTupleValue (green);
988 polydata_1->Update ();
990 line_1_data->GetCellData ()->SetScalars (line_1_colors);
993 for (std::size_t i = 0; i < cloud->
points.size (); i += level)
995 Eigen::Vector3f pc (pcs->
points[i].principal_curvature[0],
996 pcs->
points[i].principal_curvature[1],
997 pcs->
points[i].principal_curvature[2]);
998 Eigen::Vector3f normal (normals->
points[i].normal[0],
999 normals->
points[i].normal[1],
1000 normals->
points[i].normal[2]);
1001 Eigen::Vector3f pc_c = pc.cross (normal);
1004 p.x += (pcs->
points[i].pc2 * pc_c[0]) * scale;
1005 p.y += (pcs->
points[i].pc2 * pc_c[1]) * scale;
1006 p.z += (pcs->
points[i].pc2 * pc_c[2]) * scale;
1010 line_2->SetPoint2 (p.x, p.y, p.z);
1012 polydata_2->AddInputData (line_2->GetOutput ());
1014 line_2_colors->InsertNextTupleValue (blue);
1016 polydata_2->Update ();
1018 line_2_data->GetCellData ()->SetScalars (line_2_colors);
1022 alldata->AddInputData (line_1_data);
1023 alldata->AddInputData (line_2_data);
1027 createActorFromVTKDataSet (alldata->GetOutput (), actor);
1028 actor->GetMapper ()->SetScalarModeToUseCellData ();
1031 addActorToRenderer (actor, viewport);
1036 (*cloud_actor_map_)[id] = act;
1041 template <
typename Po
intT,
typename GradientT>
bool
1045 int level,
double scale,
1046 const std::string &
id,
int viewport)
1048 if (gradients->
points.size () != cloud->
points.size ())
1050 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1055 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1062 points->SetDataTypeToFloat ();
1064 data->SetNumberOfComponents (3);
1066 vtkIdType nr_gradients = (cloud->
points.size () - 1) / level + 1 ;
1067 float* pts =
new float[2 * nr_gradients * 3];
1069 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1072 p.x += gradients->
points[i].gradient[0] * scale;
1073 p.y += gradients->
points[i].gradient[1] * scale;
1074 p.z += gradients->
points[i].gradient[2] * scale;
1076 pts[2 * j * 3 + 0] = cloud->
points[i].x;
1077 pts[2 * j * 3 + 1] = cloud->
points[i].y;
1078 pts[2 * j * 3 + 2] = cloud->
points[i].z;
1079 pts[2 * j * 3 + 3] = p.x;
1080 pts[2 * j * 3 + 4] = p.y;
1081 pts[2 * j * 3 + 5] = p.z;
1083 lines->InsertNextCell(2);
1084 lines->InsertCellPoint(2*j);
1085 lines->InsertCellPoint(2*j+1);
1088 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1089 points->SetData (data);
1092 polyData->SetPoints(points);
1093 polyData->SetLines(lines);
1096 mapper->SetInputData (polyData);
1097 mapper->SetColorModeToMapScalars();
1098 mapper->SetScalarModeToUsePointData();
1102 actor->SetMapper (mapper);
1105 addActorToRenderer (actor, viewport);
1108 (*cloud_actor_map_)[id].actor = actor;
1113 template <
typename Po
intT>
bool
1117 const std::vector<int> &correspondences,
1118 const std::string &
id,
1122 corrs.resize (correspondences.size ());
1124 std::size_t index = 0;
1125 for (
auto &corr : corrs)
1127 corr.index_query = index;
1128 corr.index_match = correspondences[index];
1132 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1136 template <
typename Po
intT>
bool
1142 const std::string &
id,
1146 if (correspondences.empty ())
1148 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1153 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
1154 if (am_it != shape_actor_map_->end () && !overwrite)
1156 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1159 if (am_it == shape_actor_map_->end () && overwrite)
1164 int n_corr = int (correspondences.size () / nth);
1169 line_colors->SetNumberOfComponents (3);
1170 line_colors->SetName (
"Colors");
1171 line_colors->SetNumberOfTuples (n_corr);
1175 line_points->SetNumberOfPoints (2 * n_corr);
1178 line_cells_id->SetNumberOfComponents (3);
1179 line_cells_id->SetNumberOfTuples (n_corr);
1180 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1184 line_tcoords->SetNumberOfComponents (1);
1185 line_tcoords->SetNumberOfTuples (n_corr * 2);
1186 line_tcoords->SetName (
"Texture Coordinates");
1188 double tc[3] = {0.0, 0.0, 0.0};
1190 Eigen::Affine3f source_transformation;
1192 source_transformation.translation () = source_points->
sensor_origin_.head (3);
1193 Eigen::Affine3f target_transformation;
1195 target_transformation.translation () = target_points->
sensor_origin_.head (3);
1199 for (std::size_t i = 0; i < correspondences.size (); i += nth, ++j)
1201 if (correspondences[i].index_match == -1)
1203 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1207 PointT p_src (source_points->
points[correspondences[i].index_query]);
1208 PointT p_tgt (target_points->
points[correspondences[i].index_match]);
1210 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1211 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1213 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1215 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1216 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1218 *line_cell_id++ = 2;
1219 *line_cell_id++ = id1;
1220 *line_cell_id++ = id2;
1222 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1223 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1226 rgb[0] = vtkMath::Random (32, 255);
1227 rgb[1] = vtkMath::Random (32, 255);
1228 rgb[2] = vtkMath::Random (32, 255);
1229 line_colors->InsertTuple (i, rgb);
1231 line_colors->SetNumberOfTuples (j);
1232 line_cells_id->SetNumberOfTuples (j);
1233 line_cells->SetCells (n_corr, line_cells_id);
1234 line_points->SetNumberOfPoints (j*2);
1235 line_tcoords->SetNumberOfTuples (j*2);
1238 line_data->SetPoints (line_points);
1239 line_data->SetLines (line_cells);
1240 line_data->GetPointData ()->SetTCoords (line_tcoords);
1241 line_data->GetCellData ()->SetScalars (line_colors);
1247 createActorFromVTKDataSet (line_data, actor);
1248 actor->GetProperty ()->SetRepresentationToWireframe ();
1249 actor->GetProperty ()->SetOpacity (0.5);
1250 addActorToRenderer (actor, viewport);
1253 (*shape_actor_map_)[id] = actor;
1261 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1268 template <
typename Po
intT>
bool
1274 const std::string &
id,
1277 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1281 template <
typename Po
intT>
bool
1282 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1285 const std::string &
id,
1287 const Eigen::Vector4f& sensor_origin,
1288 const Eigen::Quaternion<float>& sensor_orientation)
1292 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1298 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1305 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1309 bool has_colors =
false;
1311 if (
auto scalars = color_handler.
getColor ())
1313 polydata->GetPointData ()->SetScalars (scalars);
1314 scalars->GetRange (minmax);
1320 createActorFromVTKDataSet (polydata, actor);
1322 actor->GetMapper ()->SetScalarRange (minmax);
1325 addActorToRenderer (actor, viewport);
1328 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1329 cloud_actor.actor = actor;
1330 cloud_actor.cells = initcells;
1334 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1335 cloud_actor.viewpoint_transformation_ = transformation;
1336 cloud_actor.actor->SetUserMatrix (transformation);
1337 cloud_actor.actor->Modified ();
1343 template <
typename Po
intT>
bool
1344 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1345 const PointCloudGeometryHandler<PointT> &geometry_handler,
1346 const ColorHandlerConstPtr &color_handler,
1347 const std::string &
id,
1349 const Eigen::Vector4f& sensor_origin,
1350 const Eigen::Quaternion<float>& sensor_orientation)
1352 if (!geometry_handler.isCapable ())
1354 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.getName ().c_str ());
1358 if (!color_handler->isCapable ())
1360 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->getName ().c_str ());
1367 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1371 bool has_colors =
false;
1373 if (
auto scalars = color_handler->getColor ())
1375 polydata->GetPointData ()->SetScalars (scalars);
1376 scalars->GetRange (minmax);
1382 createActorFromVTKDataSet (polydata, actor);
1384 actor->GetMapper ()->SetScalarRange (minmax);
1387 addActorToRenderer (actor, viewport);
1390 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1391 cloud_actor.actor = actor;
1392 cloud_actor.cells = initcells;
1393 cloud_actor.color_handlers.push_back (color_handler);
1397 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1398 cloud_actor.viewpoint_transformation_ = transformation;
1399 cloud_actor.actor->SetUserMatrix (transformation);
1400 cloud_actor.actor->Modified ();
1406 template <
typename Po
intT>
bool
1407 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1408 const GeometryHandlerConstPtr &geometry_handler,
1409 const PointCloudColorHandler<PointT> &color_handler,
1410 const std::string &
id,
1412 const Eigen::Vector4f& sensor_origin,
1413 const Eigen::Quaternion<float>& sensor_orientation)
1415 if (!geometry_handler->isCapable ())
1417 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->getName ().c_str ());
1421 if (!color_handler.isCapable ())
1423 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.getName ().c_str ());
1430 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1434 bool has_colors =
false;
1436 if (
auto scalars = color_handler.getColor ())
1438 polydata->GetPointData ()->SetScalars (scalars);
1439 scalars->GetRange (minmax);
1445 createActorFromVTKDataSet (polydata, actor);
1447 actor->GetMapper ()->SetScalarRange (minmax);
1450 addActorToRenderer (actor, viewport);
1453 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1454 cloud_actor.actor = actor;
1455 cloud_actor.cells = initcells;
1456 cloud_actor.geometry_handlers.push_back (geometry_handler);
1460 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1461 cloud_actor.viewpoint_transformation_ = transformation;
1462 cloud_actor.actor->SetUserMatrix (transformation);
1463 cloud_actor.actor->Modified ();
1469 template <
typename Po
intT>
bool
1471 const std::string &
id)
1474 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1476 if (am_it == cloud_actor_map_->end ())
1481 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1485 polydata->GetPointData ()->SetScalars (scalars);
1487 minmax[0] = std::numeric_limits<double>::min ();
1488 minmax[1] = std::numeric_limits<double>::max ();
1489 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1490 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1492 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1495 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1500 template <
typename Po
intT>
bool
1503 const std::string &
id)
1506 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1508 if (am_it == cloud_actor_map_->end ())
1515 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1519 polydata->GetPointData ()->SetScalars (scalars);
1521 minmax[0] = std::numeric_limits<double>::min ();
1522 minmax[1] = std::numeric_limits<double>::max ();
1523 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1524 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1526 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1529 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1535 template <
typename Po
intT>
bool
1538 const std::string &
id)
1541 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1543 if (am_it == cloud_actor_map_->end ())
1553 vtkIdType nr_points = cloud->
points.size ();
1554 points->SetNumberOfPoints (nr_points);
1557 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1563 for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1564 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[pts]);
1569 for (vtkIdType i = 0; i < nr_points; ++i)
1574 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[pts]);
1579 points->SetNumberOfPoints (nr_points);
1583 updateCells (cells, am_it->second.cells, nr_points);
1586 vertices->SetCells (nr_points, cells);
1589 bool has_colors =
false;
1591 if (
auto scalars = color_handler.
getColor ())
1594 polydata->GetPointData ()->SetScalars (scalars);
1595 scalars->GetRange (minmax);
1599 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1600 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1604 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1607 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1612 template <
typename Po
intT>
bool
1615 const std::vector<pcl::Vertices> &vertices,
1616 const std::string &
id,
1619 if (vertices.empty () || cloud->
points.empty ())
1624 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1629 std::vector<pcl::PCLPointField> fields;
1631 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1633 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1637 colors->SetNumberOfComponents (3);
1638 colors->SetName (
"Colors");
1639 std::uint32_t offset = fields[rgb_idx].offset;
1640 for (std::size_t i = 0; i < cloud->
size (); ++i)
1644 const pcl::RGB*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&cloud->
points[i]) + offset);
1645 unsigned char color[3];
1646 color[0] = rgb_data->r;
1647 color[1] = rgb_data->g;
1648 color[2] = rgb_data->b;
1649 colors->InsertNextTupleValue (color);
1655 vtkIdType nr_points = cloud->
points.size ();
1656 points->SetNumberOfPoints (nr_points);
1660 float *data =
static_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1663 std::vector<int> lookup;
1667 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1668 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[ptr]);
1672 lookup.resize (nr_points);
1674 for (vtkIdType i = 0; i < nr_points; ++i)
1680 lookup[i] =
static_cast<int> (j);
1681 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[ptr]);
1686 points->SetNumberOfPoints (nr_points);
1690 int max_size_of_polygon = -1;
1691 for (
const auto &vertex : vertices)
1692 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1693 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1695 if (vertices.size () > 1)
1699 vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1701 if (!lookup.empty ())
1703 for (std::size_t i = 0; i < vertices.size (); ++i, ++idx)
1705 std::size_t n_points = vertices[i].vertices.size ();
1708 for (std::size_t j = 0; j < n_points; j++, ++idx)
1709 *cell++ = lookup[vertices[i].vertices[j]];
1715 for (std::size_t i = 0; i < vertices.size (); ++i, ++idx)
1717 std::size_t n_points = vertices[i].vertices.size ();
1720 for (std::size_t j = 0; j < n_points; j++, ++idx)
1721 *cell++ = vertices[i].vertices[j];
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->
points.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->
points[i].x, &cloud->
points[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->
points[i].x, &cloud->
points[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->
points[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 ());
1876 vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1878 if (!lookup.empty ())
1880 for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
1882 std::size_t n_points = verts[i].vertices.size ();
1884 for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
1885 *cell = lookup[verts[i].vertices[j]];
1890 for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
1892 std::size_t n_points = verts[i].vertices.size ();
1894 for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
1895 *cell = verts[i].vertices[j];
1898 cells->GetData ()->SetNumberOfValues (idx);
1901 polydata->SetPolys (cells);
1906 #ifdef vtkGenericDataArray_h
1907 #undef SetTupleValue
1908 #undef InsertNextTupleValue
1909 #undef GetTupleValue