diff --git a/modules/viz/include/opencv2/viz.hpp b/modules/viz/include/opencv2/viz.hpp index 3e1663df5..2ec8fbb86 100644 --- a/modules/viz/include/opencv2/viz.hpp +++ b/modules/viz/include/opencv2/viz.hpp @@ -1,3 +1,6 @@ #pragma once +#include +#include #include + diff --git a/modules/viz/include/opencv2/viz/types.hpp b/modules/viz/include/opencv2/viz/types.hpp index 2328dc073..92b277857 100644 --- a/modules/viz/include/opencv2/viz/types.hpp +++ b/modules/viz/include/opencv2/viz/types.hpp @@ -35,15 +35,6 @@ namespace temp_viz using cv::DataDepth; using cv::DataType; - - - - struct CV_EXPORTS ModelCoefficients - { - std::vector values; - }; - - class CV_EXPORTS Color : public Scalar { public: @@ -81,6 +72,8 @@ namespace temp_viz std::vector polygons; }; + ///////////////////////////////////////////////////////////////////////////// + /// Utility functions inline Color vtkcolor(const Color& color) { @@ -90,11 +83,7 @@ namespace temp_viz } inline Vec3d vtkpoint(const Point3f& point) { return Vec3d(point.x, point.y, point.z); } - - - template inline _Tp normalized(const _Tp& v) { return v * 1/cv::norm(v); } - inline bool isNan(float x) { diff --git a/modules/viz/include/opencv2/viz/viz3d.hpp b/modules/viz/include/opencv2/viz/viz3d.hpp index 46d0be04c..b4b697b90 100644 --- a/modules/viz/include/opencv2/viz/viz3d.hpp +++ b/modules/viz/include/opencv2/viz/viz3d.hpp @@ -10,6 +10,7 @@ #include #include +#include #include namespace temp_viz @@ -25,43 +26,18 @@ namespace temp_viz void setBackgroundColor(const Color& color = Color::black()); - void addCoordinateSystem(double scale, const Affine3f& t, const String& id = "coordinate"); - void showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose = Affine3f::Identity()); void showPointCloud(const String& id, InputArray cloud, const Color& color, const Affine3f& pose = Affine3f::Identity()); bool addPointCloudNormals (const Mat &cloud, const Mat& normals, int level = 100, float scale = 0.02f, const String& id = "cloud"); - - void showLine(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white()); - void showPlane(const String& id, const Vec4f& coefs, const Color& color = Color::white()); - void showPlane(const String& id, const Vec4f& coefs, const Point3f& pt, const Color& color = Color::white()); - void showCube(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white()); - void showCylinder(const String& id, const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int num_sides, const Color& color = Color::white()); - void showCircle(const String& id, const Point3f& pt, double radius, const Color& color = Color::white()); - void showSphere(const String& id, const Point3f& pt, double radius, const Color& color = Color::white()); - void showArrow(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color = Color::white()); - - Affine3f getShapePose(const String& id); - void setShapePose(const String& id, const Affine3f &pose); - - bool addPlane (const ModelCoefficients &coefficients, const String& id = "plane"); - bool addPlane (const ModelCoefficients &coefficients, double x, double y, double z, const String& id = "plane"); - bool removeCoordinateSystem (const String& id = "coordinate"); - bool addPolygonMesh (const Mesh3d& mesh, const String& id = "polygon"); bool updatePolygonMesh (const Mesh3d& mesh, const String& id = "polygon"); bool addPolylineFromPolygonMesh (const Mesh3d& mesh, const String& id = "polyline"); - bool addText (const String &text, int xpos, int ypos, const Color& color, int fontsize = 10, const String& id = ""); - - bool addPolygon(const Mat& cloud, const Color& color, const String& id = "polygon"); - bool addSphere (const Point3f ¢er, double radius, const Color& color, const String& id = "sphere"); - - void spin (); void spinOnce (int time = 1, bool force_redraw = false); @@ -69,6 +45,9 @@ namespace temp_viz void registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie = 0); bool wasStopped() const; + + void showWidget(const String &id, const Widget &widget); + bool removeWidget(const String &id); private: Viz3d(const Viz3d&); Viz3d& operator=(const Viz3d&); diff --git a/modules/viz/include/opencv2/viz/widget_accessor.hpp b/modules/viz/include/opencv2/viz/widget_accessor.hpp new file mode 100644 index 000000000..4b1c5ef15 --- /dev/null +++ b/modules/viz/include/opencv2/viz/widget_accessor.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include +#include +#include + +namespace temp_viz +{ + class Widget; + + //The class is only that depends on VTK in its interface. + //It is indended for those users who want to develop own widgets system using VTK library API. + struct CV_EXPORTS WidgetAccessor + { + static vtkSmartPointer getActor(const Widget &widget); + }; +} diff --git a/modules/viz/include/opencv2/viz/widgets.hpp b/modules/viz/include/opencv2/viz/widgets.hpp new file mode 100644 index 000000000..122c0c7bf --- /dev/null +++ b/modules/viz/include/opencv2/viz/widgets.hpp @@ -0,0 +1,89 @@ +#pragma once + +#include + + +namespace temp_viz +{ + ///////////////////////////////////////////////////////////////////////////// + /// The base class for all widgets + class CV_EXPORTS Widget + { + public: + Widget(); + Widget(const Widget &other); + Widget& operator =(const Widget &other); + + ~Widget(); + + void copyTo(Widget &dst); + + void setColor(const Color &color); + void setPose(const Affine3f &pose); + void updatePose(const Affine3f &pose); + Affine3f getPose() const; + + private: + class Impl; + Impl* impl_; + + void create(); + void release(); + + friend struct WidgetAccessor; + }; + + + class CV_EXPORTS LineWidget : public Widget + { + public: + LineWidget(const Point3f &pt1, const Point3f &pt2, const Color &color = Color::white()); + + void setLineWidth(float line_width); + float getLineWidth(); + }; + + class CV_EXPORTS PlaneWidget : public Widget + { + public: + PlaneWidget(const Vec4f& coefs, const Color &color = Color::white()); + PlaneWidget(const Vec4f& coefs, const Point3f& pt, const Color &color = Color::white()); + }; + + class CV_EXPORTS SphereWidget : public Widget + { + 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()); + }; + + class CV_EXPORTS CylinderWidget : public Widget + { + public: + CylinderWidget(const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30, const Color &color = Color::white()); + }; + + class CV_EXPORTS CubeWidget : public Widget + { + 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/precomp.hpp b/modules/viz/src/precomp.hpp index bb59ca949..ddbef3a49 100644 --- a/modules/viz/src/precomp.hpp +++ b/modules/viz/src/precomp.hpp @@ -153,4 +153,5 @@ #endif -#include +#include "opencv2/viz/widget_accessor.hpp" +#include diff --git a/modules/viz/src/q/shapes.h b/modules/viz/src/q/shapes.h deleted file mode 100644 index 4055cd1b2..000000000 --- a/modules/viz/src/q/shapes.h +++ /dev/null @@ -1,19 +0,0 @@ -#pragma once - -#include - -namespace temp_viz -{ - vtkSmartPointer createLine (const cv::Point3f& pt1, const cv::Point3f& pt2); - vtkSmartPointer createSphere (const cv::Point3f ¢er, float radius, int sphere_resolution = 10); - vtkSmartPointer createCylinder (const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30); - vtkSmartPointer createPlane (const Vec4f& coefs); - vtkSmartPointer createPlane (const Vec4f& coefs, const Point3f& pt); - vtkSmartPointer create2DCircle (const Point3f& pt, double radius); - vtkSmartPointer createCube(const Point3f& pt_min, const Point3f& pt_max); - vtkSmartPointer createSphere (const Point3f& pt, double radius); - vtkSmartPointer createArrow (const Point3f& pt1, const Point3f& pt2); - - //brief Allocate a new unstructured grid smartpointer. For internal use only. - void allocVtkUnstructuredGrid (vtkSmartPointer &polydata); -} diff --git a/modules/viz/src/q/viz3d_impl.hpp b/modules/viz/src/q/viz3d_impl.hpp index d80881c92..f4a85e218 100644 --- a/modules/viz/src/q/viz3d_impl.hpp +++ b/modules/viz/src/q/viz3d_impl.hpp @@ -38,41 +38,6 @@ public: void spin (); void spinOnce (int time = 1, bool force_redraw = false); - /** \brief Adds 3D axes describing a coordinate system to screen at x, y, z, Roll,Pitch,Yaw - * - * \param[in] scale the scale of the axes (default: 1) - * \param[in] t transformation matrix - * - * RPY Angles - * Rotate the reference frame by the angle roll about axis x - * Rotate the reference frame by the angle pitch about axis y - * Rotate the reference frame by the angle yaw about axis z - * - * Description: - * Sets the orientation of the Prop3D. Orientation is specified as - * X,Y and Z rotations in that order, but they are performed as - * RotateZ, RotateX, and finally RotateY. - * - * All axies use right hand rule. x=red axis, y=green axis, z=blue axis - * z direction is point into the screen. - * z - * \ - * \ - * \ - * -----------> x - * | - * | - * | - * | - * | - * | - * y - */ - void addCoordinateSystem (double scale, const Affine3f& t, const String& id = "coordinate"); - - /** \brief Removes a previously added 3D axes (coordinate system) - */ - bool removeCoordinateSystem (const String& id = "coordinate"); bool removePointCloud (const String& id = "cloud"); inline bool removePolygonMesh (const String& id = "polygon") { @@ -138,18 +103,6 @@ public: interactor_->TerminateApp (); } - void showLine (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color); - void showPlane (const String& id, const cv::Vec4f &coefs, const Color& color); - void showPlane (const String& id ,const cv::Vec4f &coefs, const Point3f& pt, const Color& color); - void showCube (const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color); - 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); - void setShapePose (const String& id, const Affine3f& pose); - bool addPolygon(const cv::Mat& cloud, const Color& color, const String& id = "polygon"); bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color, bool display_length, const String& id = "arrow"); bool addArrow (const Point3f& pt1, const Point3f& pt2, const Color& color_line, const Color& color_text, const String& id = "arrow"); @@ -245,6 +198,11 @@ public: void setPosition (int x, int y); void setSize (int xw, int yw); + + void showWidget(const String &id, const Widget &widget); + bool removeWidget(const String &id); + + void all_data(); private: vtkSmartPointer interactor_; @@ -311,6 +269,9 @@ private: /** \brief Internal list with actor pointers and name IDs for shapes. */ cv::Ptr shape_actor_map_; + + /** \brief Internal list with actor pointers and name IDs for all widget actors */ + cv::Ptr widget_actor_map_; /** \brief Boolean that holds whether or not the camera parameters were manually initialized*/ bool camera_set_; diff --git a/modules/viz/src/q/viz_types.h b/modules/viz/src/q/viz_types.h index 1acfa4b1a..3dbb44f18 100644 --- a/modules/viz/src/q/viz_types.h +++ b/modules/viz/src/q/viz_types.h @@ -15,8 +15,17 @@ namespace temp_viz /** \brief Internal cell array. Used for optimizing updatePointCloud. */ vtkSmartPointer cells; }; + + // TODO This will be used to contain both cloud and shape actors + struct CV_EXPORTS WidgetActor + { + vtkSmartPointer actor; + vtkSmartPointer viewpoint_transformation_; + vtkSmartPointer cells; + }; typedef std::map CloudActorMap; typedef std::map > ShapeActorMap; + typedef std::map WidgetActorMap; } diff --git a/modules/viz/src/shapes.cpp b/modules/viz/src/shapes.cpp deleted file mode 100644 index 7fa9a2cd8..000000000 --- a/modules/viz/src/shapes.cpp +++ /dev/null @@ -1,153 +0,0 @@ -#include "precomp.hpp" - -inline float rad2deg(float alpha) { return (alpha * 57.29578f); } -inline double rad2deg(double alpha) { return (alpha * 57.29578); } - -vtkSmartPointer temp_viz::createCylinder (const cv::Point3f& pt_on_axis, const cv::Point3f& axis_direction, double radius, int numsides) -{ - const cv::Point3f pt2 = pt_on_axis + axis_direction; - vtkSmartPointer line = vtkSmartPointer::New (); - line->SetPoint1 (pt_on_axis.x, pt_on_axis.y, pt_on_axis.z); - line->SetPoint2 (pt2.x, pt2.y, pt2.z); - - vtkSmartPointer tuber = vtkSmartPointer::New (); - tuber->SetInputConnection (line->GetOutputPort ()); - tuber->SetRadius (radius); - tuber->SetNumberOfSides (numsides); - return (tuber->GetOutput ()); -} - -vtkSmartPointer temp_viz::createPlane (const cv::Vec4f& coefs) -{ - vtkSmartPointer plane = vtkSmartPointer::New (); - plane->SetNormal (coefs[0], coefs[1], coefs[2]); - double norm = cv::norm(cv::Vec3f(coefs.val)); - plane->Push (-coefs[3] / norm); - return plane->GetOutput (); -} - -vtkSmartPointer temp_viz::createPlane(const cv::Vec4f& coefs, const cv::Point3f& pt) -{ - vtkSmartPointer plane = vtkSmartPointer::New (); - cv::Point3f coefs3(coefs[0], coefs[1], coefs[2]); - double norm_sqr = 1.0 / coefs3.dot (coefs3); - plane->SetNormal(coefs[0], coefs[1], coefs[2]); - - double t = coefs3.dot(pt) + coefs[3]; - cv::Vec3f p_center = pt - coefs3 * t * norm_sqr; - plane->SetCenter (p_center[0], p_center[1], p_center[2]); - - return (plane->GetOutput ()); -} - -vtkSmartPointer temp_viz::create2DCircle (const cv::Point3f& pt, double radius) -{ - 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 ()); - - return tf->GetOutput (); -} - -vtkSmartPointer temp_viz::createCube(const cv::Point3f& pt_min, const cv::Point3f& pt_max) -{ - vtkSmartPointer cube = vtkSmartPointer::New (); - cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z); - return cube->GetOutput (); -} - -vtkSmartPointer temp_viz::createSphere (const Point3f& pt, double radius) -{ - vtkSmartPointer sphere = vtkSmartPointer::New (); - sphere->SetRadius (radius); - sphere->SetCenter (pt.x, pt.y, pt.z); - sphere->SetPhiResolution (10); - sphere->SetThetaResolution (10); - sphere->LatLongTessellationOff (); - sphere->Update (); - - return sphere->GetOutput (); -} - -vtkSmartPointer temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2) -{ - 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()); - - return transformPD->GetOutput(); -} - -vtkSmartPointer temp_viz::createLine (const cv::Point3f& pt1, const cv::Point3f& pt2) -{ - vtkSmartPointer line = vtkSmartPointer::New (); - line->SetPoint1 (pt1.x, pt1.y, pt1.z); - line->SetPoint2 (pt2.x, pt2.y, pt2.z); - line->Update (); - return line->GetOutput (); -} - -void temp_viz::allocVtkUnstructuredGrid (vtkSmartPointer &polydata) -{ - polydata = vtkSmartPointer::New (); -} - - diff --git a/modules/viz/src/simple_widgets.cpp b/modules/viz/src/simple_widgets.cpp new file mode 100644 index 000000000..45e5a9755 --- /dev/null +++ b/modules/viz/src/simple_widgets.cpp @@ -0,0 +1,277 @@ +#include "precomp.hpp" +#include + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// line widget implementation +temp_viz::LineWidget::LineWidget(const Point3f &pt1, const Point3f &pt2, const Color &color) +{ + vtkSmartPointer line = vtkSmartPointer::New(); + line->SetPoint1 (pt1.x, pt1.y, pt1.z); + line->SetPoint2 (pt2.x, pt2.y, pt2.z); + line->Update (); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput(line->GetOutput ()); + + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + actor->SetMapper(mapper); + + setColor(color); +} + +void temp_viz::LineWidget::setLineWidth(float line_width) +{ + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + actor->GetProperty()->SetLineWidth(line_width); +} + +float temp_viz::LineWidget::getLineWidth() +{ + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + return actor->GetProperty()->GetLineWidth(); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// plane widget implementation + +temp_viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, const Color &color) +{ + vtkSmartPointer plane = vtkSmartPointer::New (); + plane->SetNormal (coefs[0], coefs[1], coefs[2]); + double norm = cv::norm(cv::Vec3f(coefs.val)); + plane->Push (-coefs[3] / norm); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput(plane->GetOutput ()); + + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + actor->SetMapper(mapper); + + setColor(color); +} + +temp_viz::PlaneWidget::PlaneWidget(const Vec4f& coefs, const Point3f& pt, const Color &color) +{ + vtkSmartPointer plane = vtkSmartPointer::New (); + cv::Point3f coefs3(coefs[0], coefs[1], coefs[2]); + double norm_sqr = 1.0 / coefs3.dot (coefs3); + plane->SetNormal(coefs[0], coefs[1], coefs[2]); + + double t = coefs3.dot(pt) + coefs[3]; + cv::Vec3f p_center = pt - coefs3 * t * norm_sqr; + plane->SetCenter (p_center[0], p_center[1], p_center[2]); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput(plane->GetOutput ()); + + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + actor->SetMapper(mapper); + + setColor(color); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// sphere widget implementation + +temp_viz::SphereWidget::SphereWidget(const cv::Point3f ¢er, float radius, int sphere_resolution, const Color &color) +{ + vtkSmartPointer sphere = vtkSmartPointer::New (); + sphere->SetRadius (radius); + sphere->SetCenter (center.x, center.y, center.z); + sphere->SetPhiResolution (sphere_resolution); + sphere->SetThetaResolution (sphere_resolution); + sphere->LatLongTessellationOff (); + sphere->Update (); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput(sphere->GetOutput ()); + + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + actor->SetMapper(mapper); + + setColor(color); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// 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 temp_viz::Point3f& pt, double radius, const temp_viz::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); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// cylinder widget implementation + +temp_viz::CylinderWidget::CylinderWidget(const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides, const Color &color) +{ + const cv::Point3f pt2 = pt_on_axis + axis_direction; + vtkSmartPointer line = vtkSmartPointer::New (); + line->SetPoint1 (pt_on_axis.x, pt_on_axis.y, pt_on_axis.z); + line->SetPoint2 (pt2.x, pt2.y, pt2.z); + + vtkSmartPointer tuber = vtkSmartPointer::New (); + tuber->SetInputConnection (line->GetOutputPort ()); + tuber->SetRadius (radius); + tuber->SetNumberOfSides (numsides); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput(tuber->GetOutput ()); + + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + actor->SetMapper(mapper); + + setColor(color); +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// cylinder widget implementation + +temp_viz::CubeWidget::CubeWidget(const Point3f& pt_min, const Point3f& pt_max, const Color &color) +{ + vtkSmartPointer cube = vtkSmartPointer::New (); + cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z); + + vtkSmartPointer mapper = vtkSmartPointer::New (); + mapper->SetInput(cube->GetOutput ()); + + vtkSmartPointer actor = WidgetAccessor::getActor(*this); + actor->SetMapper(mapper); + + 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/types.cpp b/modules/viz/src/types.cpp index 0baed3a72..765913614 100644 --- a/modules/viz/src/types.cpp +++ b/modules/viz/src/types.cpp @@ -1,9 +1,7 @@ #include - - ////////////////////////////////////////////////////////////////////////////////////////////////////// -/// cv::Color +/// cv::viz::Color temp_viz::Color::Color() : Scalar(0, 0, 0) {} temp_viz::Color::Color(double gray) : Scalar(gray, gray, gray) {} diff --git a/modules/viz/src/viz3d.cpp b/modules/viz/src/viz3d.cpp index 3df43fab6..e3ebf85bf 100644 --- a/modules/viz/src/viz3d.cpp +++ b/modules/viz/src/viz3d.cpp @@ -17,11 +17,6 @@ void temp_viz::Viz3d::setBackgroundColor(const Color& color) impl_->setBackgroundColor(color); } -void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const String& id) -{ - impl_->addCoordinateSystem(scale, t, id); -} - void temp_viz::Viz3d::showPointCloud(const String& id, InputArray cloud, InputArray colors, const Affine3f& pose) { impl_->showPointCloud(id, cloud, colors, pose); @@ -72,61 +67,6 @@ void temp_viz::Viz3d::spinOnce (int time, bool force_redraw) impl_->spinOnce(time, force_redraw); } -void temp_viz::Viz3d::showLine(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color) -{ - impl_->showLine(id, pt1, pt2, color); -} - -void temp_viz::Viz3d::showPlane(const String& id, const Vec4f &coefs, const Color& color) -{ - impl_->showPlane(id, coefs, color); -} - -void temp_viz::Viz3d::showPlane(const String& id, const Vec4f &coefs, const Point3f& pt, const Color& color) -{ - impl_->showPlane(id, coefs, pt, color); -} - -void temp_viz::Viz3d::showCube(const String& id, const Point3f& pt1, const Point3f& pt2, const Color& color) -{ - impl_->showCube(id, pt1, pt2, color); -} - -void temp_viz::Viz3d::showCylinder(const String& id, const Point3f& pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, const Color& color) -{ - impl_->showCylinder(id, pt_on_axis, axis_direction, radius, num_sides, color); -} - -void temp_viz::Viz3d::showCircle(const String& id, const Point3f& pt, double radius, const Color& color) -{ - impl_->showCircle(id, pt, radius, color); -} - -void temp_viz::Viz3d::showSphere (const String& id, const Point3f& pt, double radius, const Color& color) -{ - 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); -} - -void temp_viz::Viz3d::setShapePose(const String& id, const Affine3f &pose) -{ - impl_->setShapePose(id, pose); -} - -bool temp_viz::Viz3d::removeCoordinateSystem (const String& id) -{ - return impl_->removeCoordinateSystem(id); -} - void temp_viz::Viz3d::registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void* cookie) { impl_->registerKeyboardCallback(callback, cookie); @@ -139,3 +79,12 @@ void temp_viz::Viz3d::registerMouseCallback(void (*callback)(const MouseEvent&, bool temp_viz::Viz3d::wasStopped() const { return impl_->wasStopped(); } +void temp_viz::Viz3d::showWidget(const String &id, const Widget &widget) +{ + impl_->showWidget(id, widget); +} + +bool temp_viz::Viz3d::removeWidget(const String &id) +{ + return impl_->removeWidget(id); +} diff --git a/modules/viz/src/viz3d_impl.cpp b/modules/viz/src/viz3d_impl.cpp index 7c97b20d7..ae2e06228 100644 --- a/modules/viz/src/viz3d_impl.cpp +++ b/modules/viz/src/viz3d_impl.cpp @@ -1,5 +1,4 @@ #include "precomp.hpp" -#include #include namespace temp_viz @@ -379,309 +378,6 @@ bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const return (true); } - -void temp_viz::Viz3d::VizImpl::showLine (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(); - - // If it exists just update - if (exists) - { - vtkSmartPointer actor = vtkLODActor::SafeDownCast (am_it->second); - reinterpret_cast(actor->GetMapper ())->SetInput(createLine(pt1,pt2)); - Color c = vtkcolor(color); - actor->GetProperty ()->SetColor (c.val); - actor->GetMapper ()->ScalarVisibilityOff (); - actor->Modified (); - } - else - { - // Create new line - vtkSmartPointer data = createLine (pt1, pt2); - - // Create an Actor - vtkSmartPointer actor; - createActorFromVTKDataSet (data, actor); - actor->GetProperty ()->SetRepresentationToWireframe (); - - Color c = vtkcolor(color); - 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; - } -} - -void temp_viz::Viz3d::VizImpl::showPlane (const String& id, const cv::Vec4f &coefs, 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 actor = vtkLODActor::SafeDownCast (am_it->second); - reinterpret_cast(actor->GetMapper ())->SetInput(createPlane(coefs)); - actor->GetProperty ()->SetColor (c.val); - actor->GetMapper ()->ScalarVisibilityOff (); - actor->Modified (); - } - else - { - // Create a plane - vtkSmartPointer data = createPlane (coefs); - - // Create an Actor - vtkSmartPointer 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; - } -} - -void temp_viz::Viz3d::VizImpl::showPlane (const String& id ,const cv::Vec4f &coefs, const Point3f& pt, 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 actor = vtkLODActor::SafeDownCast (am_it->second); - reinterpret_cast(actor->GetMapper ())->SetInput(createPlane(coefs, pt)); - actor->GetProperty ()->SetColor (c.val); - actor->GetMapper ()->ScalarVisibilityOff (); - actor->Modified (); - } - else - { - // Create a plane - vtkSmartPointer data = createPlane (coefs, pt); - - // Create an Actor - vtkSmartPointer 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; - } -} - -void temp_viz::Viz3d::VizImpl::showCube (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 actor = vtkLODActor::SafeDownCast (am_it->second); - reinterpret_cast(actor->GetMapper ())->SetInput(createCube(pt1, pt2)); - actor->GetProperty ()->SetColor (c.val); - actor->GetMapper ()->ScalarVisibilityOff (); - actor->Modified (); - } - else - { - // Create a plane - vtkSmartPointer data = createCube (pt1, pt2); - - // Create an Actor - vtkSmartPointer 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; - } -} - -void temp_viz::Viz3d::VizImpl::showCylinder (const String& id, const Point3f& pt_on_axis, const Point3f &axis_direction, double radius, int num_sides, 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 actor = vtkLODActor::SafeDownCast (am_it->second); - reinterpret_cast(actor->GetMapper ())->SetInput(createCylinder(pt_on_axis, axis_direction, radius, num_sides)); - actor->GetProperty ()->SetColor (c.val); - actor->GetMapper ()->ScalarVisibilityOff (); - actor->Modified (); - } - else - { - // Create a plane - vtkSmartPointer data = createCylinder(pt_on_axis, axis_direction, radius, num_sides); - - // Create an Actor - vtkSmartPointer 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; - } -} - -void temp_viz::Viz3d::VizImpl::showCircle (const String& id, const Point3f& pt, double radius, 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 actor = vtkLODActor::SafeDownCast (am_it->second); - reinterpret_cast(actor->GetMapper ())->SetInput(create2DCircle(pt, radius)); - actor->GetProperty ()->SetColor (c.val); - actor->GetMapper ()->ScalarVisibilityOff (); - actor->Modified (); - } - else - { - // Create a plane - vtkSmartPointer data = create2DCircle(pt, radius); - - // Create an Actor - vtkSmartPointer 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; - } -} - -void temp_viz::Viz3d::VizImpl::showSphere (const String& id, const Point3f& pt, double radius, 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 actor = vtkLODActor::SafeDownCast (am_it->second); - reinterpret_cast(actor->GetMapper ())->SetInput(createSphere(pt, radius)); - actor->GetProperty ()->SetColor (c.val); - actor->GetMapper ()->ScalarVisibilityOff (); - actor->Modified (); - } - else - { - // Create a plane - vtkSmartPointer data = createSphere(pt, radius); - - // Create an Actor - vtkSmartPointer 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; - } -} - -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 actor = vtkLODActor::SafeDownCast (am_it->second); - reinterpret_cast(actor->GetMapper ())->SetInput(createArrow(pt1,pt2)); - actor->GetProperty ()->SetColor (c.val); - actor->GetMapper ()->ScalarVisibilityOff (); - actor->Modified (); - } - else - { - // Create a plane - vtkSmartPointer data = createArrow(pt1,pt2); - - // Create an Actor - vtkSmartPointer 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) -{ - ShapeActorMap::iterator am_it = shape_actor_map_->find (id); - - CV_Assert(am_it != shape_actor_map_->end()); - vtkLODActor* actor = vtkLODActor::SafeDownCast(am_it->second); - return Affine3f(convertToMatx(actor->GetUserMatrix())); -} - -void temp_viz::Viz3d::VizImpl::setShapePose (const String& id, const Affine3f &pose) -{ - ShapeActorMap::iterator am_it = shape_actor_map_->find (id); - CV_Assert(am_it != shape_actor_map_->end()); - - vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second); - vtkSmartPointer matrix = convertToVtkMatrix(pose.matrix); - actor->SetUserMatrix (matrix); - actor->Modified (); -} - bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id) { CV_Assert(mesh.cloud.type() == CV_32FC3 && mesh.cloud.rows == 1 && !mesh.polygons.empty ()); @@ -1168,3 +864,36 @@ bool temp_viz::Viz3d::VizImpl::addPolygon (const cv::Mat& cloud, const Color& co return (true); } + +#include "opencv2/viz/widget_accessor.hpp" + +void temp_viz::Viz3d::VizImpl::showWidget(const String &id, const Widget &widget) +{ + WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id); + bool exists = !(wam_itr == widget_actor_map_->end()); + if (exists) + { + // Remove it if it exists and add it again + removeActorFromRenderer(wam_itr->second.actor); + } + renderer_->AddActor(WidgetAccessor::getActor(widget)); + (*widget_actor_map_)[id].actor = WidgetAccessor::getActor(widget); +} + +bool temp_viz::Viz3d::VizImpl::removeWidget(const String &id) +{ + WidgetActorMap::iterator wam_itr = widget_actor_map_->find(id); + bool exists = !(wam_itr == widget_actor_map_->end()); + + if (!exists) + { + std::cout << "[removeWidget] A widget with id <" << id << "> does not exist!" << std::endl; + return false; + } + + if (!removeActorFromRenderer (wam_itr->second.actor)) + return false; + + widget_actor_map_->erase(wam_itr); + return true; +} diff --git a/modules/viz/src/viz_main.cpp b/modules/viz/src/viz_main.cpp index 4552790ab..0d56fe0a5 100644 --- a/modules/viz/src/viz_main.cpp +++ b/modules/viz/src/viz_main.cpp @@ -1,7 +1,6 @@ #include "precomp.hpp" #include -#include #include #include @@ -17,6 +16,7 @@ temp_viz::Viz3d::VizImpl::VizImpl (const std::string &name) : style_ (vtkSmartPointer::New ()) , cloud_actor_map_ (new CloudActorMap) , shape_actor_map_ (new ShapeActorMap) + , widget_actor_map_ (new WidgetActorMap) , s_lastDone_(0.0) { renderer_ = vtkSmartPointer::New (); @@ -138,71 +138,6 @@ void temp_viz::Viz3d::VizImpl::spinOnce (int time, bool force_redraw) } } -///////////////////////////////////////////////////////////////////////////////////////////// -void temp_viz::Viz3d::VizImpl::addCoordinateSystem (double scale, const cv::Affine3f& affine, const std::string &id) -{ - 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 axes_mapper = vtkSmartPointer::New (); - axes_mapper->SetScalarModeToUsePointData (); - axes_mapper->SetInput (axes_tubes->GetOutput ()); - - vtkSmartPointer axes_actor = vtkSmartPointer::New (); - axes_actor->SetMapper (axes_mapper); - - cv::Vec3d t = affine.translation(); - axes_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; - - axes_actor->SetOrientation(0,0,0); - axes_actor->RotateWXYZ(r_angle*180/CV_PI,rvec[0], rvec[1], rvec[2]); - renderer_->AddActor (axes_actor); - - (*shape_actor_map_)[id] = axes_actor; -} - -///////////////////////////////////////////////////////////////////////////////////////////// -bool temp_viz::Viz3d::VizImpl::removeCoordinateSystem (const std::string &id) -{ - ShapeActorMap::iterator am_it = shape_actor_map_->find (id); - if (am_it == shape_actor_map_->end ()) - return false; - - // Remove it from all renderers - if (!removeActorFromRenderer(am_it->second)) - return false; - - shape_actor_map_->erase(am_it); - return true; -} - ///////////////////////////////////////////////////////////////////////////////////////////// bool temp_viz::Viz3d::VizImpl::removePointCloud (const std::string &id) { diff --git a/modules/viz/src/widget.cpp b/modules/viz/src/widget.cpp new file mode 100644 index 000000000..0657c5414 --- /dev/null +++ b/modules/viz/src/widget.cpp @@ -0,0 +1,141 @@ +#include "precomp.hpp" + +class temp_viz::Widget::Impl +{ +public: + vtkSmartPointer actor; + int ref_counter; + + Impl() : actor(vtkSmartPointer::New()) {} + + void setColor(const Color& color) + { + Color c = vtkcolor(color); + actor->GetMapper ()->ScalarVisibilityOff (); + actor->GetProperty ()->SetColor (c.val); + actor->GetProperty ()->SetEdgeColor (c.val); + actor->GetProperty ()->SetAmbient (0.8); + actor->GetProperty ()->SetDiffuse (0.8); + actor->GetProperty ()->SetSpecular (0.8); + actor->GetProperty ()->SetLighting (0); + actor->Modified (); + } + + void setPose(const Affine3f& pose) + { + vtkSmartPointer matrix = convertToVtkMatrix(pose.matrix); + actor->SetUserMatrix (matrix); + actor->Modified (); + } + + 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); + matrix = convertToVtkMatrix(updated_pose.matrix); + + actor->SetUserMatrix (matrix); + actor->Modified (); + } + + Affine3f getPose() const + { + vtkSmartPointer matrix = actor->GetUserMatrix(); + Matx44f matrix_cv = convertToMatx(matrix); + return Affine3f(matrix_cv); + } + +protected: + + static vtkSmartPointer convertToVtkMatrix (const cv::Matx44f& m) + { + vtkSmartPointer vtk_matrix = vtkSmartPointer::New (); + for (int i = 0; i < 4; i++) + for (int k = 0; k < 4; k++) + vtk_matrix->SetElement(i, k, m(i, k)); + return vtk_matrix; + } + + static cv::Matx44f convertToMatx(const vtkSmartPointer& vtk_matrix) + { + cv::Matx44f m; + for (int i = 0; i < 4; i++) + for (int k = 0; k < 4; k++) + m(i, k) = vtk_matrix->GetElement (i, k); + return m; + } +}; + + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// stream accessor implementaion + +vtkSmartPointer temp_viz::WidgetAccessor::getActor(const Widget& widget) +{ + return widget.impl_->actor; +} + +/////////////////////////////////////////////////////////////////////////////////////////////// +/// widget implementaion + +temp_viz::Widget::Widget() : impl_(0) +{ + create(); +} + +temp_viz::Widget::Widget(const Widget& other) : impl_(other.impl_) +{ + if (impl_) + CV_XADD(&impl_->ref_counter, 1); +} + +temp_viz::Widget& temp_viz::Widget::operator =(const Widget &other) +{ + if (this != &other) + { + release(); + impl_ = other.impl_; + if (impl_) + CV_XADD(&impl_->ref_counter, 1); + } + return *this; +} + +temp_viz::Widget::~Widget() +{ + release(); +} + +void temp_viz::Widget::copyTo(Widget& /*dst*/) +{ + // TODO Deep copy the data if there is any +} + +void temp_viz::Widget::setColor(const Color& color) { impl_->setColor(color); } +void temp_viz::Widget::setPose(const Affine3f& pose) { impl_->setPose(pose); } +void temp_viz::Widget::updatePose(const Affine3f& pose) { impl_->updatePose(pose); } +temp_viz::Affine3f temp_viz::Widget::getPose() const { return impl_->getPose(); } + +void temp_viz::Widget::create() +{ + if (impl_) + release(); + impl_ = new Impl(); + impl_->ref_counter = 1; +} + +void temp_viz::Widget::release() +{ + if (impl_ && CV_XADD(&impl_->ref_counter, -1) == 1) + { + delete impl_; + } +} + diff --git a/modules/viz/test/test_viz3d.cpp b/modules/viz/test/test_viz3d.cpp index 8a7472c51..6cb27dbc2 100644 --- a/modules/viz/test/test_viz3d.cpp +++ b/modules/viz/test/test_viz3d.cpp @@ -47,10 +47,9 @@ #include #include -#include +#include #include - cv::Mat cvcloud_load() { cv::Mat cloud(1, 20000, CV_32FC3); @@ -74,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)); @@ -92,25 +89,44 @@ 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.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)); + + 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); + 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)); + 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); + v.showWidget("sphere", sw); + v.showWidget("arrow", aw); + v.showWidget("circle", cw); + v.showWidget("cylinder", cyw); + v.showWidget("cube", cuw); + v.showWidget("coordinateSystem", csw); + + temp_viz::LineWidget lw2 = lw; while(!v.wasStopped()) { // 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); - v.setShapePose("sphere1", cloudPosition); - v.setShapePose("arrow1", cloudPosition); + + lw2.setColor(temp_viz::Color(col_blue, col_green, col_red)); + lw.setLineWidth(lw.getLineWidth()+pos_x * 10); + + pw.setColor(temp_viz::Color(col_blue, col_green, col_red)); + + sw.setPose(cloudPosition); + pw.setPose(cloudPosition); + aw.setPose(cloudPosition); + cw.setPose(cloudPosition); + cyw.setPose(cloudPosition); + lw.setPose(cloudPosition); + cuw.setPose(cloudPosition); angle_x += 0.1f; angle_y -= 0.1f;