PCL modules detection

CMakeLists.txt

@@ -678,6 +678,9 @@ if(USE_PCL)
   # That's why here, we are using vp_find_pcl() macro that will set PCL_DEPS_INCLUDE_DIRS and PCL_DEPS_LIBRARIES
   # that contains also VTK material location.
   vp_find_pcl(PCL_LIBRARIES PCL_DEPS_INCLUDE_DIRS PCL_DEPS_LIBRARIES)
+  set(PCL_REQUIRED_COMPONENTS "common;filters;io;visualization;segmentation")
+  # Create cmake vars corresponding to pcl components used by ViSP like VISP_HAVE_PCL_COMMON...
+  vp_detect_required_pcl_components(PCL_REQUIRED_COMPONENTS)
 endif()
 
 # ----------------------------------------------------------------------------

cmake/PCLTools.cmake

@@ -386,3 +386,16 @@ macro(vp_find_pcl pcl_libraries pcl_deps_include_dirs pcl_deps_libraries)
   mark_as_advanced($<$<CONFIG:debug:bcrypt_LOCATION)    # Requested for pcl 1.13.1 on windows
   mark_as_advanced($<$<CONFIG:release:bcrypt_LOCATION)            # Requested for pcl 1.13.1 on windows
 endmacro()
+
+# Find pcl modules
+# IN: pcl_components
+# OUT: none
+#
+macro(vp_detect_required_pcl_components pcl_components)
+  foreach(component_ ${${pcl_components}})
+    string(TOUPPER "${component_}" COMPONENT)
+    if(PCL_${COMPONENT}_FOUND)
+      set(VISP_HAVE_PCL_${COMPONENT} TRUE)
+    endif()
+  endforeach()
+endmacro()

cmake/templates/vpConfig.h.in

@@ -480,6 +480,15 @@
 // Defined if Point Cloud Library is available
 #cmakedefine VISP_HAVE_PCL
 
+// Defined if required PCL components are found
+#if defined(VISP_HAVE_PCL)
+#cmakedefine VISP_HAVE_PCL_COMMON
+#cmakedefine VISP_HAVE_PCL_FILTERS
+#cmakedefine VISP_HAVE_PCL_IO
+#cmakedefine VISP_HAVE_PCL_SEGMENTATION
+#cmakedefine VISP_HAVE_PCL_VISUALIZATION
+#endif
+
 // Defined if libdmtx is available for bar code detection
 #cmakedefine VISP_HAVE_DMTX
 

example/device/framegrabber/grabRealSense2.cpp

@@ -87,17 +87,19 @@ int main()
                                   (unsigned int)rs.getIntrinsics(RS2_STREAM_DEPTH).width);
     vpImage<uint16_t> depth(depth_display.getHeight(), depth_display.getWidth());
 
-#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS)
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS)
     std::mutex mutex;
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+#if defined(VISP_HAVE_PCL_VISUALIZATION)
     vpDisplayPCL pcl_viewer(color.getWidth() + 80, color.getHeight() + 70, "3D viewer " + vpTime::getDateTime());
     pcl_viewer.startThread(std::ref(mutex), pointcloud_color);
 #endif
+#endif
 
 #if defined(VISP_HAVE_X11)
     vpDisplayX dc(color, 10, 10, "Color image");
-    vpDisplayX di(infrared, (int)color.getWidth() + 80, 10, "Infrared image");
-    vpDisplayX dd(depth_display, 10, (int)color.getHeight() + 70, "Depth image");
+    vpDisplayX di(infrared, static_cast<int>(color.getWidth()) + 80, 10, "Infrared image");
+    vpDisplayX dd(depth_display, 10, static_cast<int>(color.getHeight()) + 70, "Depth image");
 #elif defined(VISP_HAVE_GDI)
     vpDisplayGDI dc(color, 10, 10, "Color image");
     vpDisplayGDI di(infrared, color.getWidth() + 80, 10, "Infrared image");
