From a3b1f29d23de60b4eaec656489d69e02e510ad33 Mon Sep 17 00:00:00 2001
From: Anatoly Baksheev <no@email>
Date: Wed, 8 Jan 2014 16:25:14 +0400
Subject: [PATCH] refactored spheres trajectory
---
modules/viz/doc/widget.rst | 18 ++--
modules/viz/include/opencv2/viz/widgets.hpp | 4 +-
modules/viz/src/shapes.cpp | 91 +++++++++------------
3 files changed, 49 insertions(+), 64 deletions(-)
diff --git a/modules/viz/doc/widget.rst b/modules/viz/doc/widget.rst
index 4119df083..55dca7679 100644
--- a/modules/viz/doc/widget.rst
+++ b/modules/viz/doc/widget.rst
@@ -799,23 +799,21 @@ represent the direction from previous position to the current. ::
class CV_EXPORTS WTrajectorySpheres : public Widget3D
{
public:
- WTrajectorySpheres(const std::vector<Affine3d> &path, double line_length = 0.05f,
- double init_sphere_radius = 0.021, sphere_radius = 0.007,
- Color &line_color = Color::white(), const Color &sphere_color = Color::white());
+ WTrajectorySpheres(InputArray path, double line_length = 0.05, double radius = 0.007,
+ const Color &from = Color::red(), const Color &to = Color::white());
};
viz::WTrajectorySpheres::WTrajectorySpheres
-------------------------------------------
Constructs a WTrajectorySpheres.
-.. ocv:function:: WTrajectorySpheres(const std::vector<Affine3d> &path, double 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())
+.. ocv:function:: WTrajectorySpheres(InputArray path, double line_length = 0.05, double radius = 0.007, const Color &from = Color::red(), const Color &to = Color::white());
- :param path: List of poses on a trajectory.
- :param line_length: Length of the lines.
- :param init_sphere_radius: Radius of the first sphere which represents the initial position of the camera.
- :param sphere_radius: Radius of the rest of the spheres.
- :param line_color: :ocv:class:`Color` of the lines.
- :param sphere_color: :ocv:class:`Color` of the spheres.
+ :param path: List of poses on a trajectory. Takes std::vector<Affine<T>> with T == [float | double]
+ :param line_length: Max length of the lines which point to previous position
+ :param sphere_radius: Radius of the spheres.
+ :param from: :ocv:class:`Color` for first sphere.
+ :param to: :ocv:class:`Color` for last sphere. Intermediate spheres will have interpolated color.
viz::WCloud
-----------
diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp
index ad75199f8..32665e74a 100644
--- a/modules/viz/include/opencv2/viz/widgets.hpp
+++ b/modules/viz/include/opencv2/viz/widgets.hpp
@@ -279,8 +279,8 @@ namespace cv
class CV_EXPORTS WTrajectorySpheres: public Widget3D
{
public:
- WTrajectorySpheres(const std::vector<Affine3d> &path, double line_length = 0.05, double init_sphere_radius = 0.021,
- double sphere_radius = 0.007, const Color &line_color = Color::white(), const Color &sphere_color = Color::white());
+ WTrajectorySpheres(InputArray path, double line_length = 0.05, double radius = 0.007,
+ const Color &from = Color::red(), const Color &to = Color::white());
};
/////////////////////////////////////////////////////////////////////////////
diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp
index bcd6870df..347a58b6e 100644
--- a/modules/viz/src/shapes.cpp
+++ b/modules/viz/src/shapes.cpp
@@ -1155,71 +1155,58 @@ template<> cv::viz::WTrajectoryFrustums cv::viz::Widget::cast<cv::viz::WTrajecto
///////////////////////////////////////////////////////////////////////////////////////////////
/// WTrajectorySpheres widget implementation
-cv::viz::WTrajectorySpheres::WTrajectorySpheres(const std::vector<Affine3d> &path, double line_length, double init_sphere_radius, double sphere_radius,
- const Color &line_color, const Color &sphere_color)
+cv::viz::WTrajectorySpheres::WTrajectorySpheres(InputArray _path, double line_length, double radius, const Color &from, const Color &to)
{
- vtkSmartPointer<vtkAppendPolyData> appendFilter = vtkSmartPointer<vtkAppendPolyData>::New();
- vtkIdType nr_poses = path.size();
+ CV_Assert(_path.kind() == _InputArray::STD_VECTOR || _path.kind() == _InputArray::MAT);
+ CV_Assert(_path.type() == CV_32FC(16) || _path.type() == CV_64FC(16));
- // Create color arrays
- vtkSmartPointer<vtkUnsignedCharArray> line_scalars = vtkSmartPointer<vtkUnsignedCharArray>::New();
- line_scalars->SetNumberOfComponents(3);
- line_scalars->InsertNextTuple3(line_color[2], line_color[1], line_color[0]);
+ Mat path64;
+ _path.getMat().convertTo(path64, CV_64F);
+ Affine3d *traj = path64.ptr<Affine3d>();
+ size_t total = path64.total();
- // Create color array for sphere
- vtkSmartPointer<vtkSphereSource> dummy_sphere = vtkSmartPointer<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<vtkUnsignedCharArray> sphere_scalars_init = VtkUtils::FillScalars(nr_points, sphere_color);
+ vtkSmartPointer<vtkAppendPolyData> append_filter = vtkSmartPointer<vtkAppendPolyData>::New();
- // Create the array for small sphere
- dummy_sphere->SetRadius(sphere_radius);
- dummy_sphere->Update();
- nr_points = dummy_sphere->GetOutput()->GetNumberOfCells();
- vtkSmartPointer<vtkUnsignedCharArray> sphere_scalars = VtkUtils::FillScalars(nr_points, sphere_color);
-
-
- for (vtkIdType i = 0; i < nr_poses; ++i)
+ for(size_t i = 0; i < total; ++i)
{
- Point3f new_pos = path[i].translation();
+ Vec3d curr = traj[i].translation();
vtkSmartPointer<vtkSphereSource> sphere_source = vtkSmartPointer<vtkSphereSource>::New();
- sphere_source->SetCenter(new_pos.x, new_pos.y, new_pos.z);
- if (i == 0)
+ sphere_source->SetCenter(curr.val);
+ sphere_source->SetRadius( (i == 0) ? 2 * radius : radius );
+ sphere_source->Update();
+
+ double alpha = static_cast<double>(i)/total;
+ Color c = from * (1 - alpha) + to * alpha;
+
+ vtkSmartPointer<vtkPolyData> polydata = sphere_source->GetOutput();
+ polydata->GetCellData()->SetScalars(VtkUtils::FillScalars(polydata->GetNumberOfCells(), c));
+ append_filter->AddInputConnection(polydata->GetProducerPort());
+
+ 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;
+ Vec3d prev = traj[i-1].translation();
+ Vec3d lvec = prev - curr;
+
+ if(norm(lvec) > line_length)
+ lvec = normalize(lvec) * line_length;
+
+ Vec3d lend = curr + lvec;
+
+ vtkSmartPointer<vtkLineSource> line_source = vtkSmartPointer<vtkLineSource>::New();
+ line_source->SetPoint1(curr.val);
+ line_source->SetPoint2(lend.val);
+ line_source->Update();
+ vtkSmartPointer<vtkPolyData> polydata = line_source->GetOutput();
+ polydata->GetCellData()->SetScalars(VtkUtils::FillScalars(polydata->GetNumberOfCells(), c));
+ append_filter->AddInputConnection(polydata->GetProducerPort());
}
- else
- {
- sphere_source->SetRadius(sphere_radius);
- sphere_source->Update();
- sphere_source->GetOutput()->GetCellData()->SetScalars(sphere_scalars);
- appendFilter->AddInputConnection(sphere_source->GetOutputPort());
- }
-
-
- Affine3d relativeAffine = path[i].inv() * path[i-1];
- Vec3d v = path[i].rotation() * relativeAffine.translation();
- v = normalize(v) * line_length;
-
- vtkSmartPointer<vtkLineSource> line_source = vtkSmartPointer<vtkLineSource>::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());
}
+ append_filter->Update();
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
mapper->SetScalarModeToUseCellData();
- mapper->SetInputConnection(appendFilter->GetOutputPort());
+ VtkUtils::SetInputData(mapper, append_filter->GetOutput());
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
actor->SetMapper(mapper);