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->
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)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
257 for (vtkIdType i = 0; i < nr_points; ++i)
260 if (!std::isfinite ((*cloud)[i].x) ||
261 !std::isfinite ((*cloud)[i].y) ||
262 !std::isfinite ((*cloud)[i].z))
265 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
270 points->SetNumberOfPoints (nr_points);
274 updateCells (cells, initcells, nr_points);
277 vertices->SetCells (nr_points, cells);
281 template <
typename Po
intT>
void
282 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
290 allocVtkPolyData (polydata);
292 polydata->SetVerts (vertices);
298 polydata->SetPoints (points);
300 vtkIdType nr_points = points->GetNumberOfPoints ();
303 vertices = polydata->GetVerts ();
308 updateCells (cells, initcells, nr_points);
310 vertices->SetCells (nr_points, cells);
314 template <
typename Po
intT>
bool
317 double r,
double g,
double b,
const std::string &
id,
int viewport)
324 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
325 if (am_it != shape_actor_map_->end ())
330 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
334 surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
336 all_data->AddInputData (poly_data);
340 createActorFromVTKDataSet (all_data->GetOutput (), actor);
341 actor->GetProperty ()->SetRepresentationToWireframe ();
342 actor->GetProperty ()->SetColor (r, g, b);
343 actor->GetMapper ()->ScalarVisibilityOff ();
344 removeActorFromRenderer (am_it->second, viewport);
345 addActorToRenderer (actor, viewport);
348 (*shape_actor_map_)[id] = actor;
354 createActorFromVTKDataSet (data, actor);
355 actor->GetProperty ()->SetRepresentationToWireframe ();
356 actor->GetProperty ()->SetColor (r, g, b);
357 actor->GetMapper ()->ScalarVisibilityOff ();
358 addActorToRenderer (actor, viewport);
361 (*shape_actor_map_)[id] = actor;
368 template <
typename Po
intT>
bool
371 double r,
double g,
double b,
const std::string &
id,
int viewport)
378 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
379 if (am_it != shape_actor_map_->end ())
384 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
388 surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
390 all_data->AddInputData (poly_data);
394 createActorFromVTKDataSet (all_data->GetOutput (), actor);
395 actor->GetProperty ()->SetRepresentationToWireframe ();
396 actor->GetProperty ()->SetColor (r, g, b);
397 actor->GetMapper ()->ScalarVisibilityOn ();
398 actor->GetProperty ()->BackfaceCullingOff ();
399 removeActorFromRenderer (am_it->second, viewport);
400 addActorToRenderer (actor, viewport);
403 (*shape_actor_map_)[id] = actor;
409 createActorFromVTKDataSet (data, actor);
410 actor->GetProperty ()->SetRepresentationToWireframe ();
411 actor->GetProperty ()->SetColor (r, g, b);
412 actor->GetMapper ()->ScalarVisibilityOn ();
413 actor->GetProperty ()->BackfaceCullingOff ();
414 addActorToRenderer (actor, viewport);
417 (*shape_actor_map_)[id] = actor;
423 template <
typename Po
intT>
bool
426 const std::string &
id,
int viewport)
428 return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5,
id, viewport));
432 template <
typename P1,
typename P2>
bool
437 PCL_WARN (
"[addLine] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
445 createActorFromVTKDataSet (data, actor);
446 actor->GetProperty ()->SetRepresentationToWireframe ();
447 actor->GetProperty ()->SetColor (r, g, b);
448 actor->GetMapper ()->ScalarVisibilityOff ();
449 addActorToRenderer (actor, viewport);
452 (*shape_actor_map_)[id] = actor;
457 template <
typename P1,
typename P2>
bool
462 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
468 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
469 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
470 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
471 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
472 leader->SetArrowStyleToFilled ();
473 leader->AutoLabelOn ();
475 leader->GetProperty ()->SetColor (r, g, b);
476 addActorToRenderer (leader, viewport);
479 (*shape_actor_map_)[id] = leader;
484 template <
typename P1,
typename P2>
bool
489 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
495 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
496 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
497 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
498 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
499 leader->SetArrowStyleToFilled ();
500 leader->SetArrowPlacementToPoint1 ();
502 leader->AutoLabelOn ();
504 leader->AutoLabelOff ();
506 leader->GetProperty ()->SetColor (r, g, b);
507 addActorToRenderer (leader, viewport);
510 (*shape_actor_map_)[id] = leader;
514 template <
typename P1,
typename P2>
bool
516 double r_line,
double g_line,
double b_line,
517 double r_text,
double g_text,
double b_text,
518 const std::string &
id,
int viewport)
522 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
528 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
529 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
530 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
531 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
532 leader->SetArrowStyleToFilled ();
533 leader->AutoLabelOn ();
535 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
537 leader->GetProperty ()->SetColor (r_line, g_line, b_line);
538 addActorToRenderer (leader, viewport);
541 (*shape_actor_map_)[id] = leader;
546 template <
typename P1,
typename P2>
bool
549 return (!addLine (pt1, pt2, 0.5, 0.5, 0.5,
id, viewport));
553 template <
typename Po
intT>
bool
558 PCL_WARN (
"[addSphere] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
563 data->SetRadius (radius);
564 data->SetCenter (
double (center.x), double (center.y), double (center.z));
565 data->SetPhiResolution (10);
566 data->SetThetaResolution (10);
567 data->LatLongTessellationOff ();
572 mapper->SetInputConnection (data->GetOutputPort ());
576 actor->SetMapper (mapper);
578 actor->GetProperty ()->SetRepresentationToSurface ();
579 actor->GetProperty ()->SetInterpolationToFlat ();
580 actor->GetProperty ()->SetColor (r, g, b);
581 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
582 actor->GetMapper ()->ImmediateModeRenderingOn ();
584 actor->GetMapper ()->StaticOn ();
585 actor->GetMapper ()->ScalarVisibilityOff ();
586 actor->GetMapper ()->Update ();
587 addActorToRenderer (actor, viewport);
590 (*shape_actor_map_)[id] = actor;
595 template <
typename Po
intT>
bool
598 return (addSphere (center, radius, 0.5, 0.5, 0.5,
id, viewport));
602 template<
typename Po
intT>
bool
612 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
613 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
616 vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
617 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
621 src->SetCenter (
double (center.x), double (center.y), double (center.z));
622 src->SetRadius (radius);
624 actor->GetProperty ()->SetColor (r, g, b);
631 template <
typename Po
intT>
bool
633 const std::string &text,
639 const std::string &
id,
652 if (rens_->GetNumberOfItems () <= viewport)
654 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)! \n",
661 rens_->InitTraversal ();
662 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
664 const std::string uid = tid + std::string (i,
'*');
667 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! \n"
668 "Please choose a different id and retry.\n",
679 textSource->SetText (text.c_str());
680 textSource->Update ();
683 textMapper->SetInputConnection (textSource->GetOutputPort ());
686 rens_->InitTraversal ();
687 vtkRenderer* renderer;
689 while ((renderer = rens_->GetNextItem ()))
692 if (viewport == 0 || viewport == i)
695 textActor->SetMapper (textMapper);
696 textActor->SetPosition (position.x, position.y, position.z);
697 textActor->SetScale (textScale);
698 textActor->GetProperty ()->SetColor (r, g, b);
699 textActor->SetCamera (renderer->GetActiveCamera ());
701 renderer->AddActor (textActor);
706 const std::string uid = tid + std::string (i,
'*');
707 (*shape_actor_map_)[uid] = textActor;
717 template <
typename Po
intT>
bool
719 const std::string &text,
721 double orientation[3],
726 const std::string &
id,
739 if (rens_->GetNumberOfItems () <= viewport)
741 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)!\n",
748 rens_->InitTraversal ();
749 for (std::size_t i = viewport; rens_->GetNextItem (); ++i)
751 const std::string uid = tid + std::string (i,
'*');
754 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! "
755 "Please choose a different id and retry.\n",
766 textSource->SetText (text.c_str());
767 textSource->Update ();
770 textMapper->SetInputConnection (textSource->GetOutputPort ());
773 textActor->SetMapper (textMapper);
774 textActor->SetPosition (position.x, position.y, position.z);
775 textActor->SetScale (textScale);
776 textActor->GetProperty ()->SetColor (r, g, b);
777 textActor->SetOrientation (orientation);
780 rens_->InitTraversal ();
782 for ( vtkRenderer* renderer = rens_->GetNextItem ();
784 renderer = rens_->GetNextItem (), ++i)
786 if (viewport == 0 || viewport == i)
788 renderer->AddActor (textActor);
789 const std::string uid = tid + std::string (i,
'*');
790 (*shape_actor_map_)[uid] = textActor;
798 template <
typename Po
intNT>
bool
801 int level,
float scale,
const std::string &
id,
int viewport)
803 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale,
id, viewport));
807 template <
typename Po
intT,
typename Po
intNT>
bool
811 int level,
float scale,
812 const std::string &
id,
int viewport)
814 if (normals->
size () != cloud->
size ())
816 PCL_ERROR (
"[addPointCloudNormals] The number of points differs from the number of normals!\n");
820 if (normals->
empty ())
822 PCL_WARN (
"[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
828 PCL_WARN (
"[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
835 points->SetDataTypeToFloat ();
837 data->SetNumberOfComponents (3);
840 vtkIdType nr_normals = 0;
841 float* pts =
nullptr;
846 vtkIdType point_step =
static_cast<vtkIdType
> (sqrt (
double (level)));
847 nr_normals = (
static_cast<vtkIdType
> ((cloud->
width - 1)/ point_step) + 1) *
848 (
static_cast<vtkIdType
> ((cloud->
height - 1) / point_step) + 1);
849 pts =
new float[2 * nr_normals * 3];
851 vtkIdType cell_count = 0;
852 for (vtkIdType y = 0; y < normals->
height; y += point_step)
853 for (vtkIdType x = 0; x < normals->
width; x += point_step)
855 PointT p = (*cloud)(x, y);
856 p.x += (*normals)(x, y).normal[0] * scale;
857 p.y += (*normals)(x, y).normal[1] * scale;
858 p.z += (*normals)(x, y).normal[2] * scale;
860 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
861 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
862 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
863 pts[2 * cell_count * 3 + 3] = p.x;
864 pts[2 * cell_count * 3 + 4] = p.y;
865 pts[2 * cell_count * 3 + 5] = p.z;
867 lines->InsertNextCell (2);
868 lines->InsertCellPoint (2 * cell_count);
869 lines->InsertCellPoint (2 * cell_count + 1);
875 nr_normals = (cloud->
size () - 1) / level + 1 ;
876 pts =
new float[2 * nr_normals * 3];
878 for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
881 p.x += (*normals)[i].normal[0] * scale;
882 p.y += (*normals)[i].normal[1] * scale;
883 p.z += (*normals)[i].normal[2] * scale;
885 pts[2 * j * 3 + 0] = (*cloud)[i].x;
886 pts[2 * j * 3 + 1] = (*cloud)[i].y;
887 pts[2 * j * 3 + 2] = (*cloud)[i].z;
888 pts[2 * j * 3 + 3] = p.x;
889 pts[2 * j * 3 + 4] = p.y;
890 pts[2 * j * 3 + 5] = p.z;
892 lines->InsertNextCell (2);
893 lines->InsertCellPoint (2 * j);
894 lines->InsertCellPoint (2 * j + 1);
898 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
899 points->SetData (data);
902 polyData->SetPoints (points);
903 polyData->SetLines (lines);
906 mapper->SetInputData (polyData);
907 mapper->SetColorModeToMapScalars();
908 mapper->SetScalarModeToUsePointData();
912 actor->SetMapper (mapper);
917 actor->SetUserMatrix (transformation);
920 addActorToRenderer (actor, viewport);
923 (*cloud_actor_map_)[id].actor = actor;
928 template <
typename Po
intNT>
bool
932 int level,
float scale,
933 const std::string &
id,
int viewport)
935 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
939 template <
typename Po
intT,
typename Po
intNT>
bool
944 int level,
float scale,
945 const std::string &
id,
int viewport)
949 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
955 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
963 unsigned char green[3] = {0, 255, 0};
964 unsigned char blue[3] = {0, 0, 255};
968 line_1_colors->SetNumberOfComponents (3);
969 line_1_colors->SetName (
"Colors");
971 line_2_colors->SetNumberOfComponents (3);
972 line_2_colors->SetName (
"Colors");
975 for (std::size_t i = 0; i < cloud->
size (); i+=level)
978 p.x += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[0]) * scale;
979 p.y += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[1]) * scale;
980 p.z += ((*pcs)[i].pc1 * (*pcs)[i].principal_curvature[2]) * scale;
983 line_1->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
984 line_1->SetPoint2 (p.x, p.y, p.z);
986 polydata_1->AddInputData (line_1->GetOutput ());
987 line_1_colors->InsertNextTupleValue (green);
989 polydata_1->Update ();
991 line_1_data->GetCellData ()->SetScalars (line_1_colors);
994 for (std::size_t i = 0; i < cloud->
size (); i += level)
996 Eigen::Vector3f pc ((*pcs)[i].principal_curvature[0],
997 (*pcs)[i].principal_curvature[1],
998 (*pcs)[i].principal_curvature[2]);
999 Eigen::Vector3f normal ((*normals)[i].normal[0],
1000 (*normals)[i].normal[1],
1001 (*normals)[i].normal[2]);
1002 Eigen::Vector3f pc_c = pc.cross (normal);
1005 p.x += ((*pcs)[i].pc2 * pc_c[0]) * scale;
1006 p.y += ((*pcs)[i].pc2 * pc_c[1]) * scale;
1007 p.z += ((*pcs)[i].pc2 * pc_c[2]) * scale;
1010 line_2->SetPoint1 ((*cloud)[i].x, (*cloud)[i].y, (*cloud)[i].z);
1011 line_2->SetPoint2 (p.x, p.y, p.z);
1013 polydata_2->AddInputData (line_2->GetOutput ());
1015 line_2_colors->InsertNextTupleValue (blue);
1017 polydata_2->Update ();
1019 line_2_data->GetCellData ()->SetScalars (line_2_colors);
1023 alldata->AddInputData (line_1_data);
1024 alldata->AddInputData (line_2_data);
1028 createActorFromVTKDataSet (alldata->GetOutput (), actor);
1029 actor->GetMapper ()->SetScalarModeToUseCellData ();
1032 addActorToRenderer (actor, viewport);
1037 (*cloud_actor_map_)[id] = act;
1042 template <
typename Po
intT,
typename GradientT>
bool
1046 int level,
double scale,
1047 const std::string &
id,
int viewport)
1049 if (gradients->
size () != cloud->
size ())
1051 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1056 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1063 points->SetDataTypeToFloat ();
1065 data->SetNumberOfComponents (3);
1067 vtkIdType nr_gradients = (cloud->
size () - 1) / level + 1 ;
1068 float* pts =
new float[2 * nr_gradients * 3];
1070 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1073 p.x += (*gradients)[i].gradient[0] * scale;
1074 p.y += (*gradients)[i].gradient[1] * scale;
1075 p.z += (*gradients)[i].gradient[2] * scale;
1077 pts[2 * j * 3 + 0] = (*cloud)[i].x;
1078 pts[2 * j * 3 + 1] = (*cloud)[i].y;
1079 pts[2 * j * 3 + 2] = (*cloud)[i].z;
1080 pts[2 * j * 3 + 3] = p.x;
1081 pts[2 * j * 3 + 4] = p.y;
1082 pts[2 * j * 3 + 5] = p.z;
1084 lines->InsertNextCell(2);
1085 lines->InsertCellPoint(2*j);
1086 lines->InsertCellPoint(2*j+1);
1089 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1090 points->SetData (data);
1093 polyData->SetPoints(points);
1094 polyData->SetLines(lines);
1097 mapper->SetInputData (polyData);
1098 mapper->SetColorModeToMapScalars();
1099 mapper->SetScalarModeToUsePointData();
1103 actor->SetMapper (mapper);
1106 addActorToRenderer (actor, viewport);
1109 (*cloud_actor_map_)[id].actor = actor;
1114 template <
typename Po
intT>
bool
1118 const std::vector<int> &correspondences,
1119 const std::string &
id,
1123 corrs.resize (correspondences.size ());
1125 std::size_t index = 0;
1126 for (
auto &corr : corrs)
1128 corr.index_query = index;
1129 corr.index_match = correspondences[index];
1133 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1137 template <
typename Po
intT>
bool
1143 const std::string &
id,
1147 if (correspondences.empty ())
1149 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1154 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
1155 if (am_it != shape_actor_map_->end () && !overwrite)
1157 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1160 if (am_it == shape_actor_map_->end () && overwrite)
1165 int n_corr = int (correspondences.size () / nth);
1170 line_colors->SetNumberOfComponents (3);
1171 line_colors->SetName (
"Colors");
1172 line_colors->SetNumberOfTuples (n_corr);
1176 line_points->SetNumberOfPoints (2 * n_corr);
1179 line_cells_id->SetNumberOfComponents (3);
1180 line_cells_id->SetNumberOfTuples (n_corr);
1181 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1185 line_tcoords->SetNumberOfComponents (1);
1186 line_tcoords->SetNumberOfTuples (n_corr * 2);
1187 line_tcoords->SetName (
"Texture Coordinates");
1189 double tc[3] = {0.0, 0.0, 0.0};
1191 Eigen::Affine3f source_transformation;
1193 source_transformation.translation () = source_points->
sensor_origin_.head (3);
1194 Eigen::Affine3f target_transformation;
1196 target_transformation.translation () = target_points->
sensor_origin_.head (3);
1200 for (std::size_t i = 0; i < correspondences.size (); i += nth, ++j)
1202 if (correspondences[i].index_match == -1)
1204 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1208 PointT p_src ((*source_points)[correspondences[i].index_query]);
1209 PointT p_tgt ((*target_points)[correspondences[i].index_match]);
1211 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1212 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1214 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1216 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1217 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1219 *line_cell_id++ = 2;
1220 *line_cell_id++ = id1;
1221 *line_cell_id++ = id2;
1223 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1224 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1227 rgb[0] = vtkMath::Random (32, 255);
1228 rgb[1] = vtkMath::Random (32, 255);
1229 rgb[2] = vtkMath::Random (32, 255);
1230 line_colors->InsertTuple (i, rgb);
1232 line_colors->SetNumberOfTuples (j);
1233 line_cells_id->SetNumberOfTuples (j);
1234 line_cells->SetCells (n_corr, line_cells_id);
1235 line_points->SetNumberOfPoints (j*2);
1236 line_tcoords->SetNumberOfTuples (j*2);
1239 line_data->SetPoints (line_points);
1240 line_data->SetLines (line_cells);
1241 line_data->GetPointData ()->SetTCoords (line_tcoords);
1242 line_data->GetCellData ()->SetScalars (line_colors);
1248 createActorFromVTKDataSet (line_data, actor);
1249 actor->GetProperty ()->SetRepresentationToWireframe ();
1250 actor->GetProperty ()->SetOpacity (0.5);
1251 addActorToRenderer (actor, viewport);
1254 (*shape_actor_map_)[id] = actor;
1262 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1269 template <
typename Po
intT>
bool
1275 const std::string &
id,
1278 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1282 template <
typename Po
intT>
bool
1283 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1286 const std::string &
id,
1288 const Eigen::Vector4f& sensor_origin,
1289 const Eigen::Quaternion<float>& sensor_orientation)
1293 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1299 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1306 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1310 bool has_colors =
false;
1312 if (
auto scalars = color_handler.
getColor ())
1314 polydata->GetPointData ()->SetScalars (scalars);
1315 scalars->GetRange (minmax);
1321 createActorFromVTKDataSet (polydata, actor);
1323 actor->GetMapper ()->SetScalarRange (minmax);
1326 addActorToRenderer (actor, viewport);
1329 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1330 cloud_actor.actor = actor;
1331 cloud_actor.cells = initcells;
1335 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1336 cloud_actor.viewpoint_transformation_ = transformation;
1337 cloud_actor.actor->SetUserMatrix (transformation);
1338 cloud_actor.actor->Modified ();
1344 template <
typename Po
intT>
bool
1345 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1346 const PointCloudGeometryHandler<PointT> &geometry_handler,
1347 const ColorHandlerConstPtr &color_handler,
1348 const std::string &
id,
1350 const Eigen::Vector4f& sensor_origin,
1351 const Eigen::Quaternion<float>& sensor_orientation)
1353 if (!geometry_handler.isCapable ())
1355 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.getName ().c_str ());
1359 if (!color_handler->isCapable ())
1361 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->getName ().c_str ());
1368 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1372 bool has_colors =
false;
1374 if (
auto scalars = color_handler->getColor ())
1376 polydata->GetPointData ()->SetScalars (scalars);
1377 scalars->GetRange (minmax);
1383 createActorFromVTKDataSet (polydata, actor);
1385 actor->GetMapper ()->SetScalarRange (minmax);
1388 addActorToRenderer (actor, viewport);
1391 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1392 cloud_actor.actor = actor;
1393 cloud_actor.cells = initcells;
1394 cloud_actor.color_handlers.push_back (color_handler);
1398 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1399 cloud_actor.viewpoint_transformation_ = transformation;
1400 cloud_actor.actor->SetUserMatrix (transformation);
1401 cloud_actor.actor->Modified ();
1407 template <
typename Po
intT>
bool
1408 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1409 const GeometryHandlerConstPtr &geometry_handler,
1410 const PointCloudColorHandler<PointT> &color_handler,
1411 const std::string &
id,
1413 const Eigen::Vector4f& sensor_origin,
1414 const Eigen::Quaternion<float>& sensor_orientation)
1416 if (!geometry_handler->isCapable ())
1418 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->getName ().c_str ());
1422 if (!color_handler.isCapable ())
1424 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.getName ().c_str ());
1431 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1435 bool has_colors =
false;
1437 if (
auto scalars = color_handler.getColor ())
1439 polydata->GetPointData ()->SetScalars (scalars);
1440 scalars->GetRange (minmax);
1446 createActorFromVTKDataSet (polydata, actor);
1448 actor->GetMapper ()->SetScalarRange (minmax);
1451 addActorToRenderer (actor, viewport);
1454 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1455 cloud_actor.actor = actor;
1456 cloud_actor.cells = initcells;
1457 cloud_actor.geometry_handlers.push_back (geometry_handler);
1461 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1462 cloud_actor.viewpoint_transformation_ = transformation;
1463 cloud_actor.actor->SetUserMatrix (transformation);
1464 cloud_actor.actor->Modified ();
1470 template <
typename Po
intT>
bool
1472 const std::string &
id)
1475 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1477 if (am_it == cloud_actor_map_->end ())
1484 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1488 polydata->GetPointData ()->SetScalars (scalars);
1490 minmax[0] = std::numeric_limits<double>::min ();
1491 minmax[1] = std::numeric_limits<double>::max ();
1492 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1493 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1495 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1498 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1503 template <
typename Po
intT>
bool
1506 const std::string &
id)
1509 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1511 if (am_it == cloud_actor_map_->end ())
1518 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1522 polydata->GetPointData ()->SetScalars (scalars);
1524 minmax[0] = std::numeric_limits<double>::min ();
1525 minmax[1] = std::numeric_limits<double>::max ();
1526 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1527 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1529 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1532 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1538 template <
typename Po
intT>
bool
1541 const std::string &
id)
1544 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1546 if (am_it == cloud_actor_map_->end ())
1556 vtkIdType nr_points = cloud->
size ();
1557 points->SetNumberOfPoints (nr_points);
1560 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1566 for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1567 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[pts]);
1572 for (vtkIdType i = 0; i < nr_points; ++i)
1577 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[pts]);
1582 points->SetNumberOfPoints (nr_points);
1586 updateCells (cells, am_it->second.cells, nr_points);
1589 vertices->SetCells (nr_points, cells);
1591 vertices->SetNumberOfCells(nr_points);
1594 bool has_colors =
false;
1596 if (
auto scalars = color_handler.
getColor ())
1599 polydata->GetPointData ()->SetScalars (scalars);
1600 scalars->GetRange (minmax);
1604 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1605 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1609 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1612 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1617 template <
typename Po
intT>
bool
1620 const std::vector<pcl::Vertices> &vertices,
1621 const std::string &
id,
1624 if (vertices.empty () || cloud->
points.empty ())
1629 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1634 std::vector<pcl::PCLPointField> fields;
1636 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1638 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1642 colors->SetNumberOfComponents (3);
1643 colors->SetName (
"Colors");
1644 std::uint32_t offset = fields[rgb_idx].offset;
1645 for (std::size_t i = 0; i < cloud->
size (); ++i)
1649 const pcl::RGB*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1650 unsigned char color[3];
1651 color[0] = rgb_data->r;
1652 color[1] = rgb_data->g;
1653 color[2] = rgb_data->b;
1654 colors->InsertNextTupleValue (color);
1660 vtkIdType nr_points = cloud->
size ();
1661 points->SetNumberOfPoints (nr_points);
1665 float *data =
static_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1668 std::vector<int> lookup;
1672 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1673 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1677 lookup.resize (nr_points);
1679 for (vtkIdType i = 0; i < nr_points; ++i)
1685 lookup[i] =
static_cast<int> (j);
1686 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1691 points->SetNumberOfPoints (nr_points);
1695 int max_size_of_polygon = -1;
1696 for (
const auto &vertex : vertices)
1697 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1698 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1700 if (vertices.size () > 1)
1704 vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1706 if (!lookup.empty ())
1708 for (std::size_t i = 0; i < vertices.size (); ++i, ++idx)
1710 std::size_t n_points = vertices[i].vertices.size ();
1713 for (std::size_t j = 0; j < n_points; j++, ++idx)
1714 *cell++ = lookup[vertices[i].vertices[j]];
1720 for (std::size_t i = 0; i < vertices.size (); ++i, ++idx)
1722 std::size_t n_points = vertices[i].vertices.size ();
1725 for (std::size_t j = 0; j < n_points; j++, ++idx)
1726 *cell++ = vertices[i].vertices[j];
1731 allocVtkPolyData (polydata);
1732 cell_array->GetData ()->SetNumberOfValues (idx);
1733 cell_array->Squeeze ();
1734 polydata->SetPolys (cell_array);
1735 polydata->SetPoints (points);
1738 polydata->GetPointData ()->SetScalars (colors);
1740 createActorFromVTKDataSet (polydata, actor,
false);
1745 std::size_t n_points = vertices[0].vertices.size ();
1746 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1748 if (!lookup.empty ())
1750 for (std::size_t j = 0; j < (n_points - 1); ++j)
1751 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1755 for (std::size_t j = 0; j < (n_points - 1); ++j)
1756 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1760 poly_grid->Allocate (1, 1);
1761 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1762 poly_grid->SetPoints (points);
1764 poly_grid->GetPointData ()->SetScalars (colors);
1766 createActorFromVTKDataSet (poly_grid, actor,
false);
1768 addActorToRenderer (actor, viewport);
1769 actor->GetProperty ()->SetRepresentationToSurface ();
1771 actor->GetProperty ()->BackfaceCullingOff ();
1772 actor->GetProperty ()->SetInterpolationToFlat ();
1773 actor->GetProperty ()->EdgeVisibilityOff ();
1774 actor->GetProperty ()->ShadingOff ();
1777 (*cloud_actor_map_)[id].actor = actor;
1782 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1788 template <
typename Po
intT>
bool
1791 const std::vector<pcl::Vertices> &verts,
1792 const std::string &
id)
1801 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1802 if (am_it == cloud_actor_map_->end ())
1814 vtkIdType nr_points = cloud->
size ();
1815 points->SetNumberOfPoints (nr_points);
1818 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1821 std::vector<int> lookup;
1825 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1826 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1830 lookup.resize (nr_points);
1832 for (vtkIdType i = 0; i < nr_points; ++i)
1838 lookup [i] =
static_cast<int> (j);
1839 std::copy (&(*cloud)[i].x, &(*cloud)[i].x + 3, &data[ptr]);
1844 points->SetNumberOfPoints (nr_points);
1848 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1852 std::vector<pcl::PCLPointField> fields;
1853 rgb_idx = pcl::getFieldIndex<PointT> (
"rgb", fields);
1855 rgb_idx = pcl::getFieldIndex<PointT> (
"rgba", fields);
1856 if (rgb_idx != -1 && colors)
1859 std::uint32_t offset = fields[rgb_idx].offset;
1860 for (std::size_t i = 0; i < cloud->
size (); ++i)
1864 const pcl::RGB*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&(*cloud)[i]) + offset);
1865 unsigned char color[3];
1866 color[0] = rgb_data->r;
1867 color[1] = rgb_data->g;
1868 color[2] = rgb_data->b;
1869 colors->SetTupleValue (j++, color);
1874 int max_size_of_polygon = -1;
1875 for (
const auto &vertex : verts)
1876 if (max_size_of_polygon <
static_cast<int> (vertex.vertices.size ()))
1877 max_size_of_polygon =
static_cast<int> (vertex.vertices.size ());
1881 vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1883 if (!lookup.empty ())
1885 for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
1887 std::size_t n_points = verts[i].vertices.size ();
1889 for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
1890 *cell = lookup[verts[i].vertices[j]];
1895 for (std::size_t i = 0; i < verts.size (); ++i, ++idx)
1897 std::size_t n_points = verts[i].vertices.size ();
1899 for (std::size_t j = 0; j < n_points; j++, cell++, ++idx)
1900 *cell = verts[i].vertices[j];
1903 cells->GetData ()->SetNumberOfValues (idx);
1906 polydata->SetPolys (cells);
1911 #ifdef vtkGenericDataArray_h
1912 #undef SetTupleValue
1913 #undef InsertNextTupleValue
1914 #undef GetTupleValue