44 #include <visp3/core/vpColorDepthConversion.h>
50 #include <visp3/core/vpMath.h>
51 #include <visp3/core/vpMeterPixelConversion.h>
52 #include <visp3/core/vpPixelMeterConversion.h>
67 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
83 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
84 from_point = {from_point, 0, 3};
86 return {extrinsics_params * from_point, 0, 3};
90 return vpColVector(extrinsics_params * from_point, 0, 3);
103 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
123 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
126 return {x * depth, y * depth, depth};
128 double x = 0., y = 0.;
154 const vpImage<uint16_t> &I_depth,
double depth_scale,
double depth_min,
double depth_max,
159 depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
177 const uint16_t *data,
double depth_scale,
double depth_min,
double depth_max,
double depth_width,
181 #if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
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);
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);
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())
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);
208 const auto new_dist =
vpMath::sqr(projected_pixel.get_v() - from_pixel.
get_v()) +
210 if (new_dist < min_dist || min_dist < 0) {
212 depth_pixel = curr_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);
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);
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())
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);
245 if (new_dist < min_dist || min_dist < 0) {
247 depth_pixel = curr_pixel;
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
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 ...
vpImagePoint nextInSegment(const vpImagePoint &start, const vpImagePoint &end) const
bool inSegment(const vpImagePoint &start, const vpImagePoint &end) const
Definition of the vpImage class member functions.
unsigned int getWidth() const
Type * bitmap
points toward the bitmap
unsigned int getHeight() const
static double sqr(double x)
static T clamp(const T &v, const T &lower, const T &upper)
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)