diff --git a/CMakeLists.txt b/CMakeLists.txt index b94225d85d..2ea90a04ea 100644 --- a/CMakeLists.txt +++ b/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() # ---------------------------------------------------------------------------- diff --git a/cmake/PCLTools.cmake b/cmake/PCLTools.cmake index ee1099c80a..ba7f2efa11 100644 --- a/cmake/PCLTools.cmake +++ b/cmake/PCLTools.cmake @@ -386,3 +386,16 @@ macro(vp_find_pcl pcl_libraries pcl_deps_include_dirs pcl_deps_libraries) mark_as_advanced($<$ 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::Ptr pointcloud_color(new pcl::PointCloud()); +#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(color.getWidth()) + 80, 10, "Infrared image"); + vpDisplayX dd(depth_display, 10, static_cast(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 lock(mutex); - rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, nullptr, pointcloud_color, - (unsigned char *)infrared.bitmap); + rs.acquire(reinterpret_cast(color.bitmap), reinterpret_cast(depth.bitmap), nullptr, pointcloud_color, + reinterpret_cast(infrared.bitmap)); } #else - rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth.bitmap, nullptr, (unsigned char *)infrared.bitmap); + rs.acquire(reinterpret_cast(color.bitmap), reinterpret_cast(depth.bitmap), nullptr, reinterpret_cast(infrared.bitmap)); #endif vpImageConvert::createDepthHistogram(depth, depth_display); diff --git a/example/device/framegrabber/readRealSenseData.cpp b/example/device/framegrabber/readRealSenseData.cpp index 7cc493073a..72da852b0f 100644 --- a/example/device/framegrabber/readRealSenseData.cpp +++ b/example/device/framegrabber/readRealSenseData.cpp @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -57,8 +58,15 @@ #include #if defined(VISP_HAVE_PCL) +#include +#if defined(VISP_HAVE_PCL_COMMON) +#include +#include +#endif +#if defined(VISP_HAVE_PCL_IO) #include #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 &I_color, vpImage &I_depth_raw, bool pointcloud_binary_format -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) , pcl::PointCloud::Ptr point_cloud #endif ) @@ -236,9 +244,13 @@ bool readData(int cpt, const std::string &input_directory, vpImage &I_co } } else { +#if defined(VISP_HAVE_PCL_IO) if (pcl::io::loadPCDFile(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::Ptr pointcloud(new pcl::PointCloud()); +#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 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); diff --git a/example/device/framegrabber/saveRealSenseData.cpp b/example/device/framegrabber/saveRealSenseData.cpp index 32961cfe5d..5db5e390fa 100644 --- a/example/device/framegrabber/saveRealSenseData.cpp +++ b/example/device/framegrabber/saveRealSenseData.cpp @@ -49,11 +49,18 @@ #include #if defined(VISP_HAVE_PCL) -#include +#include +#if defined(VISP_HAVE_PCL_COMMON) +#include +#include +#endif +#if defined(VISP_HAVE_PCL_IO) #include #endif +#endif #include +#include #include #include #include @@ -216,7 +223,7 @@ class vpFrameQueue // Push data to save in the queue (FIFO) void push(const vpImage &colorImg, const vpImage &depthImg, -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) const pcl::PointCloud::Ptr &pointCloud, #else const std::vector &pointCloud, @@ -255,7 +262,7 @@ class vpFrameQueue // Pop the image to save from the queue (FIFO) void pop(vpImage &colorImg, vpImage &depthImg, -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) pcl::PointCloud::Ptr &pointCloud, #else std::vector &pointCloud, @@ -294,7 +301,7 @@ class vpFrameQueue std::condition_variable m_cond; std::queue> m_queueColor; std::queue> m_queueDepth; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) std::queue::Ptr> m_queuePointCloud; #else std::queue> m_queuePointCloud; @@ -334,7 +341,7 @@ class vpStorageWorker try { vpImage colorImg; vpImage depthImg; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) pcl::PointCloud::Ptr pointCloud; #else std::vector pointCloud; @@ -388,7 +395,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 @@ -432,8 +439,10 @@ class vpStorageWorker } } 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 } } @@ -637,7 +646,7 @@ 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::Ptr pointCloud(new pcl::PointCloud); #else std::vector pointCloud; @@ -645,7 +654,7 @@ int main(int argc, const char *argv[]) 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 @@ -653,7 +662,7 @@ int main(int argc, const char *argv[]) &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 +674,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 +704,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::Ptr pointCloud_copy = pointCloud->makeShared(); save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared); #else @@ -714,7 +723,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::Ptr pointCloud_copy = pointCloud->makeShared(); save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared); #else diff --git a/modules/core/include/visp3/core/vpExponentialMap.h b/modules/core/include/visp3/core/vpExponentialMap.h index eae27af3b0..95637be0f4 100644 --- a/modules/core/include/visp3/core/vpExponentialMap.h +++ b/modules/core/include/visp3/core/vpExponentialMap.h @@ -52,25 +52,25 @@ 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 @@ -78,7 +78,7 @@ 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. diff --git a/modules/core/include/visp3/core/vpImageConvert.h b/modules/core/include/visp3/core/vpImageConvert.h index 08738343ed..baa0ad5ff1 100644 --- a/modules/core/include/visp3/core/vpImageConvert.h +++ b/modules/core/include/visp3/core/vpImageConvert.h @@ -64,12 +64,13 @@ #include #endif -#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS) #include #include #include -#include +#include +#include #include #endif @@ -143,7 +144,7 @@ class VISP_EXPORT vpImageConvert static void convert(const yarp::sig::ImageOf *src, vpImage &dest); #endif -#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS) static int depthToPointCloud(const vpImage &depth_raw, float depth_scale, const vpCameraParameters &cam_depth, pcl::PointCloud::Ptr pointcloud, diff --git a/modules/core/src/image/vpImageConvert_pcl.cpp b/modules/core/src/image/vpImageConvert_pcl.cpp index a359dce4c7..072ec4814e 100644 --- a/modules/core/src/image/vpImageConvert_pcl.cpp +++ b/modules/core/src/image/vpImageConvert_pcl.cpp @@ -40,7 +40,7 @@ #include -#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS) #include @@ -216,8 +216,16 @@ int vpImageConvert::depthToPointCloud(const vpImage &color, const vpImag #if defined(_OPENMP) std::lock_guard lock(mutex); #endif +#if PCL_VERSION_COMPARE(>=,1,14,1) pointcloud->push_back(pcl::PointXYZRGB(point_3D[0], point_3D[1], point_3D[2], color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B)); +#else + pcl::PointXYZRGB pt(color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B); + pt.x = point_3D[0]; + pt.y = point_3D[1]; + pt.z = point_3D[2]; + pointcloud->push_back(pcl::PointXYZRGB(pt)); +#endif } } } @@ -251,8 +259,16 @@ int vpImageConvert::depthToPointCloud(const vpImage &color, const vpImag #if defined(_OPENMP) std::lock_guard lock(mutex); #endif +#if PCL_VERSION_COMPARE(>=,1,14,1) pointcloud->push_back(pcl::PointXYZRGB(point_3D[0], point_3D[1], point_3D[2], color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B)); +#else + pcl::PointXYZRGB pt(color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B); + pt.x = point_3D[0]; + pt.y = point_3D[1]; + pt.z = point_3D[2]; + pointcloud->push_back(pcl::PointXYZRGB(pt)); +#endif } } } diff --git a/modules/gui/include/visp3/gui/vpDisplayPCL.h b/modules/gui/include/visp3/gui/vpDisplayPCL.h index 8cd70cb665..0d97c9aa51 100644 --- a/modules/gui/include/visp3/gui/vpDisplayPCL.h +++ b/modules/gui/include/visp3/gui/vpDisplayPCL.h @@ -36,7 +36,7 @@ #include -#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) && defined(VISP_HAVE_THREADS) #include #include diff --git a/modules/gui/include/visp3/gui/vpPclViewer.h b/modules/gui/include/visp3/gui/vpPclViewer.h index 28bc9ec785..37d51ec5d9 100644 --- a/modules/gui/include/visp3/gui/vpPclViewer.h +++ b/modules/gui/include/visp3/gui/vpPclViewer.h @@ -37,7 +37,7 @@ #include -#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) && defined(VISP_HAVE_THREADS) // System #include #include diff --git a/modules/gui/src/pointcloud/vpDisplayPCL.cpp b/modules/gui/src/pointcloud/vpDisplayPCL.cpp index c901a16f9f..64a2e5bbbc 100644 --- a/modules/gui/src/pointcloud/vpDisplayPCL.cpp +++ b/modules/gui/src/pointcloud/vpDisplayPCL.cpp @@ -33,7 +33,7 @@ #include -#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) && defined(VISP_HAVE_THREADS) #include diff --git a/modules/gui/src/pointcloud/vpPclViewer.cpp b/modules/gui/src/pointcloud/vpPclViewer.cpp index efeda9ee89..a673a2d16c 100644 --- a/modules/gui/src/pointcloud/vpPclViewer.cpp +++ b/modules/gui/src/pointcloud/vpPclViewer.cpp @@ -35,7 +35,7 @@ #ifndef DOXYGEN_SHOULD_SKIP_THIS #include -#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) && defined(VISP_HAVE_THREADS) // PCL #include diff --git a/modules/sensor/include/visp3/sensor/vpOccipitalStructure.h b/modules/sensor/include/visp3/sensor/vpOccipitalStructure.h index 07b6c5a8e3..cb1cbca048 100644 --- a/modules/sensor/include/visp3/sensor/vpOccipitalStructure.h +++ b/modules/sensor/include/visp3/sensor/vpOccipitalStructure.h @@ -47,7 +47,7 @@ #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include #endif @@ -297,7 +297,7 @@ class VISP_EXPORT vpOccipitalStructure vpColVector *acceleration_data = nullptr, vpColVector *gyroscope_data = nullptr, bool undistorted = true, double *ts = nullptr); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void acquire(unsigned char *const data_image, unsigned char *const data_depth, std::vector *const data_pointCloud, pcl::PointCloud::Ptr &pointcloud, unsigned char *const data_infrared = nullptr, vpColVector *acceleration_data = nullptr, @@ -354,7 +354,7 @@ class VISP_EXPORT vpOccipitalStructure vpCameraParameters m_visible_camera_parameters, m_depth_camera_parameters; void getPointcloud(std::vector &pointcloud); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void getPointcloud(pcl::PointCloud::Ptr &pointcloud); void getColoredPointcloud(pcl::PointCloud::Ptr &pointcloud); #endif diff --git a/modules/sensor/include/visp3/sensor/vpRealSense.h b/modules/sensor/include/visp3/sensor/vpRealSense.h index 96dfc0322c..cc27cd0235 100644 --- a/modules/sensor/include/visp3/sensor/vpRealSense.h +++ b/modules/sensor/include/visp3/sensor/vpRealSense.h @@ -50,7 +50,7 @@ #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include #include #endif @@ -335,7 +335,7 @@ class VISP_EXPORT vpRealSense virtual ~vpRealSense(); void acquire(std::vector &pointcloud); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void acquire(pcl::PointCloud::Ptr &pointcloud); void acquire(pcl::PointCloud::Ptr &pointcloud); #endif @@ -343,7 +343,7 @@ class VISP_EXPORT vpRealSense void acquire(vpImage &grey, std::vector &pointcloud); void acquire(vpImage &grey, vpImage &infrared, vpImage &depth, std::vector &pointcloud); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void acquire(vpImage &grey, pcl::PointCloud::Ptr &pointcloud); void acquire(vpImage &grey, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud); @@ -363,7 +363,7 @@ class VISP_EXPORT vpRealSense const rs::stream &stream_infrared = rs::stream::infrared, const rs::stream &stream_infrared2 = rs::stream::infrared2); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void acquire(vpImage &color, pcl::PointCloud::Ptr &pointcloud); void acquire(vpImage &color, vpImage &infrared, vpImage &depth, pcl::PointCloud::Ptr &pointcloud); diff --git a/modules/sensor/include/visp3/sensor/vpRealSense2.h b/modules/sensor/include/visp3/sensor/vpRealSense2.h index 83efbdbced..9c0e40158b 100644 --- a/modules/sensor/include/visp3/sensor/vpRealSense2.h +++ b/modules/sensor/include/visp3/sensor/vpRealSense2.h @@ -43,7 +43,7 @@ #include #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include #endif @@ -307,7 +307,7 @@ class VISP_EXPORT vpRealSense2 unsigned int *tracker_confidence = nullptr, double *ts = nullptr); #endif -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void acquire(unsigned char *const data_image, unsigned char *const data_depth, std::vector *const data_pointCloud, pcl::PointCloud::Ptr &pointcloud, unsigned char *const data_infrared = nullptr, rs2::align *const align_to = nullptr, double *ts = nullptr); @@ -399,7 +399,7 @@ class VISP_EXPORT vpRealSense2 void getGreyFrame(const rs2::frame &frame, vpImage &grey); void getNativeFrameData(const rs2::frame &frame, unsigned char *const data); void getPointcloud(const rs2::depth_frame &depth_frame, std::vector &pointcloud); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void getPointcloud(const rs2::depth_frame &depth_frame, pcl::PointCloud::Ptr &pointcloud); void getPointcloud(const rs2::depth_frame &depth_frame, const rs2::frame &color_frame, pcl::PointCloud::Ptr &pointcloud); diff --git a/modules/sensor/src/rgb-depth/occipital_structure/vpOccipitalStructure.cpp b/modules/sensor/src/rgb-depth/occipital_structure/vpOccipitalStructure.cpp index daaeb32591..807dc70318 100644 --- a/modules/sensor/src/rgb-depth/occipital_structure/vpOccipitalStructure.cpp +++ b/modules/sensor/src/rgb-depth/occipital_structure/vpOccipitalStructure.cpp @@ -304,7 +304,7 @@ void vpOccipitalStructure::acquire(unsigned char *const data_image, unsigned cha u.unlock(); } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) /*! Acquire data from Structure Core device. \param data_image : Color image buffer or nullptr if not wanted. @@ -988,7 +988,7 @@ void vpOccipitalStructure::getPointcloud(std::vector &pointcloud) } } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpOccipitalStructure::getPointcloud(pcl::PointCloud::Ptr &pointcloud) { ST::DepthFrame last_df = m_delegate.m_depthFrame; diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp b/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp index fd6ded07f0..a16ba58106 100644 --- a/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp +++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp @@ -670,7 +670,7 @@ int main() */ void vpRealSense::setEnableStream(const rs::stream &stream, bool status) { m_enableStreams[stream] = status; } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) /*! Acquire data from RealSense device. \param pointcloud : Point cloud data information. diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp b/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp index 758a60963f..91a197b75c 100644 --- a/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp +++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp @@ -470,7 +470,7 @@ void vpRealSense2::acquire(vpImage *left, vpImage } #endif // #if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0)) -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) /*! Acquire data from RealSense device. \param data_image : Color image buffer or nullptr if not wanted. @@ -882,7 +882,7 @@ void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, std::vecto } } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, pcl::PointCloud::Ptr &pointcloud) { if (m_depthScale <= std::numeric_limits::epsilon()) { diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h b/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h index d76ed2834d..a83e3bccf3 100644 --- a/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h +++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h @@ -40,7 +40,7 @@ #include #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include #endif @@ -57,13 +57,14 @@ void vp_rs_get_frame_data_impl(const rs::device *m_device, const std::mapsecond.width; - unsigned int height = (unsigned int)it_intrinsics->second.height; + unsigned int width = static_cast(it_intrinsics->second.width); + unsigned int height = static_cast(it_intrinsics->second.height); data.resize(height, width); - memcpy((unsigned char *)data.bitmap, (unsigned char *)m_device->get_frame_data(stream), + memcpy(reinterpret_cast(data.bitmap), reinterpret_cast(m_device->get_frame_data(stream)), width * height * sizeof(Type)); - } else { + } + else { throw vpException(vpException::fatalError, "RealSense Camera - stream %d not enabled!", (rs_stream)stream); } } @@ -82,7 +83,7 @@ void vp_rs_get_native_frame_data_impl(const rs::device *m_device, throw vpException(vpException::fatalError, ss.str()); } - size_t size = (size_t)(it_intrinsics->second.width * it_intrinsics->second.height); + size_t size = static_cast(it_intrinsics->second.width * it_intrinsics->second.height); switch (m_device->get_stream_format(native_stream)) { // 8 bits @@ -90,57 +91,58 @@ void vp_rs_get_native_frame_data_impl(const rs::device *m_device, std::cout << "Stream: raw8 not tested!" << std::endl; /* FALLTHRU */ case rs::format::y8: - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), size); break; // Four 10-bit luminance values encoded into a 5-byte macropixel case rs::format::raw10: std::cout << "Stream: raw10 not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), (5 * (size / 4))); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), (5 * (size / 4))); break; // 16 bits case rs::format::raw16: std::cout << "Stream: raw16 not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 2); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), size * 2); break; case rs::format::disparity16: std::cout << "Stream: disparity16 not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 2); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), size * 2); break; case rs::format::y16: case rs::format::z16: - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 2); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), size * 2); break; // 24 bits case rs::format::bgr8: case rs::format::rgb8: - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 3); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), size * 3); break; // 32 bits case rs::format::yuyv: std::cout << "Stream: yuyv not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 4); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), size * 4); break; case rs::format::bgra8: case rs::format::rgba8: - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 4); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), size * 4); break; // 96 bits case rs::format::xyz32f: std::cout << "Stream: xyz32f not tested!" << std::endl; - memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size * 3 * 4); + memcpy(data, reinterpret_cast(m_device->get_frame_data(stream)), size * 3 * 4); break; case rs::format::any: default: break; } - } else { + } + else { throw vpException(vpException::fatalError, "RealSense Camera - stream %d not enabled!", (rs_stream)native_stream); } } @@ -155,23 +157,27 @@ void vp_rs_get_color_impl(const rs::device *m_device, const std::mapsecond.width; - unsigned int height = (unsigned int)it_intrinsics->second.height; + unsigned int width = static_cast(it_intrinsics->second.width); + unsigned int height = static_cast(it_intrinsics->second.height); color.resize(height, width); if (m_device->get_stream_format(rs::stream::color) == rs::format::rgb8) { - vpImageConvert::RGBToRGBa((unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)color.bitmap, width, height); - } else if (m_device->get_stream_format(rs::stream::color) == rs::format::rgba8) { - memcpy((unsigned char *)color.bitmap, (unsigned char *)m_device->get_frame_data(rs::stream::color), + vpImageConvert::RGBToRGBa(reinterpret_cast(m_device->get_frame_data(rs::stream::color)), + reinterpret_cast(color.bitmap), width, height); + } + else if (m_device->get_stream_format(rs::stream::color) == rs::format::rgba8) { + memcpy(reinterpret_cast(color.bitmap), reinterpret_cast(m_device->get_frame_data(rs::stream::color)), width * height * sizeof(vpRGBa)); - } else if (m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) { - vpImageConvert::BGRToRGBa((unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)color.bitmap, width, height); - } else { + } + else if (m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) { + vpImageConvert::BGRToRGBa(reinterpret_cast(m_device->get_frame_data(rs::stream::color)), + reinterpret_cast(color.bitmap), width, height); + } + else { throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!"); } - } else { + } + else { throw vpException(vpException::fatalError, "RealSense Camera - color stream not enabled!"); } } @@ -186,23 +192,27 @@ void vp_rs_get_grey_impl(const rs::device *m_device, const std::mapsecond.width; - unsigned int height = (unsigned int)it_intrinsics->second.height; + unsigned int width = static_cast(it_intrinsics->second.width); + unsigned int height = static_cast(it_intrinsics->second.height); grey.resize(height, width); if (m_device->get_stream_format(rs::stream::color) == rs::format::rgb8) { - vpImageConvert::RGBToGrey((unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)grey.bitmap, width, height); - } else if (m_device->get_stream_format(rs::stream::color) == rs::format::rgba8) { - vpImageConvert::RGBaToGrey((unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)grey.bitmap, width * height); - } else if (m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) { - vpImageConvert::BGRToGrey((unsigned char *)m_device->get_frame_data(rs::stream::color), - (unsigned char *)grey.bitmap, width, height); - } else { + vpImageConvert::RGBToGrey(reinterpret_cast(m_device->get_frame_data(rs::stream::color)), + reinterpret_cast(grey.bitmap), width, height); + } + else if (m_device->get_stream_format(rs::stream::color) == rs::format::rgba8) { + vpImageConvert::RGBaToGrey(reinterpret_cast(m_device->get_frame_data(rs::stream::color)), + reinterpret_cast(grey.bitmap), width * height); + } + else if (m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) { + vpImageConvert::BGRToGrey(reinterpret_cast(m_device->get_frame_data(rs::stream::color)), + reinterpret_cast(grey.bitmap), width, height); + } + else { throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!"); } - } else { + } + else { throw vpException(vpException::fatalError, "RealSense Camera - color stream not enabled!"); } } @@ -222,19 +232,19 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::mapget_frame_data(stream_depth); + uint16_t *depth = reinterpret_cast(m_device->get_frame_data(stream_depth)); int width = it_intrinsics->second.width; int height = it_intrinsics->second.height; - pointcloud.resize((size_t)(width * height)); + pointcloud.resize(static_cast(width * height)); - for (int i = 0; i < height; i++) { - for (int j = 0; j < width; j++) { + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { float scaled_depth = depth[i * width + j] * depth_scale; - rs::float2 depth_pixel = {(float)j, (float)i}; + rs::float2 depth_pixel = { static_castj, static_casti }; depth_point = it_intrinsics->second.deproject(depth_pixel, scaled_depth); - if (depth_point.z <= 0 || depth_point.z > max_Z) { + if ((depth_point.z <= 0) || (depth_point.z > max_Z)) { depth_point.x = depth_point.y = depth_point.z = invalidDepthValue; } p3d[0] = depth_point.x; @@ -242,15 +252,16 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map(i * width + j)] = p3d; } } - } else { + } + else { pointcloud.clear(); } } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) // Retrieve point cloud void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map &m_intrinsics, float max_Z, pcl::PointCloud::Ptr &pointcloud, @@ -264,32 +275,33 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::mapsecond.width; int height = it_intrinsics->second.height; - pointcloud->width = (uint32_t)width; - pointcloud->height = (uint32_t)height; - pointcloud->resize((size_t)(width * height)); + pointcloud->width = static_cast(width); + pointcloud->height = static_cast(height); + pointcloud->resize(static_cast(width * height)); const float depth_scale = m_device->get_depth_scale(); // Fill the PointCloud2 fields. rs::float3 depth_point; - uint16_t *depth = (uint16_t *)m_device->get_frame_data(stream_depth); + uint16_t *depth = reinterpret_cast(m_device->get_frame_data(stream_depth)); - for (int i = 0; i < height; i++) { - for (int j = 0; j < width; j++) { + for (int i = 0; i < height; ++i) { + for (int j = 0; j < width; ++j) { float scaled_depth = depth[i * width + j] * depth_scale; - rs::float2 depth_pixel = {(float)j, (float)i}; + rs::float2 depth_pixel = { static_cast(j), static_cast(i) }; depth_point = it_intrinsics->second.deproject(depth_pixel, scaled_depth); - if (depth_point.z <= 0 || depth_point.z > max_Z) { + if ((depth_point.z <= 0) || (depth_point.z > max_Z)) { depth_point.x = depth_point.y = depth_point.z = invalidDepthValue; } - pointcloud->points[(size_t)(i * width + j)].x = depth_point.x; - pointcloud->points[(size_t)(i * width + j)].y = depth_point.y; - pointcloud->points[(size_t)(i * width + j)].z = depth_point.z; + pointcloud->points[static_cast(i * width + j)].x = depth_point.x; + pointcloud->points[static_cast(i * width + j)].y = depth_point.y; + pointcloud->points[static_cast(i * width + j)].z = depth_point.z; } } - } else { + } + else { pointcloud->clear(); } } @@ -311,79 +323,80 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::mapget_stream_format(rs::stream::color) != rs::format::rgb8 && - m_device->get_stream_format(rs::stream::color) != rs::format::rgba8 && - m_device->get_stream_format(rs::stream::color) != rs::format::bgr8 && - m_device->get_stream_format(rs::stream::color) != rs::format::bgra8) { + if ((m_device->get_stream_format(rs::stream::color) != rs::format::rgb8) && + (m_device->get_stream_format(rs::stream::color) != rs::format::rgba8) && + (m_device->get_stream_format(rs::stream::color) != rs::format::bgr8) && + (m_device->get_stream_format(rs::stream::color) != rs::format::bgra8)) { throw vpException(vpException::fatalError, "Color stream type must be rgb8, rgba8, bgr8 or bgra8!"); } int depth_width = it_intrinsics_depth->second.width; int depth_height = it_intrinsics_depth->second.height; - pointcloud->width = (uint32_t)depth_width; - pointcloud->height = (uint32_t)depth_height; - pointcloud->resize((size_t)(depth_width * depth_height)); + pointcloud->width = static_cast(depth_width); + pointcloud->height = static_cast(depth_height); + pointcloud->resize(static_cast(depth_width * depth_height)); const float depth_scale = m_device->get_depth_scale(); rs::extrinsics depth_2_color_extrinsic = m_device->get_extrinsics(stream_depth, stream_color); - const uint16_t *depth = (uint16_t *)m_device->get_frame_data(stream_depth); - const unsigned char *color = (unsigned char *)m_device->get_frame_data(stream_color); + const uint16_t *depth = reinterpret_cast(m_device->get_frame_data(stream_depth)); + const unsigned char *color = reinterpret_cast(m_device->get_frame_data(stream_color)); int color_width = it_intrinsics_color->second.width; int color_height = it_intrinsics_color->second.height; bool swap_rgb = m_device->get_stream_format(rs::stream::color) == rs::format::bgr8 || - m_device->get_stream_format(rs::stream::color) == rs::format::bgra8; + m_device->get_stream_format(rs::stream::color) == rs::format::bgra8; unsigned int nb_color_pixel = (m_device->get_stream_format(rs::stream::color) == rs::format::rgb8 || m_device->get_stream_format(rs::stream::color) == rs::format::bgr8) - ? 3 - : 4; + ? 3 + : 4; - int nb_threads = (int)std::thread::hardware_concurrency(); + int nb_threads = static_cast(std::thread::hardware_concurrency()); int step = depth_height / nb_threads; std::vector workers; - for (int index = 0; index < nb_threads; index++) { + for (int index = 0; index < nb_threads; ++index) { int start_index = index * step; int end_index = index == nb_threads - 1 ? depth_height : (index + 1) * step; workers.push_back(std::thread([&, start_index, end_index]() { - for (int i = start_index; i < end_index; i++) { - for (int j = 0; j < depth_width; j++) { + for (int i = start_index; i < end_index; ++i) { + for (int j = 0; j < depth_width; ++j) { float scaled_depth = depth[i * depth_width + j] * depth_scale; - rs::float2 depth_pixel = {(float)j, (float)i}; + rs::float2 depth_pixel = { static_cast(j), static_cast(i) }; rs::float3 depth_point = it_intrinsics_depth->second.deproject(depth_pixel, scaled_depth); - if (depth_point.z <= 0 || depth_point.z > max_Z) { + if ((depth_point.z <= 0) || (depth_point.z > max_Z)) { depth_point.x = depth_point.y = depth_point.z = invalidDepthValue; } - pointcloud->points[(size_t)(i * depth_width + j)].x = depth_point.x; - pointcloud->points[(size_t)(i * depth_width + j)].y = depth_point.y; - pointcloud->points[(size_t)(i * depth_width + j)].z = depth_point.z; + pointcloud->points[static_cast(i * depth_width + j)].x = depth_point.x; + pointcloud->points[static_cast(i * depth_width + j)].y = depth_point.y; + pointcloud->points[static_cast(i * depth_width + j)].z = depth_point.z; rs::float3 color_point = depth_2_color_extrinsic.transform(depth_point); rs::float2 color_pixel = it_intrinsics_color->second.project(color_point); - if (color_pixel.y < 0 || color_pixel.y >= color_height || color_pixel.x < 0 || - color_pixel.x >= color_width) { + if ((color_pixel.y < 0) || (color_pixel.y >= color_height) || (color_pixel.x < 0) || + (color_pixel.x >= color_width)) { // For out of bounds color data, default to a shade of blue in order to // visually distinguish holes. This color value is same as the librealsense // out of bounds color value. #if PCL_VERSION_COMPARE(<, 1, 1, 0) unsigned int r = 96, g = 157, b = 198; uint32_t rgb = - (static_cast(r) << 16 | static_cast(g) << 8 | static_cast(b)); + (static_cast(r) << 16 | static_cast(g) << 8 | static_cast(b)); - pointcloud->points[(size_t)(i * depth_width + j)].rgb = *reinterpret_cast(&rgb); + pointcloud->points[static_cast(i * depth_width + j)].rgb = *reinterpret_cast(&rgb); #else - pointcloud->points[(size_t)(i * depth_width + j)].r = (uint8_t)96; - pointcloud->points[(size_t)(i * depth_width + j)].g = (uint8_t)157; - pointcloud->points[(size_t)(i * depth_width + j)].b = (uint8_t)198; + pointcloud->points[static_cast(i * depth_width + j)].r = static_cast(96); + pointcloud->points[static_cast(i * depth_width + j)].g = static_cast(157); + pointcloud->points[static_cast(i * depth_width + j)].b = static_cast(198); #endif - } else { - unsigned int i_ = (unsigned int)color_pixel.y; - unsigned int j_ = (unsigned int)color_pixel.x; + } + else { + unsigned int i_ = static_cast(color_pixel.y); + unsigned int j_ = static_cast(color_pixel.x); #if PCL_VERSION_COMPARE(<, 1, 1, 0) uint32_t rgb = 0; @@ -391,38 +404,41 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) | static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 | static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]) << 16); - } else { + } + else { rgb = (static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) << 16 | static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 | static_cast(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2])); } - pointcloud->points[(size_t)(i * depth_width + j)].rgb = *reinterpret_cast(&rgb); + pointcloud->points[static_cast(i * depth_width + j)].rgb = *reinterpret_cast(&rgb); #else if (swap_rgb) { - pointcloud->points[(size_t)(i * depth_width + j)].b = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]; + pointcloud->points[static_cast(i * depth_width + j)].b = + (uint32_t)color[(i_ * static_cast(color_width) + j_) * nb_color_pixel]; pointcloud->points[(size_t)(i * depth_width + j)].g = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]; - pointcloud->points[(size_t)(i * depth_width + j)].r = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]; - } else { + (uint32_t)color[(i_ * static_cast(color_width) + j_) * nb_color_pixel + 1]; pointcloud->points[(size_t)(i * depth_width + j)].r = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]; + (uint32_t)color[(i_ * static_cast(color_width) + j_) * nb_color_pixel + 2]; + } + else { + pointcloud->points[static_cast(i * depth_width + j)].r = + (uint32_t)color[(i_ * static_cast(color_width) + j_) * nb_color_pixel]; pointcloud->points[(size_t)(i * depth_width + j)].g = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]; + (uint32_t)color[(i_ * static_cast(color_width) + j_) * nb_color_pixel + 1]; pointcloud->points[(size_t)(i * depth_width + j)].b = - (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]; + (uint32_t)color[(i_ * static_cast(color_width) + j_) * nb_color_pixel + 2]; } #endif } } } - })); + })); } std::for_each(workers.begin(), workers.end(), [](std::thread &t) { t.join(); }); - } else { + } + else { pointcloud->clear(); } } diff --git a/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_pcl.cpp b/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_pcl.cpp index 3839e01e26..f8cfad120e 100644 --- a/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_pcl.cpp +++ b/modules/sensor/test/rgb-depth/testOccipitalStructure_Core_pcl.cpp @@ -43,12 +43,10 @@ #include -#if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && (defined(VISP_HAVE_PCL)) +#if defined(VISP_HAVE_OCCIPITAL_STRUCTURE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && (defined(VISP_HAVE_PCL)) && defined(VISP_HAVE_PCL_VISUALIZATION) -#ifdef VISP_HAVE_PCL #include #include -#endif #include #include @@ -144,6 +142,8 @@ int main() std::cout << "- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl; #elif !defined( VISP_HAVE_PCL ) std::cout << "You do not have PCL 3rd party installed." << std::endl; +#elif !defined( VISP_HAVE_PCL_VISUALIZATION ) + std::cout << "You do not have PCL visualization module." << std::endl; #endif return EXIT_SUCCESS; } diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp index 25ea353468..866839352a 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_D435_align.cpp @@ -41,7 +41,7 @@ #include #if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) \ - && defined(VISP_HAVE_PCL) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) + && defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) #include #include diff --git a/modules/sensor/test/rgb-depth/testRealSense2_D435_pcl.cpp b/modules/sensor/test/rgb-depth/testRealSense2_D435_pcl.cpp index a90ce22aa5..85a54d5fbf 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_D435_pcl.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_D435_pcl.cpp @@ -42,7 +42,7 @@ #include #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_THREADS) \ - && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_PCL) + && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_VISUALIZATION) #include #include diff --git a/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp b/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp index 350d9c1013..961b7c9d2e 100644 --- a/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp +++ b/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp @@ -44,7 +44,7 @@ #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_THREADS) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) #include #include #include @@ -64,7 +64,7 @@ namespace { -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) // Global variables pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud()); pcl::PointCloud::Ptr pointcloud_color(new pcl::PointCloud()); @@ -231,7 +231,7 @@ void frame_to_mat(const rs2::frame &f, cv::Mat &img) int main(int argc, char *argv[]) { -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) bool pcl_color = false; #endif bool show_info = false; @@ -240,14 +240,14 @@ int main(int argc, char *argv[]) if (std::string(argv[i]) == "--show_info") { show_info = true; } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) else if (std::string(argv[i]) == "--pcl_color") { pcl_color = true; } #endif else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << argv[0] << " [--show_info]" -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) << " [--pcl_color]" #endif << " [--help] [-h]" @@ -292,7 +292,7 @@ int main(int argc, char *argv[]) d3.init(infrared, 0, color.getHeight() + 100, "Infrared"); std::vector pointcloud_colvector; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) std::mutex mutex; ViewerWorker viewer_colvector(false, mutex); std::thread viewer_colvector_thread(&ViewerWorker::run, &viewer_colvector); @@ -337,7 +337,7 @@ int main(int argc, char *argv[]) auto infrared_frame = data.first(RS2_STREAM_INFRARED); getNativeFrame(infrared_frame, (unsigned char *)infrared_raw.bitmap); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) getPointcloud(depth_frame, pointcloud_colvector); { @@ -379,7 +379,7 @@ int main(int argc, char *argv[]) d2.close(depth_color); d3.close(infrared); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) { std::lock_guard lock(mutex); cancelled = true; @@ -410,7 +410,7 @@ int main(int argc, char *argv[]) d2.init(depth_color, color.getWidth(), 0, "Depth"); d3.init(infrared, 0, color.getHeight() + 100, "Infrared"); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) cancelled = false; ViewerWorker viewer(pcl_color, mutex); std::thread viewer_thread(&ViewerWorker::run, &viewer); @@ -444,7 +444,7 @@ int main(int argc, char *argv[]) while (vpTime::measureTimeMs() - t_begin < 10000) { double t = vpTime::measureTimeMs(); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) { std::lock_guard lock(mutex); @@ -481,7 +481,7 @@ int main(int argc, char *argv[]) break; } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) { std::lock_guard lock(mutex); cancelled = true; @@ -541,7 +541,7 @@ int main(int argc, char *argv[]) << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl; #endif -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) // Pointcloud acquisition using std::vector + visualization // See issue #355 ViewerWorker viewer_colvector2(false, mutex); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h index 261c6766dd..c50db5f04d 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h @@ -70,7 +70,7 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker void reInitModel(const vpImage &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose = false); -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose = false); #endif @@ -99,7 +99,7 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) vp_override; virtual void setPose(const vpImage &I_color, const vpHomogeneousMatrix &cdMo) vp_override; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) virtual void setPose(const pcl::PointCloud::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo); #endif @@ -111,7 +111,7 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker virtual void track(const vpImage &) vp_override; virtual void track(const vpImage &) vp_override; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) virtual void track(const pcl::PointCloud::ConstPtr &point_cloud); #endif virtual void track(const std::vector &point_cloud, unsigned int width, unsigned int height); @@ -164,7 +164,7 @@ class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker virtual void initFaceFromLines(vpMbtPolygon &polygon) vp_override; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void segmentPointCloud(const pcl::PointCloud::ConstPtr &point_cloud); #endif void segmentPointCloud(const std::vector &point_cloud, unsigned int width, unsigned int height); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h index 91f3cab506..cbced0aaaf 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h @@ -75,7 +75,7 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker void reInitModel(const vpImage &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose = false); -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose = false); #endif @@ -102,7 +102,7 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker virtual void setPose(const vpImage &I, const vpHomogeneousMatrix &cdMo) vp_override; virtual void setPose(const vpImage &I_color, const vpHomogeneousMatrix &cdMo) vp_override; -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) virtual void setPose(const pcl::PointCloud::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo); #endif @@ -114,7 +114,7 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker virtual void track(const vpImage &) vp_override; virtual void track(const vpImage &I_color) vp_override; -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) virtual void track(const pcl::PointCloud::ConstPtr &point_cloud); #endif virtual void track(const std::vector &point_cloud, unsigned int width, unsigned int height); @@ -179,7 +179,7 @@ class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker virtual void initFaceFromLines(vpMbtPolygon &polygon) vp_override; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void segmentPointCloud(const pcl::PointCloud::ConstPtr &point_cloud); #endif void segmentPointCloud(const std::vector &point_cloud, unsigned int width, unsigned int height); diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h index b4f4067dd7..2a20a2b9a3 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h @@ -591,7 +591,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void track(std::map *> &mapOfImages); virtual void track(std::map *> &mapOfColorImages); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) virtual void track(std::map *> &mapOfImages, std::map::ConstPtr> &mapOfPointClouds); virtual void track(std::map *> &mapOfColorImages, @@ -644,7 +644,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void loadConfigFileJSON(const std::string &configFile, bool verbose = true); #endif -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) virtual void preTracking(std::map *> &mapOfImages, std::map::ConstPtr> &mapOfPointClouds); #endif @@ -740,7 +740,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void track(const vpImage &I) vp_override; virtual void track(const vpImage &I_color) vp_override; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) // Fix error: using declaration ‘using vpMbDepthDenseTracker::setPose’ conflicts with a previous // using declaration that occurs with g++ 4.6.3 on Ubuntu 12.04 #if !((__GNUC__ == 4) && (__GNUC_MINOR__ == 6)) @@ -773,7 +773,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker virtual void initMbtTracking(const vpImage *const ptr_I); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) virtual void postTracking(const vpImage *const ptr_I, const pcl::PointCloud::ConstPtr &point_cloud); virtual void preTracking(const vpImage *const ptr_I, @@ -792,7 +792,7 @@ class VISP_EXPORT vpMbGenericTracker : public vpMbTracker const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose = false, const vpHomogeneousMatrix &T = vpHomogeneousMatrix()); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) // Fix error: using declaration ‘using vpMbDepthDenseTracker::track’ conflicts with a previous // using declaration that occurs with g++ 4.6.3 on Ubuntu 12.04 #if !((__GNUC__ == 4) && (__GNUC_MINOR__ == 6)) diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h index beb1f433b6..68ec8f5dad 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h @@ -39,7 +39,7 @@ #include #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include #include #endif @@ -88,7 +88,7 @@ class VISP_EXPORT vpMbtFaceDepthDense void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces *const faces, vpUniRand &rand_gen, int polygon = -1, std::string name = ""); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const pcl::PointCloud::ConstPtr &point_cloud, unsigned int stepX, unsigned int stepY diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h index 7b59a9b625..6efddf40a6 100644 --- a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h +++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h @@ -39,7 +39,7 @@ #include #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) #include #include #endif @@ -63,7 +63,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal { ROBUST_FEATURE_ESTIMATION = 0, ROBUST_SVD_PLANE_ESTIMATION = 1, -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) PCL_PLANE_ESTIMATION = 2 #endif }; @@ -91,7 +91,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces *const faces, vpUniRand &rand_gen, int polygon = -1, std::string name = ""); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, unsigned int width, unsigned int height, const pcl::PointCloud::ConstPtr &point_cloud, vpColVector &desired_features, unsigned int stepX, unsigned int stepY @@ -128,7 +128,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal void computeNormalVisibility(double nx, double ny, double nz, const vpColVector ¢roid_point, vpColVector &face_normal); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) void computeNormalVisibility(float nx, float ny, float nz, const pcl::PointXYZ ¢roid_point, pcl::PointXYZ &face_normal); #endif @@ -279,7 +279,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal //! std::vector m_polygonLines; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) bool computeDesiredFeaturesPCL(const pcl::PointCloud::ConstPtr &point_cloud_face, vpColVector &desired_features, vpColVector &desired_normal, vpColVector ¢roid_point); @@ -315,7 +315,7 @@ class VISP_EXPORT vpMbtFaceDepthNormal #ifdef VISP_HAVE_NLOHMANN_JSON #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) NLOHMANN_JSON_SERIALIZE_ENUM(vpMbtFaceDepthNormal::vpFeatureEstimationType, { {vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION, "robust"}, {vpMbtFaceDepthNormal::ROBUST_SVD_PLANE_ESTIMATION, "robustSVD"}, diff --git a/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp index bd679514f8..61f85cac16 100644 --- a/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp +++ b/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp @@ -37,7 +37,7 @@ #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include #endif @@ -456,7 +456,7 @@ void vpMbDepthDenseTracker::reInitModel(const vpImage &I, const s initFromPose(I, cMo); } -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbDepthDenseTracker::reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose) { @@ -550,7 +550,7 @@ void vpMbDepthDenseTracker::setDepthDenseFilteringOccupancyRatio(double occupanc } } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbDepthDenseTracker::segmentPointCloud(const pcl::PointCloud::ConstPtr &point_cloud) { m_depthDenseListOfActiveFaces.clear(); @@ -746,7 +746,7 @@ void vpMbDepthDenseTracker::setPose(const vpImage &I_color, const vpHomo init(m_I); } -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbDepthDenseTracker::setPose(const pcl::PointCloud::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo) { @@ -789,7 +789,7 @@ void vpMbDepthDenseTracker::track(const vpImage &) throw vpException(vpException::fatalError, "Cannot track with a color image!"); } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbDepthDenseTracker::track(const pcl::PointCloud::ConstPtr &point_cloud) { segmentPointCloud(point_cloud); diff --git a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp index 80948122cc..23df2b1d7d 100644 --- a/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp +++ b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp @@ -37,7 +37,7 @@ #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include #endif @@ -493,7 +493,7 @@ void vpMbDepthNormalTracker::reInitModel(const vpImage &I, const initFromPose(I, cMo); } -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbDepthNormalTracker::reInitModel(const pcl::PointCloud::ConstPtr &point_cloud, const std::string &cad_name, const vpHomogeneousMatrix &cMo, bool verbose) { @@ -561,7 +561,7 @@ void vpMbDepthNormalTracker::setPose(const vpImage &I_color, const vpHom init(m_I); } -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbDepthNormalTracker::setPose(const pcl::PointCloud::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo) { @@ -594,7 +594,7 @@ void vpMbDepthNormalTracker::setUseDepthNormalTracking(const std::string &name, void vpMbDepthNormalTracker::testTracking() { } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbDepthNormalTracker::segmentPointCloud(const pcl::PointCloud::ConstPtr &point_cloud) { m_depthNormalListOfActiveFaces.clear(); @@ -867,7 +867,7 @@ void vpMbDepthNormalTracker::track(const vpImage &) throw vpException(vpException::fatalError, "Cannot track with a color image!"); } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbDepthNormalTracker::track(const pcl::PointCloud::ConstPtr &point_cloud) { segmentPointCloud(point_cloud); diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp index c8f7e5db7d..62993ff533 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp @@ -36,7 +36,7 @@ #include #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include #endif @@ -246,7 +246,7 @@ void vpMbtFaceDepthDense::addLine(vpPoint &P1, vpPoint &P2, vpMbHiddenFaces::ConstPtr &point_cloud, unsigned int stepX, unsigned int stepY diff --git a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp index aa69ae31d6..ad726e3af1 100644 --- a/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp +++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp @@ -37,7 +37,7 @@ #include #include -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON) #include #include #include @@ -146,7 +146,7 @@ void vpMbtFaceDepthNormal::addLine(vpPoint &P1, vpPoint &P2, vpMbHiddenFaces::ConstPtr &point_cloud, @@ -443,7 +443,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo // Face centroid computed by the different methods vpColVector centroid_point(3); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON) if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) { pcl::PointCloud::Ptr point_cloud_face_pcl(new pcl::PointCloud); point_cloud_face_pcl->reserve(point_cloud_face.size() / 3); @@ -604,7 +604,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo // Face centroid computed by the different methods vpColVector centroid_point(3); -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON) if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) { pcl::PointCloud::Ptr point_cloud_face_pcl(new pcl::PointCloud); point_cloud_face_pcl->reserve(point_cloud_face.size() / 3); @@ -635,7 +635,7 @@ bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo return true; } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON) bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL(const pcl::PointCloud::ConstPtr &point_cloud_face, vpColVector &desired_features, vpColVector &desired_normal, vpColVector ¢roid_point) @@ -991,7 +991,7 @@ void vpMbtFaceDepthNormal::computeNormalVisibility(double nx, double ny, double } } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_SEGMENTATION) && defined(VISP_HAVE_PCL_FILTERS) && defined(VISP_HAVE_PCL_COMMON) void vpMbtFaceDepthNormal::computeNormalVisibility(float nx, float ny, float nz, const pcl::PointXYZ ¢roid_point, pcl::PointXYZ &face_normal) { diff --git a/modules/tracker/mbt/src/vpMbGenericTracker.cpp b/modules/tracker/mbt/src/vpMbGenericTracker.cpp index 29d296cf11..a7252025ee 100644 --- a/modules/tracker/mbt/src/vpMbGenericTracker.cpp +++ b/modules/tracker/mbt/src/vpMbGenericTracker.cpp @@ -3266,7 +3266,7 @@ void vpMbGenericTracker::loadModel(const std::map &map } } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbGenericTracker::preTracking(std::map *> &mapOfImages, std::map::ConstPtr> &mapOfPointClouds) { @@ -5448,7 +5448,7 @@ void vpMbGenericTracker::track(std::map *> &m track(mapOfColorImages, mapOfPointClouds, mapOfWidths, mapOfHeights); } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) /*! Realize the tracking of the object in the image. @@ -6838,7 +6838,7 @@ void vpMbGenericTracker::TrackerWrapper::loadConfigFile(const std::string &confi #endif } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage *const ptr_I, const pcl::PointCloud::ConstPtr &point_cloud) { @@ -7347,7 +7347,7 @@ void vpMbGenericTracker::TrackerWrapper::testTracking() } void vpMbGenericTracker::TrackerWrapper::track(const vpImage & -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) I #endif ) @@ -7361,7 +7361,7 @@ void vpMbGenericTracker::TrackerWrapper::track(const vpImage & return; } -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) track(&I, nullptr); #endif } @@ -7371,7 +7371,7 @@ void vpMbGenericTracker::TrackerWrapper::track(const vpImage &) // not exposed to the public API, only for debug } -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) void vpMbGenericTracker::TrackerWrapper::track(const vpImage *const ptr_I, const pcl::PointCloud::ConstPtr &point_cloud) { diff --git a/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp b/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp index 5d1749565b..bcea5937a6 100644 --- a/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp +++ b/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.cpp @@ -1,7 +1,7 @@ //! \example ClassUsingPclViewer.cpp #include "ClassUsingPclViewer.h" -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) // PCL #include diff --git a/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.h b/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.h index 45a541348e..f66ef3041e 100644 --- a/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.h +++ b/tutorial/gui/pcl-visualizer/ClassUsingPclViewer.h @@ -4,7 +4,7 @@ //! \example ClassUsingPclViewer.h #include -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) #include #include diff --git a/tutorial/gui/pcl-visualizer/tutorial-pcl-viewer.cpp b/tutorial/gui/pcl-visualizer/tutorial-pcl-viewer.cpp index c2ddc12c67..03023ea6e3 100644 --- a/tutorial/gui/pcl-visualizer/tutorial-pcl-viewer.cpp +++ b/tutorial/gui/pcl-visualizer/tutorial-pcl-viewer.cpp @@ -4,7 +4,7 @@ // System include #include -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) // ViSP include #include @@ -187,8 +187,8 @@ int main(int argc, char *argv[]) int main() { - std::cout << "ViSP seems to have been compiled without PCL library." << std::endl; - std::cout << "Please install PCL library and recompile ViSP." << std::endl; + std::cout << "ViSP seems to have been compiled without PCL visualization module." << std::endl; + std::cout << "Please install PCL visualization module and recompile ViSP." << std::endl; return EXIT_SUCCESS; } #endif diff --git a/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl-viewer.cpp b/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl-viewer.cpp index 6ca4686235..b3467ab0e4 100644 --- a/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl-viewer.cpp +++ b/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl-viewer.cpp @@ -3,7 +3,7 @@ #include #include -#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_PCL) && defined(VISP_HAVE_THREADS) && defined(VISP_HAVE_X11) +#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION) && defined(VISP_HAVE_THREADS) && defined(VISP_HAVE_X11) #include #include #include @@ -208,6 +208,9 @@ int main() #if !defined(VISP_HAVE_PCL) std::cout << "This tutorial needs pcl library as 3rd party." << std::endl; #endif +#if !defined(VISP_HAVE_PCL_VISUALIZATION) + std::cout << "This tutorial needs pcl visualization module." << std::endl; +#endif #if !defined(VISP_HAVE_X11) std::cout << "This tutorial needs X11 3rd party enabled." << std::endl; #endif diff --git a/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl.cpp b/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl.cpp index b6961ad488..823640fd7e 100644 --- a/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl.cpp +++ b/tutorial/segmentation/color/tutorial-hsv-segmentation-pcl.cpp @@ -3,7 +3,7 @@ #include #include -#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_PCL) && defined(VISP_HAVE_X11) +#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_X11) #include #include #include diff --git a/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-rs2.cpp b/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-rs2.cpp index 349fdd3a5e..6e40ce4ed1 100644 --- a/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-rs2.cpp +++ b/tutorial/tracking/model-based/generic-apriltag/tutorial-mb-generic-tracker-apriltag-rs2.cpp @@ -109,7 +109,7 @@ state_t track(const vpImage &I, vpMbGenericTracker &tracker, doub } state_t track(std::map *> mapOfImages, -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) std::map::ConstPtr> mapOfPointclouds, #else std::map *> mapOfPointclouds, @@ -124,7 +124,7 @@ state_t track(std::map *> mapOfImages, // Track the object try { -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) tracker.track(mapOfImages, mapOfPointclouds); #else tracker.track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights); @@ -243,7 +243,7 @@ int main(int argc, const char **argv) vpImage I_depth_raw(height, width); std::map mapOfCameraTransformations; std::map *> mapOfImages; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) std::map::ConstPtr> mapOfPointclouds; pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud()); #else @@ -368,7 +368,7 @@ int main(int argc, const char **argv) // wait for a tag detection while (state != state_quit) { if (opt_use_depth) { -#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); #else realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointcloud, nullptr, @@ -381,7 +381,7 @@ int main(int argc, const char **argv) mapOfImages["Camera1"] = &I_gray; mapOfImages["Camera2"] = &I_depth; -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) mapOfPointclouds["Camera2"] = pointcloud; #else mapOfPointclouds["Camera2"] = &pointcloud; @@ -412,7 +412,7 @@ int main(int argc, const char **argv) if (state == state_tracking) { if (opt_use_depth) { -#ifdef VISP_HAVE_PCL +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) state = track(mapOfImages, mapOfPointclouds, I_gray, I_depth, depth_M_color, tracker, opt_projection_error_threshold, cMo); #else diff --git a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp index 8013831c88..daedf62334 100644 --- a/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp +++ b/tutorial/tracking/model-based/generic-rgbd/tutorial-mb-generic-tracker-rgbd.cpp @@ -10,9 +10,10 @@ #include //! [Include] -#if defined(VISP_HAVE_PCL) +#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) #include -#include +#include +#include namespace {