39 #include <visp3/core/vpColorDepthConversion.h>
45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpMeterPixelConversion.h>
47 #include <visp3/core/vpPixelMeterConversion.h>
74 from_point = { from_point, 0, 3 };
76 return { extrinsics_params * from_point, 0, 3 };
104 double x { 0. }, y { 0. };
106 return { x * depth, y * depth, depth };
127 const vpImage<uint16_t> &I_depth,
double depth_scale,
double depth_min,
double depth_max,
132 depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
153 const uint16_t *data,
double depth_scale,
double depth_min,
double depth_max,
double depth_width,
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);
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);
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())
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);
183 const auto new_dist =
vpMath::sqr(projected_pixel.get_v() - from_pixel.
get_v()) +
185 if (new_dist < min_dist || min_dist < 0) {
187 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 ...
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)