From e4e9ed876f2170409e74bd7dbca33dc3cca72413 Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Sat, 3 Aug 2013 15:40:36 +0200 Subject: [PATCH] spheres trajectory widget implementation --- modules/viz/include/opencv2/viz/widgets.hpp | 10 +- modules/viz/src/shape_widgets.cpp | 171 +++++++++++++++------------- 2 files changed, 98 insertions(+), 83 deletions(-) diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index 603d17d..c746330 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -182,14 +182,19 @@ namespace cv enum {DISPLAY_FRAMES = 1, DISPLAY_PATH = 2}; TrajectoryWidget(const std::vector &path, int display_mode = TrajectoryWidget::DISPLAY_PATH, const Color &color = Color::white(), double scale = 1.0); - TrajectoryWidget(const std::vector &path, float line_length, double init_sphere_radius, - double sphere_radius, const Color &line_color = Color::white(), const Color &sphere_color = Color::white()); TrajectoryWidget(const std::vector &path, const Matx33f &K, double scale = 1.0, const Color &color = Color::white()); // Camera frustums TrajectoryWidget(const std::vector &path, const Vec2f &fov, double scale = 1.0, const Color &color = Color::white()); // Camera frustums private: struct ApplyPath; }; + + class CV_EXPORTS SpheresTrajectoryWidget : public Widget3D + { + public: + SpheresTrajectoryWidget(const std::vector &path, float line_length = 0.05f, double init_sphere_radius = 0.021, + double sphere_radius = 0.007, const Color &line_color = Color::white(), const Color &sphere_color = Color::white()); + }; class CV_EXPORTS CloudWidget : public Widget3D { @@ -249,6 +254,7 @@ namespace cv template<> CV_EXPORTS Image3DWidget Widget::cast(); template<> CV_EXPORTS CameraPositionWidget Widget::cast(); template<> CV_EXPORTS TrajectoryWidget Widget::cast(); + template<> CV_EXPORTS SpheresTrajectoryWidget Widget::cast(); template<> CV_EXPORTS CloudWidget Widget::cast(); template<> CV_EXPORTS CloudCollectionWidget Widget::cast(); template<> CV_EXPORTS CloudNormalsWidget Widget::cast(); diff --git a/modules/viz/src/shape_widgets.cpp b/modules/viz/src/shape_widgets.cpp index caa5bf3..050b978 100644 --- a/modules/viz/src/shape_widgets.cpp +++ b/modules/viz/src/shape_widgets.cpp @@ -1181,87 +1181,6 @@ cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector &path, i WidgetAccessor::setProp(*this, actor); } -cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector &path, float line_length, double init_sphere_radius, - double sphere_radius, const Color &line_color, const Color &sphere_color) -{ - vtkSmartPointer appendFilter = vtkSmartPointer::New(); - vtkIdType nr_poses = path.size(); - - // Create color arrays - vtkSmartPointer line_scalars = vtkSmartPointer::New(); - line_scalars->SetNumberOfComponents(3); - line_scalars->InsertNextTuple3(line_color[2], line_color[1], line_color[0]); - - // Create color array for sphere - vtkSphereSource * dummy_sphere = vtkSphereSource::New(); - // Create the array for big sphere - dummy_sphere->SetRadius(init_sphere_radius); - dummy_sphere->Update(); - vtkIdType nr_points = dummy_sphere->GetOutput()->GetNumberOfCells(); - vtkSmartPointer sphere_scalars_init = vtkSmartPointer::New(); - sphere_scalars_init->SetNumberOfComponents(3); - sphere_scalars_init->SetNumberOfTuples(nr_points); - sphere_scalars_init->FillComponent(0, sphere_color[2]); - sphere_scalars_init->FillComponent(1, sphere_color[1]); - sphere_scalars_init->FillComponent(2, sphere_color[0]); - // Create the array for small sphere - dummy_sphere->SetRadius(sphere_radius); - dummy_sphere->Update(); - nr_points = dummy_sphere->GetOutput()->GetNumberOfCells(); - vtkSmartPointer sphere_scalars = vtkSmartPointer::New(); - sphere_scalars->SetNumberOfComponents(3); - sphere_scalars->SetNumberOfTuples(nr_points); - sphere_scalars->FillComponent(0, sphere_color[2]); - sphere_scalars->FillComponent(1, sphere_color[1]); - sphere_scalars->FillComponent(2, sphere_color[0]); - dummy_sphere->Delete(); - - for (vtkIdType i = 0; i < nr_poses; ++i) - { - Point3f new_pos = path[i].translation(); - - vtkSmartPointer sphere_source = vtkSmartPointer::New(); - sphere_source->SetCenter (new_pos.x, new_pos.y, new_pos.z); - if (i == 0) - { - sphere_source->SetRadius(init_sphere_radius); - sphere_source->Update(); - sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars_init); - appendFilter->AddInputConnection(sphere_source->GetOutputPort()); - continue; - } - else - { - sphere_source->SetRadius(sphere_radius); - sphere_source->Update(); - sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars); - appendFilter->AddInputConnection(sphere_source->GetOutputPort()); - } - - - Affine3f relativeAffine = path[i].inv() * path[i-1]; - Vec3f v = path[i].rotation() * relativeAffine.translation(); - v = normalize(v) * line_length; - - vtkSmartPointer line_source = vtkSmartPointer::New(); - line_source->SetPoint1(new_pos.x + v[0], new_pos.y + v[1], new_pos.z + v[2]); - line_source->SetPoint2(new_pos.x, new_pos.y, new_pos.z); - line_source->Update(); - line_source->GetOutput()->GetCellData()->SetScalars(line_scalars); - - appendFilter->AddInputConnection(line_source->GetOutputPort()); - } - - vtkSmartPointer mapper = vtkSmartPointer::New(); - mapper->SetScalarModeToUseCellData(); - mapper->SetInput(appendFilter->GetOutput()); - - vtkSmartPointer actor = vtkSmartPointer::New(); - actor->SetMapper(mapper); - - WidgetAccessor::setProp(*this, actor); -} - cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector &path, const Matx33f &K, double scale, const Color &color) { vtkSmartPointer camera = vtkSmartPointer::New(); @@ -1350,4 +1269,94 @@ template<> cv::viz::TrajectoryWidget cv::viz::Widget::castcast(); return static_cast(widget); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// spheres trajectory widget implementation + +cv::viz::SpheresTrajectoryWidget::SpheresTrajectoryWidget(const std::vector &path, float line_length, double init_sphere_radius, double sphere_radius, + const Color &line_color, const Color &sphere_color) +{ + vtkSmartPointer appendFilter = vtkSmartPointer::New(); + vtkIdType nr_poses = path.size(); + + // Create color arrays + vtkSmartPointer line_scalars = vtkSmartPointer::New(); + line_scalars->SetNumberOfComponents(3); + line_scalars->InsertNextTuple3(line_color[2], line_color[1], line_color[0]); + + // Create color array for sphere + vtkSphereSource * dummy_sphere = vtkSphereSource::New(); + // Create the array for big sphere + dummy_sphere->SetRadius(init_sphere_radius); + dummy_sphere->Update(); + vtkIdType nr_points = dummy_sphere->GetOutput()->GetNumberOfCells(); + vtkSmartPointer sphere_scalars_init = vtkSmartPointer::New(); + sphere_scalars_init->SetNumberOfComponents(3); + sphere_scalars_init->SetNumberOfTuples(nr_points); + sphere_scalars_init->FillComponent(0, sphere_color[2]); + sphere_scalars_init->FillComponent(1, sphere_color[1]); + sphere_scalars_init->FillComponent(2, sphere_color[0]); + // Create the array for small sphere + dummy_sphere->SetRadius(sphere_radius); + dummy_sphere->Update(); + nr_points = dummy_sphere->GetOutput()->GetNumberOfCells(); + vtkSmartPointer sphere_scalars = vtkSmartPointer::New(); + sphere_scalars->SetNumberOfComponents(3); + sphere_scalars->SetNumberOfTuples(nr_points); + sphere_scalars->FillComponent(0, sphere_color[2]); + sphere_scalars->FillComponent(1, sphere_color[1]); + sphere_scalars->FillComponent(2, sphere_color[0]); + dummy_sphere->Delete(); + + for (vtkIdType i = 0; i < nr_poses; ++i) + { + Point3f new_pos = path[i].translation(); + + vtkSmartPointer sphere_source = vtkSmartPointer::New(); + sphere_source->SetCenter (new_pos.x, new_pos.y, new_pos.z); + if (i == 0) + { + sphere_source->SetRadius(init_sphere_radius); + sphere_source->Update(); + sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars_init); + appendFilter->AddInputConnection(sphere_source->GetOutputPort()); + continue; + } + else + { + sphere_source->SetRadius(sphere_radius); + sphere_source->Update(); + sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars); + appendFilter->AddInputConnection(sphere_source->GetOutputPort()); + } + + + Affine3f relativeAffine = path[i].inv() * path[i-1]; + Vec3f v = path[i].rotation() * relativeAffine.translation(); + v = normalize(v) * line_length; + + vtkSmartPointer line_source = vtkSmartPointer::New(); + line_source->SetPoint1(new_pos.x + v[0], new_pos.y + v[1], new_pos.z + v[2]); + line_source->SetPoint2(new_pos.x, new_pos.y, new_pos.z); + line_source->Update(); + line_source->GetOutput()->GetCellData()->SetScalars(line_scalars); + + appendFilter->AddInputConnection(line_source->GetOutputPort()); + } + + vtkSmartPointer mapper = vtkSmartPointer::New(); + mapper->SetScalarModeToUseCellData(); + mapper->SetInput(appendFilter->GetOutput()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + WidgetAccessor::setProp(*this, actor); +} + +template<> cv::viz::SpheresTrajectoryWidget cv::viz::Widget::cast() +{ + Widget3D widget = this->cast(); + return static_cast(widget); } \ No newline at end of file -- 2.7.4