Merge branch 'kinfu-rgb'

Author: PeterBowman

CMakeLists.txt

@@ -39,7 +39,7 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake
 # Hard dependencies.
 find_package(YCM 0.11 REQUIRED)
 find_package(YARP 3.4 REQUIRED COMPONENTS os dev sig
-                               OPTIONAL_COMPONENTS cv pcl)
+                               OPTIONAL_COMPONENTS cv idl_tools pcl)
 
 # Soft dependencies.
 find_package(OpenCV QUIET)

examples/cpp/CMakeLists.txt

@@ -12,7 +12,10 @@ if(ENABLE_examples)
         add_subdirectory(exampleMeshFromCloud)
         add_subdirectory(exampleMeshFromLiveRGBD)
         add_subdirectory(exampleProcessCloud)
-        add_subdirectory(exampleSceneReconstructionClient)
+
+        if(TARGET ROBOTICSLAB::VisionIDL)
+            add_subdirectory(exampleSceneReconstructionClient)
+        endif()
     endif()
 
     add_subdirectory(exampleRemoteGrabber)

examples/cpp/exampleSceneReconstructionClient/CMakeLists.txt

@@ -6,7 +6,7 @@ if(NOT YARP_FOUND)
     find_package(YARP 3.4 REQUIRED COMPONENTS os sig)
 endif()
 
-if(NOT TARGET ROBOTICSLAB::YarpCloudUtils)
+if(NOT TARGET ROBOTICSLAB::YarpCloudUtils OR NOT TARGET ROBOTICSLAB::VisionIDL)
     find_package(ROBOTICSLAB_VISION REQUIRED)
 endif()
 
@@ -15,4 +15,5 @@ add_executable(exampleSceneReconstructionClient main.cpp)
 target_link_libraries(exampleSceneReconstructionClient YARP::YARP_os
                                                        YARP::YARP_init
                                                        YARP::YARP_sig
-                                                       ROBOTICSLAB::YarpCloudUtils)
+                                                       ROBOTICSLAB::YarpCloudUtils
+                                                       ROBOTICSLAB::VisionIDL)

examples/cpp/exampleSceneReconstructionClient/main.cpp

@@ -4,29 +4,22 @@
  * @brief Sample usage of @ref sceneReconstruction.
  */
 
-#include <yarp/conf/version.h>
-
 #include <yarp/os/LogStream.h>
 #include <yarp/os/Network.h>
 #include <yarp/os/ResourceFinder.h>
 #include <yarp/os/RpcClient.h>
 #include <yarp/os/SystemClock.h>
 #include <yarp/os/Value.h>
-#include <yarp/os/Vocab.h>
+
 #include <yarp/sig/PointCloud.h>
 
 #include <YarpCloudUtils.hpp>
+#include <SceneReconstructionIDL.h>
 
 constexpr auto DEFAULT_REMOTE = "/sceneReconstruction";
 constexpr auto DEFAULT_PREFIX = "/exampleSceneReconstructionClient";
 constexpr auto DEFAULT_COLLECTION = "meshPipeline";
 
-#if YARP_VERSION_MINOR >= 5
-constexpr auto VOCAB_GET_POINTS = yarp::os::createVocab32('g','p','c');
-#else
-constexpr auto VOCAB_GET_POINTS = yarp::os::createVocab('g','p','c');
-#endif
-
 int main(int argc, char * argv[])
 {
     yarp::os::Network yarp;
@@ -69,20 +62,22 @@ int main(int argc, char * argv[])
         return 1;
     }
 
-    yarp::os::Bottle cmd {yarp::os::Value(VOCAB_GET_POINTS, true)};
-    yarp::sig::PointCloudXYZ cloud;
+    roboticslab::SceneReconstructionIDL sceneReconstruction;
+    sceneReconstruction.yarp().attachAsClient(rpc);
+
+    auto result = sceneReconstruction.getPointsWithNormals();
 
-    if (!rpc.write(cmd, cloud))
+    if (!result.ret)
     {
         yError() << "Unable to send remote command";
         return 1;
     }
 
