diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp
index 3a1a14a75..2b3e6e734 100644
--- a/modules/viz/include/opencv2/viz/viz3d.hpp
+++ b/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);
diff --git a/modules/viz/src/precomp.hpp b/modules/viz/src/precomp.hpp
index 9076a3bb4..de7104455 100644
--- a/modules/viz/src/precomp.hpp
+++ b/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>
diff --git a/modules/viz/src/q/shapes.h b/modules/viz/src/q/shapes.h
index e0665e056..1ebc6ae80 100644
--- a/modules/viz/src/q/shapes.h
+++ b/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);
diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp
index 2b768949f..50d50bb2c 100644
--- a/modules/viz/src/q/viz3d_impl.hpp
+++ b/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);
diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp
index 586b6b5a1..3f9fcd23e 100644
--- a/modules/viz/src/shapes.cpp
+++ b/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)
{
diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp
index d209ae5d2..65d036d9e 100644
--- a/modules/viz/src/viz3d.cpp
+++ b/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);
diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp
index f9d1ff40e..af564b34c 100644
--- a/modules/viz/src/viz3d_impl.cpp
+++ b/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
diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp
index fa1d60486..8a7472c51 100644
--- a/modules/viz/test/test_viz3d.cpp
+++ b/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;