getShapePose/setShapePose simplification

little code beautification, formatting

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

@@ -25,41 +25,41 @@ namespace temp_viz
 
         void setBackgroundColor(const Color& color = Color::black());
 
-        void addCoordinateSystem(double scale, const Affine3f& t, const String &id = "coordinate");
+        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");
+        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(255,255,255));
-        void showPlane(const String &id, const Vec4f &coefs, const Color &color = Color(255,255,255));
-        void showPlane(const String &id, const Vec4f &coefs, const Point3f &pt, const Color &color = Color(255,255,255));
-        void showCube(const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color = Color(255,255,255));
-        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));
+        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);
-        bool setShapePose(const String &id, const Affine3f &pose);
+        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 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 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 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 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 addPolygon(const Mat& cloud, const Color& color, const String& id = "polygon");
 
-        bool addSphere (const Point3f &center, double radius, const Color& color, const String &id = "sphere");
+        bool addSphere (const Point3f &center, double radius, const Color& color, const String& id = "sphere");
 
 
         void spin ();

modules/viz/src/interactor_style.cpp

@@ -154,13 +154,13 @@ temp_viz::InteractorStyle::OnKeyDown ()
 {
     if (!init_)
     {
-        std::cout << "[PCLVisualizerInteractorStyle] Interactor style not initialized. Please call Initialize () before continuing" << std::endl;
+        std::cout << "Interactor style not initialized. Please call Initialize () before continuing" << std::endl;
         return;
     }
 
     if (!renderer_)
     {
-        std::cout << "[PCLVisualizerInteractorStyle] No renderer collection given! Use SetRendererCollection () before continuing." << std::endl;
+        std::cout << "No renderer given! Use SetRendererCollection () before continuing." << std::endl;
         return;
     }
 
@@ -186,21 +186,9 @@ temp_viz::InteractorStyle::OnKeyDown ()
     bool keymod = false;
     switch (modifier_)
     {
-    case KB_MOD_ALT:
-    {
-        keymod = alt;
-        break;
-    }
-    case KB_MOD_CTRL:
-    {
-        keymod = ctrl;
-        break;
-    }
-    case KB_MOD_SHIFT:
-    {
-        keymod = shift;
-        break;
-    }
+    case KB_MOD_ALT:   keymod = alt;   break;
+    case KB_MOD_CTRL:  keymod = ctrl;  break;
+    case KB_MOD_SHIFT: keymod = shift; break;
     }
 
     std::string key (Interactor->GetKeySym ());

modules/viz/src/precomp.hpp

@@ -6,12 +6,6 @@
 #include <list>
 #include <vector>
 
-/*
-#include <boost/function.hpp>
-#include <boost/bind.hpp>
-#include <boost/signals2.hpp>
-#include <boost/thread.hpp>*/
-
 #include <Eigen/Geometry>
 
 #if defined __GNUC__
@@ -140,24 +134,13 @@
 #include <vtkImageData.h>
 
 
-
 #include <vtkPolyDataNormals.h>
 #include <vtkMapper.h>
 #include <vtkSelectionNode.h>
-
-
-
 #include <vtkAbstractPicker.h>
 #include <vtkAbstractPropPicker.h>
-
 #include <vtkMatrix4x4.h>
 #include <vtkInteractorObserver.h>
-
-
-
-
-
-
 #include <vtkMapper2D.h>
 #include <vtkLeaderActor2D.h>
 #include <vtkAlgorithmOutput.h>
@@ -168,3 +151,6 @@
 #define __DEPRECATED
 #undef __DEPRECATED_DISABLED__
 #endif
+
+
+#include <q/shapes.h>

modules/viz/src/q/shapes.h

 #pragma once
 
-#include <Eigen/Core>
 #include <opencv2/viz/types.hpp>
-#include "precomp.hpp"
 
 namespace temp_viz
 {
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createLine (const cv::Point3f& pt1, const cv::Point3f& pt2);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createSphere (const cv::Point3f &center, float radius, int sphere_resolution = 10);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createCylinder (const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs);
-    CV_EXPORTS vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs, const Point3f& pt);
-    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);
-    /** \brief Allocate a new unstructured grid smartpointer. For internal use only.
-      * \param[out] polydata the resultant unstructured grid. 
-      */
-    CV_EXPORTS void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
+    vtkSmartPointer<vtkDataSet> createLine (const cv::Point3f& pt1, const cv::Point3f& pt2);
+    vtkSmartPointer<vtkDataSet> createSphere (const cv::Point3f &center, float radius, int sphere_resolution = 10);
+    vtkSmartPointer<vtkDataSet> createCylinder (const Point3f& pt_on_axis, const Point3f& axis_direction, double radius, int numsides = 30);
+    vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs);
+    vtkSmartPointer<vtkDataSet> createPlane (const Vec4f& coefs, const Point3f& pt);
+    vtkSmartPointer<vtkDataSet> create2DCircle (const Point3f& pt, double radius);
+    vtkSmartPointer<vtkDataSet> createCube(const Point3f& pt_min, const Point3f& pt_max);
+    vtkSmartPointer<vtkDataSet> createSphere (const Point3f& pt, double radius);
+    vtkSmartPointer<vtkDataSet> createArrow (const Point3f& pt1, const Point3f& pt2);
+
+    //brief Allocate a new unstructured grid smartpointer. For internal use only.
+    void allocVtkUnstructuredGrid (vtkSmartPointer<vtkUnstructuredGrid> &polydata);
 }

