display model in the viewer

Author: pattacini

CMakeLists.txt

@@ -11,6 +11,7 @@ project(web-contest-sandbox VERSION 1.0.0)
 find_package(YCM REQUIRED)
 find_package(YARP 3.3.0 REQUIRED COMPONENTS os dev sig math pcl)
 find_package(ICUBcontrib REQUIRED)
+find_package (Eigen3 3.3 REQUIRED)
 find_package(PCL REQUIRED)
 find_package(VTK REQUIRED)
 message (STATUS "VTK_VERSION: ${VTK_VERSION}")
@@ -38,7 +39,7 @@ install(TARGETS ${PROJECT_NAME}-world LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDI
 add_executable(assignment_${PROJECT_NAME} ${CMAKE_SOURCE_DIR}/src/main.cpp)
 target_compile_definitions(assignment_${PROJECT_NAME} PRIVATE _USE_MATH_DEFINES)
 target_include_directories(assignment_${PROJECT_NAME} PRIVATE ${CMAKE_SOURCE_DIR}/src)
-target_link_libraries(assignment_${PROJECT_NAME} ${YARP_LIBRARIES} ${PCL_LIBRARIES} ${VTK_LIBRARIES})
+target_link_libraries(assignment_${PROJECT_NAME} ${YARP_LIBRARIES} Eigen3::Eigen ${PCL_LIBRARIES} ${VTK_LIBRARIES})
 
 if (VTK_VERSION VERSION_GREATER_EQUAL "8.90.0")
   vtk_module_autoinit(TARGETS assignment_${PROJECT_NAME} MODULES ${VTK_LIBRARIES})

app/scripts/assignment_web-contest-sandbox-icub.xml

@@ -24,7 +24,7 @@
   </connection>
 
   <connection>
-      <from>/SFM/disp:o</from>
+      <from>/DisparityModule/disp:o</from>
       <to>/depth:i</to>
       <protocol>unix_stream</protocol>
   </connection>

assets/objects-positions.png

@@ -6,15 +6,15 @@
         <geometry>
             <mesh>
                 <uri> model://fallback-1/mesh.stl </uri>
-                <scale>0.0025 0.0025 0.0025</scale>
+                <scale>0.002 0.002 0.002</scale>
             </mesh>
         </geometry>
       </collision>
       <visual name="visual">
         <geometry>
             <mesh>
                 <uri> model://fallback-1/mesh.stl </uri>
-                <scale>0.0025 0.0025 0.0025</scale>
+                <scale>0.002 0.002 0.002</scale>
             </mesh>
         </geometry>
       </visual>

gazebo/models/fallback-1/model.sdf

@@ -6,15 +6,15 @@
         <geometry>
             <mesh>
                 <uri> model://fallback-3/mesh.stl </uri>
-                <scale>0.0025 0.0025 0.0025</scale>
+                <scale>0.002 0.002 0.002</scale>
             </mesh>
         </geometry>
       </collision>
       <visual name="visual">
         <geometry>
             <mesh>
                 <uri> model://fallback-3/mesh.stl </uri>
-                <scale>0.0025 0.0025 0.0025</scale>
+                <scale>0.002 0.002 0.002</scale>
             </mesh>
         </geometry>
       </visual>

gazebo/models/fallback-3/model.sdf

@@ -7,6 +7,8 @@
 #include <string>
 #include <cmath>
 
+#include <Eigen/Eigen>
+
 #include <yarp/os/ResourceFinder.h>
 #include <yarp/os/RFModule.h>
 #include <yarp/os/Value.h>
@@ -42,13 +44,14 @@ class Module: public yarp::os::RFModule {
     yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelFloat>> depthPort;
     yarp::os::RpcServer rpcPort;
 
+    std::string object_model;
     std::unique_ptr<viewer::Viewer> viewer;
 
     /*********************************************************************************/
     bool configure(yarp::os::ResourceFinder& rf) override {
-        std::string object_model_file = rf.findFile("models/object/mesh.stl");
-        if (object_model_file.empty()) {
-            yError() << "Unable to locate \"models/object/mesh.stl\"";
+        object_model = rf.findFile("models/object/mesh.stl");
+        if (object_model.empty()) {
+            yError() << "Unable to locate the model";
             return false;
         }
 
@@ -210,7 +213,11 @@ class Module: public yarp::os::RFModule {
             explore_table(azimuth, elevation);
             auto pc = acquire_scene();
 
-            // FILL IN THE CODE
+            Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+            T(0,3) = -.45;
+            T(1,3) = 0.;
+            T(2,3) = -.1;
+            viewer->showModel(object_model, .0025, T);
             k = 1;
         }
 

src/main.cpp

@@ -9,10 +9,14 @@
 #define VIEWER_H
 
 #include <mutex>
+#include <string>
 #include <vector>
+#include <cmath>
 
 #include <vtkSmartPointer.h>
 #include <vtkCommand.h>
+#include <vtkSTLReader.h>
+#include <vtkTransform.h>
 #include <vtkPolyDataMapper.h>
 #include <vtkPointData.h>
 #include <vtkUnsignedCharArray.h>
@@ -29,6 +33,8 @@
 #include <vtkCamera.h>
 #include <vtkInteractorStyleSwitch.h>
 
+#include <Eigen/Eigen>
+
 #include <yarp/sig/PointCloud.h>
 
 namespace viewer {
@@ -82,6 +88,10 @@ class Viewer {
     vtkSmartPointer<vtkPolyData>               vtk_pc_polydata{nullptr};
     vtkSmartPointer<vtkVertexGlyphFilter>      vtk_pc_filter{nullptr};
     vtkSmartPointer<vtkActor>                  vtk_pc_actor{nullptr};
+    vtkSmartPointer<vtkSTLReader>              vtk_mdl_reader{nullptr};
+    vtkSmartPointer<vtkPolyDataMapper>         vtk_mdl_mapper{nullptr};
+    vtkSmartPointer<vtkTransform>              vtk_mdl_transform{nullptr};
+    vtkSmartPointer<vtkActor>                  vtk_mdl_actor{nullptr};
 
 public:
     /**************************************************************************/
@@ -177,6 +187,40 @@ class Viewer {
 
         vtk_renderer->AddActor(vtk_pc_actor);
     }
+
+    /**************************************************************************/
+    void showModel(const std::string& model, const double scale,
+                   const Eigen::Matrix4d& T) {
+        std::lock_guard<std::mutex> lck(mtx);
+        if (vtk_mdl_actor) {
+            vtk_renderer->RemoveActor(vtk_mdl_actor);
+        }
+
+        vtk_mdl_reader = vtkSmartPointer<vtkSTLReader>::New();
+        vtk_mdl_reader->SetFileName(model.c_str());
+        vtk_mdl_reader->Update();
+
+        vtk_mdl_mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
+        vtk_mdl_mapper->SetInputConnection(vtk_mdl_reader->GetOutputPort());
+
+        vtk_mdl_actor = vtkSmartPointer<vtkActor>::New();
+        vtk_mdl_actor->SetMapper(vtk_mdl_mapper);
+
+        Eigen::Vector3d v;
+        v(0) = T(2,1) - T(1,2);
+        v(1) = T(0,2) - T(2,0);
+        v(2) = T(1,0) - T(0,1);
+        const auto r = std::sqrt(v(0)*v(0) + v(1)*v(1) + v(2)*v(2));
+        const auto angle = (180. / M_PI) * std::atan2(.5 * r, .5 * (T(0,0) + T(1,1) + T(2,2) - 1.));
+
+        vtk_mdl_transform = vtkSmartPointer<vtkTransform>::New();
+        vtk_mdl_transform->Translate(T(0,3), T(1,3), T(2,3));
+        vtk_mdl_transform->RotateWXYZ(angle, v(0)/r, v(1)/r, v(2)/r);
+        vtk_mdl_transform->Scale(scale, scale, scale);
+        vtk_mdl_actor->SetUserTransform(vtk_mdl_transform);
+
+        vtk_renderer->AddActor(vtk_mdl_actor);
+    }
 };
 
 }