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;
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)