circle widget and arrow widget implementation

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

@@ -56,5 +56,16 @@ namespace temp_viz
         SphereWidget(const cv::Point3f &center, float radius, int sphere_resolution = 10, const Color &color = Color::white());
     };
     
+    class CV_EXPORTS ArrowWidget : public Widget
+    {
+    public:
+        ArrowWidget(const Point3f& pt1, const Point3f& pt2, const Color &color = Color::white());
+    };
+
+    class CV_EXPORTS CircleWidget : public Widget
+    {
+    public:
+        CircleWidget(const Point3f& pt, double radius, const Color &color = Color::white());
+    };
 
 }

modules/viz/src/simple_widgets.cpp

@@ -90,3 +90,98 @@ temp_viz::SphereWidget::SphereWidget(const cv::Point3f &center, float radius, in
 
     setColor(color);
 }
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// arrow widget implementation
+
+temp_viz::ArrowWidget::ArrowWidget(const Point3f& pt1, const Point3f& pt2, const Color &color)
+{
+    vtkSmartPointer<vtkArrowSource> arrowSource = vtkSmartPointer<vtkArrowSource>::New ();
+    
+    float startPoint[3], endPoint[3];
+    startPoint[0] = pt1.x;
+    startPoint[1] = pt1.y;
+    startPoint[2] = pt1.z;
+    endPoint[0] = pt2.x;
+    endPoint[1] = pt2.y;
+    endPoint[2] = pt2.z;
+    float normalizedX[3], normalizedY[3], normalizedZ[3];
+    
+    // The X axis is a vector from start to end
+    vtkMath::Subtract(endPoint, startPoint, normalizedX);
+    float length = vtkMath::Norm(normalizedX);
+    vtkMath::Normalize(normalizedX);
+
+    // The Z axis is an arbitrary vecotr cross X
+    float arbitrary[3];
+    arbitrary[0] = vtkMath::Random(-10,10);
+    arbitrary[1] = vtkMath::Random(-10,10);
+    arbitrary[2] = vtkMath::Random(-10,10);
+    vtkMath::Cross(normalizedX, arbitrary, normalizedZ);
+    vtkMath::Normalize(normalizedZ);
+
+    // The Y axis is Z cross X
+    vtkMath::Cross(normalizedZ, normalizedX, normalizedY);
+    vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New();
+
+    // Create the direction cosine matrix
+    matrix->Identity();
+    for (unsigned int i = 0; i < 3; i++)
+    {
+        matrix->SetElement(i, 0, normalizedX[i]);
+        matrix->SetElement(i, 1, normalizedY[i]);
+        matrix->SetElement(i, 2, normalizedZ[i]);
+    }    
+
+    // Apply the transforms
+    vtkSmartPointer<vtkTransform> transform = 
+    vtkSmartPointer<vtkTransform>::New();
+    transform->Translate(startPoint);
+    transform->Concatenate(matrix);
+    transform->Scale(length, length, length);
+
+    // Transform the polydata
+    vtkSmartPointer<vtkTransformPolyDataFilter> transformPD = 
+    vtkSmartPointer<vtkTransformPolyDataFilter>::New();
+    transformPD->SetTransform(transform);
+    transformPD->SetInputConnection(arrowSource->GetOutputPort());
+    
+    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+    mapper->SetInput(transformPD->GetOutput ());
+
+    vtkSmartPointer<vtkLODActor> actor = WidgetAccessor::getActor(*this);
+    actor->SetMapper(mapper);
+    
+    setColor(color);
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////
+/// circle widget implementation
+
+temp_viz::CircleWidget::CircleWidget(const Point3f& pt, double radius, const Color &color)
+{
+    vtkSmartPointer<vtkDiskSource> disk = vtkSmartPointer<vtkDiskSource>::New ();
+    // Maybe the resolution should be lower e.g. 50 or 25
+    disk->SetCircumferentialResolution (100);
+    disk->SetInnerRadius (radius - 0.001);
+    disk->SetOuterRadius (radius + 0.001);
+    disk->SetCircumferentialResolution (20);
+
+    // Set the circle origin
+    vtkSmartPointer<vtkTransform> t = vtkSmartPointer<vtkTransform>::New ();
+    t->Identity ();
+    t->Translate (pt.x, pt.y, pt.z);
+
+    vtkSmartPointer<vtkTransformPolyDataFilter> tf = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
+    tf->SetTransform (t);
+    tf->SetInputConnection (disk->GetOutputPort ());
+    
+    vtkSmartPointer<vtkDataSetMapper> mapper = vtkSmartPointer<vtkDataSetMapper>::New ();
+    mapper->SetInput(tf->GetOutput ());
+
+    vtkSmartPointer<vtkLODActor> actor = WidgetAccessor::getActor(*this);
+    actor->SetMapper(mapper);
+    
+    setColor(color);
+}
+

modules/viz/test/test_viz3d.cpp

@@ -91,15 +91,18 @@ 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));
-    v.showArrow("arrow1", cv::Point3f(0,0,0), cv::Point3f(1,1,1), temp_viz::Color(255,0,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(1.0,1.0,1.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);
+    temp_viz::ArrowWidget aw(cv::Point3f(0,0,0), cv::Point3f(1,1,1), temp_viz::Color(255,0,0));
+    temp_viz::CircleWidget cw(cv::Point3f(0,0,0), 1.0, temp_viz::Color(0,255,0));
     
     v.showWidget("line", lw);
     v.showWidget("plane", pw);
     v.showWidget("sphere", sw);
+    v.showWidget("arrow", aw);
+    v.showWidget("circle", cw);
     
     temp_viz::LineWidget lw2 = lw;
     
@@ -115,8 +118,7 @@ TEST(Viz_viz3d, accuracy)
 //         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);
-        v.setShapePose("arrow1", cloudPosition);
+//         v.setShapePose("circle1", cloudPosition);
         lw2.setColor(temp_viz::Color(col_blue, col_green, col_red));
         lw.setLineWidth(lw.getLineWidth()+pos_x * 10);
         
@@ -124,6 +126,8 @@ TEST(Viz_viz3d, accuracy)
         
         sw.setPose(cloudPosition);
         pw.setPose(cloudPosition);
+        aw.setPose(cloudPosition);
+        cw.setPose(cloudPosition);
         
         angle_x += 0.1f;
         angle_y -= 0.1f;