From ecdd8513fd937162ef932ecd26de57c1268dbccd Mon Sep 17 00:00:00 2001 From: ozantonkal Date: Thu, 4 Jul 2013 18:19:06 +0300 Subject: [PATCH] coordinate system widget implementation, update pose fix --- modules/viz/include/opencv2/viz/widgets.hpp | 6 +++ modules/viz/src/simple_widgets.cpp | 50 +++++++++++++++++++++ modules/viz/src/widget.cpp | 5 +++ modules/viz/test/test_viz3d.cpp | 18 +++----- 4 files changed, 66 insertions(+), 13 deletions(-) diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp index 9f83d3c7b7..122c0c7bf6 100644 --- a/modules/viz/include/opencv2/viz/widgets.hpp +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -79,5 +79,11 @@ namespace temp_viz public: 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); + }; } diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp index 7f7db1b6c1..45e5a9755f 100644 --- a/modules/viz/src/simple_widgets.cpp +++ b/modules/viz/src/simple_widgets.cpp @@ -1,4 +1,5 @@ #include "precomp.hpp" +#include /////////////////////////////////////////////////////////////////////////////////////////////// /// 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 axes = vtkSmartPointer::New (); + axes->SetOrigin (0, 0, 0); + axes->SetScaleFactor (scale); + + vtkSmartPointer axes_colors = vtkSmartPointer::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 axes_data = axes->GetOutput (); + axes_data->Update (); + axes_data->GetPointData ()->SetScalars (axes_colors); + + vtkSmartPointer axes_tubes = vtkSmartPointer::New (); + axes_tubes->SetInput (axes_data); + axes_tubes->SetRadius (axes->GetScaleFactor () / 50.0); + axes_tubes->SetNumberOfSides (6); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetScalarModeToUsePointData (); + mapper->SetInput(axes_tubes->GetOutput ()); + + vtkSmartPointer 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]); +} diff --git a/modules/viz/src/widget.cpp b/modules/viz/src/widget.cpp index f61379af00..0657c54147 100644 --- a/modules/viz/src/widget.cpp +++ b/modules/viz/src/widget.cpp @@ -31,6 +31,11 @@ public: void updatePose(const Affine3f& pose) { vtkSmartPointer matrix = actor->GetUserMatrix(); + if (!matrix) + { + setPose(pose); + return ; + } Matx44f matrix_cv = convertToMatx(matrix); Affine3f updated_pose = pose * Affine3f(matrix_cv); diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 720145a22a..6cb27dbc2b 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/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); @@ -134,7 +126,7 @@ TEST(Viz_viz3d, accuracy) cw.setPose(cloudPosition); cyw.setPose(cloudPosition); lw.setPose(cloudPosition); - cuw.setPose(cloudPosition); + cuw.setPose(cloudPosition); angle_x += 0.1f; angle_y -= 0.1f;