-    yInfo() << "Got cloud of" << cloud.size() << "points";
+    yInfo() << "Got cloud of" << result.pointsWithNormals.size() << "points";
 
     if (!fileCloud.empty())
     {
-        if (!roboticslab::YarpCloudUtils::savePLY(fileCloud, cloud, binary))
+        if (!roboticslab::YarpCloudUtils::savePLY(fileCloud, result.pointsWithNormals, binary))
         {
             yWarning() << "Unable to export cloud to" << fileCloud;
         }
@@ -94,12 +89,12 @@ int main(int argc, char * argv[])
 
     if (!fileMesh.empty())
     {
-        yarp::sig::PointCloudXYZ meshPoints;
+        yarp::sig::PointCloudXYZRGBA meshPoints;
         yarp::sig::VectorOf<int> meshIndices;
 
         auto start = yarp::os::SystemClock::nowSystem();
 
-        if (!roboticslab::YarpCloudUtils::meshFromCloud(cloud, meshPoints, meshIndices, options, collection))
+        if (!roboticslab::YarpCloudUtils::meshFromCloud(result.pointsWithNormals, meshPoints, meshIndices, options, collection))
         {
             yWarning() << "Unable to reconstruct surface from cloud";
         }

libraries/CMakeLists.txt

@@ -1,7 +1,5 @@
-# Copyright: Universidad Carlos III de Madrid (C) 2013
-# Authors: Juan G. Victores
-
 add_subdirectory(TravisLib)
+add_subdirectory(VisionIDL)
 add_subdirectory(YarpCloudUtils)
 add_subdirectory(YarpCropCallback)
 add_subdirectory(YarpPlugins)

libraries/VisionIDL/CMakeLists.txt

@@ -0,0 +1,46 @@
+if(NOT YARP_idl_tools_FOUND AND (NOT DEFINED ENABLE_VisionIDL OR ENABLE_VisionIDL))
+    message(WARNING "YARP idl_tools component not found, disabling VisionIDL library")
+endif()
+
+cmake_dependent_option(ENABLE_VisionIDL "Enable/disable VisionIDL library" ON
+                       YARP_idl_tools_FOUND OFF)
+
+if(ENABLE_VisionIDL)
+
+    set(ALLOW_IDL_GENERATION ON CACHE BOOL "Detect changes and rebuild IDL files")
+    set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS vision.thrift)
+
+    yarp_idl_to_dir(INPUT_FILES vision.thrift
+                    OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}
+                    SOURCES_VAR idl_sources
+                    HEADERS_VAR idl_headers
+                    INCLUDE_DIRS_VAR idl_include_dirs
+                    PLACEMENT MERGED
+                    THRIFT_NO_NAMESPACE_PREFIX)
+
+    add_library(VisionIDL SHARED ${idl_sources} ${idl_headers})
+
+    set_target_properties(VisionIDL PROPERTIES PUBLIC_HEADER "${idl_headers}")
+
+    target_link_libraries(VisionIDL PUBLIC YARP::YARP_os
+                                           YARP::YARP_sig)
+
+    target_include_directories(VisionIDL PUBLIC $<BUILD_INTERFACE:${idl_include_dirs}>
+                                                $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>)
+
+    install(TARGETS VisionIDL
+            EXPORT ROBOTICSLAB_SPEECH
+            LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+            ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+            RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+            PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR})
+
+    set_property(GLOBAL APPEND PROPERTY _exported_targets VisionIDL)
+
+    add_library(ROBOTICSLAB::VisionIDL ALIAS VisionIDL)
+
+else()
+
+    set(ENABLE_VisionIDL OFF CACHE BOOL "Enable/disable VisionIDL library" FORCE)
+
+endif()

libraries/VisionIDL/vision.thrift

@@ -0,0 +1,65 @@
+namespace yarp roboticslab
+
+struct yarp_sig_Matrix {
+} (
+    yarp.name = "yarp::sig::Matrix"
+    yarp.includefile = "yarp/sig/Matrix.h"
+)
+
+struct yarp_sig_PointCloudXYZ {
+} (
+    yarp.name = "yarp::sig::PointCloudXYZ"
+    yarp.includefile = "yarp/sig/PointCloud.h"
+)
+
+struct yarp_sig_PointCloudXYZNormalRGBA {
+} (
+    yarp.name = "yarp::sig::PointCloudXYZNormalRGBA"
+    yarp.includefile = "yarp/sig/PointCloud.h"
+)
+
+struct return_pose {
+    1: bool ret = false;
+    2: yarp_sig_Matrix pose;
+}
+
+struct return_points {
+    1: bool ret = false;
+    2: yarp_sig_PointCloudXYZ points;
+}
+
+struct return_points_with_normals {
+    1: bool ret = false;
+    2: yarp_sig_PointCloudXYZNormalRGBA pointsWithNormals;
+}
+
+service SceneReconstructionIDL
+{
+    /**
+     * pause the scene reconstruction process
+     */
+    void pause();
+
+    /**
+     * start/resume the scene reconstruction process
+     */
+    void resume();
+
+    /**
+     * get current camera pose
+     * @return struct {bool ret, yarp_sig_Matrix pose}
+     */
+    return_pose getPose();
+
+    /**
+     * get the resulting point cloud
+     * @return struct {bool ret, yarp_sig_PointCloudXYZ points}
+     */
+    return_points getPoints();
+
+    /**
+     * get the resulting point cloud with normals and color data (if available)
+     * @return struct {bool ret, yarp_sig_PointCloudXYZNormalRGBA pointsWithNormals}
+     */
+    return_points_with_normals getPointsWithNormals();
+}

