144#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
145 static void convert(
const cv::Mat &src,
vpImage<vpRGBa> &dest,
bool flip =
false);
146 static void convert(
const cv::Mat &src,
vpImage<unsigned char> &dest,
bool flip =
false,
unsigned int nThreads = 0);
147 static void convert(
const cv::Mat &src,
vpImage<float> &dest,
bool flip =
false);
148 static void convert(
const cv::Mat &src,
vpImage<double> &dest,
bool flip =
false);
149 static void convert(
const cv::Mat &src,
vpImage<vpRGBf> &dest,
bool flip =
false);
150 static void convert(
const cv::Mat &src,
vpImage<uint16_t> &dest,
bool flip =
false);
153 static void convert(
const vpImage<float> &src, cv::Mat &dest,
bool copyData =
true);
154 static void convert(
const vpImage<double> &src, cv::Mat &dest,
bool copyData =
true);
157#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
158 template <
typename T,
bool useFullScale>
161 template <
typename T,
bool useFullScale>
164 template <
typename T,
typename U,
bool useFullScale1,
bool useFullScale2>
168 const int size = height * width;
169 dest.resize(height, width);
170#ifdef VISP_HAVE_OPENMP
171#pragma omp parallel for
173 for (
int i = 0; i < size; ++i) {
174 dest.bitmap[i].buildFrom(src.
bitmap[i]);
181 bool copyData =
true);
183 bool copyData =
true);
185 static void convert(
const vpImage<vpRGBa> &src, yarp::sig::ImageOf<yarp::sig::PixelRgba> *dest,
bool copyData =
true);
186 static void convert(
const yarp::sig::ImageOf<yarp::sig::PixelRgba> *src,
vpImage<vpRGBa> &dest,
bool copyData =
true);
188 static void convert(
const vpImage<vpRGBa> &src, yarp::sig::ImageOf<yarp::sig::PixelRgb> *dest);
189 static void convert(
const yarp::sig::ImageOf<yarp::sig::PixelRgb> *src,
vpImage<vpRGBa> &dest);
192#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS)
193#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
194 template <
typename MaskType>
195 static typename std::enable_if< std::is_same<MaskType, unsigned char>::value || std::is_same<MaskType, bool>::value,
int>::type
197 template <
typename MaskType>
218 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud,
219 std::mutex *pointcloud_mutex =
nullptr,
220 const vpImage<MaskType> *depth_mask =
nullptr,
float Z_min = 0.2,
float Z_max = 2.5)
222 int size =
static_cast<int>(depth_raw.
getSize());
223 unsigned int width = depth_raw.
getWidth();
224 unsigned int height = depth_raw.
getHeight();
226 const unsigned int index_0 = 0;
227 const unsigned int index_1 = 1;
228 const unsigned int index_2 = 2;
231 if ((width != depth_mask->getWidth()) || (height != depth_mask->getHeight())) {
234 if (pointcloud_mutex) {
235 pointcloud_mutex->lock();
238#if defined(VISP_HAVE_OPENMP)
240#pragma omp parallel for
242 for (
int p = 0; p < size; ++p) {
243 if (depth_mask->bitmap[p]) {
244 if (
static_cast<int>(depth_raw.
bitmap[p])) {
245 float Z =
static_cast<float>(depth_raw.
bitmap[p]) * depth_scale;
249 unsigned int j = p % width;
250 unsigned int i = (p - j) / width;
253 if (point_3D[index_2] > Z_min) {
254#if defined(VISP_HAVE_OPENMP)
255 std::lock_guard<std::mutex> lock(mutex);
257 pointcloud->push_back(pcl::PointXYZ(point_3D[index_0], point_3D[index_1], point_3D[index_2]));
263 pcl_size = pointcloud->size();
264 if (pointcloud_mutex) {
265 pointcloud_mutex->unlock();
269 if (pointcloud_mutex) {
270 pointcloud_mutex->lock();
273#if defined(VISP_HAVE_OPENMP)
275#pragma omp parallel for
277 for (
int p = 0; p < size; ++p) {
278 if (
static_cast<int>(depth_raw.
bitmap[p])) {
279 float Z =
static_cast<float>(depth_raw.
bitmap[p]) * depth_scale;
283 unsigned int j = p % width;
284 unsigned int i = (p - j) / width;
287 if (point_3D[index_2] >= 0.1) {
288#if defined(VISP_HAVE_OPENMP)
289 std::lock_guard<std::mutex> lock(mutex);
291 pointcloud->push_back(pcl::PointXYZ(point_3D[index_0], point_3D[index_1], point_3D[index_2]));
296 pcl_size = pointcloud->size();
297 if (pointcloud_mutex) {
298 pointcloud_mutex->unlock();
305#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
306 template <
typename MaskType>
307 static typename std::enable_if< std::is_same<MaskType, unsigned char>::value || std::is_same<MaskType, bool>::value,
int>::type
309 template <
typename MaskType>
332 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud,
333 std::mutex *pointcloud_mutex =
nullptr,
334 const vpImage<MaskType> *depth_mask =
nullptr,
float Z_min = 0.2,
float Z_max = 2.5)
336 int size =
static_cast<int>(depth_raw.
getSize());
337 unsigned int width = depth_raw.
getWidth();
338 unsigned int height = depth_raw.
getHeight();
340 const unsigned int index_0 = 0;
341 const unsigned int index_1 = 1;
342 const unsigned int index_2 = 2;
345 if ((width != depth_mask->getWidth()) || (height != depth_mask->getHeight())) {
348 if (pointcloud_mutex) {
349 pointcloud_mutex->lock();
352#if defined(VISP_HAVE_OPENMP)
354#pragma omp parallel for
356 for (
int p = 0; p < size; ++p) {
357 if (depth_mask->bitmap[p]) {
358 if (
static_cast<int>(depth_raw.
bitmap[p])) {
359 float Z =
static_cast<float>(depth_raw.
bitmap[p]) * depth_scale;
363 unsigned int j = p % width;
364 unsigned int i = (p - j) / width;
367 if (point_3D[index_2] > Z_min) {
368#if defined(VISP_HAVE_OPENMP)
369 std::lock_guard<std::mutex> lock(mutex);
371#if (VISP_HAVE_PCL_VERSION >= 0x010E01)
372 pointcloud->push_back(pcl::PointXYZRGB(point_3D[index_0], point_3D[index_1], point_3D[index_2],
373 color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B));
375 pcl::PointXYZRGB pt(color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B);
376 pt.x = point_3D[index_0];
377 pt.y = point_3D[index_1];
378 pt.z = point_3D[index_2];
379 pointcloud->push_back(pcl::PointXYZRGB(pt));
386 pcl_size = pointcloud->size();
387 if (pointcloud_mutex) {
388 pointcloud_mutex->unlock();
392 if (pointcloud_mutex) {
393 pointcloud_mutex->lock();
396#if defined(VISP_HAVE_OPENMP)
398#pragma omp parallel for
400 for (
int p = 0; p < size; ++p) {
401 if (
static_cast<int>(depth_raw.
bitmap[p])) {
402 float Z =
static_cast<float>(depth_raw.
bitmap[p]) * depth_scale;
406 unsigned int j = p % width;
407 unsigned int i = (p - j) / width;
410 if (point_3D[index_2] >= 0.1) {
411#if defined(VISP_HAVE_OPENMP)
412 std::lock_guard<std::mutex> lock(mutex);
414#if (VISP_HAVE_PCL_VERSION >= 0x010E01)
415 pointcloud->push_back(pcl::PointXYZRGB(point_3D[index_0], point_3D[index_1], point_3D[index_2],
416 color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B));
418 pcl::PointXYZRGB pt(color.bitmap[p].R, color.bitmap[p].G, color.bitmap[p].B);
419 pt.x = point_3D[index_0];
420 pt.y = point_3D[index_1];
421 pt.z = point_3D[index_2];
422 pointcloud->push_back(pcl::PointXYZRGB(pt));
428 pcl_size = pointcloud->size();
429 if (pointcloud_mutex) {
430 pointcloud_mutex->unlock();
458 static inline void YUVToRGB(
unsigned char y,
unsigned char u,
unsigned char v,
unsigned char &r,
unsigned char &g,
462 dr = floor(((0.9999695 * y) - (0.0009508 * (u - 128))) + (1.1359061 * (v - 128)));
463 dg = floor(((0.9999695 * y) - (0.3959609 * (u - 128))) - (0.5782955 * (v - 128)));
464 db = floor(((0.9999695 * y) + (2.04112 * (u - 128))) - (0.0016314 * (v - 128)));
466 dr = dr < 0. ? 0. : dr;
467 dg = dg < 0. ? 0. : dg;
468 db = db < 0. ? 0. : db;
469 dr = dr > 255. ? 255. : dr;
470 dg = dg > 255. ? 255. : dg;
471 db = db > 255. ? 255. : db;
473 r =
static_cast<unsigned char>(dr);
474 g =
static_cast<unsigned char>(dg);
475 b =
static_cast<unsigned char>(db);
477 static void YUYVToRGBa(
unsigned char *yuyv,
unsigned char *rgba,
unsigned int width,
unsigned int height);
478 static void YUYVToRGB(
unsigned char *yuyv,
unsigned char *rgb,
unsigned int width,
unsigned int height);
479 static void YUYVToGrey(
unsigned char *yuyv,
unsigned char *grey,
unsigned int size);
480 static void YUV411ToRGBa(
unsigned char *yuv,
unsigned char *rgba,
unsigned int size);
481 static void YUV411ToRGB(
unsigned char *yuv,
unsigned char *rgb,
unsigned int size);
482 static void YUV411ToGrey(
unsigned char *yuv,
unsigned char *grey,
unsigned int size);
483 static void YUV422ToRGBa(
unsigned char *yuv,
unsigned char *rgba,
unsigned int size);
484 static void YUV422ToRGB(
unsigned char *yuv,
unsigned char *rgb,
unsigned int size);
485 static void YUV422ToGrey(
unsigned char *yuv,
unsigned char *grey,
unsigned int size);
486 static void YUV420ToRGBa(
unsigned char *yuv,
unsigned char *rgba,
unsigned int width,
unsigned int height);
487 static void YUV420ToRGB(
unsigned char *yuv,
unsigned char *rgb,
unsigned int width,
unsigned int height);
488 static void YUV420ToGrey(
unsigned char *yuv,
unsigned char *grey,
unsigned int size);
490 static void YUV444ToRGBa(
unsigned char *yuv,
unsigned char *rgba,
unsigned int size);
491 static void YUV444ToRGB(
unsigned char *yuv,
unsigned char *rgb,
unsigned int size);
492 static void YUV444ToGrey(
unsigned char *yuv,
unsigned char *grey,
unsigned int size);
494 static void YV12ToRGBa(
unsigned char *yuv,
unsigned char *rgba,
unsigned int width,
unsigned int height);
495 static void YV12ToRGB(
unsigned char *yuv,
unsigned char *rgb,
unsigned int width,
unsigned int height);
496 static void YVU9ToRGBa(
unsigned char *yuv,
unsigned char *rgba,
unsigned int width,
unsigned int height);
497 static void YVU9ToRGB(
unsigned char *yuv,
unsigned char *rgb,
unsigned int width,
unsigned int height);
498 static void RGBToRGBa(
unsigned char *rgb,
unsigned char *rgba,
unsigned int size);
499 static void RGBaToRGB(
unsigned char *rgba,
unsigned char *rgb,
unsigned int size);
501 static void RGBToGrey(
unsigned char *rgb,
unsigned char *grey,
unsigned int width,
unsigned int height,
503 static void RGBToGrey(
unsigned char *rgb,
unsigned char *grey,
unsigned int size);
504 static void RGBaToGrey(
unsigned char *rgba,
unsigned char *grey,
unsigned int width,
unsigned int height,
505 unsigned int nThreads = 0);
506 static void RGBaToGrey(
unsigned char *rgba,
unsigned char *grey,
unsigned int size);
508 static void RGBToRGBa(
unsigned char *rgb,
unsigned char *rgba,
unsigned int width,
unsigned int height,
511 static void GreyToRGBa(
unsigned char *grey,
unsigned char *rgba,
unsigned int width,
unsigned int height);
512 static void GreyToRGBa(
unsigned char *grey,
unsigned char *rgba,
unsigned int size);
513 static void GreyToRGB(
unsigned char *grey,
unsigned char *rgb,
unsigned int size);
515 static void BGRToRGBa(
unsigned char *bgr,
unsigned char *rgba,
unsigned int width,
unsigned int height,
518 static void BGRToGrey(
unsigned char *bgr,
unsigned char *grey,
unsigned int width,
unsigned int height,
519 bool flip =
false,
unsigned int nThreads = 0);
521 static void BGRaToGrey(
unsigned char *bgra,
unsigned char *grey,
unsigned int width,
unsigned int height,
522 bool flip =
false,
unsigned int nThreads = 0);
523 static void BGRaToRGBa(
unsigned char *bgra,
unsigned char *rgba,
unsigned int width,
unsigned int height,
526 static void YCbCrToRGB(
unsigned char *ycbcr,
unsigned char *rgb,
unsigned int size);
527 static void YCbCrToRGBa(
unsigned char *ycbcr,
unsigned char *rgb,
unsigned int size);
528 static void YCbCrToGrey(
unsigned char *ycbcr,
unsigned char *grey,
unsigned int size);
529 static void YCrCbToRGB(
unsigned char *ycrcb,
unsigned char *rgb,
unsigned int size);
530 static void YCrCbToRGBa(
unsigned char *ycrcb,
unsigned char *rgb,
unsigned int size);
531 static void MONO16ToGrey(
unsigned char *grey16,
unsigned char *grey,
unsigned int size);
532 static void MONO16ToRGBa(
unsigned char *grey16,
unsigned char *rgba,
unsigned int size);
534 static void HSVToRGBa(
const double *hue,
const double *saturation,
const double *value,
unsigned char *rgba,
536 static void HSVToRGBa(
const unsigned char *hue,
const unsigned char *saturation,
const unsigned char *value,
537 unsigned char *rgba,
unsigned int size,
bool h_full =
true);
538 static void RGBaToHSV(
const unsigned char *rgba,
double *hue,
double *saturation,
double *value,
unsigned int size);
539 static void RGBaToHSV(
const unsigned char *rgba,
unsigned char *hue,
unsigned char *saturation,
unsigned char *value,
540 unsigned int size,
bool h_full =
true);
542 static void HSVToRGB(
const double *hue,
const double *saturation,
const double *value,
unsigned char *rgb,
544 static void HSVToRGB(
const unsigned char *hue,
const unsigned char *saturation,
const unsigned char *value,
545 unsigned char *rgb,
unsigned int size,
bool h_full =
true);
546 static void RGBToHSV(
const unsigned char *rgb,
double *hue,
double *saturation,
double *value,
unsigned int size);
547 static void RGBToHSV(
const unsigned char *rgb,
unsigned char *hue,
unsigned char *saturation,
unsigned char *value,
548 unsigned int size,
bool h_full =
true);
550#ifndef VISP_SKIP_BAYER_CONVERSION
551 static void demosaicBGGRToRGBaBilinear(
const uint8_t *bggr, uint8_t *rgba,
unsigned int width,
unsigned int height,
552 unsigned int nThreads = 0);
553 static void demosaicBGGRToRGBaBilinear(
const uint16_t *bggr, uint16_t *rgba,
unsigned int width,
unsigned int height,
554 unsigned int nThreads = 0);
556 static void demosaicGBRGToRGBaBilinear(
const uint8_t *gbrg, uint8_t *rgba,
unsigned int width,
unsigned int height,
557 unsigned int nThreads = 0);
558 static void demosaicGBRGToRGBaBilinear(
const uint16_t *gbrg, uint16_t *rgba,
unsigned int width,
unsigned int height,
559 unsigned int nThreads = 0);
561 static void demosaicGRBGToRGBaBilinear(
const uint8_t *grbg, uint8_t *rgba,
unsigned int width,
unsigned int height,
562 unsigned int nThreads = 0);
563 static void demosaicGRBGToRGBaBilinear(
const uint16_t *grbg, uint16_t *rgba,
unsigned int width,
unsigned int height,
564 unsigned int nThreads = 0);
566 static void demosaicRGGBToRGBaBilinear(
const uint8_t *rggb, uint8_t *rgba,
unsigned int width,
unsigned int height,
567 unsigned int nThreads = 0);
568 static void demosaicRGGBToRGBaBilinear(
const uint16_t *rggb, uint16_t *rgba,
unsigned int width,
unsigned int height,
569 unsigned int nThreads = 0);
571 static void demosaicBGGRToRGBaMalvar(
const uint8_t *bggr, uint8_t *rgba,
unsigned int width,
unsigned int height,
572 unsigned int nThreads = 0);
573 static void demosaicBGGRToRGBaMalvar(
const uint16_t *bggr, uint16_t *rgba,
unsigned int width,
unsigned int height,
574 unsigned int nThreads = 0);
576 static void demosaicGBRGToRGBaMalvar(
const uint8_t *gbrg, uint8_t *rgba,
unsigned int width,
unsigned int height,
577 unsigned int nThreads = 0);
578 static void demosaicGBRGToRGBaMalvar(
const uint16_t *gbrg, uint16_t *rgba,
unsigned int width,
unsigned int height,
579 unsigned int nThreads = 0);
581 static void demosaicGRBGToRGBaMalvar(
const uint8_t *grbg, uint8_t *rgba,
unsigned int width,
unsigned int height,
582 unsigned int nThreads = 0);
583 static void demosaicGRBGToRGBaMalvar(
const uint16_t *grbg, uint16_t *rgba,
unsigned int width,
unsigned int height,
584 unsigned int nThreads = 0);
586 static void demosaicRGGBToRGBaMalvar(
const uint8_t *rggb, uint8_t *rgba,
unsigned int width,
unsigned int height,
587 unsigned int nThreads = 0);
588 static void demosaicRGGBToRGBaMalvar(
const uint16_t *rggb, uint16_t *rgba,
unsigned int width,
unsigned int height,
589 unsigned int nThreads = 0);
593 static void computeYCbCrLUT();
595 static void HSV2RGB(
const double *hue,
const double *saturation,
const double *value,
unsigned char *rgba,
596 unsigned int size,
unsigned int step);
597 static void HSV2RGB(
const unsigned char *hue,
const unsigned char *saturation,
const unsigned char *value,
unsigned char *rgba,
598 unsigned int size,
unsigned int step,
bool h_full);
599 static void RGB2HSV(
const unsigned char *rgb,
double *hue,
double *saturation,
double *value,
unsigned int size,
601 static void RGB2HSV(
const unsigned char *rgb,
unsigned char *hue,
unsigned char *saturation,
unsigned char *value,
602 unsigned int size,
unsigned int step,
bool h_full);
605 static bool YCbCrLUTcomputed;
606 static int vpCrr[256];
607 static int vpCgb[256];
608 static int vpCgr[256];
609 static int vpCbb[256];
static std::enable_if< std::is_same< MaskType, unsignedchar >::value||std::is_same< MaskType, bool >::value, int >::type depthToPointCloud(const vpImage< vpRGBa > &color, const vpImage< uint16_t > &depth_raw, float depth_scale, const vpCameraParameters &cam_depth, pcl::PointCloud< pcl::PointXYZRGB >::Ptr pointcloud, std::mutex *pointcloud_mutex=nullptr, const vpImage< MaskType > *depth_mask=nullptr, float Z_min=0.2, float Z_max=2.5)
Convert a raw depth image in a textured pcl::PointCloud using a vpImage<RGBa> that is aligned with th...
static std::enable_if< std::is_same< MaskType, unsignedchar >::value||std::is_same< MaskType, bool >::value, int >::type depthToPointCloud(const vpImage< uint16_t > &depth_raw, float depth_scale, const vpCameraParameters &cam_depth, pcl::PointCloud< pcl::PointXYZ >::Ptr pointcloud, std::mutex *pointcloud_mutex=nullptr, const vpImage< MaskType > *depth_mask=nullptr, float Z_min=0.2, float Z_max=2.5)
Convert a raw depth image into a pcl::PointCloud that has no texture.