vpColorDepthConversion.cpp

/****************************************************************************
 *
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2022 by Inria. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact Inria about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://visp.inria.fr for more information.
 *
 * This software was developed at:
 * Inria Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 *
 * If you have questions regarding the use of this file, please contact
 * Inria at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Color to depth conversion.
 *
 * Authors:
 * Julien Dufour
 *
 *****************************************************************************/
 
#include <visp3/core/vpColorDepthConversion.h>
 
// System
#include <algorithm>
 
// Core
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
 
namespace
{
 
vpImagePoint adjust2DPointToBoundary(const vpImagePoint &ip, double width, double height)
{
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
  return {vpMath::clamp(ip.get_i(), 0., height), vpMath::clamp(ip.get_j(), 0., width)};
#else
  return vpImagePoint(vpMath::clamp(ip.get_i(), 0., height), vpMath::clamp(ip.get_j(), 0., width));
#endif
}
 
vpColVector transform(const vpHomogeneousMatrix &extrinsics_params, vpColVector from_point)
{
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
  from_point = {from_point, 0, 3};
  from_point.stack(1.);
  return {extrinsics_params * from_point, 0, 3};
#else
  from_point = vpColVector(from_point, 0, 3);
  from_point.stack(1.);
  return vpColVector(extrinsics_params * from_point, 0, 3);
#endif
}
 
vpImagePoint project(const vpCameraParameters &intrinsic_cam_params, const vpColVector &point)
{
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
  vpImagePoint iP{};
#else
  vpImagePoint iP;
#endif
  vpMeterPixelConversion::convertPoint(intrinsic_cam_params, point[0] / point[2], point[1] / point[2], iP);
 
  return iP;
}
 
vpColVector deproject(const vpCameraParameters &intrinsic_cam_params, const vpImagePoint &pixel, double depth)
{
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
  double x{0.}, y{0.};
  vpPixelMeterConversion::convertPoint(intrinsic_cam_params, pixel, x, y);
  return {x * depth, y * depth, depth};
#else
  double x = 0., y = 0.;
  vpPixelMeterConversion::convertPoint(intrinsic_cam_params, pixel, x, y);
 
  vpColVector p(3);
  p[0] = x * depth;
  p[1] = y * depth;
  p[2] = depth;
  return p;
#endif
}
 
} // namespace
 
vpImagePoint vpColorDepthConversion::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)
{
  return projectColorToDepth(I_depth.bitmap, depth_scale, depth_min, depth_max, I_depth.getWidth(), I_depth.getHeight(),
                             depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
}
 
vpImagePoint vpColorDepthConversion::projectColorToDepth(
    const uint16_t *data, double depth_scale, double depth_min, double depth_max, double depth_width,
    double depth_height, const vpCameraParameters &depth_intrinsics, const vpCameraParameters &color_intrinsics,
    const vpHomogeneousMatrix &color_M_depth, const vpHomogeneousMatrix &depth_M_color, const vpImagePoint &from_pixel)
{
#if (VISP_CXX_STANDARD > VISP_CXX_STANDARD_98)
  vpImagePoint depth_pixel{};
 
  // Find line start pixel
  const auto min_point = deproject(color_intrinsics, from_pixel, depth_min);
  const auto min_transformed_point = transform(depth_M_color, min_point);
  auto start_pixel = project(depth_intrinsics, min_transformed_point);
  start_pixel = adjust2DPointToBoundary(start_pixel, depth_width, depth_height);
 
  // Find line end depth pixel
  const auto max_point = deproject(color_intrinsics, from_pixel, depth_max);
  const auto max_transformed_point = transform(depth_M_color, max_point);
  auto end_pixel = project(depth_intrinsics, max_transformed_point);
  end_pixel = adjust2DPointToBoundary(end_pixel, depth_width, depth_height);
 
  // search along line for the depth pixel that it's projected pixel is the closest to the input pixel
  auto min_dist = -1.;
  for (auto curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && curr_pixel != end_pixel;
       curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) {
    const auto depth = depth_scale * data[static_cast<int>(curr_pixel.get_v() * depth_width + curr_pixel.get_u())];
    if (std::fabs(depth) <= std::numeric_limits<double>::epsilon())
      continue;
 
    const auto point = deproject(depth_intrinsics, curr_pixel, depth);
    const auto transformed_point = transform(color_M_depth, point);
    const auto projected_pixel = project(color_intrinsics, transformed_point);
 
    const auto new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) +
                          vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u());
    if (new_dist < min_dist || min_dist < 0) {
      min_dist = new_dist;
      depth_pixel = curr_pixel;
    }
  }
 
#else
  vpImagePoint depth_pixel;
 
  // Find line start pixel
  const vpColVector min_point = deproject(color_intrinsics, from_pixel, depth_min);
  const vpColVector min_transformed_point = transform(depth_M_color, min_point);
  vpImagePoint start_pixel = project(depth_intrinsics, min_transformed_point);
  start_pixel = adjust2DPointToBoundary(start_pixel, depth_width, depth_height);
 
  // Find line end depth pixel
  const vpColVector max_point = deproject(color_intrinsics, from_pixel, depth_max);
  const vpColVector max_transformed_point = transform(depth_M_color, max_point);
  vpImagePoint end_pixel = project(depth_intrinsics, max_transformed_point);
  end_pixel = adjust2DPointToBoundary(end_pixel, depth_width, depth_height);
 
  // search along line for the depth pixel that it's projected pixel is the closest to the input pixel
  double min_dist = -1.;
  for (vpImagePoint curr_pixel = start_pixel; curr_pixel.inSegment(start_pixel, end_pixel) && curr_pixel != end_pixel;
       curr_pixel = curr_pixel.nextInSegment(start_pixel, end_pixel)) {
    const double depth = depth_scale * data[static_cast<int>(curr_pixel.get_v() * depth_width + curr_pixel.get_u())];
    if (std::fabs(depth) <= std::numeric_limits<double>::epsilon())
      continue;
 
    const vpColVector point = deproject(depth_intrinsics, curr_pixel, depth);
    const vpColVector transformed_point = transform(color_M_depth, point);
    const vpImagePoint projected_pixel = project(color_intrinsics, transformed_point);
 
    const double new_dist = vpMath::sqr(projected_pixel.get_v() - from_pixel.get_v()) +
                            vpMath::sqr(projected_pixel.get_u() - from_pixel.get_u());
    if (new_dist < min_dist || min_dist < 0) {
      min_dist = new_dist;
      depth_pixel = curr_pixel;
    }
  }
#endif
 
  return depth_pixel;
}