@@ -107,14 +109,14 @@ int main()
     while (true) {
       double t = vpTime::measureTimeMs();
 
-#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS)
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS)
       {
         std::lock_guard<std::mutex> lock(mutex);
-        rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, nullptr, pointcloud_color,
-                   (unsigned char *)infrared.bitmap);
+        rs.acquire(reinterpret_cast<unsigned char *>(color.bitmap), reinterpret_cast<unsigned char *>(depth.bitmap), nullptr, pointcloud_color,
+                   reinterpret_cast<unsigned char *>(infrared.bitmap));
       }
 #else
-      rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, nullptr, (unsigned char *)infrared.bitmap);
+      rs.acquire(reinterpret_cast<unsigned char *>(color.bitmap), reinterpret_cast<unsigned char *>(depth.bitmap), nullptr, reinterpret_cast<unsigned char *>(infrared.bitmap));
 #endif
 
       vpImageConvert::createDepthHistogram(depth, depth_display);

example/device/framegrabber/readRealSenseData.cpp

@@ -48,6 +48,7 @@
 #include <thread>
 
 #include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpIoException.h>
 #include <visp3/core/vpIoTools.h>
 #include <visp3/gui/vpDisplayGDI.h>
 #include <visp3/gui/vpDisplayX.h>
@@ -57,8 +58,15 @@
 #include <visp3/io/vpVideoWriter.h>
 
 #if defined(VISP_HAVE_PCL)
+#include <pcl/pcl_config.h>
+#if defined(VISP_HAVE_PCL_COMMON)
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+#endif
+#if defined(VISP_HAVE_PCL_IO)
 #include <pcl/io/pcd_io.h>
 #endif
+#endif
 
 #define GETOPTARGS "ci:bodh"
 
@@ -155,7 +163,7 @@ bool getOptions(int argc, const char *argv[], std::string &input_directory, bool
 
 bool readData(int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_color, vpImage<uint16_t> &I_depth_raw,
               bool pointcloud_binary_format
-#if defined(VISP_HAVE_PCL)
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
               , pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud
 #endif
 )
@@ -236,9 +244,13 @@ bool readData(int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_co
     }
   }
   else {
+#if defined(VISP_HAVE_PCL_IO)
     if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename_pointcloud, *point_cloud) == -1) {
       std::cerr << "Cannot read PCD: " << filename_pointcloud << std::endl;
     }
+#else
+    throw(vpIoException(vpIoException::ioError, "Cannot read pcd file without PCL io module"));
+#endif
   }
 #endif
 
@@ -273,7 +285,9 @@ int main(int argc, const char *argv[])
 #if defined(VISP_HAVE_PCL)
   std::mutex mutex;
   pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+#if defined(VISP_HAVE_PCL_VISUALIZATION)
   vpDisplayPCL pcl_viewer;
+#endif
 #endif
 
   vpVideoWriter writer;
