setShapePose and getShapePose implementations

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

@@ -38,6 +38,9 @@ namespace temp_viz
         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));
+        
+        Affine3f getShapePose(const String &id);
+        bool 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");

modules/viz/src/q/viz3d_impl.hpp

@@ -144,6 +144,8 @@ public:
     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);
+    Affine3f getShapePose (const String &id);
+    bool setShapePose (const String &id, const Affine3f &pose);
 
     bool addPolygon(const cv::Mat& cloud, const Color& color, const std::string &id = "polygon");
     bool addArrow (const cv::Point3f &pt1, const cv::Point3f &pt2, const Color& color, bool display_length, const std::string &id = "arrow");
@@ -422,6 +424,7 @@ private:
 //void convertToVtkMatrix (const Eigen::Matrix4f &m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
 
 void convertToVtkMatrix (const cv::Matx44f& m, vtkSmartPointer<vtkMatrix4x4> &vtk_matrix);
+void convertToCvMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, cv::Matx44f &m);
 
 /** \brief Convert origin and orientation to vtkMatrix4x4
       * \param[in] origin the point cloud origin

modules/viz/src/viz3d.cpp

@@ -108,6 +108,16 @@ void temp_viz::Viz3d::showCircle(const String &id, const Point3f &pt, double rad
     impl_->showCircle(id, pt, radius, color);
 }
 
+cv::Affine3f temp_viz::Viz3d::getShapePose(const String &id)
+{
+    return impl_->getShapePose(id);
+}
+
+bool temp_viz::Viz3d::setShapePose(const String &id, const Affine3f &pose)
+{
+    return impl_->setShapePose(id, pose);
+}
+
 bool temp_viz::Viz3d::removeCoordinateSystem (const String &id)
 {
     return impl_->removeCoordinateSystem(id);

modules/viz/src/viz3d_impl.cpp

@@ -477,6 +477,51 @@ void temp_viz::Viz3d::VizImpl::showCircle (const String &id, const Point3f &pt,
     }
 }
 
+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;
+}
+
+bool 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;
+    }
+   
+    vtkLODActor* actor = vtkLODActor::SafeDownCast (am_it->second);
+    vtkSmartPointer<vtkMatrix4x4> matrix = vtkSmartPointer<vtkMatrix4x4>::New ();
+
+    convertToVtkMatrix (pose.matrix, matrix);
+
+    actor->SetUserMatrix (matrix);
+    actor->Modified ();
+
+    return (true);
+}
+
 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 ());

modules/viz/src/viz_main.cpp

@@ -1335,6 +1335,13 @@ void temp_viz::convertToVtkMatrix (const cv::Matx44f &m, vtkSmartPointer<vtkMatr
             vtk_matrix->SetElement (i, k, m (i, k));
 }
 
+void temp_viz::convertToCvMatrix (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);
+}
+
 //////////////////////////////////////////////////////////////////////////////////////////////
 void temp_viz::convertToEigenMatrix (const vtkSmartPointer<vtkMatrix4x4> &vtk_matrix, Eigen::Matrix4f &m)
 {

modules/viz/test/test_viz3d.cpp

@@ -92,20 +92,21 @@ TEST(Viz_viz3d, accuracy)
     int col_blue = 0;
     int col_green = 0;
     int col_red = 0;
+    v.showCircle("circle1", cv::Point3f(0,0,0), fabs(1.0f), temp_viz::Color(0,255,0));
     
     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.showCircle("circle1", cv::Point3f(0,0,0), fabs(pos_x), temp_viz::Color(0,255,0));
+//         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);
         
         angle_x += 0.1f;
         angle_y -= 0.1f;