diff --git a/modules/viz/include/opencv2/viz.hpp b/modules/viz/include/opencv2/viz.hpp index 5f1b7766fa..7e04f7b94a 100644 --- a/modules/viz/include/opencv2/viz.hpp +++ b/modules/viz/include/opencv2/viz.hpp @@ -64,7 +64,7 @@ namespace cv CV_EXPORTS Affine3f makeTransformToGlobal(const Vec3f& axis_x, const Vec3f& axis_y, const Vec3f& axis_z, const Vec3f& origin = Vec3f::all(0)); //! constructs camera pose from position, focal_point and up_vector (see gluLookAt() for more infromation - CV_EXPORTS Affine3f makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& up_vector); + CV_EXPORTS Affine3f makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& y_dir); //! checks float value for Nan diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index 474c1bb815..603d17df83 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace cv @@ -70,6 +71,8 @@ namespace cv public: PlaneWidget(const Vec4f& coefs, double size = 1.0, const Color &color = Color::white()); PlaneWidget(const Vec4f& coefs, const Point3f& pt, double size = 1.0, const Color &color = Color::white()); + private: + struct SetSizeImpl; }; class CV_EXPORTS SphereWidget : public Widget3D @@ -81,7 +84,7 @@ namespace cv class CV_EXPORTS ArrowWidget : public Widget3D { public: - ArrowWidget(const Point3f& pt1, const Point3f& pt2, const Color &color = Color::white()); + ArrowWidget(const Point3f& pt1, const Point3f& pt2, double thickness = 0.03, const Color &color = Color::white()); }; class CV_EXPORTS CircleWidget : public Widget3D @@ -120,7 +123,12 @@ namespace cv class CV_EXPORTS GridWidget : public Widget3D { public: - GridWidget(Vec2i dimensions, Vec2d spacing, const Color &color = Color::white()); + GridWidget(const Vec2i &dimensions, const Vec2d &spacing, const Color &color = Color::white()); + GridWidget(const Vec4f &coeffs, const Vec2i &dimensions, const Vec2d &spacing, const Color &color = Color::white()); + + private: + struct GridImpl; + }; class CV_EXPORTS Text3DWidget : public Widget3D @@ -140,6 +148,48 @@ namespace cv void setText(const String &text); String getText() const; }; + + class CV_EXPORTS ImageOverlayWidget : public Widget2D + { + public: + ImageOverlayWidget(const Mat &image, const Rect &rect); + + void setImage(const Mat &image); + }; + + class CV_EXPORTS Image3DWidget : public Widget3D + { + public: + Image3DWidget(const Mat &image, const Size &size); + Image3DWidget(const Vec3f &position, const Vec3f &normal, const Vec3f &up_vector, const Mat &image, const Size &size); + + void setImage(const Mat &image); + }; + + class CV_EXPORTS CameraPositionWidget : public Widget3D + { + public: + CameraPositionWidget(double scale = 1.0); + CameraPositionWidget(const Matx33f &K, double scale = 1.0, const Color &color = Color::white()); + CameraPositionWidget(const Vec2f &fov, double scale = 1.0, const Color &color = Color::white()); + CameraPositionWidget(const Matx33f &K, const Mat &img, double scale = 1.0, const Color &color = Color::white()); + + }; + + class CV_EXPORTS TrajectoryWidget : public Widget3D + { + public: + 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 CloudWidget : public Widget3D { @@ -151,6 +201,18 @@ namespace cv struct CreateCloudWidget; }; + class CV_EXPORTS CloudCollectionWidget : public Widget3D + { + public: + CloudCollectionWidget(); + + void addCloud(InputArray cloud, InputArray colors, const Affine3f &pose = Affine3f::Identity()); + void addCloud(InputArray cloud, const Color &color = Color::white(), const Affine3f &pose = Affine3f::Identity()); + + private: + struct CreateCloudWidget; + }; + class CV_EXPORTS CloudNormalsWidget : public Widget3D { public: @@ -183,7 +245,12 @@ namespace cv template<> CV_EXPORTS GridWidget Widget::cast(); template<> CV_EXPORTS Text3DWidget Widget::cast(); template<> CV_EXPORTS TextWidget Widget::cast(); + template<> CV_EXPORTS ImageOverlayWidget Widget::cast(); + template<> CV_EXPORTS Image3DWidget Widget::cast(); + template<> CV_EXPORTS CameraPositionWidget Widget::cast(); + template<> CV_EXPORTS TrajectoryWidget Widget::cast(); template<> CV_EXPORTS CloudWidget Widget::cast(); + template<> CV_EXPORTS CloudCollectionWidget Widget::cast(); template<> CV_EXPORTS CloudNormalsWidget Widget::cast(); template<> CV_EXPORTS MeshWidget Widget::cast(); diff --git a/modules/viz/src/cloud_widgets.cpp b/modules/viz/src/cloud_widgets.cpp index 18be023214..a30c97a32a 100644 --- a/modules/viz/src/cloud_widgets.cpp +++ b/modules/viz/src/cloud_widgets.cpp @@ -116,7 +116,7 @@ cv::viz::CloudWidget::CloudWidget(InputArray _cloud, InputArray _colors) // Filter colors Vec3b* colors_data = new Vec3b[nr_points]; - NanFilter::copy(colors, colors_data, cloud); + NanFilter::copyColor(colors, colors_data, cloud); vtkSmartPointer scalars = vtkSmartPointer::New (); scalars->SetNumberOfComponents (3); @@ -154,7 +154,6 @@ cv::viz::CloudWidget::CloudWidget(InputArray _cloud, const Color &color) Mat cloud = _cloud.getMat(); CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4); - vtkIdType nr_points; vtkSmartPointer polydata = CreateCloudWidget::create(cloud, nr_points); @@ -184,6 +183,227 @@ template<> cv::viz::CloudWidget cv::viz::Widget::cast() return static_cast(widget); } +/////////////////////////////////////////////////////////////////////////////////////////////// +/// Cloud Collection Widget implementation + +struct cv::viz::CloudCollectionWidget::CreateCloudWidget +{ + static inline vtkSmartPointer create(const Mat &cloud, vtkIdType &nr_points) + { + vtkSmartPointer polydata = vtkSmartPointer::New (); + vtkSmartPointer vertices = vtkSmartPointer::New (); + + polydata->SetVerts (vertices); + + vtkSmartPointer points = polydata->GetPoints(); + vtkSmartPointer initcells; + nr_points = cloud.total(); + + if (!points) + { + points = vtkSmartPointer::New (); + if (cloud.depth() == CV_32F) + points->SetDataTypeToFloat(); + else if (cloud.depth() == CV_64F) + points->SetDataTypeToDouble(); + polydata->SetPoints (points); + } + points->SetNumberOfPoints (nr_points); + + if (cloud.depth() == CV_32F) + { + // Get a pointer to the beginning of the data array + Vec3f *data_beg = vtkpoints_data(points); + Vec3f *data_end = NanFilter::copy(cloud, data_beg, cloud); + nr_points = data_end - data_beg; + } + else if (cloud.depth() == CV_64F) + { + // Get a pointer to the beginning of the data array + Vec3d *data_beg = vtkpoints_data(points); + Vec3d *data_end = NanFilter::copy(cloud, data_beg, cloud); + nr_points = data_end - data_beg; + } + points->SetNumberOfPoints (nr_points); + + // Update cells + vtkSmartPointer cells = vertices->GetData (); + // If no init cells and cells has not been initialized... + if (!cells) + cells = vtkSmartPointer::New (); + + // If we have less values then we need to recreate the array + if (cells->GetNumberOfTuples () < nr_points) + { + cells = vtkSmartPointer::New (); + + // If init cells is given, and there's enough data in it, use it + if (initcells && initcells->GetNumberOfTuples () >= nr_points) + { + cells->DeepCopy (initcells); + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + } + else + { + // If the number of tuples is still too small, we need to recreate the array + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + vtkIdType *cell = cells->GetPointer (0); + // Fill it with 1s + std::fill_n (cell, nr_points * 2, 1); + cell++; + for (vtkIdType i = 0; i < nr_points; ++i, cell += 2) + *cell = i; + // Save the results in initcells + initcells = vtkSmartPointer::New (); + initcells->DeepCopy (cells); + } + } + else + { + // The assumption here is that the current set of cells has more data than needed + cells->SetNumberOfComponents (2); + cells->SetNumberOfTuples (nr_points); + } + + // Set the cells and the vertices + vertices->SetCells (nr_points, cells); + return polydata; + } + + static void createMapper(vtkSmartPointer actor, vtkSmartPointer poly_data, Vec3d& minmax) + { + vtkDataSetMapper *mapper = vtkDataSetMapper::SafeDownCast(actor->GetMapper()); + if (!mapper) + { + // This is the first cloud + vtkSmartPointer mapper_new = vtkSmartPointer::New (); + mapper_new->SetInputConnection (poly_data->GetProducerPort()); + + mapper_new->SetScalarRange(minmax.val); + mapper_new->SetScalarModeToUsePointData (); + + bool interpolation = (poly_data && poly_data->GetNumberOfCells () != poly_data->GetNumberOfVerts ()); + + mapper_new->SetInterpolateScalarsBeforeMapping(interpolation); + mapper_new->ScalarVisibilityOn(); + mapper_new->ImmediateModeRenderingOff(); + + actor->SetNumberOfCloudPoints (int (std::max(1, poly_data->GetNumberOfPoints () / 10))); + actor->GetProperty()->SetInterpolationToFlat(); + actor->GetProperty()->BackfaceCullingOn(); + actor->SetMapper(mapper_new); + return ; + } + + vtkPolyData *data = vtkPolyData::SafeDownCast(mapper->GetInput()); + CV_Assert(data); + + vtkSmartPointer appendFilter = vtkSmartPointer::New(); + appendFilter->AddInputConnection(mapper->GetInput()->GetProducerPort()); + appendFilter->AddInputConnection(poly_data->GetProducerPort()); + mapper->SetInputConnection(appendFilter->GetOutputPort()); + + // Update the number of cloud points + vtkIdType old_cloud_points = actor->GetNumberOfCloudPoints(); + actor->SetNumberOfCloudPoints (int (std::max(1, old_cloud_points+poly_data->GetNumberOfPoints () / 10))); + } +}; + +cv::viz::CloudCollectionWidget::CloudCollectionWidget() +{ + // Just create the actor + vtkSmartPointer actor = vtkSmartPointer::New(); + WidgetAccessor::setProp(*this, actor); +} + +void cv::viz::CloudCollectionWidget::addCloud(InputArray _cloud, InputArray _colors, const Affine3f &pose) +{ + Mat cloud = _cloud.getMat(); + Mat colors = _colors.getMat(); + CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4); + CV_Assert(colors.type() == CV_8UC3 && cloud.size() == colors.size()); + + if (cloud.isContinuous() && colors.isContinuous()) + { + cloud.reshape(cloud.channels(), 1); + colors.reshape(colors.channels(), 1); + } + + vtkIdType nr_points; + vtkSmartPointer polydata = CreateCloudWidget::create(cloud, nr_points); + + // Filter colors + Vec3b* colors_data = new Vec3b[nr_points]; + NanFilter::copyColor(colors, colors_data, cloud); + + vtkSmartPointer scalars = vtkSmartPointer::New (); + scalars->SetNumberOfComponents (3); + scalars->SetNumberOfTuples (nr_points); + scalars->SetArray (colors_data->val, 3 * nr_points, 0); + + // Assign the colors + polydata->GetPointData ()->SetScalars (scalars); + + // Transform the poly data based on the pose + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->SetMatrix(convertToVtkMatrix(pose.matrix)); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetTransform(transform); + transform_filter->SetInputConnection(polydata->GetProducerPort()); + transform_filter->Update(); + + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getProp(*this)); + CV_Assert(actor); + + Vec3d minmax(scalars->GetRange()); + CreateCloudWidget::createMapper(actor, transform_filter->GetOutput(), minmax); +} + +void cv::viz::CloudCollectionWidget::addCloud(InputArray _cloud, const Color &color, const Affine3f &pose) +{ + Mat cloud = _cloud.getMat(); + CV_Assert(cloud.type() == CV_32FC3 || cloud.type() == CV_64FC3 || cloud.type() == CV_32FC4 || cloud.type() == CV_64FC4); + + vtkIdType nr_points; + vtkSmartPointer polydata = CreateCloudWidget::create(cloud, nr_points); + + vtkSmartPointer scalars = vtkSmartPointer::New (); + scalars->SetNumberOfComponents (3); + scalars->SetNumberOfTuples (nr_points); + scalars->FillComponent(0, color[2]); + scalars->FillComponent(1, color[1]); + scalars->FillComponent(2, color[0]); + + // Assign the colors + polydata->GetPointData ()->SetScalars (scalars); + + // Transform the poly data based on the pose + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->SetMatrix(convertToVtkMatrix(pose.matrix)); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetTransform(transform); + transform_filter->SetInputConnection(polydata->GetProducerPort()); + transform_filter->Update(); + + vtkLODActor *actor = vtkLODActor::SafeDownCast(WidgetAccessor::getProp(*this)); + CV_Assert(actor); + + Vec3d minmax(scalars->GetRange()); + CreateCloudWidget::createMapper(actor, transform_filter->GetOutput(), minmax); +} + +template<> cv::viz::CloudCollectionWidget cv::viz::Widget::cast() +{ + Widget3D widget = this->cast(); + return static_cast(widget); +} + /////////////////////////////////////////////////////////////////////////////////////////////// /// Cloud Normals Widget implementation @@ -359,7 +579,8 @@ cv::viz::MeshWidget::MeshWidget(const Mesh3d &mesh) vtkSmartPointer points = vtkSmartPointer::New (); vtkIdType nr_points = mesh.cloud.total(); - int * look_up = new int[nr_points]; + Mat look_up_mat(1, nr_points, CV_32SC1); + int * look_up = look_up_mat.ptr(); points->SetNumberOfPoints (nr_points); // Copy data from cloud to vtkPoints @@ -384,7 +605,7 @@ cv::viz::MeshWidget::MeshWidget(const Mesh3d &mesh) { Vec3b * colors_data = 0; colors_data = new Vec3b[nr_points]; - NanFilter::copy(mesh.colors, colors_data, mesh.cloud); + NanFilter::copyColor(mesh.colors, colors_data, mesh.cloud); scalars = vtkSmartPointer::New (); scalars->SetNumberOfComponents (3); diff --git a/modules/viz/src/precomp.hpp b/modules/viz/src/precomp.hpp index df3a970404..12a042c154 100644 --- a/modules/viz/src/precomp.hpp +++ b/modules/viz/src/precomp.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -107,6 +108,7 @@ #include #include #include +#include #include #include @@ -131,7 +133,9 @@ #include #include #include - +#include +#include +#include #include #include diff --git a/modules/viz/src/shape_widgets.cpp b/modules/viz/src/shape_widgets.cpp index e2cb9a5600..caa5bf3588 100644 --- a/modules/viz/src/shape_widgets.cpp +++ b/modules/viz/src/shape_widgets.cpp @@ -50,6 +50,26 @@ template<> cv::viz::LineWidget cv::viz::Widget::cast() /////////////////////////////////////////////////////////////////////////////////////////////// /// plane widget implementation +struct cv::viz::PlaneWidget::SetSizeImpl +{ + template + static vtkSmartPointer setSize(const Vec<_Tp, 3> ¢er, vtkSmartPointer poly_data, double size) + { + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->Translate(center[0], center[1], center[2]); + transform->Scale(size, size, size); + transform->Translate(-center[0], -center[1], -center[2]); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetInput(poly_data); + transform_filter->SetTransform(transform); + transform_filter->Update(); + + return transform_filter->GetOutput(); + } +}; + cv::viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, double size, const Color &color) { vtkSmartPointer plane = vtkSmartPointer::New (); @@ -57,12 +77,14 @@ cv::viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, double size, const Color & double norm = cv::norm(Vec3f(coefs.val)); plane->Push (-coefs[3] / norm); + Vec3d p_center; + plane->GetOrigin(p_center.val); + vtkSmartPointer mapper = vtkSmartPointer::New (); - mapper->SetInput(plane->GetOutput ()); + mapper->SetInput(SetSizeImpl::setSize(p_center, plane->GetOutput(), size)); vtkSmartPointer actor = vtkSmartPointer::New(); actor->SetMapper(mapper); - actor->SetScale(size); WidgetAccessor::setProp(*this, actor); setColor(color); @@ -80,11 +102,10 @@ cv::viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, const Point3f& pt, double plane->SetCenter (p_center[0], p_center[1], p_center[2]); vtkSmartPointer mapper = vtkSmartPointer::New (); - mapper->SetInput(plane->GetOutput ()); + mapper->SetInput(SetSizeImpl::setSize(p_center, plane->GetOutput(), size)); vtkSmartPointer actor = vtkSmartPointer::New(); actor->SetMapper(mapper); - actor->SetScale(size); WidgetAccessor::setProp(*this, actor); setColor(color); @@ -128,9 +149,13 @@ template<> cv::viz::SphereWidget cv::viz::Widget::cast() /////////////////////////////////////////////////////////////////////////////////////////////// /// arrow widget implementation -cv::viz::ArrowWidget::ArrowWidget(const Point3f& pt1, const Point3f& pt2, const Color &color) +cv::viz::ArrowWidget::ArrowWidget(const Point3f& pt1, const Point3f& 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); float startPoint[3], endPoint[3]; startPoint[0] = pt1.x; @@ -265,19 +290,24 @@ template<> cv::viz::CylinderWidget cv::viz::Widget::cast cube = vtkSmartPointer::New (); - cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z); +{ + vtkSmartPointer mapper = vtkSmartPointer::New (); + if (wire_frame) + { + vtkSmartPointer cube = vtkSmartPointer::New(); + cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z); + mapper->SetInput(cube->GetOutput ()); + } + else + { + vtkSmartPointer cube = vtkSmartPointer::New (); + cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z); + mapper->SetInput(cube->GetOutput ()); + } - vtkSmartPointer mapper = vtkSmartPointer::New (); - mapper->SetInput(cube->GetOutput ()); - vtkSmartPointer actor = vtkSmartPointer::New(); actor->SetMapper(mapper); - if (wire_frame) - actor->GetProperty ()->SetRepresentationToWireframe (); - WidgetAccessor::setProp(*this, actor); setColor(color); } @@ -410,26 +440,88 @@ template<> cv::viz::PolyLineWidget cv::viz::Widget::cast grid = vtkSmartPointer::New(); - - // Add 1 to dimensions because in ImageData dimensions is the number of lines - // - however here it means number of cells - grid->SetDimensions(dimensions[0]+1, dimensions[1]+1, 1); - grid->SetSpacing(spacing[0], spacing[1], 0.); - - // Set origin of the grid to be the middle of the grid - grid->SetOrigin(dimensions[0] * spacing[0] * (-0.5), dimensions[1] * spacing[1] * (-0.5), 0); + static vtkSmartPointer createGrid(const Vec2i &dimensions, const Vec2d &spacing) + { + // Create the grid using image data + vtkSmartPointer grid = vtkSmartPointer::New(); + + // Add 1 to dimensions because in ImageData dimensions is the number of lines + // - however here it means number of cells + grid->SetDimensions(dimensions[0]+1, dimensions[1]+1, 1); + grid->SetSpacing(spacing[0], spacing[1], 0.); + + // Set origin of the grid to be the middle of the grid + grid->SetOrigin(dimensions[0] * spacing[0] * (-0.5), dimensions[1] * spacing[1] * (-0.5), 0); + + // Extract the edges so we have the grid + vtkSmartPointer filter = vtkSmartPointer::New(); + filter->SetInputConnection(grid->GetProducerPort()); + filter->Update(); + return filter->GetOutput(); + } +}; + +cv::viz::GridWidget::GridWidget(const Vec2i &dimensions, const Vec2d &spacing, const Color &color) +{ + vtkSmartPointer grid = GridImpl::createGrid(dimensions, spacing); vtkSmartPointer mapper = vtkSmartPointer::New(); - mapper->SetInput(grid); + mapper->SetInputConnection(grid->GetProducerPort()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + WidgetAccessor::setProp(*this, actor); + setColor(color); +} + +cv::viz::GridWidget::GridWidget(const Vec4f &coefs, const Vec2i &dimensions, const Vec2d &spacing, const Color &color) +{ + vtkSmartPointer grid = GridImpl::createGrid(dimensions, spacing); + + // Estimate the transform to set the normal based on the coefficients + Vec3f normal(coefs[0], coefs[1], coefs[2]); + Vec3f up_vector(0.0f, 1.0f, 0.0f); // Just set as default + double push_distance = -coefs[3]/cv::norm(Vec3f(coefs.val)); + Vec3f u,v,n; + n = normalize(normal); + u = normalize(up_vector.cross(n)); + v = n.cross(u); + + vtkSmartPointer mat_trans = vtkSmartPointer::New(); + mat_trans->SetElement(0,0,u[0]); + mat_trans->SetElement(0,1,u[1]); + mat_trans->SetElement(0,2,u[2]); + mat_trans->SetElement(1,0,v[0]); + mat_trans->SetElement(1,1,v[1]); + mat_trans->SetElement(1,2,v[2]); + mat_trans->SetElement(2,0,n[0]); + mat_trans->SetElement(2,1,n[1]); + mat_trans->SetElement(2,2,n[2]); + // Inverse rotation (orthogonal, so just take transpose) + mat_trans->Transpose(); + mat_trans->SetElement(0,3,n[0] * push_distance); + mat_trans->SetElement(1,3,n[1] * push_distance); + mat_trans->SetElement(2,3,n[2] * push_distance); + mat_trans->SetElement(3,3,1); + + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->SetMatrix(mat_trans); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetTransform(transform); + transform_filter->SetInputConnection(grid->GetProducerPort()); + transform_filter->Update(); + + vtkSmartPointer mapper = vtkSmartPointer::New(); + mapper->SetInputConnection(transform_filter->GetOutputPort()); + vtkSmartPointer actor = vtkSmartPointer::New(); actor->SetMapper(mapper); - // Show it as wireframe - actor->GetProperty ()->SetRepresentationToWireframe (); WidgetAccessor::setProp(*this, actor); setColor(color); } @@ -533,3 +625,729 @@ cv::String cv::viz::TextWidget::getText() const CV_Assert(actor); return actor->GetInput(); } + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// image overlay widget implementation + +cv::viz::ImageOverlayWidget::ImageOverlayWidget(const Mat &image, const Rect &rect) +{ + CV_Assert(!image.empty() && image.depth() == CV_8U); + + // Create the vtk image and set its parameters based on input image + vtkSmartPointer vtk_image = vtkSmartPointer::New(); + ConvertToVtkImage::convert(image, vtk_image); + + // Need to flip the image as the coordinates are different in OpenCV and VTK + vtkSmartPointer flipFilter = vtkSmartPointer::New(); + flipFilter->SetFilteredAxis(1); // Vertical flip + flipFilter->SetInputConnection(vtk_image->GetProducerPort()); + flipFilter->Update(); + + // Scale the image based on the Rect + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->Scale(double(image.cols)/rect.width,double(image.rows)/rect.height,1.0); + + vtkSmartPointer image_reslice = vtkSmartPointer::New(); + image_reslice->SetResliceTransform(transform); + image_reslice->SetInputConnection(flipFilter->GetOutputPort()); + image_reslice->SetOutputDimensionality(2); + image_reslice->InterpolateOn(); + image_reslice->AutoCropOutputOn(); + + vtkSmartPointer imageMapper = vtkSmartPointer::New(); + imageMapper->SetInputConnection(image_reslice->GetOutputPort()); + imageMapper->SetColorWindow(255); // OpenCV color + imageMapper->SetColorLevel(127.5); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(imageMapper); + actor->SetPosition(rect.x, rect.y); + + WidgetAccessor::setProp(*this, actor); +} + +void cv::viz::ImageOverlayWidget::setImage(const Mat &image) +{ + CV_Assert(!image.empty() && image.depth() == CV_8U); + + vtkActor2D *actor = vtkActor2D::SafeDownCast(WidgetAccessor::getProp(*this)); + CV_Assert(actor); + + vtkImageMapper *mapper = vtkImageMapper::SafeDownCast(actor->GetMapper()); + CV_Assert(mapper); + + // Create the vtk image and set its parameters based on input image + vtkSmartPointer vtk_image = vtkSmartPointer::New(); + ConvertToVtkImage::convert(image, vtk_image); + + // Need to flip the image as the coordinates are different in OpenCV and VTK + vtkSmartPointer flipFilter = vtkSmartPointer::New(); + flipFilter->SetFilteredAxis(1); // Vertical flip + flipFilter->SetInputConnection(vtk_image->GetProducerPort()); + flipFilter->Update(); + + mapper->SetInputConnection(flipFilter->GetOutputPort()); +} + +template<> cv::viz::ImageOverlayWidget cv::viz::Widget::cast() +{ + Widget2D widget = this->cast(); + return static_cast(widget); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// image 3D widget implementation + +cv::viz::Image3DWidget::Image3DWidget(const Mat &image, const Size &size) +{ + CV_Assert(!image.empty() && image.depth() == CV_8U); + + // Create the vtk image and set its parameters based on input image + vtkSmartPointer vtk_image = vtkSmartPointer::New(); + ConvertToVtkImage::convert(image, vtk_image); + + // Need to flip the image as the coordinates are different in OpenCV and VTK + vtkSmartPointer flipFilter = vtkSmartPointer::New(); + flipFilter->SetFilteredAxis(1); // Vertical flip + flipFilter->SetInputConnection(vtk_image->GetProducerPort()); + flipFilter->Update(); + + Vec3d plane_center(size.width * 0.5, size.height * 0.5, 0.0); + + vtkSmartPointer plane = vtkSmartPointer::New(); + plane->SetCenter(plane_center[0], plane_center[1], plane_center[2]); + plane->SetNormal(0.0, 0.0, 1.0); + + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->Translate(plane_center[0], plane_center[1], plane_center[2]); + transform->Scale(size.width, size.height, 1.0); + transform->Translate(-plane_center[0], -plane_center[1], -plane_center[2]); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetTransform(transform); + transform_filter->SetInputConnection(plane->GetOutputPort()); + transform_filter->Update(); + + // Apply the texture + vtkSmartPointer texture = vtkSmartPointer::New(); + texture->SetInputConnection(flipFilter->GetOutputPort()); + + vtkSmartPointer texturePlane = vtkSmartPointer::New(); + texturePlane->SetInputConnection(transform_filter->GetOutputPort()); + + vtkSmartPointer planeMapper = vtkSmartPointer::New(); + planeMapper->SetInputConnection(texturePlane->GetOutputPort()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(planeMapper); + actor->SetTexture(texture); + + WidgetAccessor::setProp(*this, actor); +} + +cv::viz::Image3DWidget::Image3DWidget(const Vec3f &position, const Vec3f &normal, const Vec3f &up_vector, const Mat &image, const Size &size) +{ + CV_Assert(!image.empty() && image.depth() == CV_8U); + + // Create the vtk image and set its parameters based on input image + vtkSmartPointer vtk_image = vtkSmartPointer::New(); + ConvertToVtkImage::convert(image, vtk_image); + + // Need to flip the image as the coordinates are different in OpenCV and VTK + vtkSmartPointer flipFilter = vtkSmartPointer::New(); + flipFilter->SetFilteredAxis(1); // Vertical flip + flipFilter->SetInputConnection(vtk_image->GetProducerPort()); + flipFilter->Update(); + + vtkSmartPointer plane = vtkSmartPointer::New(); + plane->SetCenter(0.0, 0.0, 0.0); + plane->SetNormal(0.0, 0.0, 1.0); + + // Compute the transformation matrix for drawing the camera frame in a scene + Vec3f u,v,n; + n = normalize(normal); + u = normalize(up_vector.cross(n)); + v = n.cross(u); + + vtkSmartPointer mat_trans = vtkSmartPointer::New(); + mat_trans->SetElement(0,0,u[0]); + mat_trans->SetElement(0,1,u[1]); + mat_trans->SetElement(0,2,u[2]); + mat_trans->SetElement(1,0,v[0]); + mat_trans->SetElement(1,1,v[1]); + mat_trans->SetElement(1,2,v[2]); + mat_trans->SetElement(2,0,n[0]); + mat_trans->SetElement(2,1,n[1]); + mat_trans->SetElement(2,2,n[2]); + // Inverse rotation (orthogonal, so just take transpose) + mat_trans->Transpose(); + // Then translate the coordinate frame to camera position + mat_trans->SetElement(0,3,position[0]); + mat_trans->SetElement(1,3,position[1]); + mat_trans->SetElement(2,3,position[2]); + mat_trans->SetElement(3,3,1); + + // Apply the texture + vtkSmartPointer texture = vtkSmartPointer::New(); + texture->SetInputConnection(flipFilter->GetOutputPort()); + + vtkSmartPointer texturePlane = vtkSmartPointer::New(); + texturePlane->SetInputConnection(plane->GetOutputPort()); + + // Apply the transform after texture mapping + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->SetMatrix(mat_trans); + transform->Scale(size.width, size.height, 1.0); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetTransform(transform); + transform_filter->SetInputConnection(texturePlane->GetOutputPort()); + transform_filter->Update(); + + vtkSmartPointer planeMapper = vtkSmartPointer::New(); + planeMapper->SetInputConnection(transform_filter->GetOutputPort()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(planeMapper); + actor->SetTexture(texture); + + WidgetAccessor::setProp(*this, actor); +} + +void cv::viz::Image3DWidget::setImage(const Mat &image) +{ + CV_Assert(!image.empty() && image.depth() == CV_8U); + + vtkActor *actor = vtkActor::SafeDownCast(WidgetAccessor::getProp(*this)); + CV_Assert(actor); + + // Create the vtk image and set its parameters based on input image + vtkSmartPointer vtk_image = vtkSmartPointer::New(); + ConvertToVtkImage::convert(image, vtk_image); + + // Need to flip the image as the coordinates are different in OpenCV and VTK + vtkSmartPointer flipFilter = vtkSmartPointer::New(); + flipFilter->SetFilteredAxis(1); // Vertical flip + flipFilter->SetInputConnection(vtk_image->GetProducerPort()); + flipFilter->Update(); + + // Apply the texture + vtkSmartPointer texture = vtkSmartPointer::New(); + texture->SetInputConnection(flipFilter->GetOutputPort()); + + actor->SetTexture(texture); +} + +template<> cv::viz::Image3DWidget cv::viz::Widget::cast() +{ + Widget3D widget = this->cast(); + return static_cast(widget); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// camera position widget implementation + +cv::viz::CameraPositionWidget::CameraPositionWidget(double scale) +{ + vtkSmartPointer axes = vtkSmartPointer::New (); + axes->SetOrigin (0, 0, 0); + axes->SetScaleFactor (scale); + + vtkSmartPointer axes_colors = vtkSmartPointer::New (); + axes_colors->Allocate (6); + axes_colors->InsertNextValue (0.0); + axes_colors->InsertNextValue (0.0); + axes_colors->InsertNextValue (0.5); + axes_colors->InsertNextValue (0.5); + axes_colors->InsertNextValue (1.0); + axes_colors->InsertNextValue (1.0); + + vtkSmartPointer axes_data = axes->GetOutput (); + axes_data->Update (); + axes_data->GetPointData ()->SetScalars (axes_colors); + + vtkSmartPointer axes_tubes = vtkSmartPointer::New (); + axes_tubes->SetInput (axes_data); + axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0); + axes_tubes->SetNumberOfSides (6); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetScalarModeToUsePointData (); + mapper->SetInput(axes_tubes->GetOutput ()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + WidgetAccessor::setProp(*this, actor); +} + +cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, double scale, const Color &color) +{ + vtkSmartPointer camera = vtkSmartPointer::New(); + float f_x = K(0,0); + float f_y = K(1,1); + float c_y = K(1,2); + float aspect_ratio = f_y / f_x; + // Assuming that this is an ideal camera (c_y and c_x are at the center of the image) + float fovy = 2.0f * atan2(c_y,f_y) * 180 / CV_PI; + + 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); + + double planesArray[24]; + camera->GetFrustumPlanes(aspect_ratio, planesArray); + + vtkSmartPointer planes = vtkSmartPointer::New(); + planes->SetFrustumPlanes(planesArray); + + vtkSmartPointer frustumSource = + vtkSmartPointer::New(); + frustumSource->SetPlanes(planes); + frustumSource->Update(); + + vtkSmartPointer filter = vtkSmartPointer::New(); + filter->SetInput(frustumSource->GetOutput()); + filter->Update(); + + vtkSmartPointer mapper = vtkSmartPointer::New(); + mapper->SetInput(filter->GetOutput()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + WidgetAccessor::setProp(*this, actor); + setColor(color); +} + + +cv::viz::CameraPositionWidget::CameraPositionWidget(const Vec2f &fov, double scale, const Color &color) +{ + vtkSmartPointer camera = vtkSmartPointer::New(); + + camera->SetViewAngle(fov[1] * 180 / CV_PI); // Vertical field of view + 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); + + double aspect_ratio = tan(fov[0] * 0.5) / tan(fov[1] * 0.5); + + double planesArray[24]; + camera->GetFrustumPlanes(aspect_ratio, planesArray); + + vtkSmartPointer planes = vtkSmartPointer::New(); + planes->SetFrustumPlanes(planesArray); + + vtkSmartPointer frustumSource = + vtkSmartPointer::New(); + frustumSource->SetPlanes(planes); + frustumSource->Update(); + + // Extract the edges so we have the grid + vtkSmartPointer filter = vtkSmartPointer::New(); + filter->SetInput(frustumSource->GetOutput()); + filter->Update(); + + vtkSmartPointer mapper = vtkSmartPointer::New(); + mapper->SetInput(filter->GetOutput()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + WidgetAccessor::setProp(*this, actor); + setColor(color); +} + +cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, const Mat &image, double scale, const Color &color) +{ + CV_Assert(!image.empty() && image.depth() == CV_8U); + + // Create a camera + vtkSmartPointer camera = vtkSmartPointer::New(); + float f_x = K(0,0); + float f_y = K(1,1); + float c_y = K(1,2); + float aspect_ratio = float(image.cols)/float(image.rows); + // Assuming that this is an ideal camera (c_y and c_x are at the center of the image) + float fovy = 2.0f * atan2(c_y,f_y) * 180 / CV_PI; + float far_end_height = 2.0f * c_y * scale / f_y; + + // Create the vtk image + vtkSmartPointer vtk_image = vtkSmartPointer::New(); + ConvertToVtkImage::convert(image, vtk_image); + + // Adjust a pixel of the vtk_image + vtk_image->SetScalarComponentFromDouble(0, image.rows-1, 0, 0, color[2]); + vtk_image->SetScalarComponentFromDouble(0, image.rows-1, 0, 1, color[1]); + vtk_image->SetScalarComponentFromDouble(0, image.rows-1, 0, 2, color[0]); + + // Need to flip the image as the coordinates are different in OpenCV and VTK + vtkSmartPointer flipFilter = vtkSmartPointer::New(); + flipFilter->SetFilteredAxis(1); // Vertical flip + flipFilter->SetInputConnection(vtk_image->GetProducerPort()); + flipFilter->Update(); + + Vec3d plane_center(0.0, 0.0, scale); + + vtkSmartPointer plane = vtkSmartPointer::New(); + plane->SetCenter(plane_center[0], plane_center[1], plane_center[2]); + plane->SetNormal(0.0, 0.0, 1.0); + + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + transform->Translate(plane_center[0], plane_center[1], plane_center[2]); + transform->Scale(far_end_height*aspect_ratio, far_end_height, 1.0); + transform->RotateY(180.0); + transform->Translate(-plane_center[0], -plane_center[1], -plane_center[2]); + + // Apply the texture + vtkSmartPointer texture = vtkSmartPointer::New(); + texture->SetInputConnection(flipFilter->GetOutputPort()); + + vtkSmartPointer texturePlane = vtkSmartPointer::New(); + texturePlane->SetInputConnection(plane->GetOutputPort()); + + vtkSmartPointer transform_filter = vtkSmartPointer::New(); + transform_filter->SetTransform(transform); + transform_filter->SetInputConnection(texturePlane->GetOutputPort()); + transform_filter->Update(); + + // Create frustum + 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); + + double planesArray[24]; + camera->GetFrustumPlanes(aspect_ratio, planesArray); + + vtkSmartPointer planes = vtkSmartPointer::New(); + planes->SetFrustumPlanes(planesArray); + + vtkSmartPointer frustumSource = + vtkSmartPointer::New(); + frustumSource->SetPlanes(planes); + frustumSource->Update(); + + vtkSmartPointer filter = vtkSmartPointer::New(); + filter->SetInput(frustumSource->GetOutput()); + filter->Update(); + + // Frustum needs to be textured or else it can't be combined with image + vtkSmartPointer frustum_texture = vtkSmartPointer::New(); + frustum_texture->SetInputConnection(filter->GetOutputPort()); + // Texture mapping with only one pixel from the image to have constant color + frustum_texture->SetSRange(0.0, 0.0); + frustum_texture->SetTRange(0.0, 0.0); + + vtkSmartPointer appendFilter = vtkSmartPointer::New(); + appendFilter->AddInputConnection(frustum_texture->GetOutputPort()); + appendFilter->AddInputConnection(transform_filter->GetOutputPort()); + + vtkSmartPointer planeMapper = vtkSmartPointer::New(); + planeMapper->SetInputConnection(appendFilter->GetOutputPort()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(planeMapper); + actor->SetTexture(texture); + + WidgetAccessor::setProp(*this, actor); +} + +template<> cv::viz::CameraPositionWidget cv::viz::Widget::cast() +{ + Widget3D widget = this->cast(); + return static_cast(widget); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// trajectory widget implementation + +struct cv::viz::TrajectoryWidget::ApplyPath +{ + static void applyPath(vtkSmartPointer poly_data, vtkSmartPointer append_filter, const std::vector &path) + { + vtkIdType nr_points = path.size(); + + for (vtkIdType i = 0; i < nr_points; ++i) + { + vtkSmartPointer new_data = vtkSmartPointer::New(); + new_data->DeepCopy(poly_data); + + // Transform the default coordinate frame + vtkSmartPointer transform = vtkSmartPointer::New(); + transform->PreMultiply(); + vtkSmartPointer mat_trans = vtkSmartPointer::New(); + mat_trans = convertToVtkMatrix(path[i].matrix); + transform->SetMatrix(mat_trans); + + vtkSmartPointer filter = vtkSmartPointer::New(); + filter->SetInput(new_data); + filter->SetTransform(transform); + filter->Update(); + + append_filter->AddInputConnection(filter->GetOutputPort()); + } + } +}; + +cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector &path, int display_mode, const Color &color, double scale) +{ + vtkSmartPointer appendFilter = vtkSmartPointer::New(); + + // Bitwise and with 3 in order to limit the domain to 2 bits + if ((~display_mode & 3) ^ TrajectoryWidget::DISPLAY_PATH) + { + // Create a poly line along the path + vtkIdType nr_points = path.size(); + + vtkSmartPointer points = vtkSmartPointer::New (); + vtkSmartPointer polyData = vtkSmartPointer::New (); + vtkSmartPointer polyLine = vtkSmartPointer::New (); + + points->SetDataTypeToFloat(); + points->SetNumberOfPoints(nr_points); + polyLine->GetPointIds()->SetNumberOfIds(nr_points); + + Vec3f *data_beg = vtkpoints_data(points); + + for (vtkIdType i = 0; i < nr_points; ++i) + { + Vec3f cam_pose = path[i].translation(); + *data_beg++ = cam_pose; + polyLine->GetPointIds()->SetId(i,i); + } + + vtkSmartPointer cells = vtkSmartPointer::New(); + cells->InsertNextCell(polyLine); + + polyData->SetPoints(points); + polyData->SetLines(cells); + + // Set the color for polyData + vtkSmartPointer colors = vtkSmartPointer::New(); + colors->SetNumberOfComponents(3); + colors->SetNumberOfTuples(nr_points); + colors->FillComponent(0, color[2]); + colors->FillComponent(1, color[1]); + colors->FillComponent(2, color[0]); + + polyData->GetPointData()->SetScalars(colors); + appendFilter->AddInputConnection(polyData->GetProducerPort()); + } + + if ((~display_mode & 3) ^ TrajectoryWidget::DISPLAY_FRAMES) + { + // Create frames and transform along the path + vtkSmartPointer axes = vtkSmartPointer::New(); + axes->SetOrigin (0, 0, 0); + axes->SetScaleFactor (scale); + + vtkSmartPointer axes_colors = vtkSmartPointer::New (); + axes_colors->SetNumberOfComponents(3); + axes_colors->InsertNextTuple3(255,0,0); + axes_colors->InsertNextTuple3(255,0,0); + axes_colors->InsertNextTuple3(0,255,0); + axes_colors->InsertNextTuple3(0,255,0); + axes_colors->InsertNextTuple3(0,0,255); + axes_colors->InsertNextTuple3(0,0,255); + + vtkSmartPointer axes_data = axes->GetOutput (); + axes_data->Update (); + axes_data->GetPointData ()->SetScalars (axes_colors); + + vtkSmartPointer axes_tubes = vtkSmartPointer::New (); + axes_tubes->SetInput (axes_data); + axes_tubes->SetRadius (axes->GetScaleFactor() / 50.0); + axes_tubes->SetNumberOfSides (6); + axes_tubes->Update(); + + ApplyPath::applyPath(axes_tubes->GetOutput(), appendFilter, path); + } + + vtkSmartPointer mapper = vtkSmartPointer::New(); + mapper->SetScalarModeToUsePointData (); + mapper->SetInput(appendFilter->GetOutput()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + 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(); + float f_x = K(0,0); + float f_y = K(1,1); + float c_y = K(1,2); + float aspect_ratio = f_y / f_x; + // Assuming that this is an ideal camera (c_y and c_x are at the center of the image) + float fovy = 2.0f * atan2(c_y,f_y) * 180 / CV_PI; + + 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); + + double planesArray[24]; + camera->GetFrustumPlanes(aspect_ratio, planesArray); + + vtkSmartPointer planes = vtkSmartPointer::New(); + planes->SetFrustumPlanes(planesArray); + + vtkSmartPointer frustumSource = vtkSmartPointer::New(); + frustumSource->SetPlanes(planes); + frustumSource->Update(); + + // Extract the edges + vtkSmartPointer filter = vtkSmartPointer::New(); + filter->SetInput(frustumSource->GetOutput()); + filter->Update(); + + vtkSmartPointer appendFilter = vtkSmartPointer::New(); + ApplyPath::applyPath(filter->GetOutput(), appendFilter, path); + + vtkSmartPointer mapper = vtkSmartPointer::New(); + mapper->SetInput(appendFilter->GetOutput()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + WidgetAccessor::setProp(*this, actor); + setColor(color); +} + +cv::viz::TrajectoryWidget::TrajectoryWidget(const std::vector &path, const Vec2f &fov, double scale, const Color &color) +{ + vtkSmartPointer camera = vtkSmartPointer::New(); + + camera->SetViewAngle(fov[1] * 180 / CV_PI); // Vertical field of view + 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); + + double aspect_ratio = tan(fov[0] * 0.5) / tan(fov[1] * 0.5); + + double planesArray[24]; + camera->GetFrustumPlanes(aspect_ratio, planesArray); + + vtkSmartPointer planes = vtkSmartPointer::New(); + planes->SetFrustumPlanes(planesArray); + + vtkSmartPointer frustumSource = vtkSmartPointer::New(); + frustumSource->SetPlanes(planes); + frustumSource->Update(); + + // Extract the edges + vtkSmartPointer filter = vtkSmartPointer::New(); + filter->SetInput(frustumSource->GetOutput()); + filter->Update(); + + vtkSmartPointer appendFilter = vtkSmartPointer::New(); + ApplyPath::applyPath(filter->GetOutput(), appendFilter, path); + + vtkSmartPointer mapper = vtkSmartPointer::New(); + mapper->SetInput(appendFilter->GetOutput()); + + vtkSmartPointer actor = vtkSmartPointer::New(); + actor->SetMapper(mapper); + + WidgetAccessor::setProp(*this, actor); + setColor(color); +} + +template<> cv::viz::TrajectoryWidget cv::viz::Widget::cast() +{ + Widget3D widget = this->cast(); + return static_cast(widget); +} \ No newline at end of file diff --git a/modules/viz/src/types.cpp b/modules/viz/src/types.cpp index 7cb25a0368..871c2dbcb6 100644 --- a/modules/viz/src/types.cpp +++ b/modules/viz/src/types.cpp @@ -111,7 +111,7 @@ struct cv::viz::Mesh3d::loadMeshImpl Vec3b point_color; poly_colors->GetTupleValue (i, point_color.val); - //RGB or BGR? should we swap channels???? + std::swap(point_color[0], point_color[2]); // RGB -> BGR mesh_colors[i] = point_color; } } diff --git a/modules/viz/src/viz.cpp b/modules/viz/src/viz.cpp index 1d53b5d25a..94e4be53d5 100644 --- a/modules/viz/src/viz.cpp +++ b/modules/viz/src/viz.cpp @@ -19,6 +19,32 @@ cv::Affine3f cv::viz::makeTransformToGlobal(const Vec3f& axis_x, const Vec3f& ax return Affine3f(R, origin); } +cv::Affine3f cv::viz::makeCameraPose(const Vec3f& position, const Vec3f& focal_point, const Vec3f& y_dir) +{ + // Compute the transformation matrix for drawing the camera frame in a scene + Vec3f u,v,n; + n = normalize(focal_point - position); + u = normalize(y_dir.cross(n)); + v = n.cross(u); + + Matx44f pose_mat; + pose_mat.zeros(); + pose_mat(0,0) = u[0]; + pose_mat(0,1) = u[1]; + pose_mat(0,2) = u[2]; + pose_mat(1,0) = v[0]; + pose_mat(1,1) = v[1]; + pose_mat(1,2) = v[2]; + pose_mat(2,0) = n[0]; + pose_mat(2,1) = n[1]; + pose_mat(2,2) = n[2]; + pose_mat(3,0) = position[0]; + pose_mat(3,1) = position[1]; + pose_mat(3,2) = position[2]; + pose_mat(3,3) = 1.0f; + pose_mat = pose_mat.t(); + return pose_mat; +} vtkSmartPointer cv::viz::convertToVtkMatrix (const cv::Matx44f &m) { diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 0220d9c652..ca4b75bbf7 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -847,7 +847,7 @@ bool cv::viz::Viz3d::VizImpl::addModelFromPLYFile (const std::string &filename, return (true); } -bool cv::viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& mesh, const std::string &id) +bool cv::viz::Viz3d::VizImpl::addPolylineFromPolygonMesh (const Mesh3d& /*mesh*/, const std::string &/*id*/) { // CV_Assert(mesh.cloud.rows == 1 && mesh.cloud.type() == CV_32FC3); // @@ -1017,7 +1017,7 @@ void cv::viz::Viz3d::VizImpl::setWindowName (const std::string &name) void cv::viz::Viz3d::VizImpl::setWindowPosition (int x, int y) { window_->SetPosition (x, y); } void cv::viz::Viz3d::VizImpl::setWindowSize (int xw, int yw) { window_->SetSize (xw, yw); } -bool cv::viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) +bool cv::viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& /*mesh*/, const Mat& /*mask*/, const std::string &/*id*/) { // CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ()); // CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3)); @@ -1205,7 +1205,7 @@ return true; } -bool cv::viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& mesh, const cv::Mat& mask, const std::string &id) +bool cv::viz::Viz3d::VizImpl::updatePolygonMesh (const Mesh3d& /*mesh*/, const cv::Mat& /*mask*/, const std::string &/*id*/) { // CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ()); // CV_Assert(mesh.colors.empty() || (!mesh.colors.empty() && mesh.colors.size() == mesh.cloud.size() && mesh.colors.type() == CV_8UC3)); diff --git a/modules/viz/src/viz3d_impl.hpp b/modules/viz/src/viz3d_impl.hpp index f3f245c93a..c6bef09885 100644 --- a/modules/viz/src/viz3d_impl.hpp +++ b/modules/viz/src/viz3d_impl.hpp @@ -327,6 +327,31 @@ namespace cv } return output; } + + static _Out* copyColor(const Mat& source, _Out* output, const Mat& nan_mask) + { + CV_Assert(DataDepth<_Tp>::value == source.depth() && source.size() == nan_mask.size()); + CV_Assert(nan_mask.channels() == 3 || nan_mask.channels() == 4); + CV_DbgAssert(DataDepth<_Msk>::value == nan_mask.depth()); + + int s_chs = source.channels(); + int m_chs = nan_mask.channels(); + + for(int y = 0; y < source.rows; ++y) + { + const _Tp* srow = source.ptr<_Tp>(y); + const _Msk* mrow = nan_mask.ptr<_Msk>(y); + + for(int x = 0; x < source.cols; ++x, srow += s_chs, mrow += m_chs) + if (!isNan(mrow[0]) && !isNan(mrow[1]) && !isNan(mrow[2])) + { + *output = _Out(srow); + std::swap((*output)[0], (*output)[2]); // BGR -> RGB + ++output; + } + } + return output; + } }; template @@ -339,6 +364,17 @@ namespace cv return table[nan_mask.depth() - 5](source, output, nan_mask); } + + template + static inline Vec<_Tp, 3>* copyColor(const Mat& source, Vec<_Tp, 3>* output, const Mat& nan_mask) + { + CV_Assert(nan_mask.depth() == CV_32F || nan_mask.depth() == CV_64F); + + typedef Vec<_Tp, 3>* (*copy_func)(const Mat&, Vec<_Tp, 3>*, const Mat&); + const static copy_func table[2] = { &NanFilter::Impl<_Tp, float>::copyColor, &NanFilter::Impl<_Tp, double>::copyColor }; + + return table[nan_mask.depth() - 5](source, output, nan_mask); + } }; struct ApplyAffine @@ -374,6 +410,63 @@ namespace cv 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); } + + struct ConvertToVtkImage + { + struct Impl + { + static void copyImageMultiChannel(const Mat &image, vtkSmartPointer output) + { + int i_chs = image.channels(); + + for (int i = 0; i < image.rows; ++i) + { + const unsigned char * irows = image.ptr(i); + for (int j = 0; j < image.cols; ++j, irows += i_chs) + { + unsigned char * vrows = static_cast(output->GetScalarPointer(j,i,0)); + memcpy(vrows, irows, i_chs); + std::swap(vrows[0], vrows[2]); // BGR -> RGB + } + } + output->Modified(); + } + + static void copyImageSingleChannel(const Mat &image, vtkSmartPointer output) + { + for (int i = 0; i < image.rows; ++i) + { + const unsigned char * irows = image.ptr(i); + for (int j = 0; j < image.cols; ++j, ++irows) + { + unsigned char * vrows = static_cast(output->GetScalarPointer(j,i,0)); + *vrows = *irows; + } + } + output->Modified(); + } + }; + + static void convert(const Mat &image, vtkSmartPointer output) + { + // Create the vtk image + output->SetDimensions(image.cols, image.rows, 1); + output->SetNumberOfScalarComponents(image.channels()); + output->SetScalarTypeToUnsignedChar(); + output->AllocateScalars(); + + int i_chs = image.channels(); + if (i_chs > 1) + { + // Multi channel images are handled differently because of BGR <-> RGB + Impl::copyImageMultiChannel(image, output); + } + else + { + Impl::copyImageSingleChannel(image, output); + } + } + }; } } diff --git a/modules/viz/src/widget.cpp b/modules/viz/src/widget.cpp index 8f9e90404e..b295843f98 100644 --- a/modules/viz/src/widget.cpp +++ b/modules/viz/src/widget.cpp @@ -142,10 +142,6 @@ void cv::viz::Widget3D::setColor(const Color &color) actor->GetMapper ()->ScalarVisibilityOff (); actor->GetProperty ()->SetColor (c.val); actor->GetProperty ()->SetEdgeColor (c.val); - actor->GetProperty ()->SetAmbient (0.8); - actor->GetProperty ()->SetDiffuse (0.8); - actor->GetProperty ()->SetSpecular (0.8); - actor->GetProperty ()->SetLighting (0); actor->Modified (); } diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index f2ba913d00..66f811884d 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -88,9 +89,11 @@ TEST(Viz_viz3d, accuracy) viz::Color color = viz::Color::black(); viz::LineWidget lw(Point3f(0, 0, 0), Point3f(4.f, 4.f,4.f), viz::Color::green()); - viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0), 5.0); - viz::SphereWidget sw(Point3f(0, 0, 0), 0.5); - viz::ArrowWidget aw(Point3f(0, 0, 0), Point3f(1, 1, 1), viz::Color::red()); + viz::PlaneWidget pw(Vec4f(0.0,1.0,2.0,3.0)); + viz::PlaneWidget pw2(Vec4f(0.0,1.0,2.0,3.0), 2.0, viz::Color::red()); + viz::PlaneWidget pw3(Vec4f(0.0,1.0,2.0,3.0), 3.0, viz::Color::blue()); + viz::SphereWidget sw(Point3f(0, 0, 0), 0.2); + viz::ArrowWidget aw(Point3f(0, 0, 0), Point3f(1, 1, 1), 0.01, viz::Color::red()); viz::CircleWidget cw(Point3f(0, 0, 0), 0.5, 0.01, viz::Color::green()); viz::CylinderWidget cyw(Point3f(0, 0, 0), Point3f(-1, -1, -1), 0.5, 30, viz::Color::green()); viz::CubeWidget cuw(Point3f(-2, -2, -2), Point3f(-1, -1, -1)); @@ -99,18 +102,20 @@ TEST(Viz_viz3d, accuracy) viz::CloudWidget pcw(cloud, colors); viz::CloudWidget pcw2(cloud, viz::Color::magenta()); - viz.showWidget("line", lw); +// viz.showWidget("line", lw); viz.showWidget("plane", pw); - viz.showWidget("sphere", sw); - viz.showWidget("arrow", aw); - viz.showWidget("circle", cw); - viz.showWidget("cylinder", cyw); - viz.showWidget("cube", cuw); + viz.showWidget("plane2", pw2); + viz.showWidget("plane3", pw3); +// viz.showWidget("sphere", sw); +// viz.showWidget("arrow", aw); +// viz.showWidget("circle", cw); +// viz.showWidget("cylinder", cyw); +// viz.showWidget("cube", cuw); viz.showWidget("coordinateSystem", csw); - viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0))); - viz.showWidget("text",tw); - viz.showWidget("pcw",pcw); - viz.showWidget("pcw2",pcw2); +// viz.showWidget("coordinateSystem2", viz::CoordinateSystemWidget(2.0), Affine3f().translate(Vec3f(2, 0, 0))); +// viz.showWidget("text",tw); +// viz.showWidget("pcw",pcw); +// viz.showWidget("pcw2",pcw2); // viz::LineWidget lw2 = lw; // v.showPointCloud("cld",cloud, colors); @@ -125,13 +130,55 @@ TEST(Viz_viz3d, accuracy) viz::PolyLineWidget plw(points, viz::Color::green()); - viz.showWidget("polyline", plw); +// viz.showWidget("polyline", plw); // lw = v.getWidget("polyline").cast(); - viz::Mesh3d::Ptr mesh = cv::viz::Mesh3d::loadMesh("horse.ply"); + viz::Mesh3d mesh = cv::viz::Mesh3d::loadMesh("horse.ply"); - viz::MeshWidget mw(*mesh); - viz.showWidget("mesh", mw); + viz::MeshWidget mw(mesh); +// viz.showWidget("mesh", mw); + + Mat img = imread("opencv.png"); +// resize(img, img, Size(50,50)); +// viz.showWidget("img", viz::ImageOverlayWidget(img, Point2i(50,50))); + + Matx33f K(657, 0, 320, + 0, 657, 240, + 0, 0, 1); + + viz::CameraPositionWidget cpw(Vec3f(0.5, 0.5, 3.0), Vec3f(0.0,0.0,0.0), Vec3f(0.0,-1.0,0.0), 0.5); + viz::CameraPositionWidget cpw2(0.5); + viz::CameraPositionWidget frustum(K, 2.0, viz::Color::green()); +// viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red()); + viz::CameraPositionWidget frustum2(K, 4.0, viz::Color::red()); + viz::CameraPositionWidget frustum3(Vec2f(CV_PI, CV_PI/2), 4.0); + viz::Text3DWidget t3w1("Camera1", Point3f(0.4, 0.6, 3.0), 0.1); + viz::Text3DWidget t3w2("Camera2", Point3f(0,0,0), 0.1); + +// viz.showWidget("CameraPositionWidget", cpw); +// viz.showWidget("CameraPositionWidget2", cpw2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5))); +// viz.showWidget("camera_label", t3w1); +// viz.showWidget("camera_label2", t3w2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5))); +// viz.showWidget("frustrum", frustum, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5))); +// viz.showWidget("frustrum2", frustum2, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5))); +// viz.showWidget("frustum3", frustum3, Affine3f(0.524, 0, 0, Vec3f(-1.0, 0.5, 0.5))); + + std::vector trajectory; + + trajectory.push_back(Affine3f().translate(Vec3f(0.5,0.5,0.5))); + trajectory.push_back(Affine3f().translate(Vec3f(1.0,0.0,0.0))); + trajectory.push_back(Affine3f().translate(Vec3f(2.0,0.5,0.0))); + trajectory.push_back(Affine3f(0.5, 0.0, 0.0, Vec3f(1.0,0.0,1.0))); +// + viz.showWidget("trajectory1", viz::TrajectoryWidget(trajectory, viz::Color(0,255,255), true, 0.5)); + viz.showWidget("trajectory2", viz::TrajectoryWidget(trajectory, K, 1.0, viz::Color(255,0,255))); + + + +// viz.showWidget("trajectory1", viz::TrajectoryWidget(trajectory/*, viz::Color::yellow()*/)); + +// viz.showWidget("CameraPositionWidget2", cpw2); +// viz.showWidget("CameraPositionWidget3", cpw3); viz.spin(); @@ -156,13 +203,16 @@ TEST(Viz_viz3d, accuracy) //plw.setColor(viz::Color(col_blue, col_green, col_red)); - sw.setPose(cloudPosition); +// sw.setPose(cloudPosition); // pw.setPose(cloudPosition); aw.setPose(cloudPosition); cw.setPose(cloudPosition); cyw.setPose(cloudPosition); + + frustum.setPose(cloudPosition); // lw.setPose(cloudPosition); - cuw.setPose(cloudPosition); +// cpw.updatePose(Affine3f(0.1,0.0,0.0, cv::Vec3f(0.0,0.0,0.0))); +// cpw.setPose(cloudPosition); // cnw.setPose(cloudPosition); // v.showWidget("pcw",pcw, cloudPosition); // v.showWidget("pcw2",pcw2, cloudPosition2);