@@ -289,7 +303,7 @@ int main(int argc, const char *argv[])
   while (!quit) {
     double t = vpTime::measureTimeMs();
 
-#if defined(VISP_HAVE_PCL)
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
     {
       std::lock_guard<std::mutex> lock(mutex);
       quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format, pointcloud);
@@ -311,7 +325,7 @@ int main(int argc, const char *argv[])
       else {
         d2.init(I_depth, I_color.getWidth() + 10, 0, "Depth image");
       }
-#if defined(VISP_HAVE_PCL)
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
       pcl_viewer.setPosition(I_color.getWidth() + 10, I_color.getHeight() + 70);
       pcl_viewer.setWindowName("3D point cloud");
       pcl_viewer.startThread(std::ref(mutex), pointcloud);

example/device/framegrabber/saveRealSenseData.cpp

@@ -49,11 +49,22 @@
 #include <thread>
 
 #if defined(VISP_HAVE_PCL)
-#include <pcl/common/common.h>
+#include <pcl/pcl_config.h>
+#if defined(VISP_HAVE_PCL_COMMON)
+#if PCL_VERSION_COMPARE(>=,1,14,1)
+#include <pcl/impl/point_types.hpp>
+#else
+#include <pcl/point_types.h>
+#endif
+#include <pcl/point_cloud.h>
+#endif
+#if defined(VISP_HAVE_PCL_IO)
 #include <pcl/io/pcd_io.h>
 #endif
+#endif
 
 #include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpIoException.h>
 #include <visp3/core/vpIoTools.h>
 #include <visp3/core/vpXmlParserCamera.h>
 #include <visp3/gui/vpDisplayGDI.h>
@@ -216,7 +227,7 @@ class vpFrameQueue
 
   // Push data to save in the queue (FIFO)
   void push(const vpImage<vpRGBa> &colorImg, const vpImage<uint16_t> &depthImg,
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
             const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
 #else
             const std::vector<vpColVector> &pointCloud,
@@ -255,7 +266,7 @@ class vpFrameQueue
 
   // Pop the image to save from the queue (FIFO)
   void pop(vpImage<vpRGBa> &colorImg, vpImage<uint16_t> &depthImg,
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
            pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
 #else
            std::vector<vpColVector> &pointCloud,
@@ -294,7 +305,7 @@ class vpFrameQueue
   std::condition_variable m_cond;
   std::queue<vpImage<vpRGBa>> m_queueColor;
   std::queue<vpImage<uint16_t>> m_queueDepth;
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
   std::queue<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_queuePointCloud;
 #else
   std::queue<std::vector<vpColVector>> m_queuePointCloud;
@@ -334,7 +345,7 @@ class vpStorageWorker
     try {
       vpImage<vpRGBa> colorImg;
       vpImage<uint16_t> depthImg;
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
       pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud;
 #else
       std::vector<vpColVector> pointCloud;
@@ -388,7 +399,7 @@ class vpStorageWorker
               std::ofstream file_pointcloud(filename_point_cloud.c_str(), std::ios::out | std::ios::binary);
 
               if (file_pointcloud.is_open()) {
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
                 uint32_t width = pointCloud->width;
                 uint32_t height = pointCloud->height;
                 // true if pointcloud does not contain NaN or Inf, not handled currently
@@ -406,7 +417,7 @@ class vpStorageWorker
                     vpIoTools::writeBinaryValueLE(file_pointcloud, pt.y);
                     vpIoTools::writeBinaryValueLE(file_pointcloud, pt.z);
                   }
-                }
+              }
 #else
                 uint32_t width = m_size_width;
                 uint32_t height = m_size_height;
@@ -429,14 +440,16 @@ class vpStorageWorker
                   }
                 }
 #endif
-              }
             }
+          }
             else {
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO)
               pcl::io::savePCDFileBinary(filename_point_cloud, *pointCloud);
+#elif defined(VISP_HAVE_PCL)
+              throw(vpIoException(vpIoException::fatalError, "Cannot save as pcd files without PCL io module"));
 #endif
             }
-          }
+        }
 
           if (m_save_infrared) {
             ss.str("");
@@ -448,13 +461,13 @@ class vpStorageWorker
           }
 
           m_cpt++;
-        }
       }
     }
+  }
     catch (const vpFrameQueue::vpCancelled_t &) {
       std::cout << "Receive cancel vpFrameQueue." << std::endl;
     }
-  }
+}
 
 private:
   vpFrameQueue &m_queue;
@@ -614,12 +627,12 @@ int main(int argc, const char *argv[])
     vpHomogeneousMatrix depth_M_color;
     if (!use_aligned_stream) {
       depth_M_color = realsense.getTransformation(rs::stream::color, rs::stream::depth);
-    }
+  }
 #endif
     std::ofstream file(std::string(output_directory + "/depth_M_color.txt"));
     depth_M_color.save(file);
     file.close();
-  }
+}
 
   vpFrameQueue save_queue;
   vpStorageWorker storage(std::ref(save_queue), std::cref(output_directory), save_color, save_depth, save_pointcloud,
@@ -637,23 +650,23 @@ int main(int argc, const char *argv[])
 
   int nb_saves = 0;
   bool quit = false;
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
   pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZ>);
 #else
   std::vector<vpColVector> pointCloud;
 #endif
   while (!quit) {
     if (use_aligned_stream) {
 #ifdef USE_REALSENSE2
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
       realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud, nullptr,
                         &align_to);
 #else
       realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud, nullptr,
                         &align_to);
 #endif
 #else
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
       realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
                         (unsigned char *)I_infrared.bitmap, nullptr, rs::stream::rectified_color,
                         rs::stream::depth_aligned_to_rectified_color);