modules/viz/src/shapes.cpp

-#include <q/shapes.h>
+#include "precomp.hpp"
 
-inline float rad2deg (float alpha)
-{ return (alpha * 57.29578f); }
-
-inline double rad2deg (double alpha){return (alpha * 57.29578);}
+inline float rad2deg(float alpha) { return (alpha * 57.29578f); }
+inline double rad2deg(double alpha) { return (alpha * 57.29578); }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createCylinder (const cv::Point3f& pt_on_axis, const cv::Point3f& axis_direction, double radius, int numsides)
 {
@@ -23,21 +21,20 @@ vtkSmartPointer<vtkDataSet> temp_viz::createPlane (const cv::Vec4f& coefs)
 {
     vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
     plane->SetNormal (coefs[0], coefs[1], coefs[2]);
-    double norm = cv::norm (cv::Vec3f (coefs[0], coefs[1], coefs[2]));
+    double norm = cv::norm(cv::Vec3f(coefs.val));
     plane->Push (-coefs[3] / norm);
-    return (plane->GetOutput ());
+    return plane->GetOutput ();
 }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createPlane(const cv::Vec4f& coefs, const cv::Point3f& pt)
 {
     vtkSmartPointer<vtkPlaneSource> plane = vtkSmartPointer<vtkPlaneSource>::New ();
-    cv::Point3f coefs3 (coefs[0], coefs[1], coefs[2]);    
+    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]);
+    plane->SetNormal(coefs[0], coefs[1], coefs[2]);
 
-    double t = coefs3.dot (pt) + coefs[3];
-    cv::Vec3f p_center;
-    p_center = pt - coefs3 * t * norm_sqr;
+    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 ());
@@ -61,14 +58,14 @@ vtkSmartPointer<vtkDataSet> temp_viz::create2DCircle (const cv::Point3f& pt, dou
     tf->SetTransform (t);
     tf->SetInputConnection (disk->GetOutputPort ());
     
-    return (tf->GetOutput ());
+    return tf->GetOutput ();
 }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createCube(const cv::Point3f& pt_min, const cv::Point3f& pt_max)
 {
     vtkSmartPointer<vtkCubeSource> cube = vtkSmartPointer<vtkCubeSource>::New ();
     cube->SetBounds (pt_min.x, pt_max.x, pt_min.y, pt_max.y, pt_min.z, pt_max.z);
-    return (cube->GetOutput ());
+    return cube->GetOutput ();
 }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double radius)
@@ -81,7 +78,7 @@ vtkSmartPointer<vtkDataSet> temp_viz::createSphere (const Point3f& pt, double ra
     sphere->LatLongTessellationOff ();
     sphere->Update ();
     
-    return (sphere->GetOutput ());
+    return sphere->GetOutput ();
 }
 
 vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Point3f& pt2)
@@ -136,22 +133,21 @@ vtkSmartPointer<vtkDataSet> temp_viz::createArrow (const Point3f& pt1, const Poi
     transformPD->SetTransform(transform);
     transformPD->SetInputConnection(arrowSource->GetOutputPort());
     
-    return (transformPD->GetOutput());
+    return transformPD->GetOutput();
 }
 
-////////////////////////////////////////////////////////////////////////////////////////////
 vtkSmartPointer<vtkDataSet> temp_viz::createLine (const cv::Point3f& pt1, const cv::Point3f& pt2)
 {
-  vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::New ();
-  line->SetPoint1 (pt1.x, pt1.y, pt1.z);
-  line->SetPoint2 (pt2.x, pt2.y, pt2.z);
-  line->Update ();
-  return line->GetOutput ();
+    vtkSmartPointer<vtkLineSource> line = vtkSmartPointer<vtkLineSource>::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<vtkUnstructuredGrid> &polydata)
 {
-  polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();
+    polydata = vtkSmartPointer<vtkUnstructuredGrid>::New ();
 }
 
 

modules/viz/src/viz3d.cpp

