Visual Servoing Platform  version 3.6.1 under development (2023-12-06)
vpColorDepthConversion.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2023 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 
49 namespace
50 {
51 
60 vpImagePoint adjust2DPointToBoundary(const vpImagePoint &ip, double width, double height)
61 {
62  return { vpMath::clamp(ip.get_i(), 0., height), vpMath::clamp(ip.get_j(), 0., width) };
63 }
64 
72 vpColVector transform(const vpHomogeneousMatrix &extrinsics_params, vpColVector from_point)
73 {
74  from_point = { from_point, 0, 3 };
75  from_point.stack(1.);
76  return { extrinsics_params * from_point, 0, 3 };
77 }
78 
86 vpImagePoint project(const vpCameraParameters &intrinsic_cam_params, const vpColVector &point)
87 {
88  vpImagePoint iP {};
89  vpMeterPixelConversion::convertPoint(intrinsic_cam_params, point[0] / point[2], point[1] / point[2], iP);
90 
91  return iP;
92 }
93 
102 vpColVector deproject(const vpCameraParameters &intrinsic_cam_params, const vpImagePoint &pixel, double depth)
103 {
104  double x { 0. }, y { 0. };
105  vpPixelMeterConversion::convertPoint(intrinsic_cam_params, pixel, x, y);
106  return { x * depth, y * depth, depth };
107 }
108 
109 } // namespace
110 
127  const vpImage<uint16_t> &I_depth, double depth_scale, double depth_min, double depth_max,
128  const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics,
129  const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
130 {
131  return projectColorToDepth(I_depth.bitmap, depth_scale, depth_min, depth_max, I_depth.getWidth(), I_depth.getHeight(),
132  depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
133 }
134 
153  const uint16_t *data, double depth_scale, double depth_min, double depth_max, double depth_width,
154  double depth_height, const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics,
155  const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
156 {
157  vpImagePoint depth_pixel {};
158 
159  // Find line start pixel
160  const auto min_point = deproject(color_intrinsics, from_pixel, depth_min);
161  const auto min_transformed_point = transform(depth_M_color, min_point);
162  auto start_pixel = project(depth_intrinsics, min_transformed_point);
163  start_pixel = adjust2DPointToBoundary(start_pixel, depth_width, depth_height);
164 
165  // Find line end depth pixel
166  const auto max_point = deproject(color_intrinsics, from_pixel, depth_max);
167  const auto max_transformed_point = transform(depth_M_color, max_point);
168  auto end_pixel = project(depth_intrinsics, max_transformed_point);
169  end_pixel = adjust2DPointToBoundary(end_pixel, depth_width, depth_height);
170 
171  // search along line for the depth pixel that it's projected pixel is the closest to the input pixel
172  auto min_dist = -1.;
173  for (auto curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && curr_pixel != end_pixel;
174  curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) {
175  const auto depth = depth_scale * data[static_cast<int>(curr_pixel.get_v() * depth_width + curr_pixel.get_u())];
176  if (std::fabs(depth) <= std::numeric_limits<double>::epsilon())
177  continue;
178 
179  const auto point = deproject(depth_intrinsics, curr_pixel, depth);
180  const auto transformed_point = transform(color_M_depth, point);
181  const auto projected_pixel = project(color_intrinsics, transformed_point);
182 
183  const auto new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) +
184  vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u());
185  if (new_dist < min_dist || min_dist < 0) {
186  min_dist = new_dist;
187  depth_pixel = curr_pixel;
188  }
189  }
190 
191  return depth_pixel;
192 }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
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
double get_u() const
Definition: vpImagePoint.h:136
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:69
unsigned int getWidth() const
Definition: vpImage.h:240
Type * bitmap
points toward the bitmap
Definition: vpImage.h:139
unsigned int getHeight() const
Definition: vpImage.h:182
static double sqr(double x)
Definition: vpMath.h:201
static T clamp(const T &v, const T &lower, const T &upper)
Definition: vpMath.h:216
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)