@@ -665,7 +678,7 @@ int main(int argc, const char *argv[])
 #endif
     }
     else {
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
       realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
                         (unsigned char *)I_infrared.bitmap, nullptr);
 #else
@@ -695,7 +708,7 @@ int main(int argc, const char *argv[])
     vpDisplay::flush(I_infrared);
 
     if (save && !click_to_save) {
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
       pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy = pointCloud->makeShared();
       save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared);
 #else
@@ -714,7 +727,7 @@ int main(int argc, const char *argv[])
         case vpMouseButton::button1:
           if (save) {
             nb_saves++;
-#ifdef VISP_HAVE_PCL
+#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
             pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy = pointCloud->makeShared();
             save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared);
 #else

modules/core/include/visp3/core/vpExponentialMap.h

@@ -52,33 +52,33 @@
   The exponential map gives the relationship between the velocity of a moving
   body and its displacement:
 
-  \f[ \exp({^c}{\bf v}_c) = {^{c(t)}}{\bf M}_{c(t+\Delta t)} \f]
+  \f[ \exp({^c}{\bf v}_c(t - \Delta t)) = {^{c(t - \Delta t)}}{\bf M}_{c(t)} \f]
 
-  where \f$ {^c}{\bf v}_c\f$ is the velocity skew vector applied during \f$\Delta t\f$
-  seconds at point \f$ c \f$ in frame \f$ c \f$, while \f$ {^{c(t)}}{\bf M}_{c(t+\Delta t)} \f$
+  where \f$ {^c}{\bf v}_c(t - \Delta t)\f$ is the velocity skew vector at the previous iteration applied during \f$\Delta t\f$
+  seconds at point \f$ c \f$ in frame \f$ c \f$, while \f$ {^{c(t- \Delta t)}}{\bf M}_{c(t)} \f$
   is the corresponding displacement.
 
   This class allows to compute the direct or the inverse exponential map.
 
   - The direct exponential map allows to compute the displacement
-    \f${^{c(t)}}{\bf M}_{c(t+\Delta t)}\f$ using \f${^c}{\bf v}_c\f$ as input:
-    \f[ {^{o}}{\bf M}_{c(t+\Delta t)} = {^{o}}{\bf M}_{c(t)} \exp({^c}{\bf v}_c) \f]
+    \f${^{c(t - \Delta t)}}{\bf M}_{c(t)}\f$ using \f${^c}{\bf v}_c(t - \Delta t)\f$ as input:
+    \f[ {^{o}}{\bf M}_{c(t)} = {^{o}}{\bf M}_{c(t - \Delta t)} \exp({^c}{\bf v}_c(t - \Delta t)) \f]
     where \f$ o \f$ is a reference frame.
-    With direct(), the velocity skew vector \f$ {^c}{\bf v}_c \f$ is applied during 1 second
+    With direct(), the velocity skew vector \f$ {^c}{\bf v}_c(t - \Delta t) \f$ is applied during 1 second
     considering \f$ \Delta t = 1\f$. With direct(const vpColVector &, const double &)
     the sampling time can be set to an other value where the second
     argument is \f$ \Delta t \f$.
 
   - The inverse exponential map allows to compute the velocity skew vector \f$
-    \bf {^c}{\bf v}_c \f$ from the displacement \f$ {^{c(t)}}{\bf M}_{c(t+\Delta t)}\f$
+    {^c}{\bf v}_c(t - \Delta t) \f$ from the displacement \f$ {^{c(t - \Delta t)}}{\bf M}_{c(t)}\f$
     measured during a time interval \f$ \Delta t \f$. With inverse() the time interval
     also called sampling time is set to 1 second. With
     inverse(const vpHomogeneousMatrix &, const double &) the sampling time can
     be set to an other value where the second
     argument is \f$ \Delta t \f$.
 
   A displacement \f$ \bf M \f$ is represented as an homogeneous matrix implemented in
-  vpHomogeneousMatrix. A velocities \f$ \bf v \f$ is represented as a
+  vpHomogeneousMatrix. A velocity \f$ \bf v \f$ is represented as a
   6 dimension velocity skew vector \f$ [v, \omega] \f$, where \f$ v \f$
   is a velocity translation vector with values in m/s and \f$ \omega \f$ a
   velocity rotation vector with values expressed in rad/s.