@@ -9,16 +9,15 @@ temp_viz::Viz3d::Viz3d(const String& window_name) : impl_(new VizImpl(window_nam
 
 temp_viz::Viz3d::~Viz3d()
 {
-
+    delete impl_;
 }
 
-
 void temp_viz::Viz3d::setBackgroundColor(const Color& color)
 {
     impl_->setBackgroundColor(color);
 }
 
-void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const String &id)
+void temp_viz::Viz3d::addCoordinateSystem(double scale, const Affine3f& t, const String& id)
 {
     impl_->addCoordinateSystem(scale, t, id);
 }
@@ -38,22 +37,22 @@ bool temp_viz::Viz3d::addPointCloudNormals (const Mat &cloud, const Mat& normals
     return impl_->addPointCloudNormals(cloud, normals, level, scale, id);
 }
 
-bool temp_viz::Viz3d::addPolygonMesh (const Mesh3d& mesh, const String &id)
+bool temp_viz::Viz3d::addPolygonMesh (const Mesh3d& mesh, const String& id)
 {
     return impl_->addPolygonMesh(mesh, Mat(), id);
 }
 
-bool temp_viz::Viz3d::updatePolygonMesh (const Mesh3d& mesh, const String &id)
+bool temp_viz::Viz3d::updatePolygonMesh (const Mesh3d& mesh, const String& id)
 {
     return impl_->updatePolygonMesh(mesh, Mat(), id);
 }
 
-bool temp_viz::Viz3d::addPolylineFromPolygonMesh (const Mesh3d& mesh, const String &id)
+bool temp_viz::Viz3d::addPolylineFromPolygonMesh (const Mesh3d& mesh, const String& id)
 {
     return impl_->addPolylineFromPolygonMesh(mesh, id);
 }
 
-bool temp_viz::Viz3d::addText (const String &text, int xpos, int ypos, const Color& color, int fontsize, const String &id)
+bool temp_viz::Viz3d::addText (const String &text, int xpos, int ypos, const Color& color, int fontsize, const String& id)
 {
     return impl_->addText(text, xpos, ypos, color, fontsize, id);
 }
@@ -73,57 +72,57 @@ 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)
+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)
+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)
+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)
+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)
+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)
+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)
+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)
+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)
+cv::Affine3f temp_viz::Viz3d::getShapePose(const String& id)
 {
     return impl_->getShapePose(id);
 }
 
-bool temp_viz::Viz3d::setShapePose(const String &id, const Affine3f &pose)
+void temp_viz::Viz3d::setShapePose(const String& id, const Affine3f &pose)
 {
-    return impl_->setShapePose(id, pose);
+    impl_->setShapePose(id, pose);
 }
 
-bool temp_viz::Viz3d::removeCoordinateSystem (const String &id)
+bool temp_viz::Viz3d::removeCoordinateSystem (const String& id)
 {
     return impl_->removeCoordinateSystem(id);
 }

modules/viz/src/viz3d_impl.cpp

@@ -53,7 +53,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
 
     // If the cloud already exists, update otherwise create new one
     CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-    bool exist = (am_it == cloud_actor_map_->end());
+    bool exist = am_it == cloud_actor_map_->end();
     if (exist)
     {
         // Add as new cloud
@@ -181,7 +181,7 @@ void temp_viz::Viz3d::VizImpl::showPointCloud(const String& id, InputArray _clou
 
     // If the cloud already exists, update otherwise create new one
     CloudActorMap::iterator am_it = cloud_actor_map_->find (id);
-    bool exist = (am_it == cloud_actor_map_->end());
+    bool exist = am_it == cloud_actor_map_->end();
     if (exist)
     {
         // Add as new cloud
@@ -292,7 +292,7 @@ bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const
     CV_Assert(cloud.size() == normals.size() && cloud.type() == CV_32FC3 && normals.type() == CV_32FC3);
 
     if (cloud_actor_map_->find (id) != cloud_actor_map_->end ())
-        return (false);
+        return false;
 
     vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
     vtkSmartPointer<vtkCellArray> lines = vtkSmartPointer<vtkCellArray>::New();
@@ -380,11 +380,11 @@ bool temp_viz::Viz3d::VizImpl::addPointCloudNormals (const cv::Mat &cloud, const
 }
 
 
-void temp_viz::Viz3d::VizImpl::showLine (const String &id, const cv::Point3f &pt1, const cv::Point3f &pt2, const Color &color)
+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());
+    bool exists = am_it != shape_actor_map_->end();
     
     // If it exists just update
     if (exists)
@@ -416,11 +416,11 @@ void temp_viz::Viz3d::VizImpl::showLine (const String &id, const cv::Point3f &pt
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coefs, const Color &color)
+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());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -451,11 +451,11 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id, const cv::Vec4f &coe
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coefs, const cv::Point3f &pt, const Color &color)
+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());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -486,11 +486,11 @@ void temp_viz::Viz3d::VizImpl::showPlane (const String &id ,const cv::Vec4f &coe
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showCube (const String &id, const Point3f &pt1, const Point3f &pt2, const Color &color)
+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());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -521,11 +521,11 @@ void temp_viz::Viz3d::VizImpl::showCube (const String &id, const Point3f &pt1, c
     }
 }
 
