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 template <
typename Po
intT>
bool 71 const std::string &
id,
int viewport)
75 return (addPointCloud<PointT> (cloud, geometry_handler,
id, viewport));
79 template <
typename Po
intT>
bool 83 const std::string &
id,
int viewport)
87 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
91 if (pcl::traits::has_color<PointT>())
101 template <
typename Po
intT>
bool 105 const std::string &
id,
int viewport)
111 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
112 am_it->second.geometry_handlers.push_back (geometry_handler);
122 template <
typename Po
intT>
bool 126 const std::string &
id,
int viewport)
130 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
144 template <
typename Po
intT>
bool 148 const std::string &
id,
int viewport)
151 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
152 if (am_it != cloud_actor_map_->end ())
156 am_it->second.color_handlers.push_back (color_handler);
165 template <
typename Po
intT>
bool 170 const std::string &
id,
int viewport)
173 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
174 if (am_it != cloud_actor_map_->end ())
178 am_it->second.geometry_handlers.push_back (geometry_handler);
179 am_it->second.color_handlers.push_back (color_handler);
186 template <
typename Po
intT>
bool 191 const std::string &
id,
int viewport)
195 PCL_WARN (
"[addPointCloud] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
207 template <
typename Po
intT>
void 208 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
216 allocVtkPolyData (polydata);
218 polydata->SetVerts (vertices);
222 vertices = polydata->GetVerts ();
226 vtkIdType nr_points = cloud->
points.size ();
232 points->SetDataTypeToFloat ();
233 polydata->SetPoints (points);
235 points->SetNumberOfPoints (nr_points);
238 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
243 for (vtkIdType i = 0; i < nr_points; ++i)
244 memcpy (&data[i * 3], &cloud->
points[i].x, 12);
249 for (vtkIdType i = 0; i < nr_points; ++i)
252 if (!pcl_isfinite (cloud->
points[i].x) ||
253 !pcl_isfinite (cloud->
points[i].y) ||
254 !pcl_isfinite (cloud->
points[i].z))
257 memcpy (&data[j * 3], &cloud->
points[i].x, 12);
261 points->SetNumberOfPoints (nr_points);
265 updateCells (cells, initcells, nr_points);
268 vertices->SetCells (nr_points, cells);
272 template <
typename Po
intT>
void 273 pcl::visualization::PCLVisualizer::convertPointCloudToVTKPolyData (
281 allocVtkPolyData (polydata);
283 polydata->SetVerts (vertices);
289 polydata->SetPoints (points);
291 vtkIdType nr_points = points->GetNumberOfPoints ();
294 vertices = polydata->GetVerts ();
299 updateCells (cells, initcells, nr_points);
301 vertices->SetCells (nr_points, cells);
305 template <
typename Po
intT>
bool 308 double r,
double g,
double b,
const std::string &
id,
int viewport)
315 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
316 if (am_it != shape_actor_map_->end ())
321 #if VTK_MAJOR_VERSION < 6 322 all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
324 all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
329 #if VTK_MAJOR_VERSION < 6 330 surface_filter->AddInput (vtkUnstructuredGrid::SafeDownCast (data));
332 surface_filter->AddInputData (vtkUnstructuredGrid::SafeDownCast (data));
335 #if VTK_MAJOR_VERSION < 6 336 all_data->AddInput (poly_data);
338 all_data->AddInputData (poly_data);
343 createActorFromVTKDataSet (all_data->GetOutput (), actor);
344 actor->GetProperty ()->SetRepresentationToWireframe ();
345 actor->GetProperty ()->SetColor (r, g, b);
346 actor->GetMapper ()->ScalarVisibilityOff ();
347 removeActorFromRenderer (am_it->second, viewport);
348 addActorToRenderer (actor, viewport);
351 (*shape_actor_map_)[id] = actor;
357 createActorFromVTKDataSet (data, actor);
358 actor->GetProperty ()->SetRepresentationToWireframe ();
359 actor->GetProperty ()->SetColor (r, g, b);
360 actor->GetMapper ()->ScalarVisibilityOff ();
361 addActorToRenderer (actor, viewport);
364 (*shape_actor_map_)[id] = actor;
371 template <
typename Po
intT>
bool 374 double r,
double g,
double b,
const std::string &
id,
int viewport)
381 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
382 if (am_it != shape_actor_map_->end ())
387 #if VTK_MAJOR_VERSION < 6 388 all_data->AddInput (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
390 all_data->AddInputData (reinterpret_cast<vtkPolyDataMapper*> ((vtkActor::SafeDownCast (am_it->second))->GetMapper ())->GetInput ());
395 #if VTK_MAJOR_VERSION < 6 396 surface_filter->SetInput (vtkUnstructuredGrid::SafeDownCast (data));
398 surface_filter->SetInputData (vtkUnstructuredGrid::SafeDownCast (data));
401 #if VTK_MAJOR_VERSION < 6 402 all_data->AddInput (poly_data);
404 all_data->AddInputData (poly_data);
409 createActorFromVTKDataSet (all_data->GetOutput (), actor);
410 actor->GetProperty ()->SetRepresentationToWireframe ();
411 actor->GetProperty ()->SetColor (r, g, b);
412 actor->GetMapper ()->ScalarVisibilityOn ();
413 actor->GetProperty ()->BackfaceCullingOff ();
414 removeActorFromRenderer (am_it->second, viewport);
415 addActorToRenderer (actor, viewport);
418 (*shape_actor_map_)[id] = actor;
424 createActorFromVTKDataSet (data, actor);
425 actor->GetProperty ()->SetRepresentationToWireframe ();
426 actor->GetProperty ()->SetColor (r, g, b);
427 actor->GetMapper ()->ScalarVisibilityOn ();
428 actor->GetProperty ()->BackfaceCullingOff ();
429 addActorToRenderer (actor, viewport);
432 (*shape_actor_map_)[id] = actor;
438 template <
typename Po
intT>
bool 441 const std::string &
id,
int viewport)
443 return (!addPolygon<PointT> (cloud, 0.5, 0.5, 0.5,
id, viewport));
447 template <
typename P1,
typename P2>
bool 452 PCL_WARN (
"[addLine] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
460 createActorFromVTKDataSet (data, actor);
461 actor->GetProperty ()->SetRepresentationToWireframe ();
462 actor->GetProperty ()->SetColor (r, g, b);
463 actor->GetMapper ()->ScalarVisibilityOff ();
464 addActorToRenderer (actor, viewport);
467 (*shape_actor_map_)[id] = actor;
472 template <
typename P1,
typename P2>
bool 477 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
483 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
484 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
485 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
486 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
487 leader->SetArrowStyleToFilled ();
488 leader->AutoLabelOn ();
490 leader->GetProperty ()->SetColor (r, g, b);
491 addActorToRenderer (leader, viewport);
494 (*shape_actor_map_)[id] = leader;
499 template <
typename P1,
typename P2>
bool 504 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
510 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
511 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
512 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
513 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
514 leader->SetArrowStyleToFilled ();
515 leader->SetArrowPlacementToPoint1 ();
517 leader->AutoLabelOn ();
519 leader->AutoLabelOff ();
521 leader->GetProperty ()->SetColor (r, g, b);
522 addActorToRenderer (leader, viewport);
525 (*shape_actor_map_)[id] = leader;
529 template <
typename P1,
typename P2>
bool 531 double r_line,
double g_line,
double b_line,
532 double r_text,
double g_text,
double b_text,
533 const std::string &
id,
int viewport)
537 PCL_WARN (
"[addArrow] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
543 leader->GetPositionCoordinate ()->SetCoordinateSystemToWorld ();
544 leader->GetPositionCoordinate ()->SetValue (pt1.x, pt1.y, pt1.z);
545 leader->GetPosition2Coordinate ()->SetCoordinateSystemToWorld ();
546 leader->GetPosition2Coordinate ()->SetValue (pt2.x, pt2.y, pt2.z);
547 leader->SetArrowStyleToFilled ();
548 leader->AutoLabelOn ();
550 leader->GetLabelTextProperty()->SetColor(r_text, g_text, b_text);
552 leader->GetProperty ()->SetColor (r_line, g_line, b_line);
553 addActorToRenderer (leader, viewport);
556 (*shape_actor_map_)[id] = leader;
561 template <
typename P1,
typename P2>
bool 564 return (!
addLine (pt1, pt2, 0.5, 0.5, 0.5,
id, viewport));
568 template <
typename Po
intT>
bool 573 PCL_WARN (
"[addSphere] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
578 data->SetRadius (radius);
579 data->SetCenter (
double (center.x), double (center.y), double (center.z));
580 data->SetPhiResolution (10);
581 data->SetThetaResolution (10);
582 data->LatLongTessellationOff ();
587 mapper->SetInputConnection (data->GetOutputPort ());
591 actor->SetMapper (mapper);
593 actor->GetProperty ()->SetRepresentationToSurface ();
594 actor->GetProperty ()->SetInterpolationToFlat ();
595 actor->GetProperty ()->SetColor (r, g, b);
596 actor->GetMapper ()->ImmediateModeRenderingOn ();
597 actor->GetMapper ()->StaticOn ();
598 actor->GetMapper ()->ScalarVisibilityOff ();
599 actor->GetMapper ()->Update ();
600 addActorToRenderer (actor, viewport);
603 (*shape_actor_map_)[id] = actor;
608 template <
typename Po
intT>
bool 611 return (
addSphere (center, radius, 0.5, 0.5, 0.5,
id, viewport));
615 template<
typename Po
intT>
bool 625 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
626 vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
629 #if VTK_MAJOR_VERSION < 6 630 vtkAlgorithm *algo = actor->GetMapper ()->GetInput ()->GetProducerPort ()->GetProducer ();
632 vtkAlgorithm *algo = actor->GetMapper ()->GetInputAlgorithm ();
634 vtkSphereSource *src = vtkSphereSource::SafeDownCast (algo);
638 src->SetCenter (
double (center.x), double (center.y), double (center.z));
639 src->SetRadius (radius);
641 actor->GetProperty ()->SetColor (r, g, b);
648 template <
typename Po
intT>
bool 650 const std::string &text,
656 const std::string &
id,
667 PCL_WARN (
"[addText3D] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
672 textSource->SetText (text.c_str());
673 textSource->Update ();
676 textMapper->SetInputConnection (textSource->GetOutputPort ());
679 rens_->InitTraversal ();
680 vtkRenderer* renderer = NULL;
682 while ((renderer = rens_->GetNextItem ()) != NULL)
685 if (viewport == 0 || viewport == i)
688 textActor->SetMapper (textMapper);
689 textActor->SetPosition (position.x, position.y, position.z);
690 textActor->SetScale (textScale);
691 textActor->GetProperty ()->SetColor (r, g, b);
692 textActor->SetCamera (renderer->GetActiveCamera ());
694 renderer->AddActor (textActor);
699 std::string alternate_tid = tid;
700 alternate_tid.append(i,
'*');
702 (*shape_actor_map_)[(viewport == 0) ? tid : alternate_tid] = textActor;
712 template <
typename Po
intNT>
bool 715 int level,
float scale,
const std::string &
id,
int viewport)
717 return (addPointCloudNormals<PointNT, PointNT> (cloud, cloud, level, scale,
id, viewport));
721 template <
typename Po
intT,
typename Po
intNT>
bool 725 int level,
float scale,
726 const std::string &
id,
int viewport)
730 PCL_ERROR (
"[addPointCloudNormals] The number of points differs from the number of normals!\n");
735 PCL_WARN (
"[addPointCloudNormals] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
742 points->SetDataTypeToFloat ();
744 data->SetNumberOfComponents (3);
747 vtkIdType nr_normals = 0;
753 vtkIdType point_step =
static_cast<vtkIdType
> (sqrt (
double (level)));
754 nr_normals = (
static_cast<vtkIdType
> ((cloud->
width - 1)/ point_step) + 1) *
755 (static_cast<vtkIdType> ((cloud->
height - 1) / point_step) + 1);
756 pts =
new float[2 * nr_normals * 3];
758 vtkIdType cell_count = 0;
759 for (vtkIdType y = 0; y < normals->
height; y += point_step)
760 for (vtkIdType x = 0; x < normals->
width; x += point_step)
762 PointT p = (*cloud)(x, y);
763 p.x += (*normals)(x, y).normal[0] * scale;
764 p.y += (*normals)(x, y).normal[1] * scale;
765 p.z += (*normals)(x, y).normal[2] * scale;
767 pts[2 * cell_count * 3 + 0] = (*cloud)(x, y).x;
768 pts[2 * cell_count * 3 + 1] = (*cloud)(x, y).y;
769 pts[2 * cell_count * 3 + 2] = (*cloud)(x, y).z;
770 pts[2 * cell_count * 3 + 3] = p.x;
771 pts[2 * cell_count * 3 + 4] = p.y;
772 pts[2 * cell_count * 3 + 5] = p.z;
774 lines->InsertNextCell (2);
775 lines->InsertCellPoint (2 * cell_count);
776 lines->InsertCellPoint (2 * cell_count + 1);
782 nr_normals = (cloud->
points.size () - 1) / level + 1 ;
783 pts =
new float[2 * nr_normals * 3];
785 for (vtkIdType i = 0, j = 0; j < nr_normals; j++, i = j * level)
788 p.x += normals->
points[i].normal[0] * scale;
789 p.y += normals->
points[i].normal[1] * scale;
790 p.z += normals->
points[i].normal[2] * scale;
792 pts[2 * j * 3 + 0] = cloud->
points[i].x;
793 pts[2 * j * 3 + 1] = cloud->
points[i].y;
794 pts[2 * j * 3 + 2] = cloud->
points[i].z;
795 pts[2 * j * 3 + 3] = p.x;
796 pts[2 * j * 3 + 4] = p.y;
797 pts[2 * j * 3 + 5] = p.z;
799 lines->InsertNextCell (2);
800 lines->InsertCellPoint (2 * j);
801 lines->InsertCellPoint (2 * j + 1);
805 data->SetArray (&pts[0], 2 * nr_normals * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
806 points->SetData (data);
809 polyData->SetPoints (points);
810 polyData->SetLines (lines);
813 #if VTK_MAJOR_VERSION < 6 814 mapper->SetInput (polyData);
816 mapper->SetInputData (polyData);
818 mapper->SetColorModeToMapScalars();
819 mapper->SetScalarModeToUsePointData();
823 actor->SetMapper (mapper);
826 addActorToRenderer (actor, viewport);
829 (*cloud_actor_map_)[id].actor = actor;
834 template <
typename Po
intNT>
bool 838 int level,
float scale,
839 const std::string &
id,
int viewport)
841 return (addPointCloudPrincipalCurvatures<PointNT, PointNT> (cloud, cloud, pcs, level, scale,
id, viewport));
845 template <
typename Po
intT,
typename Po
intNT>
bool 850 int level,
float scale,
851 const std::string &
id,
int viewport)
855 pcl::console::print_error (
"[addPointCloudPrincipalCurvatures] The number of points differs from the number of principal curvatures/normals!\n");
861 PCL_WARN (
"[addPointCloudPrincipalCurvatures] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
869 unsigned char green[3] = {0, 255, 0};
870 unsigned char blue[3] = {0, 0, 255};
874 line_1_colors->SetNumberOfComponents (3);
875 line_1_colors->SetName (
"Colors");
877 line_2_colors->SetNumberOfComponents (3);
878 line_2_colors->SetName (
"Colors");
881 for (
size_t i = 0; i < cloud->
points.size (); i+=level)
884 p.x += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[0]) * scale;
885 p.y += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[1]) * scale;
886 p.z += (pcs->
points[i].pc1 * pcs->
points[i].principal_curvature[2]) * scale;
890 line_1->SetPoint2 (p.x, p.y, p.z);
892 #if VTK_MAJOR_VERSION < 6 893 polydata_1->AddInput (line_1->GetOutput ());
895 polydata_1->AddInputData (line_1->GetOutput ());
897 line_1_colors->InsertNextTupleValue (green);
899 polydata_1->Update ();
901 line_1_data->GetCellData ()->SetScalars (line_1_colors);
904 for (
size_t i = 0; i < cloud->
points.size (); i += level)
906 Eigen::Vector3f pc (pcs->
points[i].principal_curvature[0],
907 pcs->
points[i].principal_curvature[1],
908 pcs->
points[i].principal_curvature[2]);
909 Eigen::Vector3f normal (normals->
points[i].normal[0],
910 normals->
points[i].normal[1],
911 normals->
points[i].normal[2]);
912 Eigen::Vector3f pc_c = pc.cross (normal);
915 p.x += (pcs->
points[i].pc2 * pc_c[0]) * scale;
916 p.y += (pcs->
points[i].pc2 * pc_c[1]) * scale;
917 p.z += (pcs->
points[i].pc2 * pc_c[2]) * scale;
921 line_2->SetPoint2 (p.x, p.y, p.z);
923 #if VTK_MAJOR_VERSION < 6 924 polydata_2->AddInput (line_2->GetOutput ());
926 polydata_2->AddInputData (line_2->GetOutput ());
929 line_2_colors->InsertNextTupleValue (blue);
931 polydata_2->Update ();
933 line_2_data->GetCellData ()->SetScalars (line_2_colors);
937 #if VTK_MAJOR_VERSION < 6 938 alldata->AddInput (line_1_data);
939 alldata->AddInput (line_2_data);
941 alldata->AddInputData (line_1_data);
942 alldata->AddInputData (line_2_data);
947 createActorFromVTKDataSet (alldata->GetOutput (), actor);
948 actor->GetMapper ()->SetScalarModeToUseCellData ();
951 addActorToRenderer (actor, viewport);
956 (*cloud_actor_map_)[id] = act;
961 template <
typename Po
intT,
typename GradientT>
bool 965 int level,
double scale,
966 const std::string &
id,
int viewport)
970 PCL_ERROR (
"[addPointCloudGradients] The number of points differs from the number of gradients!\n");
975 PCL_WARN (
"[addPointCloudGradients] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
982 points->SetDataTypeToFloat ();
984 data->SetNumberOfComponents (3);
986 vtkIdType nr_gradients = (cloud->
points.size () - 1) / level + 1 ;
987 float* pts =
new float[2 * nr_gradients * 3];
989 for (vtkIdType i = 0, j = 0; j < nr_gradients; j++, i = j * level)
992 p.x += gradients->
points[i].gradient[0] * scale;
993 p.y += gradients->
points[i].gradient[1] * scale;
994 p.z += gradients->
points[i].gradient[2] * scale;
996 pts[2 * j * 3 + 0] = cloud->
points[i].x;
997 pts[2 * j * 3 + 1] = cloud->
points[i].y;
998 pts[2 * j * 3 + 2] = cloud->
points[i].z;
999 pts[2 * j * 3 + 3] = p.x;
1000 pts[2 * j * 3 + 4] = p.y;
1001 pts[2 * j * 3 + 5] = p.z;
1003 lines->InsertNextCell(2);
1004 lines->InsertCellPoint(2*j);
1005 lines->InsertCellPoint(2*j+1);
1008 data->SetArray (&pts[0], 2 * nr_gradients * 3, 0, vtkFloatArray::VTK_DATA_ARRAY_DELETE);
1009 points->SetData (data);
1012 polyData->SetPoints(points);
1013 polyData->SetLines(lines);
1016 #if VTK_MAJOR_VERSION < 6 1017 mapper->SetInput (polyData);
1019 mapper->SetInputData (polyData);
1021 mapper->SetColorModeToMapScalars();
1022 mapper->SetScalarModeToUsePointData();
1026 actor->SetMapper (mapper);
1029 addActorToRenderer (actor, viewport);
1032 (*cloud_actor_map_)[id].actor = actor;
1037 template <
typename Po
intT>
bool 1041 const std::vector<int> &correspondences,
1042 const std::string &
id,
1046 corrs.resize (correspondences.size ());
1049 for (pcl::Correspondences::iterator corrs_it (corrs.begin ()); corrs_it != corrs.end (); ++corrs_it)
1051 corrs_it->index_query = index;
1052 corrs_it->index_match = correspondences[index];
1056 return (addCorrespondences<PointT> (source_points, target_points, corrs,
id, viewport));
1060 template <
typename Po
intT>
bool 1066 const std::string &
id,
1070 if (correspondences.empty ())
1072 PCL_DEBUG (
"[addCorrespondences] An empty set of correspondences given! Nothing to display.\n");
1077 ShapeActorMap::iterator am_it = shape_actor_map_->find (
id);
1078 if (am_it != shape_actor_map_->end () && overwrite ==
false)
1080 PCL_WARN (
"[addCorrespondences] A set of correspondences with id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1082 }
else if (am_it == shape_actor_map_->end () && overwrite ==
true)
1087 int n_corr = int (correspondences.size () / nth);
1092 line_colors->SetNumberOfComponents (3);
1093 line_colors->SetName (
"Colors");
1094 line_colors->SetNumberOfTuples (n_corr);
1098 line_points->SetNumberOfPoints (2 * n_corr);
1101 line_cells_id->SetNumberOfComponents (3);
1102 line_cells_id->SetNumberOfTuples (n_corr);
1103 vtkIdType *line_cell_id = line_cells_id->GetPointer (0);
1107 line_tcoords->SetNumberOfComponents (1);
1108 line_tcoords->SetNumberOfTuples (n_corr * 2);
1109 line_tcoords->SetName (
"Texture Coordinates");
1111 double tc[3] = {0.0, 0.0, 0.0};
1113 Eigen::Affine3f source_transformation;
1115 source_transformation.translation () = source_points->
sensor_origin_.head (3);
1116 Eigen::Affine3f target_transformation;
1118 target_transformation.translation () = target_points->
sensor_origin_.head (3);
1122 for (
size_t i = 0; i < correspondences.size (); i += nth, ++j)
1124 if (correspondences[i].index_match == -1)
1126 PCL_WARN (
"[addCorrespondences] No valid index_match for correspondence %d\n", i);
1130 PointT p_src (source_points->
points[correspondences[i].index_query]);
1131 PointT p_tgt (target_points->
points[correspondences[i].index_match]);
1133 p_src.getVector3fMap () = source_transformation * p_src.getVector3fMap ();
1134 p_tgt.getVector3fMap () = target_transformation * p_tgt.getVector3fMap ();
1136 int id1 = j * 2 + 0, id2 = j * 2 + 1;
1138 line_points->SetPoint (id1, p_src.x, p_src.y, p_src.z);
1139 line_points->SetPoint (id2, p_tgt.x, p_tgt.y, p_tgt.z);
1141 *line_cell_id++ = 2;
1142 *line_cell_id++ = id1;
1143 *line_cell_id++ = id2;
1145 tc[0] = 0.; line_tcoords->SetTuple (id1, tc);
1146 tc[0] = 1.; line_tcoords->SetTuple (id2, tc);
1149 rgb[0] = vtkMath::Random (32, 255);
1150 rgb[1] = vtkMath::Random (32, 255);
1151 rgb[2] = vtkMath::Random (32, 255);
1152 line_colors->InsertTuple (i, rgb);
1154 line_colors->SetNumberOfTuples (j);
1155 line_cells_id->SetNumberOfTuples (j);
1156 line_cells->SetCells (n_corr, line_cells_id);
1157 line_points->SetNumberOfPoints (j*2);
1158 line_tcoords->SetNumberOfTuples (j*2);
1161 line_data->SetPoints (line_points);
1162 line_data->SetLines (line_cells);
1163 line_data->GetPointData ()->SetTCoords (line_tcoords);
1164 line_data->GetCellData ()->SetScalars (line_colors);
1170 createActorFromVTKDataSet (line_data, actor);
1171 actor->GetProperty ()->SetRepresentationToWireframe ();
1172 actor->GetProperty ()->SetOpacity (0.5);
1173 addActorToRenderer (actor, viewport);
1176 (*shape_actor_map_)[id] = actor;
1184 #if VTK_MAJOR_VERSION < 6 1185 reinterpret_cast<vtkPolyDataMapper*
>(actor->GetMapper ())->SetInput (line_data);
1187 reinterpret_cast<vtkPolyDataMapper*
> (actor->GetMapper ())->SetInputData (line_data);
1195 template <
typename Po
intT>
bool 1201 const std::string &
id,
1204 return (addCorrespondences<PointT> (source_points, target_points, correspondences, nth,
id, viewport,
true));
1208 template <
typename Po
intT>
bool 1209 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1212 const std::string &
id,
1214 const Eigen::Vector4f& sensor_origin,
1215 const Eigen::Quaternion<float>& sensor_orientation)
1219 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1225 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1232 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1236 bool has_colors =
false;
1239 if (color_handler.
getColor (scalars))
1241 polydata->GetPointData ()->SetScalars (scalars);
1242 scalars->GetRange (minmax);
1248 createActorFromVTKDataSet (polydata, actor);
1250 actor->GetMapper ()->SetScalarRange (minmax);
1253 addActorToRenderer (actor, viewport);
1256 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1257 cloud_actor.
actor = actor;
1258 cloud_actor.
cells = initcells;
1264 cloud_actor.
actor->SetUserMatrix (transformation);
1265 cloud_actor.
actor->Modified ();
1271 template <
typename Po
intT>
bool 1272 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1275 const std::string &
id,
1277 const Eigen::Vector4f& sensor_origin,
1278 const Eigen::Quaternion<float>& sensor_orientation)
1282 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler.
getName ().c_str ());
1288 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler->
getName ().c_str ());
1295 convertPointCloudToVTKPolyData<PointT> (geometry_handler, polydata, initcells);
1299 bool has_colors =
false;
1302 if (color_handler->
getColor (scalars))
1304 polydata->GetPointData ()->SetScalars (scalars);
1305 scalars->GetRange (minmax);
1311 createActorFromVTKDataSet (polydata, actor);
1313 actor->GetMapper ()->SetScalarRange (minmax);
1316 addActorToRenderer (actor, viewport);
1319 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1320 cloud_actor.
actor = actor;
1321 cloud_actor.
cells = initcells;
1328 cloud_actor.
actor->SetUserMatrix (transformation);
1329 cloud_actor.
actor->Modified ();
1335 template <
typename Po
intT>
bool 1336 pcl::visualization::PCLVisualizer::fromHandlersToScreen (
1339 const std::string &
id,
1341 const Eigen::Vector4f& sensor_origin,
1342 const Eigen::Quaternion<float>& sensor_orientation)
1346 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid geometry handler (%s)!\n",
id.c_str (), geometry_handler->
getName ().c_str ());
1352 PCL_WARN (
"[fromHandlersToScreen] PointCloud <%s> requested with an invalid color handler (%s)!\n",
id.c_str (), color_handler.
getName ().c_str ());
1359 convertPointCloudToVTKPolyData (geometry_handler, polydata, initcells);
1363 bool has_colors =
false;
1366 if (color_handler.
getColor (scalars))
1368 polydata->GetPointData ()->SetScalars (scalars);
1369 scalars->GetRange (minmax);
1375 createActorFromVTKDataSet (polydata, actor);
1377 actor->GetMapper ()->SetScalarRange (minmax);
1380 addActorToRenderer (actor, viewport);
1383 CloudActor& cloud_actor = (*cloud_actor_map_)[id];
1384 cloud_actor.
actor = actor;
1385 cloud_actor.
cells = initcells;
1392 cloud_actor.
actor->SetUserMatrix (transformation);
1393 cloud_actor.
actor->Modified ();
1399 template <
typename Po
intT>
bool 1401 const std::string &
id)
1404 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1406 if (am_it == cloud_actor_map_->end ())
1411 convertPointCloudToVTKPolyData<PointT> (cloud, polydata, am_it->second.cells);
1415 polydata->GetPointData ()->SetScalars (scalars);
1417 minmax[0] = std::numeric_limits<double>::min ();
1418 minmax[1] = std::numeric_limits<double>::max ();
1419 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1420 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1423 #if VTK_MAJOR_VERSION < 6 1424 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1426 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1432 template <
typename Po
intT>
bool 1435 const std::string &
id)
1438 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1440 if (am_it == cloud_actor_map_->end ())
1447 convertPointCloudToVTKPolyData (geometry_handler, polydata, am_it->second.cells);
1451 polydata->GetPointData ()->SetScalars (scalars);
1453 minmax[0] = std::numeric_limits<double>::min ();
1454 minmax[1] = std::numeric_limits<double>::max ();
1455 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1456 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1459 #if VTK_MAJOR_VERSION < 6 1460 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1462 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1469 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 ())
1487 vtkIdType nr_points = cloud->
points.size ();
1488 points->SetNumberOfPoints (nr_points);
1491 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1497 for (vtkIdType i = 0; i < nr_points; ++i, pts += 3)
1498 memcpy (&data[pts], &cloud->
points[i].x, 12);
1503 for (vtkIdType i = 0; i < nr_points; ++i)
1509 memcpy (&data[pts], &cloud->
points[i].x, 12);
1514 points->SetNumberOfPoints (nr_points);
1518 updateCells (cells, am_it->second.cells, nr_points);
1521 vertices->SetCells (nr_points, cells);
1527 scalars->GetRange (minmax);
1529 polydata->GetPointData ()->SetScalars (scalars);
1531 am_it->second.actor->GetMapper ()->ImmediateModeRenderingOff ();
1532 am_it->second.actor->GetMapper ()->SetScalarRange (minmax);
1535 #if VTK_MAJOR_VERSION < 6 1536 reinterpret_cast<vtkPolyDataMapper*
>(am_it->second.actor->GetMapper ())->SetInput (polydata);
1538 reinterpret_cast<vtkPolyDataMapper*
> (am_it->second.actor->GetMapper ())->SetInputData (polydata);
1544 template <
typename Po
intT>
bool 1547 const std::vector<pcl::Vertices> &vertices,
1548 const std::string &
id,
1551 if (vertices.empty () || cloud->
points.empty ())
1556 PCL_WARN (
"[addPolygonMesh] The id <%s> already exists! Please choose a different id and retry.\n",
id.c_str ());
1561 std::vector<pcl::PCLPointField> fields;
1569 colors->SetNumberOfComponents (3);
1570 colors->SetName (
"Colors");
1573 for (
size_t i = 0; i < cloud->
size (); ++i)
1577 memcpy (&rgb_data, reinterpret_cast<const char*> (&cloud->
points[i]) + fields[rgb_idx].offset, sizeof (
pcl::RGB));
1578 unsigned char color[3];
1579 color[0] = rgb_data.r;
1580 color[1] = rgb_data.g;
1581 color[2] = rgb_data.b;
1582 colors->InsertNextTupleValue (color);
1588 vtkIdType nr_points = cloud->
points.size ();
1589 points->SetNumberOfPoints (nr_points);
1593 float *data =
static_cast<vtkFloatArray*
> (points->GetData ())->GetPointer (0);
1596 std::vector<int> lookup;
1600 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1601 memcpy (&data[ptr], &cloud->
points[i].x, sizeof (
float) * 3);
1605 lookup.resize (nr_points);
1607 for (vtkIdType i = 0; i < nr_points; ++i)
1613 lookup[i] =
static_cast<int> (j);
1614 memcpy (&data[ptr], &cloud->
points[i].x, sizeof (
float) * 3);
1619 points->SetNumberOfPoints (nr_points);
1623 int max_size_of_polygon = -1;
1624 for (
size_t i = 0; i < vertices.size (); ++i)
1625 if (max_size_of_polygon < static_cast<int> (vertices[i].vertices.size ()))
1626 max_size_of_polygon =
static_cast<int> (vertices[i].vertices.size ());
1628 if (vertices.size () > 1)
1632 vtkIdType *cell = cell_array->WritePointer (vertices.size (), vertices.size () * (max_size_of_polygon + 1));
1634 if (lookup.size () > 0)
1636 for (
size_t i = 0; i < vertices.size (); ++i, ++idx)
1638 size_t n_points = vertices[i].vertices.size ();
1641 for (
size_t j = 0; j < n_points; j++, ++idx)
1642 *cell++ = lookup[vertices[i].vertices[j]];
1648 for (
size_t i = 0; i < vertices.size (); ++i, ++idx)
1650 size_t n_points = vertices[i].vertices.size ();
1653 for (
size_t j = 0; j < n_points; j++, ++idx)
1654 *cell++ = vertices[i].vertices[j];
1659 allocVtkPolyData (polydata);
1660 cell_array->GetData ()->SetNumberOfValues (idx);
1661 cell_array->Squeeze ();
1662 polydata->SetPolys (cell_array);
1663 polydata->SetPoints (points);
1666 polydata->GetPointData ()->SetScalars (colors);
1668 createActorFromVTKDataSet (polydata, actor,
false);
1673 size_t n_points = vertices[0].vertices.size ();
1674 polygon->GetPointIds ()->SetNumberOfIds (n_points - 1);
1676 if (lookup.size () > 0)
1678 for (
size_t j = 0; j < (n_points - 1); ++j)
1679 polygon->GetPointIds ()->SetId (j, lookup[vertices[0].vertices[j]]);
1683 for (
size_t j = 0; j < (n_points - 1); ++j)
1684 polygon->GetPointIds ()->SetId (j, vertices[0].vertices[j]);
1687 allocVtkUnstructuredGrid (poly_grid);
1688 poly_grid->Allocate (1, 1);
1689 poly_grid->InsertNextCell (polygon->GetCellType (), polygon->GetPointIds ());
1690 poly_grid->SetPoints (points);
1692 poly_grid->GetPointData ()->SetScalars (colors);
1694 createActorFromVTKDataSet (poly_grid, actor,
false);
1696 addActorToRenderer (actor, viewport);
1697 actor->GetProperty ()->SetRepresentationToSurface ();
1699 actor->GetProperty ()->BackfaceCullingOff ();
1700 actor->GetProperty ()->SetInterpolationToFlat ();
1701 actor->GetProperty ()->EdgeVisibilityOff ();
1702 actor->GetProperty ()->ShadingOff ();
1705 (*cloud_actor_map_)[id].actor = actor;
1710 (*cloud_actor_map_)[id].viewpoint_transformation_ = transformation;
1716 template <
typename Po
intT>
bool 1719 const std::vector<pcl::Vertices> &verts,
1720 const std::string &
id)
1729 CloudActorMap::iterator am_it = cloud_actor_map_->find (
id);
1730 if (am_it == cloud_actor_map_->end ())
1742 vtkIdType nr_points = cloud->
points.size ();
1743 points->SetNumberOfPoints (nr_points);
1746 float *data = (
static_cast<vtkFloatArray*
> (points->GetData ()))->GetPointer (0);
1749 std::vector<int> lookup;
1753 for (vtkIdType i = 0; i < nr_points; ++i, ptr += 3)
1754 memcpy (&data[ptr], &cloud->
points[i].x, sizeof (
float) * 3);
1758 lookup.resize (nr_points);
1760 for (vtkIdType i = 0; i < nr_points; ++i)
1766 lookup [i] =
static_cast<int> (j);
1767 memcpy (&data[ptr], &cloud->
points[i].x, sizeof (
float) * 3);
1772 points->SetNumberOfPoints (nr_points);
1776 vtkUnsignedCharArray* colors = vtkUnsignedCharArray::SafeDownCast (polydata->GetPointData ()->GetScalars ());
1780 std::vector<pcl::PCLPointField> fields;
1784 if (rgb_idx != -1 && colors)
1788 for (
size_t i = 0; i < cloud->
size (); ++i)
1793 reinterpret_cast<const char*> (&cloud->
points[i]) + fields[rgb_idx].offset,
1795 unsigned char color[3];
1796 color[0] = rgb_data.r;
1797 color[1] = rgb_data.g;
1798 color[2] = rgb_data.b;
1799 colors->SetTupleValue (j++, color);
1804 int max_size_of_polygon = -1;
1805 for (
size_t i = 0; i < verts.size (); ++i)
1806 if (max_size_of_polygon < static_cast<int> (verts[i].vertices.size ()))
1807 max_size_of_polygon =
static_cast<int> (verts[i].vertices.size ());
1811 vtkIdType *cell = cells->WritePointer (verts.size (), verts.size () * (max_size_of_polygon + 1));
1813 if (lookup.size () > 0)
1815 for (
size_t i = 0; i < verts.size (); ++i, ++idx)
1817 size_t n_points = verts[i].vertices.size ();
1819 for (
size_t j = 0; j < n_points; j++, cell++, ++idx)
1820 *cell = lookup[verts[i].vertices[j]];
1825 for (
size_t i = 0; i < verts.size (); ++i, ++idx)
1827 size_t n_points = verts[i].vertices.size ();
1829 for (
size_t j = 0; j < n_points; j++, cell++, ++idx)
1830 *cell = verts[i].vertices[j];
1833 cells->GetData ()->SetNumberOfValues (idx);
1836 polydata->SetPolys (cells);
XYZ handler class for PointCloud geometry.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
int getFieldIndex(const pcl::PCLPointCloud2 &cloud, const std::string &field_name)
Get the index of a specified field (i.e., dimension/channel)
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
ColorHandler::ConstPtr ColorHandlerConstPtr
bool addPointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud", int viewport=0)
Add a Point Cloud (templated) to screen.
virtual void getGeometry(vtkSmartPointer< vtkPoints > &points) const =0
Obtain the actual point geometry for the input dataset in VTK format.
static void convertToVtkMatrix(const Eigen::Matrix4f &m, vtkSmartPointer< vtkMatrix4x4 > &vtk_matrix)
Convert Eigen::Matrix4f to vtkMatrix4x4.
std::vector< ColorHandlerConstPtr > color_handlers
A vector of color handlers that can be used for rendering the data.
bool addPointCloudIntensityGradients(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const typename pcl::PointCloud< GradientT >::ConstPtr &gradients, int level=100, double scale=1e-6, const std::string &id="cloud", int viewport=0)
Add the estimated surface intensity gradients of a Point Cloud to screen.
bool addPolygon(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, double r, double g, double b, const std::string &id="polygon", int viewport=0)
Add a polygon (polyline) that represents the input point cloud (connects all points in order) ...
bool addPointCloudPrincipalCurvatures(const typename pcl::PointCloud< PointNT >::ConstPtr &cloud, const typename pcl::PointCloud< pcl::PrincipalCurvatures >::ConstPtr &pcs, int level=100, float scale=1.0f, const std::string &id="cloud", int viewport=0)
Add the estimated principal curvatures of a Point Cloud to screen.
Base Handler class for PointCloud colors.
uint32_t height
The point cloud height (if organized as an image-structure).
bool addSphere(const PointT ¢er, double radius, const std::string &id="sphere", int viewport=0)
Add a sphere shape from a point and a radius.
bool isOrganized() const
Return whether a dataset is organized (e.g., arranged in a structured grid).
bool isCapable() const
Checl if this handler is capable of handling the input data or not.
bool addCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const std::vector< int > &correspondences, const std::string &id="correspondences", int viewport=0)
Add the specified correspondences to the display.
Eigen::Vector4f sensor_origin_
Sensor acquisition pose (origin/translation).
A structure representing RGB color information.
uint32_t width
The point cloud width (if organized as an image-structure).
bool updateCorrespondences(const typename pcl::PointCloud< PointT >::ConstPtr &source_points, const typename pcl::PointCloud< PointT >::ConstPtr &target_points, const pcl::Correspondences &correspondences, int nth, const std::string &id="correspondences", int viewport=0)
Update the specified correspondences to the display.
vtkSmartPointer< vtkIdTypeArray > cells
Internal cell array.
Define methods or creating 3D shapes from parametric models.
Handler for predefined user colors.
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
RGB handler class for colors.
bool addPolygonMesh(const pcl::PolygonMesh &polymesh, const std::string &id="polygon", int viewport=0)
Add a PolygonMesh object to screen.
Eigen::Quaternionf sensor_orientation_
Sensor acquisition pose (rotation).
vtkSmartPointer< vtkLODActor > actor
The actor holding the data to render.
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
bool contains(const std::string &id) const
Check if the cloud, shape, or coordinate with the given id was already added to this vizualizer...
bool addArrow(const P1 &pt1, const P2 &pt2, double r, double g, double b, const std::string &id="arrow", int viewport=0)
Add a line arrow segment between two points, and display the distance between them.
bool updatePointCloud(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::string &id="cloud")
Updates the XYZ data for an existing cloud object id on screen.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
bool isCapable() const
Check if this handler is capable of handling the input data or not.
Base handler class for PointCloud geometry.
std::vector< GeometryHandlerConstPtr > geometry_handlers
A vector of geometry handlers that can be used for rendering the data.
virtual std::string getName() const =0
Abstract getName method.
GeometryHandler::ConstPtr GeometryHandlerConstPtr
bool addLine(const P1 &pt1, const P2 &pt2, const std::string &id="line", int viewport=0)
Add a line segment from two points.
bool addText3D(const std::string &text, const PointT &position, double textScale=1.0, double r=1.0, double g=1.0, double b=1.0, const std::string &id="", int viewport=0)
Add a 3d text to the scene.
A point structure representing Euclidean xyz coordinates, and the RGB color.
virtual std::string getName() const =0
Abstract getName method.
bool updateSphere(const PointT ¢er, double radius, double r, double g, double b, const std::string &id="sphere")
Update an existing sphere shape from a point and a radius.
vtkSmartPointer< vtkMatrix4x4 > viewpoint_transformation_
The viewpoint transformation matrix.
bool addPointCloudNormals(const typename pcl::PointCloud< PointNT >::ConstPtr &cloud, int level=100, float scale=0.02f, const std::string &id="cloud", int viewport=0)
Add the estimated surface normals of a Point Cloud to screen.
virtual bool getColor(vtkSmartPointer< vtkDataArray > &scalars) const =0
Obtain the actual color for the input dataset as vtk scalars.
PCL_EXPORTS vtkSmartPointer< vtkDataSet > createLine(const Eigen::Vector4f &pt1, const Eigen::Vector4f &pt2)
Create a line shape from two points.
bool updatePolygonMesh(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, const std::vector< pcl::Vertices > &vertices, const std::string &id="polygon")
Update a PolygonMesh object on screen.
PCL_EXPORTS void print_error(const char *format,...)
Print an error message on stream with colors.