///////////////////////////////////////////////////////////////////////////////////////////////
/// camera position widget implementation
+
+cv::viz::CameraPositionWidget::CameraPositionWidget(double scale)
+{
+ vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New ();
+ axes->SetOrigin (0, 0, 0);
+ axes->SetScaleFactor (scale);
+
+ vtkSmartPointer<vtkFloatArray> axes_colors = vtkSmartPointer<vtkFloatArray>::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<vtkPolyData> axes_data = axes->GetOutput ();
+ axes_data->Update ();
+ axes_data->GetPointData ()->SetScalars (axes_colors);
+
+ vtkSmartPointer<vtkTubeFilter> axes_tubes = vtkSmartPointer<vtkTubeFilter>::New ();
+ axes_tubes->SetInput (axes_data);
+ axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0);
+ axes_tubes->SetNumberOfSides (6);
+
+ vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+ mapper->SetScalarModeToUsePointData ();
+ mapper->SetInput(axes_tubes->GetOutput ());
+
+ vtkSmartPointer<vtkLODActor> actor = vtkSmartPointer<vtkLODActor>::New();
+ actor->SetMapper(mapper);
+
+ WidgetAccessor::setProp(*this, actor);
+}
+
cv::viz::CameraPositionWidget::CameraPositionWidget(const Vec3f &position, const Vec3f &look_at, const Vec3f &up_vector, double scale)
{
vtkSmartPointer<vtkAxes> axes = vtkSmartPointer<vtkAxes>::New ();
actor->SetMapper(mapper);
WidgetAccessor::setProp(*this, actor);
-}
\ No newline at end of file
+}
+
+cv::viz::CameraPositionWidget::CameraPositionWidget(const Matx33f &K, const Color &color)
+{
+ vtkSmartPointer<vtkCamera> camera = vtkSmartPointer<vtkCamera>::New();
+ float focal_length = K(0,0);
+ float c_x = K(0,2);
+ float c_y = K(1,2);
+ float aspect_ratio = c_x / c_y;
+ float img_width = c_x * 2;
+ float img_height = c_y * 2;
+ // 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,focal_length) * 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);
+
+ double planesArray[24];
+ camera->GetFrustumPlanes(aspect_ratio, planesArray);
+
+ vtkSmartPointer<vtkPlanes> planes = vtkSmartPointer<vtkPlanes>::New();
+ planes->SetFrustumPlanes(planesArray);
+
+ vtkSmartPointer<vtkFrustumSource> frustumSource =
+ vtkSmartPointer<vtkFrustumSource>::New();
+ frustumSource->SetPlanes(planes);
+ frustumSource->Update();
+
+ // Extract the edges so we have the grid
+ vtkSmartPointer<vtkExtractEdges> filter = vtkSmartPointer<vtkExtractEdges>::New();
+ filter->SetInput(frustumSource->GetOutput());
+ filter->Update();
+
+ vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+ mapper->SetInput(filter->GetOutput());
+
+ vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
+ actor->SetMapper(mapper);
+
+ WidgetAccessor::setProp(*this, actor);
+ setColor(color);
+}
// 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, viz::Color::green());
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("CameraPositionWidget2", cpw2);
// viz.showWidget("CameraPositionWidget3", cpw3);
aw.setPose(cloudPosition);
cw.setPose(cloudPosition);
cyw.setPose(cloudPosition);
+
+ frustum.setPose(cloudPosition);
// lw.setPose(cloudPosition);
// cpw.updatePose(Affine3f(0.1,0.0,0.0, cv::Vec3f(0.0,0.0,0.0)));
// cpw.setPose(cloudPosition);