Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpColorDepthConversion.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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
54namespace
55{
56
65vpImagePoint 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
81vpColVector 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
101vpImagePoint 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
121vpColVector 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.
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 ...
double get_j() const
vpImagePoint nextInSegment(const vpImagePoint &start, const vpImagePoint &end) const
double get_u() const
bool inSegment(const vpImagePoint &start, const vpImagePoint &end) const
double get_i() const
double get_v() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
Type * bitmap
points toward the bitmap
Definition vpImage.h:139
unsigned int getHeight() const
Definition vpImage.h:184
static double sqr(double x)
Definition vpMath.h:124
static T clamp(const T &v, const T &lower, const T &upper)
Definition vpMath.h:139
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)