diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index 1ca792539..735b9d2b9 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -174,6 +174,10 @@ namespace cv CameraPositionWidget(const Vec3f &position, const Vec3f &look_at, const Vec3f &up_vector, 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); + + private: + struct CopyImpl; }; class CV_EXPORTS TrajectoryWidget : public Widget3D diff --git a/modules/viz/src/shape_widgets.cpp b/modules/viz/src/shape_widgets.cpp index 737b6928f..47e20d360 100644 --- a/modules/viz/src/shape_widgets.cpp +++ b/modules/viz/src/shape_widgets.cpp @@ -918,6 +918,58 @@ template<> cv::viz::Image3DWidget cv::viz::Widget::cast( /////////////////////////////////////////////////////////////////////////////////////////////// /// camera position widget implementation +struct cv::viz::CameraPositionWidget::CopyImpl +{ + 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 copyImage(const Mat &image, vtkSmartPointer output) + { + 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); + } + } +}; + + cv::viz::CameraPositionWidget::CameraPositionWidget(double scale) { vtkSmartPointer axes = vtkSmartPointer::New (); @@ -1099,6 +1151,103 @@ cv::viz::CameraPositionWidget::CameraPositionWidget(const Vec2f &fov, double sca setColor(color); } +cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, const Mat &image, double scale) +{ + 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 and set its parameters based on input image + vtkSmartPointer vtk_image = vtkSmartPointer::New(); + vtk_image->SetDimensions(image.cols, image.rows, 1); + vtk_image->SetNumberOfScalarComponents(image.channels()); + vtk_image->SetScalarTypeToUnsignedChar(); + vtk_image->AllocateScalars(); + + CopyImpl::copyImage(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(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); +} + /////////////////////////////////////////////////////////////////////////////////////////////// /// trajectory widget implementation