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 &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 &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> 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 &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 &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 appendFilter = vtkSmartPointer::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 line_scalars = vtkSmartPointer::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(); + size_t total = path64.total(); - // Create color array for sphere - vtkSmartPointer dummy_sphere = vtkSmartPointer::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 = VtkUtils::FillScalars(nr_points, sphere_color); + vtkSmartPointer append_filter = vtkSmartPointer::New(); - // Create the array for small sphere - dummy_sphere->SetRadius(sphere_radius); - dummy_sphere->Update(); - nr_points = dummy_sphere->GetOutput()->GetNumberOfCells(); - vtkSmartPointer 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 sphere_source = vtkSmartPointer::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(i)/total; + Color c = from * (1 - alpha) + to * alpha; + + vtkSmartPointer 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 line_source = vtkSmartPointer::New(); + line_source->SetPoint1(curr.val); + line_source->SetPoint2(lend.val); + line_source->Update(); + vtkSmartPointer 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 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()); } + append_filter->Update(); vtkSmartPointer mapper = vtkSmartPointer::New(); mapper->SetScalarModeToUseCellData(); - mapper->SetInputConnection(appendFilter->GetOutputPort()); + VtkUtils::SetInputData(mapper, append_filter->GetOutput()); vtkSmartPointer actor = vtkSmartPointer::New(); actor->SetMapper(mapper);