Visual Servoing Platform version 3.7.0
Loading...
Searching...
No Matches
vpImageConvert.h
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Convert image types.
32 */
33
38
39#ifndef VP_IMAGE_CONVERT_H
40#define VP_IMAGE_CONVERT_H
41
42#include <stdint.h>
43
44// image
45#include <visp3/core/vpConfig.h>
46#include <visp3/core/vpImage.h>
47// color
48#include <visp3/core/vpHSV.h>
49#include <visp3/core/vpRGBa.h>
50
51#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC)
52#include <opencv2/imgproc/imgproc.hpp>
53#if (VISP_HAVE_OPENCV_VERSION < 0x050000)
54#include <opencv2/imgproc/types_c.h>
55#endif
56#endif
57
58#ifdef VISP_HAVE_YARP
59#include <yarp/sig/Image.h>
60#endif
61
62#if defined(_WIN32)
63
64// Mute warning with clang-cl
65// warning : non-portable path to file '<WinSock2.h>'; specified path differs in case from file name on disk [-Wnonportable-system-include-path]
66// warning : non-portable path to file '<Windows.h>'; specified path differs in case from file name on disk [-Wnonportable-system-include-path]
67#if defined(__clang__)
68# pragma clang diagnostic push
69# pragma clang diagnostic ignored "-Wnonportable-system-include-path"
70#endif
71
72// Include WinSock2.h before windows.h to ensure that winsock.h is not
73// included by windows.h since winsock.h and winsock2.h are incompatible
74#include <WinSock2.h>
75#include <windows.h>
76
77#if defined(__clang__)
78# pragma clang diagnostic pop
79#endif
80#endif
81
82#ifdef VISP_HAVE_OPENMP
83#include <omp.h>
84#endif
85
86#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && defined(VISP_HAVE_THREADS)
87#include <mutex>
88#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
89#include <type_traits>
90#endif
91#include <visp3/core/vpColVector.h>
92#include <visp3/core/vpImageException.h>
93#include <visp3/core/vpPixelMeterConversion.h>
94
95#include <pcl/pcl_config.h>
96#include <pcl/point_types.h>
97#include <pcl/point_cloud.h>
98#endif
99
114class VISP_EXPORT vpImageConvert
115{
116
117public:
118 static void createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage<vpRGBa> &dest_rgba);
119 static void createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage<unsigned char> &dest_depth);
120
121 static void createDepthHistogram(const vpImage<float> &src_depth, vpImage<vpRGBa> &dest_depth);
122 static void createDepthHistogram(const vpImage<float> &src_depth, vpImage<unsigned char> &dest_depth);
123
124 static void convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> &dest);
125 static void convert(const vpImage<vpRGBa> &src, vpImage<unsigned char> &dest, unsigned int nThreads = 0);
126
127 static void convert(const vpImage<float> &src, vpImage<unsigned char> &dest);
128 static void convert(const vpImage<vpRGBf> &src, vpImage<vpRGBa> &dest);
129 static void convert(const vpImage<unsigned char> &src, vpImage<float> &dest);
130
131 static void convert(const vpImage<double> &src, vpImage<unsigned char> &dest);
132 static void convert(const vpImage<unsigned char> &src, vpImage<double> &dest);
133
134 static void convert(const vpImage<uint16_t> &src, vpImage<unsigned char> &dest, unsigned char bitshift = 8);
135 static void convert(const vpImage<unsigned char> &src, vpImage<uint16_t> &dest, unsigned char bitshift = 8);
136
142 template <typename Type> static void convert(const vpImage<Type> &src, vpImage<Type> &dest) { dest = src; }
143
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);
151 static void convert(const vpImage<vpRGBa> &src, cv::Mat &dest);
152 static void convert(const vpImage<unsigned char> &src, cv::Mat &dest, bool copyData = true);
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);
155 static void convert(const vpImage<vpRGBf> &src, cv::Mat &dest);
156#endif
157#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
158 template <typename T, bool useFullScale>
159 static void convert(const vpImage<vpRGBa> &src, vpImage<vpHSV<T, useFullScale>> &dest);
160
161 template <typename T, bool useFullScale>
162 static void convert(const vpImage<vpHSV<T, useFullScale>> &src, vpImage<vpRGBa> &dest);
163
164 template <typename T, typename U, bool useFullScale1, bool useFullScale2>
165 static typename std::enable_if<!std::is_same<T, U>::value, void>::type convert(const vpImage<vpHSV<T, useFullScale1>> &src, vpImage<vpHSV<U, useFullScale2>> &dest)
166 {
167 const int height = src.getHeight(), width = src.getWidth();
168 const int size = height * width;
169 dest.resize(height, width);
170#ifdef VISP_HAVE_OPENMP
171#pragma omp parallel for
172#endif
173 for (int i = 0; i < size; ++i) {
174 dest.bitmap[i].buildFrom(src.bitmap[i]);
175 }
176 }
177#endif
178
179#ifdef VISP_HAVE_YARP
180 static void convert(const vpImage<unsigned char> &src, yarp::sig::ImageOf<yarp::sig::PixelMono> *dest,
181 bool copyData = true);
182 static void convert(const yarp::sig::ImageOf<yarp::sig::PixelMono> *src, vpImage<unsigned char> &dest,
183 bool copyData = true);
184
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);
187
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);
190#endif
191
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
196#else
197 template <typename MaskType>
198 static int
199#endif
217 float depth_scale, const vpCameraParameters &cam_depth,
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)
221 {
222 int size = static_cast<int>(depth_raw.getSize());
223 unsigned int width = depth_raw.getWidth();
224 unsigned int height = depth_raw.getHeight();
225 int pcl_size = 0;
226 const unsigned int index_0 = 0;
227 const unsigned int index_1 = 1;
228 const unsigned int index_2 = 2;
229
230 if (depth_mask) {
231 if ((width != depth_mask->getWidth()) || (height != depth_mask->getHeight())) {
232 throw(vpImageException(vpImageException::notInitializedError, "Depth image and mask size differ"));
233 }
234 if (pointcloud_mutex) {
235 pointcloud_mutex->lock();
236 }
237 pointcloud->clear();
238#if defined(VISP_HAVE_OPENMP)
239 std::mutex mutex;
240#pragma omp parallel for
241#endif
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;
246 if (Z < Z_max) {
247 double x = 0;
248 double y = 0;
249 unsigned int j = p % width;
250 unsigned int i = (p - j) / width;
251 vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
252 vpColVector point_3D({ x * Z, y * Z, Z });
253 if (point_3D[index_2] > Z_min) {
254#if defined(VISP_HAVE_OPENMP)
255 std::lock_guard<std::mutex> lock(mutex);
256#endif
257 pointcloud->push_back(pcl::PointXYZ(point_3D[index_0], point_3D[index_1], point_3D[index_2]));
258 }
259 }
260 }
261 }
262 }
263 pcl_size = pointcloud->size();
264 if (pointcloud_mutex) {
265 pointcloud_mutex->unlock();
266 }
267 }
268 else {
269 if (pointcloud_mutex) {
270 pointcloud_mutex->lock();
271 }
272 pointcloud->clear();
273#if defined(VISP_HAVE_OPENMP)
274 std::mutex mutex;
275#pragma omp parallel for
276#endif
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;
280 if (Z < Z_max) {
281 double x = 0;
282 double y = 0;
283 unsigned int j = p % width;
284 unsigned int i = (p - j) / width;
285 vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
286 vpColVector point_3D({ x * Z, y * Z, Z, 1 });
287 if (point_3D[index_2] >= 0.1) {
288#if defined(VISP_HAVE_OPENMP)
289 std::lock_guard<std::mutex> lock(mutex);
290#endif
291 pointcloud->push_back(pcl::PointXYZ(point_3D[index_0], point_3D[index_1], point_3D[index_2]));
292 }
293 }
294 }
295 }
296 pcl_size = pointcloud->size();
297 if (pointcloud_mutex) {
298 pointcloud_mutex->unlock();
299 }
300 }
301
302 return pcl_size;
303 }
304
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
308#else
309 template <typename MaskType>
310 static int
311#endif
331 float depth_scale, const vpCameraParameters &cam_depth,
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)
335 {
336 int size = static_cast<int>(depth_raw.getSize());
337 unsigned int width = depth_raw.getWidth();
338 unsigned int height = depth_raw.getHeight();
339 int pcl_size = 0;
340 const unsigned int index_0 = 0;
341 const unsigned int index_1 = 1;
342 const unsigned int index_2 = 2;
343
344 if (depth_mask) {
345 if ((width != depth_mask->getWidth()) || (height != depth_mask->getHeight())) {
346 throw(vpImageException(vpImageException::notInitializedError, "Depth image and mask size differ"));
347 }
348 if (pointcloud_mutex) {
349 pointcloud_mutex->lock();
350 }
351 pointcloud->clear();
352#if defined(VISP_HAVE_OPENMP)
353 std::mutex mutex;
354#pragma omp parallel for
355#endif
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;
360 if (Z < Z_max) {
361 double x = 0;
362 double y = 0;
363 unsigned int j = p % width;
364 unsigned int i = (p - j) / width;
365 vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
366 vpColVector point_3D({ x * Z, y * Z, Z });
367 if (point_3D[index_2] > Z_min) {
368#if defined(VISP_HAVE_OPENMP)
369 std::lock_guard<std::mutex> lock(mutex);
370#endif
371#if (VISP_HAVE_PCL_VERSION >= 0x010E01) // 1.14.1
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));
374#else
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));
380#endif
381 }
382 }
383 }
384 }
385 }
386 pcl_size = pointcloud->size();
387 if (pointcloud_mutex) {
388 pointcloud_mutex->unlock();
389 }
390 }
391 else {
392 if (pointcloud_mutex) {
393 pointcloud_mutex->lock();
394 }
395 pointcloud->clear();
396#if defined(VISP_HAVE_OPENMP)
397 std::mutex mutex;
398#pragma omp parallel for
399#endif
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;
403 if (Z < Z_max) {
404 double x = 0;
405 double y = 0;
406 unsigned int j = p % width;
407 unsigned int i = (p - j) / width;
408 vpPixelMeterConversion::convertPoint(cam_depth, j, i, x, y);
409 vpColVector point_3D({ x * Z, y * Z, Z, 1 });
410 if (point_3D[index_2] >= 0.1) {
411#if defined(VISP_HAVE_OPENMP)
412 std::lock_guard<std::mutex> lock(mutex);
413#endif
414#if (VISP_HAVE_PCL_VERSION >= 0x010E01) // 1.14.1
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));
417#else
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));
423#endif
424 }
425 }
426 }
427 }
428 pcl_size = pointcloud->size();
429 if (pointcloud_mutex) {
430 pointcloud_mutex->unlock();
431 }
432 }
433
434 return pcl_size;
435 }
436#endif
437
438 static void split(const vpImage<vpRGBa> &src, vpImage<unsigned char> *pR, vpImage<unsigned char> *pG,
440
441 static void merge(const vpImage<unsigned char> *R, const vpImage<unsigned char> *G, const vpImage<unsigned char> *B,
443
458 static inline void YUVToRGB(unsigned char y, unsigned char u, unsigned char v, unsigned char &r, unsigned char &g,
459 unsigned char &b)
460 {
461 double dr, dg, db;
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)));
465
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;
472
473 r = static_cast<unsigned char>(dr);
474 g = static_cast<unsigned char>(dg);
475 b = static_cast<unsigned char>(db);
476 }
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);
489
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);
493
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);
500
501 static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height,
502 bool flip = false);
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);
507
508 static void RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int width, unsigned int height,
509 bool flip = false);
510
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);
514
515 static void BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height,
516 bool flip = false);
517
518 static void BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height,
519 bool flip = false, unsigned int nThreads = 0);
520
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,
524 bool flip = false);
525
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);
533
534 static void HSVToRGBa(const double *hue, const double *saturation, const double *value, unsigned char *rgba,
535 unsigned int size);
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);
541
542 static void HSVToRGB(const double *hue, const double *saturation, const double *value, unsigned char *rgb,
543 unsigned int size);
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);
549
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);
555
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);
560
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);
565
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);
570
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);
575
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);
580
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);
585
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);
590#endif
591
592private:
593 static void computeYCbCrLUT();
594
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,
600 unsigned int step);
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);
603
604private:
605 static bool YCbCrLUTcomputed;
606 static int vpCrr[256];
607 static int vpCgb[256];
608 static int vpCgr[256];
609 static int vpCbb[256];
610};
611
612#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
621template <typename T, bool useFullScale>
623{
624 const int height = src.getHeight(), width = src.getWidth();
625 const int size = height * width;
626 dest.resize(height, width);
627#ifdef VISP_HAVE_OPENMP
628#pragma omp parallel for
629#endif
630 for (int i = 0; i < size; ++i) {
631 dest.bitmap[i].buildFrom(src.bitmap[i]);
632 }
633}
634
643template <typename T, bool useFullScale>
645{
646 const int height = src.getHeight(), width = src.getWidth();
647 const int size = height * width;
648 dest.resize(height, width);
649#ifdef VISP_HAVE_OPENMP
650#pragma omp parallel for
651#endif
652 for (int i = 0; i < size; ++i) {
653 dest.bitmap[i].buildFrom(src.bitmap[i]);
654 }
655}
656#endif
657END_VISP_NAMESPACE
658#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Class implementing the HSV pixel format.
Definition vpHSV.h:135
static void convert(const vpImage< Type > &src, vpImage< Type > &dest)
static std::enable_if<!std::is_same< T, U >::value, void >::type convert(const vpImage< vpHSV< T, useFullScale1 > > &src, vpImage< vpHSV< U, useFullScale2 > > &dest)
static void YUVToRGB(unsigned char y, unsigned char u, unsigned char v, unsigned char &r, unsigned char &g, unsigned char &b)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Error that can be emitted by the vpImage class and its derivatives.
@ notInitializedError
Image not initialized.
Definition of the vpImage class member functions.
Definition vpImage.h:131
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getSize() const
Definition vpImage.h:221
Type * bitmap
points toward the bitmap
Definition vpImage.h:135
unsigned int getHeight() const
Definition vpImage.h:181
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
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.