createArrow and showArrow implementation without label

modules/viz/include/opencv2/viz/viz3d.hpp

@@ -39,6 +39,7 @@ namespace temp_viz
         void showCylinder(const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color = Color(255,255,255));
         void showCircle(const String &id, const Point3f &pt, double radius, const Color &color = Color(255,255,255));
         void showSphere (const String &id, const Point3f &pt, double radius, const Color &color = Color(255,255,255));
+        void showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255));
         
         Affine3f getShapePose(const String &id);
         bool setShapePose(const String &id, const Affine3f &pose);

modules/viz/src/precomp.hpp

@@ -49,6 +49,7 @@
 #include <vtkDiskSource.h>
 #include <vtkPlaneSource.h>
 #include <vtkSphereSource.h>
+#include <vtkArrowSource.h>
 #include <vtkIdentityTransform.h>
 #include <vtkTransform.h>
 #include <vtkTransformPolyDataFilter.h>

modules/viz/src/q/shapes.h

@@ -14,6 +14,7 @@ namespace temp_viz
     CV_EXPORTS vtkSmartPointer<vtkDataSet> create2DCircle (const Point3f& pt, double radius);
     CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube(const Point3f& pt_min, const Point3f& pt_max);
     CV_EXPORTS vtkSmartPointer<vtkDataSet> createSphere (const Point3f& pt, double radius);
+    CV_EXPORTS vtkSmartPointer<vtkDataSet> createArrow (const Point3f& pt1, const Point3f& pt2);
 //     CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const Point3f& pt, const Quaternionf& qt,  );
 //     CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (const Eigen::Vector3f &translation, const Eigen::Quaternionf &rotation, double width, double height, double depth);
 //     CV_EXPORTS vtkSmartPointer<vtkDataSet> createCube (double x_min, double x_max, double y_min, double y_max, double z_min, double z_max);

modules/viz/src/q/viz3d_impl.hpp

@@ -145,6 +145,7 @@ public:
     void showCylinder (const String &id, const Point3f &pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color &color);
     void showCircle (const String &id, const Point3f &pt, double radius, const Color &color);
     void showSphere (const String &id, const Point3f &pt, double radius, const Color &color);
+    void showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color);
     
     Affine3f getShapePose (const String &id);
     bool setShapePose (const String &id, const Affine3f &pose);

modules/viz/src/shapes.cpp

@@ -84,6 +84,61 @@ vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double ra
     return (sphere->GetOutput ());
 }
 
+vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2)
+{
+    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());
+    
+    return (transformPD->GetOutput());
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////
 vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const temp_viz::ModelCoefficients &coefficients, int numsides)
 {

modules/viz/src/viz3d.cpp

@@ -113,6 +113,11 @@ void temp_viz::Viz3d::showSphere (const String &id, const Point3f &pt, double ra
     impl_->showSphere(id, pt, radius, color);
 }
 
+void temp_viz::Viz3d::showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
+{
+    impl_->showArrow(id,pt1,pt2,color);
+}
+
 cv::Affine3f temp_viz::Viz3d::getShapePose(const String &id)
 {
     return impl_->getShapePose(id);

modules/viz/src/viz3d_impl.cpp

@@ -512,6 +512,42 @@ void temp_viz::Viz3d::VizImpl::showSphere (const String &id, const Point3f &pt,
     }
 }
 
+void temp_viz::Viz3d::VizImpl::showArrow (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
+{
+    // Check if this Id already exists
+    ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
+    bool exists = (am_it != shape_actor_map_->end());
+    Color c = vtkcolor(color);
+    
+    // If it exists just update
+    if (exists)
+    {
+        vtkSmartPointer<vtkLODActor> actor = vtkLODActor::SafeDownCast (am_it->second);
+        reinterpret_cast<vtkDataSetMapper*>(actor->GetMapper ())->SetInput(createArrow(pt1,pt2));
+        actor->GetProperty ()->SetColor (c.val);
+        actor->GetMapper ()->ScalarVisibilityOff ();
+        actor->Modified ();
+    }
+    else
+    {
+        // Create a plane
+        vtkSmartPointer<vtkDataSet> data = createArrow(pt1,pt2);
+
+        // Create an Actor
+        vtkSmartPointer<vtkLODActor> actor;
+        createActorFromVTKDataSet (data, actor);
+        //  actor->GetProperty ()->SetRepresentationToWireframe ();
+        actor->GetProperty ()->SetRepresentationToSurface ();
+        actor->GetProperty ()->SetLighting (false);
+        actor->GetProperty ()->SetColor (c.val);
+        actor->GetMapper ()->ScalarVisibilityOff ();
+        renderer_->AddActor(actor);
+
+        // Save the pointer/ID pair to the global actor map
+        (*shape_actor_map_)[id] = actor;
+    }
+}
+
 cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String &id)
 {
     // Get the shape with the id and return the pose

modules/viz/test/test_viz3d.cpp

@@ -94,6 +94,7 @@ TEST(Viz_viz3d, accuracy)
     int col_red = 0;
     v.showCircle("circle1", cv::Point3f(0,0,0), 1.0, temp_viz::Color(0,255,0));
     v.showSphere("sphere1", cv::Point3f(0,0,0), 0.5, temp_viz::Color(0,0,255));
+    v.showArrow("arrow1", cv::Point3f(0,0,0), cv::Point3f(1,1,1), temp_viz::Color(255,0,0));
     
     while(!v.wasStopped())
     {
@@ -109,6 +110,7 @@ TEST(Viz_viz3d, accuracy)
 //         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("sphere1", cloudPosition);
+        v.setShapePose("arrow1", cloudPosition);
         
         angle_x += 0.1f;
         angle_y -= 0.1f;