programs/sceneReconstruction/CMakeLists.txt

@@ -12,7 +12,7 @@ if(NOT DEFINED ENABLE_sceneReconstruction OR ENABLE_sceneReconstruction)
 endif()
 
 cmake_dependent_option(ENABLE_sceneReconstruction "Enable/disable sceneReconstruction program" ON
-                       "TARGET opencv_rgbd;OpenCV_VERSION VERSION_GREATER_EQUAL 4.0;_opencv_nonfree;YARP_cv_FOUND" OFF)
+                       "TARGET opencv_rgbd;OpenCV_VERSION VERSION_GREATER_EQUAL 4.0;_opencv_nonfree;YARP_cv_FOUND;ENABLE_VisionIDL" OFF)
 
 if(ENABLE_sceneReconstruction)
 
@@ -31,7 +31,8 @@ if(ENABLE_sceneReconstruction)
                                               YARP::YARP_dev
                                               YARP::YARP_sig
                                               YARP::YARP_cv
-                                              opencv_rgbd)
+                                              opencv_rgbd
+                                              ROBOTICSLAB::VisionIDL)
 
     if(OpenCV_VERSION VERSION_GREATER_EQUAL 4.2)
         target_sources(sceneReconstruction PRIVATE DynaFu.cpp)
@@ -43,7 +44,7 @@ if(ENABLE_sceneReconstruction)
         target_compile_definitions(sceneReconstruction PRIVATE HAVE_KINFU_LS)
     endif()
 
-    if(OpenCV_VERSION VERSION_GREATER_EQUAL 4.5.3)
+    if(OpenCV_VERSION VERSION_GREATER_EQUAL 4.5.5)
         target_sources(sceneReconstruction PRIVATE ColoredKinFu.cpp)
         target_compile_definitions(sceneReconstruction PRIVATE HAVE_COLORED_KINFU)
     endif()

programs/sceneReconstruction/ColoredKinFu.cpp

@@ -10,21 +10,49 @@
 
 using namespace roboticslab;
 
+template <>
+void KinectFusionImpl<cv::colored_kinfu::ColoredKinFu>::getCloud(yarp::sig::PointCloudXYZNormalRGBA & cloudWithNormals) const
+{
+    cv::Mat points, normals, colors;
+
+    mtx.lock();
+    handle->getCloud(points, normals, colors);
+    mtx.unlock();
+
+    cloudWithNormals.resize(points.rows);
+
+    for (auto i = 0; i < points.rows; i++)
+    {
+        using color_t = unsigned char;
+        const auto & point = points.at<cv::Vec4f>(i);
+        const auto & normal = normals.at<cv::Vec4f>(i);
+        const auto & color = colors.at<cv::Vec4f>(i);
+
+        cloudWithNormals(i) = {
+            {point[0], point[1], point[2]},
+            {normal[0], normal[1], normal[2]},
+            {static_cast<color_t>(color[0]), static_cast<color_t>(color[1]), static_cast<color_t>(color[2]), static_cast<color_t>(color[3])}
+        };
+    }
+}
+
 template <>
 bool KinectFusionImpl<cv::colored_kinfu::ColoredKinFu>::update(const yarp::sig::ImageOf<yarp::sig::PixelFloat> & depthFrame,
-                                                               const yarp::sig::FlexImage & rgbFrame)
+                                                               const yarp::sig::FlexImage & colorFrame)
 {
     // Cast away constness so that toCvMat accepts the YARP image. This function
     // does not alter the inner structure of PixelFloat images anyway.
     auto & nonConstDepthFrame = const_cast<yarp::sig::ImageOf<yarp::sig::PixelFloat> &>(depthFrame);
     cv::Mat depthMat = yarp::cv::toCvMat(nonConstDepthFrame);
 
     yarp::sig::ImageOf<yarp::sig::PixelBgr> bgrFrame;
-    bgrFrame.copy(rgbFrame);
+    bgrFrame.copy(colorFrame); // convert to BGR (probably from RGB)
     cv::Mat bgrMat = yarp::cv::toCvMat(bgrFrame);
 
     cv::UMat depthUmat;
     depthMat.convertTo(depthUmat, depthMat.type(), 1000.0); // OpenCV uses milimeters
+
+    std::lock_guard<std::mutex> lock(mtx);
     return handle->update(depthUmat, bgrMat);
 }
 
