diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index 32665e74ab..12373cd133 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -279,6 +279,7 @@ namespace cv class CV_EXPORTS WTrajectorySpheres: public Widget3D { public: + //! Takes vector> and displays trajectory of the given path 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/precomp.hpp b/modules/viz/src/precomp.hpp index 155612724e..3f4feaf046 100644 --- a/modules/viz/src/precomp.hpp +++ b/modules/viz/src/precomp.hpp @@ -221,6 +221,22 @@ namespace cv return scalars; } }; + + inline vtkSmartPointer vtkmatrix(const cv::Matx44d &matrix) + { + vtkSmartPointer vtk_matrix = vtkSmartPointer::New(); + vtk_matrix->DeepCopy(matrix.val); + return vtk_matrix; + } + + inline Color vtkcolor(const Color& color) + { + Color scaled_color = color * (1.0/255.0); + std::swap(scaled_color[0], scaled_color[2]); + return scaled_color; + } + + template inline _Tp normalized(const _Tp& v) { return v * 1/norm(v); } } } diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp index cc674dcf49..f4b68a9fb8 100644 --- a/modules/viz/src/shapes.cpp +++ b/modules/viz/src/shapes.cpp @@ -70,6 +70,35 @@ template<> cv::viz::WLine cv::viz::Widget::cast() return static_cast(widget); } +/////////////////////////////////////////////////////////////////////////////////////////////// +/// sphere widget implementation + +cv::viz::WSphere::WSphere(const Point3d ¢er, double radius, int sphere_resolution, const Color &color) +{ + vtkSmartPointer sphere = vtkSmartPointer::New(); + sphere->SetRadius(radius); + sphere->SetCenter(center.x, center.y, center.z); + sphere->SetPhiResolution(sphere_resolution); + sphere->SetThetaResolution(sphere_resolution); + sphere->LatLongTessellationOff(); + sphere->Update(); + + vtkSmartPointer mapper = vtkSmartPointer::New(); + VtkUtils::SetInputData(mapper, sphere->GetOutput()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + WidgetAccessor::setProp(*this, actor); + setColor(color); +} + +template<> cv::viz::WSphere cv::viz::Widget::cast() +{ + Widget3D widget = this->cast(); + return static_cast(widget); +} + /////////////////////////////////////////////////////////////////////////////////////////////// /// plane widget implementation @@ -143,51 +172,21 @@ template<> cv::viz::WPlane cv::viz::Widget::cast() return static_cast(widget); } -/////////////////////////////////////////////////////////////////////////////////////////////// -/// sphere widget implementation - -cv::viz::WSphere::WSphere(const Point3d ¢er, double radius, int sphere_resolution, const Color &color) -{ - vtkSmartPointer sphere = vtkSmartPointer::New(); - sphere->SetRadius(radius); - sphere->SetCenter(center.x, center.y, center.z); - sphere->SetPhiResolution(sphere_resolution); - sphere->SetThetaResolution(sphere_resolution); - sphere->LatLongTessellationOff(); - sphere->Update(); - - vtkSmartPointer mapper = vtkSmartPointer::New(); - VtkUtils::SetInputData(mapper, sphere->GetOutput()); - - vtkSmartPointer actor = vtkSmartPointer::New(); - actor->SetMapper(mapper); - - WidgetAccessor::setProp(*this, actor); - setColor(color); -} - -template<> cv::viz::WSphere cv::viz::Widget::cast() -{ - Widget3D widget = this->cast(); - return static_cast(widget); -} - /////////////////////////////////////////////////////////////////////////////////////////////// /// arrow widget implementation cv::viz::WArrow::WArrow(const Point3d& pt1, const Point3d& pt2, double thickness, const Color &color) { - vtkSmartPointer arrowSource = vtkSmartPointer::New(); - arrowSource->SetShaftRadius(thickness); - // The thickness and radius of the tip are adjusted based on the thickness of the arrow - arrowSource->SetTipRadius(thickness * 3.0); - arrowSource->SetTipLength(thickness * 10.0); + vtkSmartPointer arrow_source = vtkSmartPointer::New(); + arrow_source->SetShaftRadius(thickness); + arrow_source->SetTipRadius(thickness * 3.0); + arrow_source->SetTipLength(thickness * 10.0); RNG rng = theRNG(); Vec3d arbitrary(rng.uniform(-10.0, 10.0), rng.uniform(-10.0, 10.0), rng.uniform(-10.0, 10.0)); Vec3d startPoint(pt1.x, pt1.y, pt1.z), endPoint(pt2.x, pt2.y, pt2.z); - double length = cv::norm(endPoint - startPoint); + double length = norm(endPoint - startPoint); Vec3d xvec = normalized(endPoint - startPoint); Vec3d zvec = normalized(xvec.cross(arbitrary)); @@ -204,7 +203,7 @@ cv::viz::WArrow::WArrow(const Point3d& pt1, const Point3d& pt2, double thickness // Transform the polydata vtkSmartPointer transformPD = vtkSmartPointer::New(); transformPD->SetTransform(transform); - transformPD->SetInputConnection(arrowSource->GetOutputPort()); + transformPD->SetInputConnection(arrow_source->GetOutputPort()); transformPD->Update(); vtkSmartPointer mapper = vtkSmartPointer::New(); @@ -908,14 +907,14 @@ namespace cv { namespace viz { namespace actor->SetTexture(texture); } - static vtkSmartPointer createFrustrum(double aspect_ratio, double fovy, double scale) + static vtkSmartPointer createFrustum(double aspect_ratio, double fovy, double scale) { vtkSmartPointer camera = vtkSmartPointer::New(); camera->SetViewAngle(fovy); camera->SetPosition(0.0, 0.0, 0.0); camera->SetViewUp(0.0, 1.0, 0.0); camera->SetFocalPoint(0.0, 0.0, 1.0); - camera->SetClippingRange(0.01, scale); + camera->SetClippingRange(1e-9, scale); double planesArray[24]; camera->GetFrustumPlanes(aspect_ratio, planesArray); @@ -956,7 +955,7 @@ cv::viz::WCameraPosition::WCameraPosition(const Matx33d &K, double scale, const double fovy = 2.0 * atan2(c_y, f_y) * 180 / CV_PI; double aspect_ratio = f_y / f_x; - vtkSmartPointer polydata = CameraPositionUtils::createFrustrum(aspect_ratio, fovy, scale); + vtkSmartPointer polydata = CameraPositionUtils::createFrustum(aspect_ratio, fovy, scale); vtkSmartPointer mapper = vtkSmartPointer::New(); VtkUtils::SetInputData(mapper, polydata); @@ -973,7 +972,7 @@ cv::viz::WCameraPosition::WCameraPosition(const Vec2d &fov, double scale, const double aspect_ratio = tan(fov[0] * 0.5) / tan(fov[1] * 0.5); double fovy = fov[1] * 180 / CV_PI; - vtkSmartPointer polydata = CameraPositionUtils::createFrustrum(aspect_ratio, fovy, scale); + vtkSmartPointer polydata = CameraPositionUtils::createFrustum(aspect_ratio, fovy, scale); vtkSmartPointer mapper = vtkSmartPointer::New(); VtkUtils::SetInputData(mapper, polydata); diff --git a/modules/viz/src/vizimpl.hpp b/modules/viz/src/vizimpl.hpp index d5274b49ce..03d5c0325e 100644 --- a/modules/viz/src/vizimpl.hpp +++ b/modules/viz/src/vizimpl.hpp @@ -179,34 +179,16 @@ private: bool removeActorFromRenderer(const vtkSmartPointer &actor); }; - namespace cv { namespace viz { - inline vtkSmartPointer vtkmatrix(const cv::Matx44d &matrix) - { - vtkSmartPointer vtk_matrix = vtkSmartPointer::New(); - vtk_matrix->DeepCopy(matrix.val); - return vtk_matrix; - } - struct color_tag {}; struct gray_tag {}; inline Vec3b fetchRgb(const unsigned char* color, color_tag) { return Vec3b(color[2], color[1], color[0]); } inline Vec3b fetchRgb(const unsigned char* color, gray_tag) { return Vec3b(color[0], color[0], color[0]); } - inline Vec3d vtkpoint(const Point3f& point) { return Vec3d(point.x, point.y, point.z); } - template inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); } - - inline Color vtkcolor(const Color& color) - { - Color scaled_color = color * (1.0/255.0); - std::swap(scaled_color[0], scaled_color[2]); - return scaled_color; - } - struct ConvertToVtkImage { struct Impl diff --git a/modules/viz/src/vtk/vtkTrajectorySource.cpp b/modules/viz/src/vtk/vtkTrajectorySource.cpp index 1510208819..e098a1d553 100644 --- a/modules/viz/src/vtk/vtkTrajectorySource.cpp +++ b/modules/viz/src/vtk/vtkTrajectorySource.cpp @@ -59,9 +59,8 @@ void cv::viz::vtkTrajectorySource::SetTrajectory(InputArray _traj) Mat traj; _traj.getMat().convertTo(traj, CV_64F); - const Affine3d* dpath = _traj.getMat().ptr(); - - size_t total = _traj.total(); + const Affine3d* dpath = traj.ptr(); + size_t total = traj.total(); points = vtkSmartPointer::New(); points->SetDataType(VTK_DOUBLE); @@ -81,7 +80,6 @@ void cv::viz::vtkTrajectorySource::SetTrajectory(InputArray _traj) } } - cv::Mat cv::viz::vtkTrajectorySource::ExtractPoints(InputArray _traj) { CV_Assert(_traj.kind() == _InputArray::STD_VECTOR || _traj.kind() == _InputArray::MAT); diff --git a/modules/viz/src/widget.cpp b/modules/viz/src/widget.cpp index 23099ba728..b0e84037fd 100644 --- a/modules/viz/src/widget.cpp +++ b/modules/viz/src/widget.cpp @@ -140,14 +140,11 @@ void cv::viz::Widget::setRenderingProperty(int property, double value) { if (!actor->GetMapper()->GetInput()->GetPointData()->GetNormals()) { + vtkSmartPointer mapper = vtkPolyDataMapper::SafeDownCast(actor->GetMapper()); vtkSmartPointer normals = vtkSmartPointer::New(); -#if VTK_MAJOR_VERSION <= 5 - normals->SetInput(actor->GetMapper()->GetInput()); -#else - normals->SetInputData(actor->GetMapper()->GetInput()); -#endif + VtkUtils::SetInputData(normals, mapper->GetInput()); normals->Update(); - vtkDataSetMapper::SafeDownCast(actor->GetMapper())->SetInputConnection(normals->GetOutputPort()); + VtkUtils::SetInputData(mapper, normals->GetOutput()); } actor->GetProperty()->SetInterpolationToGouraud(); break; @@ -156,14 +153,11 @@ void cv::viz::Widget::setRenderingProperty(int property, double value) { if (!actor->GetMapper()->GetInput()->GetPointData()->GetNormals()) { + vtkSmartPointer mapper = vtkPolyDataMapper::SafeDownCast(actor->GetMapper()); vtkSmartPointer normals = vtkSmartPointer::New(); -#if VTK_MAJOR_VERSION <= 5 - normals->SetInput(actor->GetMapper()->GetInput()); -#else - normals->SetInputData(actor->GetMapper()->GetInput()); -#endif + VtkUtils::SetInputData(normals, mapper->GetInput()); normals->Update(); - vtkDataSetMapper::SafeDownCast(actor->GetMapper())->SetInputConnection(normals->GetOutputPort()); + VtkUtils::SetInputData(mapper, normals->GetOutput()); } actor->GetProperty()->SetInterpolationToPhong(); break; diff --git a/modules/viz/test/test_precomp.hpp b/modules/viz/test/test_precomp.hpp index efc89b27f5..e4fe78420c 100644 --- a/modules/viz/test/test_precomp.hpp +++ b/modules/viz/test/test_precomp.hpp @@ -78,6 +78,21 @@ namespace cv { return Path::combine(cvtest::TS::ptr()->get_data_path(), "dragon.ply"); } + + template + inline std::vector< Affine3<_Tp> > generate_test_trajectory() + { + std::vector< Affine3<_Tp> > result; + + for (int i = 0, j = 0; i <= 270; i += 3, j += 10) + { + double x = 2 * cos(i * 3 * CV_PI/180.0) * (1.0 + 0.5 * cos(1.2 + i * 1.2 * CV_PI/180.0)); + double y = 0.25 + i/270.0 + sin(j * CV_PI/180.0) * 0.2 * sin(0.6 + j * 1.5 * CV_PI/180.0); + double z = 2 * sin(i * 3 * CV_PI/180.0) * (1.0 + 0.5 * cos(1.2 + i * CV_PI/180.0)); + result.push_back(viz::makeCameraPose(Vec3d(x, y, z), Vec3d::all(0.0), Vec3d(0.0, 1.0, 0.0))); + } + return result; + } } #endif diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 6bc3622c63..aec326508e 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -48,46 +48,21 @@ TEST(Viz_viz3d, develop) cv::Mat cloud = cv::viz::readCloud(get_dragon_ply_file_path()); //cv::viz::Mesh3d mesh = cv::viz::Mesh3d::load(get_dragon_ply_file_path()); - //theRNG().fill(mesh.colors, RNG::UNIFORM, 0, 255); cv::viz::Viz3d viz("abc"); viz.setBackgroundColor(cv::viz::Color::mlab()); - viz.showWidget("coo", cv::viz::WCoordinateSystem(0.1)); - - -// double c = cos(CV_PI/6); -// std::vector pts; -// pts.push_back(Vec3d(0, 0.0, -1.0)); -// pts.push_back(Vec3d(1, c, -0.5)); -// pts.push_back(Vec3d(2, c, 0.5)); -// pts.push_back(Vec3d(3, 0.0, 1.0)); -// pts.push_back(Vec3d(4, -c, 0.5)); -// pts.push_back(Vec3d(5, -c, -0.5)); - -// viz.showWidget("pl", cv::viz::WPolyLine(Mat(pts), cv::viz::Color::green())); - - //viz.showWidget("pl", cv::viz::WPolyLine(cloud.colRange(0, 100), cv::viz::Color::green())); - //viz.spin(); - - //cv::Mat colors(cloud.size(), CV_8UC3, cv::Scalar(0, 255, 0)); + viz.showWidget("coo", cv::viz::WCoordinateSystem(1)); //viz.showWidget("h", cv::viz::Widget::fromPlyFile("d:/horse-red.ply")); //viz.showWidget("a", cv::viz::WArrow(cv::Point3f(0,0,0), cv::Point3f(1,1,1))); - std::vector gt, es; - cv::viz::readTrajectory(gt, "d:/Datasets/trajs/gt%05d.xml"); + //---->>>>> + //std::vector gt, es; + //cv::viz::readTrajectory(gt, "d:/Datasets/trajs/gt%05d.xml"); //cv::viz::readTrajectory(es, "d:/Datasets/trajs/es%05d.xml"); - gt.resize(20); - - Affine3d inv = gt[0].inv(); - for(size_t i = 0; i < gt.size(); ++i) - gt[i] = inv * gt[i]; - - //viz.showWidget("gt", viz::WTrajectory(gt, viz::WTrajectory::PATH, 1.f, viz::Color::blue()), gt[0].inv()); - viz.showWidget("gt", viz::WTrajectory(gt, viz::WTrajectory::BOTH, 0.01f, viz::Color::blue())); - - //viz.showWidget("tr", viz::WTrajectory(es, viz::WTrajectory::PATH, 1.f, viz::Color::red()), gt[0].inv()); + //cv::Mat cloud = cv::viz::readCloud(get_dragon_ply_file_path()); + //---->>>>> //theRNG().fill(colors, cv::RNG::UNIFORM, 0, 255); //viz.showWidget("c", cv::viz::WCloud(cloud, colors)); diff --git a/modules/viz/test/tests_simple.cpp b/modules/viz/test/tests_simple.cpp index 6f61a806c6..93428c6b95 100644 --- a/modules/viz/test/tests_simple.cpp +++ b/modules/viz/test/tests_simple.cpp @@ -156,6 +156,49 @@ TEST(Viz, DISABLED_show_sampled_normals) viz.spin(); } +TEST(Viz, DISABLED_show_trajectories) +{ + std::vector path = generate_test_trajectory(), sub0, sub1, sub2, sub3, sub4, sub5; + + Mat(path).rowRange(0, path.size()/10+1).copyTo(sub0); + Mat(path).rowRange(path.size()/10, path.size()/5+1).copyTo(sub1); + Mat(path).rowRange(path.size()/5, 11*path.size()/12).copyTo(sub2); + Mat(path).rowRange(11*path.size()/12, path.size()).copyTo(sub3); + Mat(path).rowRange(3*path.size()/4, 33*path.size()/40).copyTo(sub4); + Mat(path).rowRange(33*path.size()/40, 9*path.size()/10).copyTo(sub5); + Matx33d K(1024.0, 0.0, 320.0, 0.0, 1024.0, 240.0, 0.0, 0.0, 1.0); + + Viz3d viz("show_trajectories"); + viz.showWidget("coos", WCoordinateSystem()); + viz.showWidget("sub0", WTrajectorySpheres(sub0, 0.25, 0.07)); + viz.showWidget("sub1", WTrajectory(sub1, WTrajectory::PATH, 0.2, Color::brown())); + viz.showWidget("sub2", WTrajectory(sub2, WTrajectory::FRAMES, 0.2)); + viz.showWidget("sub3", WTrajectory(sub3, WTrajectory::BOTH, 0.2, Color::green())); + viz.showWidget("sub4", WTrajectoryFrustums(sub4, K, 0.3)); + viz.showWidget("sub5", WTrajectoryFrustums(sub5, Vec2d(0.78, 0.78), 0.15)); + + int i = 0; + while(!viz.wasStopped()) + { + double a = --i % 360; + Vec3d pose(sin(a * CV_PI/180), 0.7, cos(a * CV_PI/180)); + viz.setViewerPose(makeCameraPose(pose * 7.5, Vec3d(0.0, 0.5, 0.0), Vec3d(0.0, 0.1, 0.0))); + viz.spinOnce(20, true); + } + //viz.spin(); +} + +TEST(Viz, DISABLED_show_trajectory_reposition) +{ + std::vector path = generate_test_trajectory(); + + Viz3d viz("show_trajectory_reposition_to_origin"); + viz.showWidget("coos", WCoordinateSystem()); + viz.showWidget("sub3", WTrajectory(Mat(path).rowRange(0, path.size()/3), WTrajectory::BOTH, 0.2, Color::brown()), path.front().inv()); + viz.spin(); +} + + TEST(Viz, DISABLED_spin_twice_____________________________TODO_UI_BUG) { Mesh3d mesh = Mesh3d::load(get_dragon_ply_file_path());