Visual Servoing Platform  version 3.5.1 under development (2022-08-10)
vpColorDepthConversion.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2022 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Color to depth conversion.
33  *
34  * Authors:
35  * Julien Dufour
36  *
37  *****************************************************************************/
38 
44 #include <visp3/core/vpColorDepthConversion.h>
45 
46 // System
47 #include <algorithm>
48 
49 // Core
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMeterPixelConversion.h>
52 #include <visp3/core/vpPixelMeterConversion.h>
53 
54 namespace
55 {
56 
65 vpImagePoint adjust2DPointToBoundary(const vpImagePoint &ip, double width, double height)
66 {
67 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
68  return {vpMath::clamp(ip.get_i(), 0., height), vpMath::clamp(ip.get_j(), 0., width)};
69 #else
70  return vpImagePoint(vpMath::clamp(ip.get_i(), 0., height), vpMath::clamp(ip.get_j(), 0., width));
71 #endif
72 }
73 
81 vpColVector transform(const vpHomogeneousMatrix &extrinsics_params, vpColVector from_point)
82 {
83 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
84  from_point = {from_point, 0, 3};
85  from_point.stack(1.);
86  return {extrinsics_params * from_point, 0, 3};
87 #else
88  from_point = vpColVector(from_point, 0, 3);
89  from_point.stack(1.);
90  return vpColVector(extrinsics_params * from_point, 0, 3);
91 #endif
92 }
93 
101 vpImagePoint project(const vpCameraParameters &intrinsic_cam_params, const vpColVector &point)
102 {
103 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
104  vpImagePoint iP{};
105 #else
106  vpImagePoint iP;
107 #endif
108  vpMeterPixelConversion::convertPoint(intrinsic_cam_params, point[0] / point[2], point[1] / point[2], iP);
109 
110  return iP;
111 }
112 
121 vpColVector deproject(const vpCameraParameters &intrinsic_cam_params, const vpImagePoint &pixel, double depth)
122 {
123 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
124  double x{0.}, y{0.};
125  vpPixelMeterConversion::convertPoint(intrinsic_cam_params, pixel, x, y);
126  return {x * depth, y * depth, depth};
127 #else
128  double x = 0., y = 0.;
129  vpPixelMeterConversion::convertPoint(intrinsic_cam_params, pixel, x, y);
130 
131  vpColVector p(3);
132  p[0] = x * depth;
133  p[1] = y * depth;
134  p[2] = depth;
135  return p;
136 #endif
137 }
138 
139 } // namespace
140 
154  const vpImage<uint16_t> &I_depth, double depth_scale, double depth_min, double depth_max,
155  const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics,
156  const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
157 {
158  return projectColorToDepth(I_depth.bitmap, depth_scale, depth_min, depth_max, I_depth.getWidth(), I_depth.getHeight(),
159  depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
160 }
161 
177  const uint16_t *data, double depth_scale, double depth_min, double depth_max, double depth_width,
178  double depth_height, const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics,
179  const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
180 {
181 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
182  vpImagePoint depth_pixel{};
183 
184  // Find line start pixel
185  const auto min_point = deproject(color_intrinsics, from_pixel, depth_min);
186  const auto min_transformed_point = transform(depth_M_color, min_point);
187  auto start_pixel = project(depth_intrinsics, min_transformed_point);
188  start_pixel = adjust2DPointToBoundary(start_pixel, depth_width, depth_height);
189 
190  // Find line end depth pixel
191  const auto max_point = deproject(color_intrinsics, from_pixel, depth_max);
192  const auto max_transformed_point = transform(depth_M_color, max_point);
193  auto end_pixel = project(depth_intrinsics, max_transformed_point);
194  end_pixel = adjust2DPointToBoundary(end_pixel, depth_width, depth_height);
195 
196  // search along line for the depth pixel that it's projected pixel is the closest to the input pixel
197  auto min_dist = -1.;
198  for (auto curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && curr_pixel != end_pixel;
199  curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) {
200  const auto depth = depth_scale * data[static_cast<int>(curr_pixel.get_v() * depth_width + curr_pixel.get_u())];
201  if (std::fabs(depth) <= std::numeric_limits<double>::epsilon())
202  continue;
203 
204  const auto point = deproject(depth_intrinsics, curr_pixel, depth);
205  const auto transformed_point = transform(color_M_depth, point);
206  const auto projected_pixel = project(color_intrinsics, transformed_point);
207 
208  const auto new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) +
209  vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u());
210  if (new_dist < min_dist || min_dist < 0) {
211  min_dist = new_dist;
212  depth_pixel = curr_pixel;
213  }
214  }
215 
216 #else
217  vpImagePoint depth_pixel;
218 
219  // Find line start pixel
220  const vpColVector min_point = deproject(color_intrinsics, from_pixel, depth_min);
221  const vpColVector min_transformed_point = transform(depth_M_color, min_point);
222  vpImagePoint start_pixel = project(depth_intrinsics, min_transformed_point);
223  start_pixel = adjust2DPointToBoundary(start_pixel, depth_width, depth_height);
224 
225  // Find line end depth pixel
226  const vpColVector max_point = deproject(color_intrinsics, from_pixel, depth_max);
227  const vpColVector max_transformed_point = transform(depth_M_color, max_point);
228  vpImagePoint end_pixel = project(depth_intrinsics, max_transformed_point);
229  end_pixel = adjust2DPointToBoundary(end_pixel, depth_width, depth_height);
230 
231  // search along line for the depth pixel that it's projected pixel is the closest to the input pixel
232  double min_dist = -1.;
233  for (vpImagePoint curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && curr_pixel != end_pixel;
234  curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) {
235  const double depth = depth_scale * data[static_cast<int>(curr_pixel.get_v() * depth_width + curr_pixel.get_u())];
236  if (std::fabs(depth) <= std::numeric_limits<double>::epsilon())
237  continue;
238 
239  const vpColVector point = deproject(depth_intrinsics, curr_pixel, depth);
240  const vpColVector transformed_point = transform(color_M_depth, point);
241  const vpImagePoint projected_pixel = project(color_intrinsics, transformed_point);
242 
243  const double new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) +
244  vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u());
245  if (new_dist < min_dist || min_dist < 0) {
246  min_dist = new_dist;
247  depth_pixel = curr_pixel;
248  }
249  }
250 #endif
251 
252  return depth_pixel;
253 }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
void stack(double d)
static vpImagePoint projectColorToDepth(const vpImage< uint16_t > &I_depth, double depth_scale, double depth_min, double depth_max, const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics, const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:89
double get_j() const
Definition: vpImagePoint.h:132
vpImagePoint nextInSegment(const vpImagePoint &start, const vpImagePoint &end) const
Definition: vpImagePoint.h:222
double get_u() const
Definition: vpImagePoint.h:143
bool inSegment(const vpImagePoint &start, const vpImagePoint &end) const
Definition: vpImagePoint.h:169
double get_i() const
Definition: vpImagePoint.h:121
double get_v() const
Definition: vpImagePoint.h:154
Definition of the vpImage class member functions.
Definition: vpImage.h:73
unsigned int getWidth() const
Definition: vpImage.h:246
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
unsigned int getHeight() const
Definition: vpImage.h:188
static double sqr(double x)
Definition: vpMath.h:123
static T clamp(const T &v, const T &lower, const T &upper)
Definition: vpMath.h:138
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)