@@ -42,9 +70,9 @@ namespace roboticslab
 
 std::unique_ptr<KinectFusion> makeColoredKinFu(const yarp::os::Searchable & config,
                                                const yarp::sig::IntrinsicParams & depthIntrinsic,
-                                               const yarp::sig::IntrinsicParams & rgbIntrinsic,
+                                               const yarp::sig::IntrinsicParams & colorIntrinsic,
                                                int depthWidth, int depthHeight,
-                                               int rgbWidth, int rgbHeight)
+                                               int colorWidth, int colorHeight)
 {
     using Params = cv::colored_kinfu::Params;
 
@@ -65,20 +93,20 @@ std::unique_ptr<KinectFusion> makeColoredKinFu(const yarp::os::Searchable & conf
     yCInfo(KINFU) << "principal point (X):" << depthIntrinsic.principalPointX;
     yCInfo(KINFU) << "principal point (Y):" << depthIntrinsic.principalPointY;
 
-    yCInfo(KINFU) << "--- CAMERA PARAMETERS (RGB) ---";
+    yCInfo(KINFU) << "--- CAMERA PARAMETERS (color) ---";
 
-    params->rgb_frameSize = cv::Size(rgbWidth, rgbHeight);
-    yCInfo(KINFU) << "width:" << rgbWidth;
-    yCInfo(KINFU) << "height:" << rgbHeight;
+    params->rgb_frameSize = cv::Size(colorWidth, colorHeight);
+    yCInfo(KINFU) << "width:" << colorWidth;
+    yCInfo(KINFU) << "height:" << colorHeight;
 
-    params->rgb_intr = cv::Matx33f(rgbIntrinsic.focalLengthX,                         0, rgbIntrinsic.principalPointX,
-                                                           0, rgbIntrinsic.focalLengthY, rgbIntrinsic.principalPointY,
-                                                           0,                         0,                            1);
+    params->rgb_intr = cv::Matx33f(colorIntrinsic.focalLengthX,                           0, colorIntrinsic.principalPointX,
+                                                             0, colorIntrinsic.focalLengthY, colorIntrinsic.principalPointY,
+                                                             0,                           0,                              1);
 
-    yCInfo(KINFU) << "focal length (X):" << rgbIntrinsic.focalLengthX;
-    yCInfo(KINFU) << "focal length (Y):" << rgbIntrinsic.focalLengthY;
-    yCInfo(KINFU) << "principal point (X):" << rgbIntrinsic.principalPointX;
-    yCInfo(KINFU) << "principal point (Y):" << rgbIntrinsic.principalPointY;
+    yCInfo(KINFU) << "focal length (X):" << colorIntrinsic.focalLengthX;
+    yCInfo(KINFU) << "focal length (Y):" << colorIntrinsic.focalLengthY;
+    yCInfo(KINFU) << "principal point (X):" << colorIntrinsic.principalPointX;
+    yCInfo(KINFU) << "principal point (Y):" << colorIntrinsic.principalPointY;
 
     yCInfo(KINFU) << "--- ALGORITHM PARAMETERS ---";
 
@@ -203,7 +231,7 @@ std::unique_ptr<KinectFusion> makeColoredKinFu(const yarp::os::Searchable & conf
         yCInfo(KINFU) << "volumePoseTransl (DEFAULT):" << transl[0] << transl[1] << transl[2];
     }
 
-    if (config.check("volumeType", "type of voxel volume (tsdf, hashtsdf)"))
+    if (config.check("volumeType", "type of voxel volume (tsdf, hashtsdf, coloredtsdf)"))
     {
         std::string volumeType = config.find("volumeType").asString();