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 (!pcl_isfinite (cloud->
points[i].x) ||
261 !pcl_isfinite (cloud->
points[i].y) ||
262 !pcl_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 #if VTK_MAJOR_VERSION < 6
330 all_data->AddInput (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
332 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
337 #if VTK_MAJOR_VERSION < 6
338 surface_filter->AddInput (vtkUnstructuredGrid::SafeDownCast (data));
340 surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
343 #if VTK_MAJOR_VERSION < 6
344 all_data->AddInput (poly_data);
346 all_data->AddInputData (poly_data);
351 createActorFromVTKDataSet (all_data->GetOutput (), actor);
352 actor->GetProperty ()->SetRepresentationToWireframe ();
353 actor->GetProperty ()->SetColor (r, g, b);
354 actor->GetMapper ()->ScalarVisibilityOff ();
355 removeActorFromRenderer (am_it->second, viewport);
356 addActorToRenderer (actor, viewport);
359 (*shape_actor_map_)[id] = actor;
365 createActorFromVTKDataSet (data, actor);
366 actor->GetProperty ()->SetRepresentationToWireframe ();
367 actor->GetProperty ()->SetColor (r, g, b);
368 actor->GetMapper ()->ScalarVisibilityOff ();
369 addActorToRenderer (actor, viewport);
372 (*shape_actor_map_)[id] = actor;
379 template <
typename Po
intT>
bool
382 double r,
double g,
double b,
const std::string &
id,
int viewport)
389 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
390 if (am_it != shape_actor_map_->end ())
395 #if VTK_MAJOR_VERSION < 6
396 all_data->AddInput (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
398 all_data->AddInputData (
reinterpret_cast<vtkPolyDataMapper*
> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
403 #if VTK_MAJOR_VERSION < 6
404 surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
406 surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
409 #if VTK_MAJOR_VERSION < 6
410 all_data->AddInput (poly_data);
412 all_data->AddInputData (poly_data);
417 createActorFromVTKDataSet (all_data->GetOutput (), actor);
418 actor->GetProperty ()->SetRepresentationToWireframe ();
419 actor->GetProperty ()->SetColor (r, g, b);
420 actor->GetMapper ()->ScalarVisibilityOn ();
421 actor->GetProperty ()->BackfaceCullingOff ();
422 removeActorFromRenderer (am_it->second, viewport);
423 addActorToRenderer (actor, viewport);
426 (*shape_actor_map_)[id] = actor;
432 createActorFromVTKDataSet (data, actor);
433 actor->GetProperty ()->SetRepresentationToWireframe ();
434 actor->GetProperty ()->SetColor (r, g, b);
435 actor->GetMapper ()->ScalarVisibilityOn ();
436 actor->GetProperty ()->BackfaceCullingOff ();
437 addActorToRenderer (actor, viewport);
440 (*shape_actor_map_)[id] = actor;
446 template <
typename Po
intT>
bool
449 const std::string &
id,
int viewport)
451 return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5,
id, viewport));
455 template <
typename P1,
typename P2>
bool
460 PCL_WARN (
"[addLine] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
468 createActorFromVTKDataSet (data, actor);
469 actor->GetProperty ()->SetRepresentationToWireframe ();
470 actor->GetProperty ()->SetColor (r, g, b);
471 actor->GetMapper ()->ScalarVisibilityOff ();
472 addActorToRenderer (actor, viewport);
475 (*shape_actor_map_)[id] = actor;
480 template <
typename P1,
typename P2>
bool
485 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
491 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
492 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
493 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
494 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
495 leader->SetArrowStyleToFilled ();
496 leader->AutoLabelOn ();
498 leader->GetProperty ()->SetColor (r, g, b);
499 addActorToRenderer (leader, viewport);
502 (*shape_actor_map_)[id] = leader;
507 template <
typename P1,
typename P2>
bool
512 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
518 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
519 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
520 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
521 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
522 leader->SetArrowStyleToFilled ();
523 leader->SetArrowPlacementToPoint1 ();
525 leader->AutoLabelOn ();
527 leader->AutoLabelOff ();
529 leader->GetProperty ()->SetColor (r, g, b);
530 addActorToRenderer (leader, viewport);
533 (*shape_actor_map_)[id] = leader;
537 template <
typename P1,
typename P2>
bool
539 double r_line,
double g_line,
double b_line,
540 double r_text,
double g_text,
double b_text,
541 const std::string &
id,
int viewport)
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->AutoLabelOn ();
558 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
560 leader->GetProperty ()->SetColor (r_line, g_line, b_line);
561 addActorToRenderer (leader, viewport);
564 (*shape_actor_map_)[id] = leader;
569 template <
typename P1,
typename P2>
bool
572 return (!addLine (pt1, pt2, 0.5, 0.5, 0.5,
id, viewport));
576 template <
typename Po
intT>
bool
581 PCL_WARN (
"[addSphere] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
586 data->SetRadius (radius);
587 data->SetCenter (
double (center.x), double (center.y), double (center.z));
588 data->SetPhiResolution (10);
589 data->SetThetaResolution (10);
590 data->LatLongTessellationOff ();
595 mapper->SetInputConnection (data->GetOutputPort ());
599 actor->SetMapper (mapper);
601 actor->GetProperty ()->SetRepresentationToSurface ();
602 actor->GetProperty ()->SetInterpolationToFlat ();
603 actor->GetProperty ()->SetColor (r, g, b);
604 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
605 actor->GetMapper ()->ImmediateModeRenderingOn ();
607 actor->GetMapper ()->StaticOn ();
608 actor->GetMapper ()->ScalarVisibilityOff ();
609 actor->GetMapper ()->Update ();
610 addActorToRenderer (actor, viewport);
613 (*shape_actor_map_)[id] = actor;
618 template <
typename Po
intT>
bool
621 return (addSphere (center, radius, 0.5, 0.5, 0.5,
id, viewport));
625 template<
typename Po
intT>
bool
635 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
636 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
639 #if VTK_MAJOR_VERSION < 6
640 vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
642 vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
644 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
648 src->SetCenter (
double (center.x), double (center.y), double (center.z));
649 src->SetRadius (radius);
651 actor->GetProperty ()->SetColor (r, g, b);
658 template <
typename Po
intT>
bool
660 const std::string &text,
666 const std::string &
id,
679 if (rens_->GetNumberOfItems () <= viewport)
681 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
688 rens_->InitTraversal ();
689 for (
size_t i = viewport; rens_->GetNextItem () != NULL; ++i)
691 const std::string uid = tid + std::string (i,
'*');
694 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! "
695 "Please choose a different id and retry.\n",
706 textSource->SetText (text.c_str());
707 textSource->Update ();
710 textMapper->SetInputConnection (textSource->GetOutputPort ());
713 rens_->InitTraversal ();
714 vtkRenderer* renderer;
716 while ((renderer = rens_->GetNextItem ()) != NULL)
719 if (viewport == 0 || viewport == i)
722 textActor->SetMapper (textMapper);
723 textActor->SetPosition (position.x, position.y, position.z);
724 textActor->SetScale (textScale);
725 textActor->GetProperty ()->SetColor (r, g, b);
726 textActor->SetCamera (renderer->GetActiveCamera ());
728 renderer->AddActor (textActor);
733 const std::string uid = tid + std::string (i,
'*');
734 (*shape_actor_map_)[uid] = textActor;
744 template <
typename Po
intT>
bool
746 const std::string &text,
748 double orientation[3],
753 const std::string &
id,
766 if (rens_->GetNumberOfItems () <= viewport)
768 PCL_ERROR (
"[addText3D] The viewport [%d] doesn't exist (id <%s>)! ",
775 rens_->InitTraversal ();
776 for (
size_t i = viewport; rens_->GetNextItem () != NULL; ++i)
778 const std::string uid = tid + std::string (i,
'*');
781 PCL_ERROR (
"[addText3D] The id <%s> already exists in viewport [%d]! "
782 "Please choose a different id and retry.\n",
793 textSource->SetText (text.c_str());
794 textSource->Update ();
797 textMapper->SetInputConnection (textSource->GetOutputPort ());
800 textActor->SetMapper (textMapper);
801 textActor->SetPosition (position.x, position.y, position.z);
802 textActor->SetScale (textScale);
803 textActor->GetProperty ()->SetColor (r, g, b);
804 textActor->SetOrientation (orientation);
807 rens_->InitTraversal ();
809 for ( vtkRenderer* renderer = rens_->GetNextItem ();
811 renderer = rens_->GetNextItem (), ++i)
813 if (viewport == 0 || viewport == i)
815 renderer->AddActor (textActor);
816 const std::string uid = tid + std::string (i,
'*');
817 (*shape_actor_map_)[uid] = textActor;
825 template <
typename Po
intNT>
bool
828 int level,
float scale,
const std::string &
id,
int viewport)
830 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale,
id, viewport));
834 template <
typename Po
intT,
typename Po
intNT>
bool
838 int level,
float scale,
839 const std::string &
id,
int viewport)
843 PCL_ERROR (
"[addPointCloudNormals] The number of points differs from the number of normals!\n");
847 if (normals->
empty ())
849 PCL_WARN (
"[addPointCloudNormals] An empty normal cloud is given! Nothing to display.\n");
855 PCL_WARN (
"[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
862 points->SetDataTypeToFloat ();
864 data->SetNumberOfComponents (3);
867 vtkIdType nr_normals = 0;
873 vtkIdType point_step =
static_cast<vtkIdType
> (sqrt (
double (level)));
874 nr_normals = (
static_cast<vtkIdType
> ((cloud->
width - 1)/ point_step) + 1) *
875 (
static_cast<vtkIdType
> ((cloud->
height - 1) / point_step) + 1);
876 pts =
new float[2 * nr_normals * 3];
878 vtkIdType cell_count = 0;
879 for (vtkIdType y = 0; y < normals->
height; y += point_step)
880 for (vtkIdType x = 0; x < normals->
width; x += point_step)
882 PointT p = (*cloud)(x, y);
883 p.x += (*normals)(x, y).normal[0] * scale;
884 p.y += (*normals)(x, y).normal[1] * scale;
885 p.z += (*normals)(x, y).normal[2] * scale;
887 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
888 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
889 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
890 pts[2 * cell_count * 3 + 3] = p.x;
891 pts[2 * cell_count * 3 + 4] = p.y;
892 pts[2 * cell_count * 3 + 5] = p.z;
894 lines->InsertNextCell (2);
895 lines->InsertCellPoint (2 * cell_count);
896 lines->InsertCellPoint (2 * cell_count + 1);
902 nr_normals = (cloud->
points.size () - 1) / level + 1 ;
903 pts =
new float[2 * nr_normals * 3];
905 for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
908 p.x += normals->
points[i].normal[0] * scale;
909 p.y += normals->
points[i].normal[1] * scale;
910 p.z += normals->
points[i].normal[2] * scale;
912 pts[2 * j * 3 + 0] = cloud->
points[i].x;
913 pts[2 * j * 3 + 1] = cloud->
points[i].y;
914 pts[2 * j * 3 + 2] = cloud->
points[i].z;
915 pts[2 * j * 3 + 3] = p.x;
916 pts[2 * j * 3 + 4] = p.y;
917 pts[2 * j * 3 + 5] = p.z;
919 lines->InsertNextCell (2);
920 lines->InsertCellPoint (2 * j);
921 lines->InsertCellPoint (2 * j + 1);
925 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
926 points->SetData (data);
929 polyData->SetPoints (points);
930 polyData->SetLines (lines);
933 #if VTK_MAJOR_VERSION < 6
934 mapper->SetInput (polyData);
936 mapper->SetInputData (polyData);
938 mapper->SetColorModeToMapScalars();
939 mapper->SetScalarModeToUsePointData();
943 actor->SetMapper (mapper);
948 actor->SetUserMatrix (transformation);
951 addActorToRenderer (actor, viewport);
954 (*cloud_actor_map_)[id].actor = actor;
959 template <
typename Po
intNT>
bool
963 int level,
float scale,
964 const std::string &
id,
int viewport)
966 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
970 template <
typename Po
intT,
typename Po
intNT>
bool
975 int level,
float scale,
976 const std::string &
id,
int viewport)
980 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
986 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
994 unsigned char green[3] = {0, 255, 0};
995 unsigned char blue[3] = {0, 0, 255};
999 line_1_colors->SetNumberOfComponents (3);
1000 line_1_colors->SetName (
"Colors");
1002 line_2_colors->SetNumberOfComponents (3);
1003 line_2_colors->SetName (
"Colors");
1006 for (
size_t i = 0; i < cloud->
points.size (); i+=level)
1009 p.x += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[0]) * scale;
1010 p.y += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[1]) * scale;
1011 p.z += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[2]) * scale;
1015 line_1->SetPoint2 (p.x, p.y, p.z);
1017 #if VTK_MAJOR_VERSION < 6
1018 polydata_1->AddInput (line_1->GetOutput ());
1020 polydata_1->AddInputData (line_1->GetOutput ());
1022 line_1_colors->InsertNextTupleValue (green);
1024 polydata_1->Update ();
1026 line_1_data->GetCellData ()->SetScalars (line_1_colors);
1029 for (
size_t i = 0; i < cloud->
points.size (); i += level)
1031 Eigen::Vector3f pc (pcs->
points[i].principal_curvature[0],
1032 pcs->
points[i].principal_curvature[1],
1033 pcs->
points[i].principal_curvature[2]);
1034 Eigen::Vector3f normal (normals->
points[i].normal[0],
1035 normals->
points[i].normal[1],
1036 normals->
points[i].normal[2]);
1037 Eigen::Vector3f pc_c = pc.cross (normal);
1040 p.x += (pcs->
points[i].pc2 * pc_c[0]) * scale;
1041 p.y += (pcs->
points[i].pc2 * pc_c[1]) * scale;
1042 p.z += (pcs->
points[i].pc2 * pc_c[2]) * scale;
1046 line_2->SetPoint2 (p.x, p.y, p.z);
1048 #if VTK_MAJOR_VERSION < 6
1049 polydata_2->AddInput (line_2->GetOutput ());
1051 polydata_2->AddInputData (line_2->GetOutput ());
1054 line_2_colors->InsertNextTupleValue (blue);
1056 polydata_2->Update ();
1058 line_2_data->GetCellData ()->SetScalars (line_2_colors);
1062 #if VTK_MAJOR_VERSION < 6
1063 alldata->AddInput (line_1_data);
1064 alldata->AddInput (line_2_data);
1066 alldata->AddInputData (line_1_data);
1067 alldata->AddInputData (line_2_data);
1072 createActorFromVTKDataSet (alldata->GetOutput (), actor);
1073 actor->GetMapper ()->SetScalarModeToUseCellData ();
1076 addActorToRenderer (actor, viewport);
1081 (*cloud_actor_map_)[id] = act;
1086 template <
typename Po
intT,
typename GradientT>
bool
1090 int level,
double scale,
1091 const std::string &
id,
int viewport)
1093 if (gradients->
points.size () != cloud->
points.size ())
1095 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
1100 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1107 points->SetDataTypeToFloat ();
1109 data->SetNumberOfComponents (3);
1111 vtkIdType nr_gradients = (cloud->
points.size () - 1) / level + 1 ;
1112 float* pts =
new float[2 * nr_gradients * 3];
1114 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
1117 p.x += gradients->
points[i].gradient[0] * scale;
1118 p.y += gradients->
points[i].gradient[1] * scale;
1119 p.z += gradients->
points[i].gradient[2] * scale;
1121 pts[2 * j * 3 + 0] = cloud->
points[i].x;
1122 pts[2 * j * 3 + 1] = cloud->
points[i].y;
1123 pts[2 * j * 3 + 2] = cloud->
points[i].z;
1124 pts[2 * j * 3 + 3] = p.x;
1125 pts[2 * j * 3 + 4] = p.y;
1126 pts[2 * j * 3 + 5] = p.z;
1128 lines->InsertNextCell(2);
1129 lines->InsertCellPoint(2*j);
1130 lines->InsertCellPoint(2*j+1);
1133 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1134 points->SetData (data);
1137 polyData->SetPoints(points);
1138 polyData->SetLines(lines);
1141 #if VTK_MAJOR_VERSION < 6
1142 mapper->SetInput (polyData);
1144 mapper->SetInputData (polyData);
1146 mapper->SetColorModeToMapScalars();
1147 mapper->SetScalarModeToUsePointData();
1151 actor->SetMapper (mapper);
1154 addActorToRenderer (actor, viewport);
1157 (*cloud_actor_map_)[id].actor = actor;
1162 template <
typename Po
intT>
bool
1166 const std::vector<int> &correspondences,
1167 const std::string &
id,
1171 corrs.resize (correspondences.size ());
1174 for (pcl::Correspondences::iterator corrs_it (corrs.begin ()); corrs_it != corrs.end (); ++corrs_it)
1176 corrs_it->index_query = index;
1177 corrs_it->index_match = correspondences[index];
1181 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1185 template <
typename Po
intT>
bool
1191 const std::string &
id,
1195 if (correspondences.empty ())
1197 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1202 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
1203 if (am_it != shape_actor_map_->end () && overwrite ==
false)
1205 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1207 }
else if (am_it == shape_actor_map_->end () && overwrite ==
true)
1212 int n_corr = int (correspondences.size () / nth);
1217 line_colors->SetNumberOfComponents (3);
1218 line_colors->SetName (
"Colors");
1219 line_colors->SetNumberOfTuples (n_corr);
1223 line_points->SetNumberOfPoints (2 * n_corr);
1226 line_cells_id->SetNumberOfComponents (3);
1227 line_cells_id->SetNumberOfTuples (n_corr);
1228 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1232 line_tcoords->SetNumberOfComponents (1);
1233 line_tcoords->SetNumberOfTuples (n_corr * 2);
1234 line_tcoords->SetName (
"Texture Coordinates");
1236 double tc[3] = {0.0, 0.0, 0.0};
1238 Eigen::Affine3f source_transformation;
1240 source_transformation.translation () = source_points->
sensor_origin_.head (3);
1241 Eigen::Affine3f target_transformation;
1243 target_transformation.translation () = target_points->
sensor_origin_.head (3);
1247 for (
size_t i = 0; i < correspondences.size (); i += nth, ++j)
1249 if (correspondences[i].index_match == -1)
1251 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1255 PointT p_src (source_points->
points[correspondences[i].index_query]);
1256 PointT p_tgt (target_points->
points[correspondences[i].index_match]);
1258 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1259 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1261 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1263 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1264 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1266 *line_cell_id++ = 2;
1267 *line_cell_id++ = id1;
1268 *line_cell_id++ = id2;
1270 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1271 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1274 rgb[0] = vtkMath::Random (32, 255);
1275 rgb[1] = vtkMath::Random (32, 255);
1276 rgb[2] = vtkMath::Random (32, 255);
1277 line_colors->InsertTuple (i, rgb);
1279 line_colors->SetNumberOfTuples (j);
1280 line_cells_id->SetNumberOfTuples (j);
1281 line_cells->SetCells (n_corr, line_cells_id);
1282 line_points->SetNumberOfPoints (j*2);
1283 line_tcoords->SetNumberOfTuples (j*2);
1286 line_data->SetPoints (line_points);
1287 line_data->SetLines (line_cells);
1288 line_data->GetPointData ()->SetTCoords (line_tcoords);
1289 line_data->GetCellData ()->SetScalars (line_colors);
1295 createActorFromVTKDataSet (line_data, actor);
1296 actor->GetProperty ()->SetRepresentationToWireframe ();
1297 actor->GetProperty ()->SetOpacity (0.5);
1298 addActorToRenderer (actor, viewport);
1301 (*shape_actor_map_)[id] = actor;
1309 #if VTK_MAJOR_VERSION < 6
1310 reinterpret_cast<vtkPolyDataMapper*
>(actor->GetMapper ())->SetInput (line_data);
1312 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1320 template <
typename Po
intT>
bool
1326 const std::string &
id,
1329 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1333 template <
typename Po
intT>
bool
1334 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1337 const std::string &
id,
1339 const Eigen::Vector4f& sensor_origin,
1340 const Eigen::Quaternion<float>& sensor_orientation)
1344 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1350 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1357 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1361 bool has_colors =
false;
1364 if (color_handler.
getColor (scalars))
1366 polydata->GetPointData ()->SetScalars (scalars);
1367 scalars->GetRange (minmax);
1373 createActorFromVTKDataSet (polydata, actor);
1375 actor->GetMapper ()->SetScalarRange (minmax);
1378 addActorToRenderer (actor, viewport);
1381 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1382 cloud_actor.actor = actor;
1383 cloud_actor.cells = initcells;
1387 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1388 cloud_actor.viewpoint_transformation_ = transformation;
1389 cloud_actor.actor->SetUserMatrix (transformation);
1390 cloud_actor.actor->Modified ();
1396 template <
typename Po
intT>
bool
1397 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1398 const PointCloudGeometryHandler<PointT> &geometry_handler,
1399 const ColorHandlerConstPtr &color_handler,
1400 const std::string &
id,
1402 const Eigen::Vector4f& sensor_origin,
1403 const Eigen::Quaternion<float>& sensor_orientation)
1405 if (!geometry_handler.isCapable ())
1407 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.getName ().c_str ());
1411 if (!color_handler->isCapable ())
1413 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->getName ().c_str ());
1420 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1424 bool has_colors =
false;
1427 if (color_handler->getColor (scalars))
1429 polydata->GetPointData ()->SetScalars (scalars);
1430 scalars->GetRange (minmax);
1436 createActorFromVTKDataSet (polydata, actor);
1438 actor->GetMapper ()->SetScalarRange (minmax);
1441 addActorToRenderer (actor, viewport);
1444 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1445 cloud_actor.actor = actor;
1446 cloud_actor.cells = initcells;
1447 cloud_actor.color_handlers.push_back (color_handler);
1451 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1452 cloud_actor.viewpoint_transformation_ = transformation;
1453 cloud_actor.actor->SetUserMatrix (transformation);
1454 cloud_actor.actor->Modified ();
1460 template <
typename Po
intT>
bool
1461 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1462 const GeometryHandlerConstPtr &geometry_handler,
1463 const PointCloudColorHandler<PointT> &color_handler,
1464 const std::string &
id,
1466 const Eigen::Vector4f& sensor_origin,
1467 const Eigen::Quaternion<float>& sensor_orientation)
1469 if (!geometry_handler->isCapable ())
1471 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->getName ().c_str ());
1475 if (!color_handler.isCapable ())
1477 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.getName ().c_str ());
1484 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1488 bool has_colors =
false;
1491 if (color_handler.getColor (scalars))
1493 polydata->GetPointData ()->SetScalars (scalars);
1494 scalars->GetRange (minmax);
1500 createActorFromVTKDataSet (polydata, actor);
1502 actor->GetMapper ()->SetScalarRange (minmax);
1505 addActorToRenderer (actor, viewport);
1508 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1509 cloud_actor.actor = actor;
1510 cloud_actor.cells = initcells;
1511 cloud_actor.geometry_handlers.push_back (geometry_handler);
1515 convertToVtkMatrix (sensor_origin, sensor_orientation, transformation);
1516 cloud_actor.viewpoint_transformation_ = transformation;
1517 cloud_actor.actor->SetUserMatrix (transformation);
1518 cloud_actor.actor->Modified ();
1524 template <
typename Po
intT>
bool
1526 const std::string &
id)
1529 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1531 if (am_it == cloud_actor_map_->end ())
1536 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1540 polydata->GetPointData ()->SetScalars (scalars);
1542 minmax[0] = std::numeric_limits<double>::min ();
1543 minmax[1] = std::numeric_limits<double>::max ();
1544 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1545 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1547 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1550 #if VTK_MAJOR_VERSION < 6
1551 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1553 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1559 template <
typename Po
intT>
bool
1562 const std::string &
id)
1565 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1567 if (am_it == cloud_actor_map_->end ())
1574 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1578 polydata->GetPointData ()->SetScalars (scalars);
1580 minmax[0] = std::numeric_limits<double>::min ();
1581 minmax[1] = std::numeric_limits<double>::max ();
1582 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1583 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1585 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1588 #if VTK_MAJOR_VERSION < 6
1589 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1591 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1598 template <
typename Po
intT>
bool
1601 const std::string &
id)
1604 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1606 if (am_it == cloud_actor_map_->end ())
1616 vtkIdType nr_points = cloud->
points.size ();
1617 points->SetNumberOfPoints (nr_points);
1620 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1626 for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1627 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[pts]);
1632 for (vtkIdType i = 0; i < nr_points; ++i)
1637 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[pts]);
1642 points->SetNumberOfPoints (nr_points);
1646 updateCells (cells, am_it->second.cells, nr_points);
1649 vertices->SetCells (nr_points, cells);
1652 bool has_colors =
false;
1655 if (color_handler.
getColor (scalars))
1658 polydata->GetPointData ()->SetScalars (scalars);
1659 scalars->GetRange (minmax);
1663 #if VTK_RENDERING_BACKEND_OPENGL_VERSION < 2
1664 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1668 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1671 #if VTK_MAJOR_VERSION < 6
1672 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1674 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1680 template <
typename Po
intT>
bool
1683 const std::vector<pcl::Vertices> &vertices,
1684 const std::string &
id,
1687 if (vertices.empty () || cloud->
points.empty ())
1692 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1697 std::vector<pcl::PCLPointField> fields;
1705 colors->SetNumberOfComponents (3);
1706 colors->SetName (
"Colors");
1707 uint32_t offset = fields[rgb_idx].offset;
1708 for (
size_t i = 0; i < cloud->
size (); ++i)
1712 const pcl::RGB*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&cloud->
points[i]) + offset);
1713 unsigned char color[3];
1714 color[0] = rgb_data->r;
1715 color[1] = rgb_data->g;
1716 color[2] = rgb_data->b;
1717 colors->InsertNextTupleValue (color);
1723 vtkIdType nr_points = cloud->
points.size ();
1724 points->SetNumberOfPoints (nr_points);
1728 float *data =
static_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1731 std::vector<int> lookup;
1735 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1736 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[ptr]);
1740 lookup.resize (nr_points);
1742 for (vtkIdType i = 0; i < nr_points; ++i)
1748 lookup[i] =
static_cast<int> (j);
1749 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[ptr]);
1754 points->SetNumberOfPoints (nr_points);
1758 int max_size_of_polygon = -1;
1759 for (
size_t i = 0; i < vertices.size (); ++i)
1760 if (max_size_of_polygon <
static_cast<int> (vertices[i].vertices.size ()))
1761 max_size_of_polygon =
static_cast<int> (vertices[i].vertices.size ());
1763 if (vertices.size () > 1)
1767 vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1769 if (lookup.size () > 0)
1771 for (
size_t i = 0; i < vertices.size (); ++i, ++idx)
1773 size_t n_points = vertices[i].vertices.size ();
1776 for (
size_t j = 0; j < n_points; j++, ++idx)
1777 *cell++ = lookup[vertices[i].vertices[j]];
1783 for (
size_t i = 0; i < vertices.size (); ++i, ++idx)
1785 size_t n_points = vertices[i].vertices.size ();
1788 for (
size_t j = 0; j < n_points; j++, ++idx)
1789 *cell++ = vertices[i].vertices[j];
1794 allocVtkPolyData (polydata);
1795 cell_array->GetData ()->SetNumberOfValues (idx);
1796 cell_array->Squeeze ();
1797 polydata->SetPolys (cell_array);
1798 polydata->SetPoints (points);
1801 polydata->GetPointData ()->SetScalars (colors);
1803 createActorFromVTKDataSet (polydata, actor,
false);
1808 size_t n_points = vertices[0].vertices.size ();
1809 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1811 if (lookup.size () > 0)
1813 for (
size_t j = 0; j < (n_points - 1); ++j)
1814 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1818 for (
size_t j = 0; j < (n_points - 1); ++j)
1819 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1823 poly_grid->Allocate (1, 1);
1824 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1825 poly_grid->SetPoints (points);
1827 poly_grid->GetPointData ()->SetScalars (colors);
1829 createActorFromVTKDataSet (poly_grid, actor,
false);
1831 addActorToRenderer (actor, viewport);
1832 actor->GetProperty ()->SetRepresentationToSurface ();
1834 actor->GetProperty ()->BackfaceCullingOff ();
1835 actor->GetProperty ()->SetInterpolationToFlat ();
1836 actor->GetProperty ()->EdgeVisibilityOff ();
1837 actor->GetProperty ()->ShadingOff ();
1840 (*cloud_actor_map_)[id].actor = actor;
1845 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1851 template <
typename Po
intT>
bool
1854 const std::vector<pcl::Vertices> &verts,
1855 const std::string &
id)
1864 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1865 if (am_it == cloud_actor_map_->end ())
1877 vtkIdType nr_points = cloud->
points.size ();
1878 points->SetNumberOfPoints (nr_points);
1881 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1884 std::vector<int> lookup;
1888 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1889 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[ptr]);
1893 lookup.resize (nr_points);
1895 for (vtkIdType i = 0; i < nr_points; ++i)
1901 lookup [i] =
static_cast<int> (j);
1902 std::copy (&cloud->
points[i].x, &cloud->
points[i].x + 3, &data[ptr]);
1907 points->SetNumberOfPoints (nr_points);
1911 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1915 std::vector<pcl::PCLPointField> fields;
1919 if (rgb_idx != -1 && colors)
1922 uint32_t offset = fields[rgb_idx].offset;
1923 for (
size_t i = 0; i < cloud->
size (); ++i)
1927 const pcl::RGB*
const rgb_data =
reinterpret_cast<const pcl::RGB*
>(
reinterpret_cast<const char*
> (&cloud->
points[i]) + offset);
1928 unsigned char color[3];
1929 color[0] = rgb_data->r;
1930 color[1] = rgb_data->g;
1931 color[2] = rgb_data->b;
1932 colors->SetTupleValue (j++, color);
1937 int max_size_of_polygon = -1;
1938 for (
size_t i = 0; i < verts.size (); ++i)
1939 if (max_size_of_polygon <
static_cast<int> (verts[i].vertices.size ()))
1940 max_size_of_polygon =
static_cast<int> (verts[i].vertices.size ());
1944 vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1946 if (lookup.size () > 0)
1948 for (
size_t i = 0; i < verts.size (); ++i, ++idx)
1950 size_t n_points = verts[i].vertices.size ();
1952 for (
size_t j = 0; j < n_points; j++, cell++, ++idx)
1953 *cell = lookup[verts[i].vertices[j]];
1958 for (
size_t i = 0; i < verts.size (); ++i, ++idx)
1960 size_t n_points = verts[i].vertices.size ();
1962 for (
size_t j = 0; j < n_points; j++, cell++, ++idx)
1963 *cell = verts[i].vertices[j];
1966 cells->GetData ()->SetNumberOfValues (idx);
1969 polydata->SetPolys (cells);
1974 #ifdef vtkGenericDataArray_h
1975 #undef SetTupleValue
1976 #undef InsertNextTupleValue
1977 #undef GetTupleValue