Visual Servoing Platform  version 3.6.1 under development (2024-07-18)
vpColorDepthConversion.cpp
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  * Color to depth conversion.
32  */
33 
39 #include <visp3/core/vpColorDepthConversion.h>
40 
41 // System
42 #include <algorithm>
43 
44 // Core
45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPixelMeterConversion.h>
48 
50 
51 #ifndef DOXYGEN_SHOULD_SKIP_THIS
52 namespace
53 {
54 
63 vpImagePoint adjust2DPointToBoundary(const vpImagePoint &ip, double width, double height)
64 {
65 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
66  return { vpMath::clamp(ip.get_i(), 0., height), vpMath::clamp(ip.get_j(), 0., width) };
67 #else
68  return vpImagePoint(vpMath::clamp(ip.get_i(), 0., height), vpMath::clamp(ip.get_j(), 0., width));
69 #endif
70 }
71 
79 vpColVector transform(const vpHomogeneousMatrix &extrinsics_params, vpColVector from_point)
80 {
81 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
82  from_point = { from_point, 0, 3 };
83  from_point.stack(1.);
84  return { extrinsics_params * from_point, 0, 3 };
85 #else
86  from_point = vpColVector(from_point, 0, 3);
87  from_point.stack(1.);
88  return vpColVector(extrinsics_params * from_point, 0, 3);
89 #endif
90 }
91 
99 vpImagePoint project(const vpCameraParameters &intrinsic_cam_params, const vpColVector &point)
100 {
101 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
102  vpImagePoint iP {};
103 #else
104  vpImagePoint iP;
105 #endif
106  vpMeterPixelConversion::convertPoint(intrinsic_cam_params, point[0] / point[2], point[1] / point[2], iP);
107 
108  return iP;
109 }
110 
119 vpColVector deproject(const vpCameraParameters &intrinsic_cam_params, const vpImagePoint &pixel, double depth)
120 {
121 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
122  double x { 0. }, y { 0. };
123  vpPixelMeterConversion::convertPoint(intrinsic_cam_params, pixel, x, y);
124  return { x * depth, y * depth, depth };
125 #else
126  double x = 0., y = 0.;
127  vpPixelMeterConversion::convertPoint(intrinsic_cam_params, pixel, x, y);
128 
129  vpColVector p(3);
130  p[0] = x * depth;
131  p[1] = y * depth;
132  p[2] = depth;
133  return p;
134 #endif
135 }
136 
137 } // namespace
138 
139 #endif // DOXYGEN_SHOULD_SKIP_THIS
140 
157  const vpImage<uint16_t> &I_depth, double depth_scale, double depth_min, double depth_max,
158  const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics,
159  const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
160 {
161  return projectColorToDepth(I_depth.bitmap, depth_scale, depth_min, depth_max, I_depth.getWidth(), I_depth.getHeight(),
162  depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
163 }
164 
183  const uint16_t *data, double depth_scale, double depth_min, double depth_max, double depth_width,
184  double depth_height, const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics,
185  const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
186 {
187 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
188  vpImagePoint depth_pixel {};
189 
190  // Find line start pixel
191  const auto min_point = deproject(color_intrinsics, from_pixel, depth_min);
192  const auto min_transformed_point = transform(depth_M_color, min_point);
193  auto start_pixel = project(depth_intrinsics, min_transformed_point);
194  start_pixel = adjust2DPointToBoundary(start_pixel, depth_width, depth_height);
195 
196  // Find line end depth pixel
197  const auto max_point = deproject(color_intrinsics, from_pixel, depth_max);
198  const auto max_transformed_point = transform(depth_M_color, max_point);
199  auto end_pixel = project(depth_intrinsics, max_transformed_point);
200  end_pixel = adjust2DPointToBoundary(end_pixel, depth_width, depth_height);
201 
202  // search along line for the depth pixel that it's projected pixel is the closest to the input pixel
203  auto min_dist = -1.;
204  for (auto curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && (curr_pixel != end_pixel);
205  curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) {
206  const auto depth = depth_scale * data[static_cast<int>((curr_pixel.get_v() * depth_width) + curr_pixel.get_u())];
207  bool stop_for_loop = false;
208  if (std::fabs(depth) <= std::numeric_limits<double>::epsilon()) {
209  stop_for_loop = true;
210  }
211  if (!stop_for_loop) {
212  const auto point = deproject(depth_intrinsics, curr_pixel, depth);
213  const auto transformed_point = transform(color_M_depth, point);
214  const auto projected_pixel = project(color_intrinsics, transformed_point);
215 
216  const auto new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) +
217  vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u());
218  if ((new_dist < min_dist) || (min_dist < 0)) {
219  min_dist = new_dist;
220  depth_pixel = curr_pixel;
221  }
222  }
223  }
224 
225 #else
226  vpImagePoint depth_pixel;
227 
228  // Find line start pixel
229  const vpColVector min_point = deproject(color_intrinsics, from_pixel, depth_min);
230  const vpColVector min_transformed_point = transform(depth_M_color, min_point);
231  vpImagePoint start_pixel = project(depth_intrinsics, min_transformed_point);
232  start_pixel = adjust2DPointToBoundary(start_pixel, depth_width, depth_height);
233 
234  // Find line end depth pixel
235  const vpColVector max_point = deproject(color_intrinsics, from_pixel, depth_max);
236  const vpColVector max_transformed_point = transform(depth_M_color, max_point);
237  vpImagePoint end_pixel = project(depth_intrinsics, max_transformed_point);
238  end_pixel = adjust2DPointToBoundary(end_pixel, depth_width, depth_height);
239 
240  // search along line for the depth pixel that it's projected pixel is the closest to the input pixel
241  double min_dist = -1.;
242  for (vpImagePoint curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && curr_pixel != end_pixel;
243  curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) {
244  const double depth = depth_scale * data[static_cast<int>(curr_pixel.get_v() * depth_width + curr_pixel.get_u())];
245 
246  bool stop_for_loop = false;
247  if (std::fabs(depth) <= std::numeric_limits<double>::epsilon()) {
248  stop_for_loop = true;
249  }
250  if (!stop_for_loop) {
251  const vpColVector point = deproject(depth_intrinsics, curr_pixel, depth);
252  const vpColVector transformed_point = transform(color_M_depth, point);
253  const vpImagePoint projected_pixel = project(color_intrinsics, transformed_point);
254 
255  const double new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) +
256  vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u());
257  if (new_dist < min_dist || min_dist < 0) {
258  min_dist = new_dist;
259  depth_pixel = curr_pixel;
260  }
261  }
262  }
263 #endif
264  return depth_pixel;
265 }
266 END_VISP_NAMESPACE
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
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:82
double get_j() const
Definition: vpImagePoint.h:125
vpImagePoint nextInSegment(const vpImagePoint &start, const vpImagePoint &end) const
Definition: vpImagePoint.h:220
double get_u() const
Definition: vpImagePoint.h:136
bool inSegment(const vpImagePoint &start, const vpImagePoint &end) const
Definition: vpImagePoint.h:162
double get_i() const
Definition: vpImagePoint.h:114
double get_v() const
Definition: vpImagePoint.h:147
Definition of the vpImage class member functions.
Definition: vpImage.h:131
unsigned int getWidth() const
Definition: vpImage.h:242
Type * bitmap
points toward the bitmap
Definition: vpImage.h:135
unsigned int getHeight() const
Definition: vpImage.h:181
static double sqr(double x)
Definition: vpMath.h:203
static T clamp(const T &v, const T &lower, const T &upper)
Definition: vpMath.h:218
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)