vpColorDepthConversion.cpp

/*
 * ViSP, open source Visual Servoing Platform software.
 * Copyright (C) 2005 - 2024 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 https://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.
 */

 
#include <visp3/core/vpColorDepthConversion.h>
 
// System
#include <algorithm>
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
#include <optional>
#endif
 
// Core
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
 
BEGIN_VISP_NAMESPACE
 
#ifndef DOXYGEN_SHOULD_SKIP_THIS
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
}
 
#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
double getDepth(const float *data, const std::optional<double> depth_scale, const double &offset)
{
  (void)depth_scale;
  return static_cast<double>(data[static_cast<int>(offset)]);
}
 
double getDepth(const uint16_t *data, const std::optional<double> depth_scale, const double &offset)
{
  return *depth_scale * static_cast<double>(data[static_cast<int>(offset)]);
}

template<typename Type>
vpImagePoint project_color_to_depth(
  const Type *data, std::optional<double> depth_scale, const double &depth_min, const double &depth_max, const double &depth_width,
  const 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)
{
  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 = getDepth(data, depth_scale, (curr_pixel.get_v() * depth_width) + curr_pixel.get_u());
    bool stop_for_loop = false;
    if (std::fabs(depth) <= std::numeric_limits<double>::epsilon()) {
      stop_for_loop = true;
    }
    if (!stop_for_loop) {
      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;
      }
    }
  }
  return depth_pixel;
}
#else
double getDepth(const float *data, const double *depth_scale, const double &offset)
{
  (void)depth_scale;
  return static_cast<double>(data[static_cast<int>(offset)]);
}
 
double getDepth(const uint16_t *data, const double *depth_scale, const double &offset)
{
  return *depth_scale * static_cast<double>(data[static_cast<int>(offset)]);
}
 
template<typename Type>
vpImagePoint project_color_to_depth(
  const Type *data, const double *depth_scale, const double &depth_min, const double &depth_max, const double &depth_width,
  const 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)
{
  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 = getDepth(data, depth_scale, (curr_pixel.get_v() * depth_width) + curr_pixel.get_u());
 
    bool stop_for_loop = false;
    if (std::fabs(depth) <= std::numeric_limits<double>::epsilon()) {
      stop_for_loop = true;
    }
    if (!stop_for_loop) {
      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;
      }
    }
  }
  return depth_pixel;
}
#endif
} // namespace
 
#endif // DOXYGEN_SHOULD_SKIP_THIS

vpImagePoint vpColorDepthConversion::projectColorToDepth(
  const vpImage<uint16_t> &I_depth, const double &depth_scale, const double &depth_min, const 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, const double &depth_scale, const double &depth_min, const double &depth_max, const double &depth_width,
  const 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_17)
  return project_color_to_depth(data, std::optional<double>(depth_scale), depth_min, depth_max, depth_width, depth_height,
                             depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
 
#else
  return project_color_to_depth(data, &depth_scale, depth_min, depth_max, depth_width, depth_height,
                             depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
#endif
}

vpImagePoint vpColorDepthConversion::projectColorToDepth(
  const vpImage<float> &I_depth, const double &depth_min, const 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_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 float *data, const double &depth_min, const double &depth_max, const double &depth_width,
  const 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_17)
  return project_color_to_depth(data, std::nullopt, depth_min, depth_max, depth_width, depth_height,
                             depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
 
#else
  return project_color_to_depth(data, nullptr, depth_min, depth_max, depth_width, depth_height,
                             depth_intrinsics, color_intrinsics, color_M_depth, depth_M_color, from_pixel);
#endif
}
END_VISP_NAMESPACE