coordinate system widget implementation, update pose fix

modules/viz/include/opencv2/viz/widgets.hpp

@@ -80,4 +80,10 @@ namespace temp_viz
         CubeWidget(const Point3f& pt_min, const Point3f& pt_max, const Color &color = Color::white());
     };
     
+    class CV_EXPORTS CoordinateSystemWidget : public Widget
+    {
+    public:
+        CoordinateSystemWidget(double scale, const Affine3f& affine);
+    };
+
 }

modules/viz/src/simple_widgets.cpp

@@ -1,4 +1,5 @@
 #include "precomp.hpp"
+#include <opencv2/calib3d.hpp>
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
 /// line widget implementation
@@ -225,3 +226,52 @@ temp_viz::CubeWidget::CubeWidget(const Point3f& pt_min, const Point3f& pt_max, c
     
     setColor(color);
 }
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// coordinate system widget implementation
+
+temp_viz::CoordinateSystemWidget::CoordinateSystemWidget(double scale, const Affine3f& affine)
+{
+    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 = WidgetAccessor::getActor(*this);
+    actor->SetMapper(mapper);
+
+    cv::Vec3d t = affine.translation();
+    actor->SetPosition (t[0], t[1], t[2]);
+
+    cv::Matx33f m = affine.rotation();
+
+    cv::Vec3f rvec;
+    cv::Rodrigues(m, rvec);
+
+    float r_angle = cv::norm(rvec);
+    rvec *= 1.f/r_angle;
+
+    actor->SetOrientation(0,0,0);
+    actor->RotateWXYZ(r_angle*180/CV_PI,rvec[0], rvec[1], rvec[2]);
+}

modules/viz/src/widget.cpp

@@ -31,6 +31,11 @@ public:
     void updatePose(const Affine3f& pose)
     {
         vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
+        if (!matrix)
+        {
+            setPose(pose);
+            return ;
+        }
         Matx44f matrix_cv = convertToMatx(matrix);
 
         Affine3f updated_pose = pose * Affine3f(matrix_cv);

modules/viz/test/test_viz3d.cpp

@@ -73,8 +73,6 @@ TEST(Viz_viz3d, accuracy)
 
     v.setBackgroundColor();
 
-    v.addCoordinateSystem(1.0, cv::Affine3f::Identity());
-
     cv::Mat cloud = cvcloud_load();
 
     cv::Mat colors(cloud.size(), CV_8UC3, cv::Scalar(0, 255, 0));
@@ -91,7 +89,7 @@ TEST(Viz_viz3d, accuracy)
     int col_blue = 0;
     int col_green = 0;
     int col_red = 0;
-//     v.showCircle("circle1", cv::Point3f(0,0,0), 1.0, temp_viz::Color(0,255,0));
+
     temp_viz::LineWidget lw(cv::Point3f(0.0,0.0,0.0), cv::Point3f(4.0,4.0,4.0), temp_viz::Color(0,255,0));
     temp_viz::PlaneWidget pw(cv::Vec4f(0.0,1.0,2.0,3.0));
     temp_viz::SphereWidget sw(cv::Point3f(0,0,0), 0.5);
@@ -99,6 +97,7 @@ TEST(Viz_viz3d, accuracy)
     temp_viz::CircleWidget cw(cv::Point3f(0,0,0), 1.0, temp_viz::Color(0,255,0));
     temp_viz::CylinderWidget cyw(cv::Point3f(0,0,0), cv::Point3f(-1,-1,-1), 0.5, 30, temp_viz::Color(0,255,0));
     temp_viz::CubeWidget cuw(cv::Point3f(-2,-2,-2), cv::Point3f(-1,-1,-1), temp_viz::Color(0,0,255));
+    temp_viz::CoordinateSystemWidget csw(1.0f, cv::Affine3f::Identity());
     
     v.showWidget("line", lw);
     v.showWidget("plane", pw);
@@ -107,6 +106,7 @@ TEST(Viz_viz3d, accuracy)
     v.showWidget("circle", cw);
     v.showWidget("cylinder", cyw);
     v.showWidget("cube", cuw);
+    v.showWidget("coordinateSystem", csw);
     
     temp_viz::LineWidget lw2 = lw;
     
@@ -114,15 +114,7 @@ TEST(Viz_viz3d, accuracy)
     {
         // Creating new point cloud with id cloud1
         cv::Affine3f cloudPosition(angle_x, angle_y, angle_z, cv::Vec3f(pos_x, pos_y, pos_z));
-//         v.showPointCloud("cloud1", cloud, temp_viz::Color(col_blue, col_green, col_red), cloudPosition);
-//         v.showLine("line1", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
-//         v.showLine("line2", cv::Point3f(0.0,0.0,0.0), cv::Point3f(1.0f-pos_x, pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
-//         v.showLine("line3", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, 1.0f-pos_y, pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
-//         v.showLine("line4", cv::Point3f(0.0,0.0,0.0), cv::Point3f(pos_x, pos_y, 1.0f-pos_z) , temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
-//         v.showPlane("plane1", cv::Vec4f(pos_x*pos_y,pos_y,pos_z,pos_x+pos_y*pos_z), temp_viz::Color(255-col_blue, 255-col_green, 255-col_red));
-//         v.showCube("cube1", cv::Point3f(pos_x, pos_y, pos_z), cv::Point3f(pos_x+0.5, pos_y+0.5, pos_z+0.5), temp_viz::Color(255,150,50));
-//         v.showCylinder("cylinder1", cv::Point3f(0,0,0), cv::Point3f(pos_x, 1.0, 1.0), 0.5, 5*pos_x+3, temp_viz::Color(0,255,0));
-//         v.setShapePose("circle1", cloudPosition);
+
         lw2.setColor(temp_viz::Color(col_blue, col_green, col_red));
         lw.setLineWidth(lw.getLineWidth()+pos_x * 10);