From f97c3c8b06278e61767accf3d135f071936147db Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Thu, 4 Jul 2013 16:54:46 +0300 Subject: [PATCH] circle widget and arrow widget implementation --- modules/viz/include/opencv2/viz/widgets.hpp | 11 +++ modules/viz/src/simple_widgets.cpp | 97 ++++++++++++++++++++- modules/viz/test/test_viz3d.cpp | 12 ++- 3 files changed, 115 insertions(+), 5 deletions(-) diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index 69c2c56241..3535f486ae 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -55,6 +55,17 @@ namespace temp_viz public: SphereWidget(const cv::Point3f ¢er, 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()); + }; } diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index dcca437baa..51edc9b134 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -89,4 +89,99 @@ temp_viz::SphereWidget::SphereWidget(const cv::Point3f ¢er, float radius, in actor->SetMapper(mapper); setColor(color); -} \ No newline at end of file +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// arrow widget implementation + +temp_viz::ArrowWidget::ArrowWidget(const Point3f& pt1, const Point3f& pt2, const Color &color) +{ + vtkSmartPointer arrowSource = vtkSmartPointer::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 matrix = vtkSmartPointer::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 transform = + vtkSmartPointer::New(); + transform->Translate(startPoint); + transform->Concatenate(matrix); + transform->Scale(length, length, length); + + // Transform the polydata + vtkSmartPointer transformPD = + vtkSmartPointer::New(); + transformPD->SetTransform(transform); + transformPD->SetInputConnection(arrowSource->GetOutputPort()); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput(transformPD->GetOutput ()); + + vtkSmartPointer 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 disk = vtkSmartPointer::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 t = vtkSmartPointer::New (); + t->Identity (); + t->Translate (pt.x, pt.y, pt.z); + + vtkSmartPointer tf = vtkSmartPointer::New (); + tf->SetTransform (t); + tf->SetInputConnection (disk->GetOutputPort ()); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput(tf->GetOutput ()); + + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + actor->SetMapper(mapper); + + setColor(color); +} + diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index cf666d8023..55e590e16b 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/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;