-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)
+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());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -556,11 +556,11 @@ void temp_viz::Viz3d::VizImpl::showCylinder (const String &id, const Point3f &pt
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt, double radius, const Color &color)
+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());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -591,11 +591,11 @@ void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt,
     }
 }
 
-void temp_viz::Viz3d::VizImpl::showSphere (const String &id, const Point3f &pt, double radius, const Color &color)
+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());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     // If it exists just update
     if (exists)
@@ -626,11 +626,11 @@ 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)
+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());
+    bool exists = am_it != shape_actor_map_->end();
     Color c = vtkcolor(color);
     
     // If it exists just update
@@ -662,49 +662,24 @@ void temp_viz::Viz3d::VizImpl::showArrow (const String &id, const Point3f &pt1,
     }
 }
 
-cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String &id)
+cv::Affine3f temp_viz::Viz3d::VizImpl::getShapePose (const String& id)
 {
-    // Get the shape with the id and return the pose
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
-    
-    if (!exists)
-    {
-        std::cout << "[getShapePose] A shape with id <" << id << "> does not exist!" << std::endl;
-        return Affine3f();
-    }
 
-    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
-
-    vtkSmartPointer<vtkMatrix4x4> matrix = actor->GetUserMatrix();
-    
-    Matx44f pose_mat;
-    convertToCvMatrix(matrix, pose_mat);
-    
-    Affine3f pose;
-    pose.matrix = pose_mat;
-    return pose;
+    CV_Assert(am_it != shape_actor_map_->end());
+    vtkLODActor* actor = vtkLODActor::SafeDownCast(am_it->second);
+    return Affine3f(convertToMatx(actor->GetUserMatrix()));
 }
 
-bool temp_viz::Viz3d::VizImpl::setShapePose (const String &id, const Affine3f &pose)
+void temp_viz::Viz3d::VizImpl::setShapePose (const String& id, const Affine3f &pose)
 {
     ShapeActorMap::iterator am_it = shape_actor_map_->find (id);
-    bool exists = (am_it != shape_actor_map_->end());
-    
-    if (!exists)
-    {
-        return false;
-    }
+    CV_Assert(am_it != shape_actor_map_->end());
    
     vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
-    vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
-
-    convertToVtkMatrix (pose.matrix, matrix);
-
+    vtkSmartPointer<vtkMatrix4x4> matrix = convertToVtkMatrix(pose.matrix);
     actor->SetUserMatrix (matrix);
     actor->Modified ();
-
-    return (true);
 }
 
 bool temp_viz::Viz3d::VizImpl::addPolygonMesh (const Mesh3d& mesh, const Mat& mask, const std::string &id)

modules/viz/src/viz_main.cpp

@@ -97,14 +97,12 @@ void temp_viz::Viz3d::VizImpl::saveScreenshot (const std::string &file) { style_
 /////////////////////////////////////////////////////////////////////////////////////////////
 void temp_viz::Viz3d::VizImpl::registerMouseCallback(void (*callback)(const MouseEvent&, void*), void* cookie)
 {
-    // Register the callback function in the interactor style
     style_->registerMouseCallback(callback, cookie);
 }
 
 /////////////////////////////////////////////////////////////////////////////////////////////
 void temp_viz::Viz3d::VizImpl::registerKeyboardCallback(void (*callback)(const KeyboardEvent&, void*), void* cookie)
 {
-    // Register the callback function in the interactor style
     style_->registerKeyboardCallback(callback, cookie);
 }
 
@@ -1205,6 +1203,15 @@ void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointer<vtkMatr
             vtk_matrix->SetElement (i, k, m (i, k));
 }
 
+vtkSmartPointer<vtkMatrix4x4> temp_viz::convertToVtkMatrix (const cv::Matx44f &m)
+{
+    vtkSmartPointer<vtkMatrix4x4> vtk_matrix = vtkSmartPointer<vtkMatrix4x4>::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;
+}
+
 void temp_viz::convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, cv::Matx44f &m)
 {
     for (int i = 0; i < 4; i++)
@@ -1212,6 +1219,16 @@ void temp_viz::convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matri
             m(i,k) = vtk_matrix->GetElement (i, k);
 }
 
+
+cv::Matx44f temp_viz::convertToMatx(const vtkSmartPointer<vtkMatrix4x4>& 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;
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m)
 {