/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          https://www.mrpt.org/                            |
   |                                                                           |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file        |
   | See: https://www.mrpt.org/Authors - All rights reserved.                  |
   | Released under BSD License. See details in https://www.mrpt.org/License   |
   +---------------------------------------------------------------------------+
   */

#include "vision-precomp.h"  // Precompiled headers
//
#include <mrpt/io/CFileInputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/io/CTextFileLinesParser.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/serialization/optional_serialization.h>
#include <mrpt/serialization/stl_serialization.h>
#include <mrpt/system/os.h>
#include <mrpt/vision/CFeature.h>
#include <mrpt/vision/types.h>
#include <mrpt/vision/utils.h>

#include <iostream>

using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::io;
using namespace std;

IMPLEMENTS_SERIALIZABLE(CFeature, CSerializable, mrpt::vision)

// --------------------------------------------------
//			loadFromConfigFile
// --------------------------------------------------
/**  Load all the params from a config source, in the format described in
 * saveToConfigFile()
 */
void TMultiResDescMatchOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& cfg, const std::string& section)
{
  useOriFilter = cfg.read_bool(section, "useOriFilter", true, false);
  oriThreshold = cfg.read_double(section, "oriThreshold", 0.2, false);
  lastSeenThreshold = cfg.read_int(section, "lastSeenThreshold", 10, false);
  timesSeenThreshold = cfg.read_int(section, "timesSeenThreshold", 5, false);
  minFeaturesToFind = cfg.read_int(section, "minFeaturesToFind", 5, false);
  minFeaturesToBeLost = cfg.read_int(section, "minFeaturesToBeLost", 5, false);

  useDepthFilter = cfg.read_bool(section, "useDepthFilter", true, false);

  matchingThreshold = cfg.read_double(section, "matchingThreshold", 1e4, false);
  matchingRatioThreshold = cfg.read_double(section, "matchingRatioThreshold", 0.5, false);

  lowScl1 = cfg.read_int(section, "lowScl1", 0, false);
  lowScl2 = cfg.read_int(section, "lowScl1", 0, false);
  highScl1 = cfg.read_int(section, "highScl1", 6, false);
  highScl2 = cfg.read_int(section, "highScl2", 6, false);

  searchAreaSize = cfg.read_uint64_t(section, "searchAreaSize", 20, false);
}

// --------------------------------------------------
//			saveToConfigFile
// --------------------------------------------------
void TMultiResDescMatchOptions::saveToConfigFile(
    mrpt::config::CConfigFileBase& cfg, const std::string& section) const
{
  if (useOriFilter)
  {
    cfg.write(section, "useOriFilter", "true");
    cfg.write(section, "oriThreshold", oriThreshold);
  }
  else
    cfg.write(section, "useOriFilter", "false");

  if (useDepthFilter)
    cfg.write(section, "useDepthFilter", "true");
  else
    cfg.write(section, "useDepthFilter", "false");

  cfg.write(section, "matchingThreshold", matchingThreshold);
  cfg.write(section, "matchingRatioThreshold", matchingRatioThreshold);
  cfg.write(section, "lowScl1", lowScl1);
  cfg.write(section, "lowScl2", lowScl2);
  cfg.write(section, "highScl1", highScl1);
  cfg.write(section, "highScl2", highScl2);

  cfg.write(section, "searchAreaSize", searchAreaSize);
  cfg.write(section, "lastSeenThreshold", lastSeenThreshold);
  cfg.write(section, "timesSeenThreshold", timesSeenThreshold);
  cfg.write(section, "minFeaturesToFind", minFeaturesToFind);
  cfg.write(section, "minFeaturesToBeLost", minFeaturesToBeLost);
}  // end-saveToConfigFile

// --------------------------------------------------
//			dumpToTextStream
// --------------------------------------------------
void TMultiResDescMatchOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [vision::TMultiResDescMatchOptions] ------------ \n";
  out << "Use orientation filter?:        ";
  if (useOriFilter)
  {
    out << "Yes\n";
    out << mrpt::format("· Orientation threshold:        %.1f deg\n", RAD2DEG(oriThreshold));
  }
  else
    out << "No\n";
  out << "Use depth filter?:              ";
  if (useDepthFilter)
    out << "Yes\n";
  else
  {
    out << "No\n";
    out << mrpt::format("Lowest scale in list1:          %d\n", lowScl1);
    out << mrpt::format("Highest scale in list1:         %d\n", highScl1);
    out << mrpt::format("Lowest scale in list2:          %d\n", lowScl2);
    out << mrpt::format("Highest scale in list2:         %d\n", highScl2);
  }
  out << mrpt::format("#frames last seen threshold:    %d\n", lastSeenThreshold);
  out << mrpt::format("#frames to be stable threshold: %d\n", timesSeenThreshold);
  out << mrpt::format("min. # features in system:      %d\n", minFeaturesToFind);
  out << mrpt::format("min. # features to be lost:     %d\n", minFeaturesToBeLost);
  out << mrpt::format("Matching threshold:             %.2f\n", matchingThreshold);
  out << mrpt::format("Matching ratio threshold:       %.2f\n", matchingRatioThreshold);
  out << mrpt::format("Size of the search window:      %d px\n", searchAreaSize);
  out << "-------------------------------------------------------- \n";
}  // end-dumpToTextStream

// --------------------------------------------------
//			loadFromConfigFile
// --------------------------------------------------
/**  Load all the params from a config source, in the format described in
 * saveToConfigFile()
 */
void TMultiResDescOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& cfg, const std::string& section)
{
  basePSize = cfg.read_uint64_t(section, "basePSize", 23, false);
  comLScl = cfg.read_int(section, "comLScl", 0, false);
  comHScl = cfg.read_int(section, "comHScl", 6, false);
  sg1 = cfg.read_double(section, "sg1", 0.5, false);
  sg2 = cfg.read_double(section, "sg2", 7.5, false);
  sg3 = cfg.read_double(section, "sg3", 8.0, false);
  computeDepth = cfg.read_bool(section, "computeDepth", true, false);
  blurImage = cfg.read_bool(section, "blurImage", true, false);
  fx = cfg.read_double(section, "fx", 0.0, false);
  cx = cfg.read_double(section, "cx", 0.0, false);
  cy = cfg.read_double(section, "cy", 0.0, false);
  baseline = cfg.read_double(section, "baseline", 0.0, false);
  computeHashCoeffs = cfg.read_bool(section, "computeHashCoeffs", false, false);

  cfg.read_vector(section, "scales", vector<double>(), scales, false);
  if (scales.size() < 1)
  {
    scales.resize(7);
    scales[0] = 0.5;
    scales[1] = 0.8;
    scales[2] = 1.0;
    scales[3] = 1.2;
    scales[4] = 1.5;
    scales[5] = 1.8;
    scales[6] = 2.0;
  }  // end-if
}

// --------------------------------------------------
//			saveToConfigFile
// --------------------------------------------------
void TMultiResDescOptions::saveToConfigFile(
    mrpt::config::CConfigFileBase& cfg, const std::string& section) const
{
  cfg.write(section, "basePSize", basePSize);
  cfg.write(section, "comLScl", comLScl);
  cfg.write(section, "comHScl", comHScl);
  cfg.write(section, "sg1", sg1);
  cfg.write(section, "sg2", sg2);
  cfg.write(section, "sg3", sg3);

  cfg.write(section, "computeDepth", computeDepth ? "true" : "false");
  cfg.write(section, "blurImage", blurImage ? "true" : "false");
  cfg.write(section, "fx", fx);
  cfg.write(section, "cx", cx);
  cfg.write(section, "cy", cy);
  cfg.write(section, "baseline", baseline);
  cfg.write(section, "computeHashCoeffs", computeHashCoeffs ? "true" : "false");

  char buf[300];
  for (double scale : scales) mrpt::system::os::sprintf(buf, 300, "%.2f ", scale);
  cfg.write(section, "scales", buf);
}  // end-saveToConfigFile

// --------------------------------------------------
//			dumpToTextStream
// --------------------------------------------------
void TMultiResDescOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [vision::TMultiResDescOptions] ------------ \n";
  out << mrpt::format("Base patch size:                %d px\n", basePSize);
  out << mrpt::format("Lowest scale to compute:        %d\n", comLScl);
  out << mrpt::format("Highest scale to compute:       %d\n", comHScl);
  out << mrpt::format("Image smoothing sigma:          %.2f px\n", sg1);
  out << mrpt::format("Orientation histogram sigma:    %.2f\n", sg2);
  out << mrpt::format("Descriptor histogram sigma:     %.2f\n", sg3);
  out << "Compute depth:                  ";
  if (computeDepth)
  {
    out << "Yes\n";
    out << mrpt::format("Focal length:                   %.2f px\n", fx);
    out << mrpt::format("Principal point (cx):           %.2f px\n", cx);
    out << mrpt::format("Principal point (cy):           %.2f px\n", cy);
    out << mrpt::format("Baseline:                       %.2f m\n", baseline);
  }
  else
    out << "No\n";

  out << "Compute Hash Coeffs:            ";
  if (computeHashCoeffs)
    out << "Yes\n";
  else
    out << "No\n";

  out << "Blur image previously:          ";
  if (blurImage)
    out << "Yes\n";
  else
    out << "No\n";

  out << "Scales:                         ";
  for (double scale : scales) out << mrpt::format("%.2f ", scale);
  out << "\n";
  out << "-------------------------------------------------------- \n";
}  // end-dumpToTextStream

void CFeature::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [vision::CFeature] ------------ \n";
  out << mrpt::format("Feature ID:                     %d\n", (int)keypoint.ID);
  out << mrpt::format(
      "Coordinates:                    (%.2f,%.2f) px\n", keypoint.pt.x, keypoint.pt.y);
  out << mrpt::format("PatchSize:                      %d\n", patchSize);
  out << "Type:                           ";
  out << mrpt::typemeta::TEnumType<decltype(type)>::value2name(type) << "\n";
  out << "Status:                         ";
  switch (track_status)
  {
    case 0:
      out << "Idle\n";
      break;
    case 1:
      out << "[KLT] Out of bounds [KLT]\n";
      break;
    case 5:
      out << "[KLT] Tracked\n";
      break;
    case 10:
      out << "[KLT] Lost\n";
      break;
  }

  out << mrpt::format("Response:                       %.2f\n", response);
  out << mrpt::format("Main orientation:               %.2f\n", orientation);
  out << mrpt::format("Initial Depth:                  %.2f m\n", initialDepth);
  out << mrpt::format("Depth:                          %.2f m\n", depth);
  out << mrpt::format("3D point:                       (%.2f,%.2f,%.2f) m\n", p3D.x, p3D.y, p3D.z);
  out << "Is point feature?:              ";
  isPointFeature() ? out << "Yes\n" : out << "No\n";

  out << "Has SIFT descriptor?:           ";
  descriptors.hasDescriptorSIFT() ? out << "Yes\n" : out << "No\n";
  out << "Has SURF descriptor?:           ";
  descriptors.hasDescriptorSURF() ? out << "Yes\n" : out << "No\n";
  out << "Has Spin image descriptor?:     ";
  descriptors.hasDescriptorSpinImg() ? out << "Yes\n" : out << "No\n";
  out << "Has Polar descriptor?:          ";
  descriptors.hasDescriptorPolarImg() ? out << "Yes\n" : out << "No\n";
  out << "Has Log Polar descriptor?:      ";
  descriptors.hasDescriptorLogPolarImg() ? out << "Yes\n" : out << "No\n";
  out << "Has ORB descriptor?:			";
  descriptors.hasDescriptorORB() ? out << "Yes\n" : out << "No\n";
  //# added by Raghavender Sahdev
  out << "Has BLD descriptor?:			";
  descriptors.hasDescriptorBLD() ? out << "Yes\n" : out << "No\n";
  out << "Has LATCH descriptor?:			";
  descriptors.hasDescriptorLATCH() ? out << "Yes\n" : out << "No\n";

}  // end dumpToTextStream

void CFeature::dumpToConsole() const { dumpToTextStream(std::cout); }
uint8_t CFeature::serializeGetVersion() const { return 2; }
void CFeature::serializeTo(mrpt::serialization::CArchive& out) const
{
  // The coordinates:
  out << keypoint.pt.x << keypoint.pt.y << keypoint.ID << patch << patchSize << (uint32_t)type
      << (uint32_t)keypoint.track_status << keypoint.response << orientation << keypoint.octave
      << user_flags << depth << initialDepth << p3D << descriptors.SIFT << descriptors.SURF
      << descriptors.SpinImg << descriptors.SpinImg_range_rows << descriptors.PolarImg
      << descriptors.LogPolarImg << descriptors.polarImgsNoRotation << descriptors.ORB
      << descriptors.BLD << descriptors.LATCH;
}

void CFeature::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    case 1:
    case 2:
    {
      // The coordinates:
      uint32_t aux_type, aux_KLTS;
      in >> keypoint.pt.x >> keypoint.pt.y >> keypoint.ID >> patch >> patchSize >> aux_type >>
          aux_KLTS >> keypoint.response >> orientation >> keypoint.octave >> user_flags;
      if (version > 0)
      {
        in >> depth >> initialDepth >> p3D;
      }
      in >> descriptors.SIFT >> descriptors.SURF >> descriptors.SpinImg >>
          descriptors.SpinImg_range_rows >> descriptors.PolarImg >> descriptors.LogPolarImg >>
          descriptors.polarImgsNoRotation >> descriptors.BLD >> descriptors.LATCH;
      if (version > 1) in >> descriptors.ORB;

      type = (TKeyPointMethod)aux_type;
      track_status = (TFeatureTrackStatus)aux_KLTS;
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/****************************************************
        Class CFEATURE
*****************************************************/

// Return false only for Blob detectors (SIFT, SURF)
bool CFeature::isPointFeature() const { return type == featSIFT || type == featSURF; }

// --------------------------------------------------
//			patchCorrelationTo
// --------------------------------------------------
float CFeature::patchCorrelationTo(const CFeature& oFeature) const
{
  MRPT_START
  ASSERT_(patch);

  ASSERT_(patch->getWidth() == oFeature.patch->getWidth());
  ASSERT_(patch->getHeight() == oFeature.patch->getHeight());
  ASSERT_(patch->getHeight() > 0 && patch->getWidth() > 0);
  size_t x_max, y_max;
  double max_val;
  mrpt::vision::openCV_cross_correlation(*patch, *oFeature.patch, x_max, y_max, max_val);

  // Value as "distance" in the range [0,1], best = 0
  return d2f(0.5 - 0.5 * max_val);

  MRPT_END
}

// --------------------------------------------------
//			descriptorDistanceTo
// --------------------------------------------------
float CFeature::descriptorDistanceTo(
    const CFeature& oFeature, TDescriptorType descriptorToUse, bool normalize_distances) const
{
  MRPT_START

  // If we are not ask for a specific descriptor, select the first one found:
  if (descriptorToUse == descAny)
  {
    if (descriptors.hasDescriptorSIFT())
      descriptorToUse = descSIFT;
    else if (descriptors.hasDescriptorSURF())
      descriptorToUse = descSURF;
    else if (descriptors.hasDescriptorSpinImg())
      descriptorToUse = descSpinImages;
    else if (descriptors.hasDescriptorPolarImg())
      descriptorToUse = descPolarImages;
    else if (descriptors.hasDescriptorLogPolarImg())
      descriptorToUse = descLogPolarImages;
    else if (descriptors.hasDescriptorORB())
      descriptorToUse = descORB;
    // # added by Raghavender Sahdev - BLD/LATCH descriptors
    else if (descriptors.hasDescriptorBLD())
      descriptorToUse = descBLD;
    else if (descriptors.hasDescriptorLATCH())
      descriptorToUse = descLATCH;
    else
      THROW_EXCEPTION("Feature has no descriptors and descriptorToUse=descAny");
  }

  switch (descriptorToUse)
  {
    case descSIFT:
      return descriptorSIFTDistanceTo(oFeature, normalize_distances);
    case descSURF:
      return descriptorSURFDistanceTo(oFeature, normalize_distances);
    case descSpinImages:
      return descriptorSpinImgDistanceTo(oFeature, normalize_distances);
    case descPolarImages:
    {
      float minAng;
      return descriptorPolarImgDistanceTo(oFeature, minAng, normalize_distances);
    }
    case descLogPolarImages:
    {
      float minAng;
      return descriptorLogPolarImgDistanceTo(oFeature, minAng, normalize_distances);
    }
    case descORB:
      return float(descriptorORBDistanceTo(oFeature));
    // # added by Raghavender Sahdev
    case descBLD:
      return (descriptorBLDDistanceTo(oFeature));
    case descLATCH:
      return (descriptorLATCHDistanceTo(oFeature));
    default:
      THROW_EXCEPTION_FMT("Unknown value for 'descriptorToUse'=%u", (unsigned)descriptorToUse);
  }

  MRPT_END
}

// --------------------------------------------------
// descriptorSIFTDistanceTo
// --------------------------------------------------
float CFeature::descriptorSIFTDistanceTo(const CFeature& oFeature, bool normalize_distances) const
{
  ASSERT_(descriptors.SIFT);
  ASSERT_(oFeature.descriptors.SIFT);
  ASSERT_(descriptors.SIFT->size() == oFeature.descriptors.SIFT->size());
  ASSERT_(descriptors.hasDescriptorSIFT() && oFeature.descriptors.hasDescriptorSIFT());

  float dist = 0.0f;
  std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
  for (itDesc1 = descriptors.SIFT->begin(), itDesc2 = oFeature.descriptors.SIFT->begin();
       itDesc1 != descriptors.SIFT->end(); itDesc1++, itDesc2++)
  {
    dist += square(*itDesc1 - *itDesc2);
  }
  if (normalize_distances) dist /= descriptors.SIFT->size();
  dist = sqrt(dist);
  if (normalize_distances) dist /= 64.0f;
  return dist;
}  // end descriptorSIFTDistanceTo

// --------------------------------------------------
// descriptorSURFDistanceTo
// --------------------------------------------------
float CFeature::descriptorSURFDistanceTo(const CFeature& oFeature, bool normalize_distances) const
{
  ASSERT_(descriptors.SURF);
  ASSERT_(descriptors.SURF->size() == oFeature.descriptors.SURF->size());
  ASSERT_(descriptors.hasDescriptorSURF() && oFeature.descriptors.hasDescriptorSURF());

  float dist = 0.0f;
  std::vector<float>::const_iterator itDesc1, itDesc2;
  for (itDesc1 = descriptors.SURF->begin(), itDesc2 = oFeature.descriptors.SURF->begin();
       itDesc1 != descriptors.SURF->end(); itDesc1++, itDesc2++)
  {
    dist += square(*itDesc1 - *itDesc2);
  }
  if (normalize_distances) dist /= descriptors.SURF->size();
  dist = sqrt(dist);
  if (normalize_distances) dist /= 0.20f;  // JL: Ad-hoc value! Investigate where does this come
  // from...
  return dist;
}  // end descriptorSURFDistanceTo

// --------------------------------------------------
// descriptorSpinImgDistanceTo
// --------------------------------------------------
float CFeature::descriptorSpinImgDistanceTo(
    const CFeature& oFeature, bool normalize_by_vector_length) const
{
  ASSERT_(descriptors.SpinImg);

  ASSERT_(descriptors.SpinImg->size() == oFeature.descriptors.SpinImg->size());
  ASSERT_(descriptors.hasDescriptorSpinImg() && oFeature.descriptors.hasDescriptorSpinImg());
  ASSERT_(!descriptors.SpinImg->empty());
  float dist = 0.0f;
  std::vector<float>::const_iterator itDesc1, itDesc2;
  for (itDesc1 = descriptors.SpinImg->begin(), itDesc2 = oFeature.descriptors.SpinImg->begin();
       itDesc1 != descriptors.SpinImg->end(); itDesc1++, itDesc2++)
  {
    dist += square(*itDesc1 - *itDesc2);
  }

  if (normalize_by_vector_length) dist /= 0.25f * descriptors.SpinImg->size();

  return sqrt(dist);
}  // end descriptorSpinImgDistanceTo

// --------------------------------------------------
//        descriptorPolarImgDistanceTo
// --------------------------------------------------
float CFeature::internal_distanceBetweenPolarImages(
    const CMatrixF& desc1,
    const CMatrixF& desc2,
    float& minDistAngle,
    bool normalize_distances,
    bool dont_shift_angle)
{
  MRPT_START

  // Find the smallest distance:
  unsigned int delta, i, j, ii, height = desc1.rows(), width = desc1.cols();
  float dist, minDist = 0;

  //#define LM_CORR_BIAS_MEAN

#define LM_CORR_METHOD_EUCLID
  //#define LM_CORR_METHOD_MANHATTAN
  //#define LM_CORR_METHOD_CORRELATION

#if defined(LM_CORR_BIAS_MEAN) || defined(LM_CORR_METHOD_CORRELATION)
  const float desc1_mean = desc1.sum() / d2f(width * height);
  const float desc2_mean = desc2.sum() / d2f(width * height);
#endif

  CVectorFloat distances(height, 0);  // Distances for each shift

  for (delta = 0; delta < height; delta++)
  {
#if defined(LM_CORR_METHOD_CORRELATION)
    float s11 = 0;
    float s22 = 0;
    float s12 = 0;
#endif
    // Compute the mean distance between desc1[t] and desc2[t-delta]:
    dist = 0;
    for (i = 0; i < height; i++)
    {
      ii = (i + delta) % height;  // Shifted index
      for (j = 0; j < width; j++)
      {
#ifdef LM_CORR_METHOD_EUCLID
#ifdef LM_CORR_BIAS_MEAN
        dist += square(desc1(i, j) - desc1_mean - desc2(ii, j) + desc2_mean);
#else
        dist += square(desc1(i, j) - desc2(ii, j));
#endif
#elif defined(LM_CORR_METHOD_MANHATTAN)
#ifdef LM_CORR_BIAS_MEAN
        dist += std::abs(desc1(i, j) - desc1_mean - desc2(ii, j) + desc2_mean);
#else
        dist += std::abs(desc1(i, j) - desc2(ii, j));
#endif
#elif defined(LM_CORR_METHOD_CORRELATION)
        float d1 = desc1(i, j) - desc1_mean;
        float d2 = desc2(ii, j) - desc2_mean;
        s11 += square(d1);
        s22 += square(d2);
        s12 += d1 * d2;
#else
#error A LM_CORR_METHOD_XXX method must be selected!
#endif
      }
    }

    // Average:
    if (normalize_distances) dist /= d2f(width * height);

#ifdef LM_CORR_METHOD_EUCLID
    dist = sqrt(dist);
#endif

#if defined(LM_CORR_METHOD_CORRELATION)
    dist = 1 - (s12 / sqrt(s11 * s22));
#endif

    distances[delta] = dist;
    if (!delta && dont_shift_angle)
    {
      distances.resize(1);
      break;
    }
  }  // end for delta

  size_t minDistIdx;
  minDist = distances.minCoeff(minDistIdx);

  double dist_mean, dist_std;
  mrpt::math::meanAndStd(distances, dist_mean, dist_std);

#if 0
	{
		cout << "min dist: " << minDist << endl;

		static mrpt::gui::CDisplayWindowPlots	win("distances");
		win.plot(distances,"b.4");
		CImage img1(desc1);
		win.image(img1,0,-0.5,0.4*width,0.5,"img1");

		CImage img2(desc2);
		win.image(img2,0.6*width,-0.5,0.4*width,0.5,"img2");

		//win.axis_fit();
		win.waitForKey();
	}
#endif

  // Output:
  minDistAngle = minDistIdx * d2f(M_2PI / width);
  return minDist;

  MRPT_END
}

// --------------------------------------------------
//        descriptorPolarImgDistanceTo
// --------------------------------------------------
float CFeature::descriptorPolarImgDistanceTo(
    const CFeature& oFeature, float& minDistAngle, bool normalize_distances) const
{
  MRPT_START

  ASSERT_(descriptors.PolarImg);
  ASSERT_EQUAL_(descriptors.PolarImg->rows(), oFeature.descriptors.PolarImg->rows());
  ASSERT_EQUAL_(descriptors.PolarImg->cols(), oFeature.descriptors.PolarImg->cols());
  ASSERT_(descriptors.hasDescriptorPolarImg() && oFeature.descriptors.hasDescriptorPolarImg());
  ASSERT_GT_(descriptors.PolarImg->rows(), 1);
  ASSERT_GT_(descriptors.PolarImg->cols(), 1);

  // Call the common method for computing these distances:
  return internal_distanceBetweenPolarImages(
      *descriptors.PolarImg, *oFeature.descriptors.PolarImg, minDistAngle, normalize_distances,
      descriptors.polarImgsNoRotation);

  MRPT_END
}  // end descriptorPolarImgDistanceTo

// --------------------------------------------------
//        descriptorLogPolarImgDistanceTo
// --------------------------------------------------
float CFeature::descriptorLogPolarImgDistanceTo(
    const CFeature& oFeature, float& minDistAngle, bool normalize_distances) const
{
  MRPT_START

  ASSERT_(descriptors.LogPolarImg);
  ASSERT_(descriptors.LogPolarImg->rows() == oFeature.descriptors.LogPolarImg->rows());
  ASSERT_(descriptors.LogPolarImg->cols() == oFeature.descriptors.LogPolarImg->cols());
  ASSERT_(
      descriptors.hasDescriptorLogPolarImg() && oFeature.descriptors.hasDescriptorLogPolarImg());
  ASSERT_(descriptors.LogPolarImg->rows() > 1 && descriptors.LogPolarImg->cols() > 1);

  // Call the common method for computing these distances:
  return internal_distanceBetweenPolarImages(
      *descriptors.LogPolarImg, *oFeature.descriptors.LogPolarImg, minDistAngle,
      normalize_distances, descriptors.polarImgsNoRotation);

  MRPT_END
}  // end descriptorPolarImgDistanceTo

// --------------------------------------------------
//        descriptorORBDistanceTo
// --------------------------------------------------
uint8_t CFeature::descriptorORBDistanceTo(const CFeature& oFeature) const
{
  ASSERT_(descriptors.hasDescriptorORB() && oFeature.descriptors.hasDescriptorORB());
  ASSERT_(descriptors.ORB->size() == oFeature.descriptors.ORB->size());
  const std::vector<uint8_t>& t_desc = *descriptors.ORB;
  const std::vector<uint8_t>& o_desc = *oFeature.descriptors.ORB;

  // Descriptors XOR + Hamming weight
  uint8_t distance = 0;
  for (uint8_t k = 0; k < t_desc.size(); ++k)
  {
    // from : Wegner, Peter (1960), "A technique for counting ones in a
    // binary computer", Communications of the ACM 3 (5): 322,
    // doi:10.1145/367236.367286
    uint8_t x_or = t_desc[k] ^ o_desc[k];
    uint8_t count;
    for (count = 0; x_or; count++)  // ...
      x_or &= x_or - 1;             // ...
    distance += count;
  }

  return distance;
}

// # added by Raghavender Sahdev
// --------------------------------------------------
// descriptorBLDDistanceTo
// --------------------------------------------------
float CFeature::descriptorBLDDistanceTo(const CFeature& oFeature, bool normalize_distances) const
{
  ASSERT_(descriptors.BLD->size() == oFeature.descriptors.BLD->size());
  ASSERT_(descriptors.hasDescriptorBLD() && oFeature.descriptors.hasDescriptorBLD());

  float dist = 0.0f;
  std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
  for (itDesc1 = descriptors.BLD->begin(), itDesc2 = oFeature.descriptors.BLD->begin();
       itDesc1 != descriptors.BLD->end(); itDesc1++, itDesc2++)
  {
    dist += square(*itDesc1 - *itDesc2);
  }
  if (normalize_distances) dist /= descriptors.BLD->size();
  dist = sqrt(dist);
  if (normalize_distances) dist /= 64.0f;
  return dist;
}  // end descriptorBLDDistanceTo

// --------------------------------------------------
// descriptorLATCHDistanceTo
// --------------------------------------------------
float CFeature::descriptorLATCHDistanceTo(const CFeature& oFeature, bool normalize_distances) const
{
  ASSERT_(descriptors.LATCH->size() == oFeature.descriptors.LATCH->size());
  ASSERT_(descriptors.hasDescriptorLATCH() && oFeature.descriptors.hasDescriptorLATCH());

  float dist = 0.0f;
  std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
  for (itDesc1 = descriptors.LATCH->begin(), itDesc2 = oFeature.descriptors.LATCH->begin();
       itDesc1 != descriptors.LATCH->end(); itDesc1++, itDesc2++)
  {
    dist += square(*itDesc1 - *itDesc2);
  }
  if (normalize_distances) dist /= descriptors.LATCH->size();
  dist = sqrt(dist);
  if (normalize_distances) dist /= 64.0f;
  return dist;
}  // end descriptorLATCHDistanceTo

// --------------------------------------------------
//              saveToTextFile
// --------------------------------------------------
void CFeature::saveToTextFile(const std::string& filename, bool APPEND)
{
  MRPT_START
  //    "%% Dump of mrpt::vision::CFeatureList. Each line format is:\n"
  //    "%% ID TYPE X Y ORIENTATION SCALE TRACK_STATUS RESPONSE HAS_SIFT
  //    [SIFT] HAS_SURF [SURF] HAS_MULTI [MULTI_i] HAS_ORB [ORB]"
  //    "%% \\---------------------- feature ------------------/ \\---------
  //    descriptors -------/\n"
  //    "%% with:\n"
  //    "%%  TYPE  : The used detector: 0:KLT, 1: Harris, 2: BCD, 3: SIFT, 4:
  //    SURF, 5: Beacon, 6: FAST\n"
  //    "%%  HAS_* : 1 if a descriptor of that type is associated to the
  //    feature. \n"
  //    "%%  SIFT  : Present if HAS_SIFT=1: N DESC_0 ... DESC_N-1 \n"
  //    "%%  SURF  : Present if HAS_SURF=1: N DESC_0 ... DESC_N-1 \n"
  //	  "%%  MULTI : Present if HAS_MULTI=1: SCALE ORI N DESC_0 ... DESC_N-1"
  //	  "%%  ORB   : Present if HAS_ORB=1: VALUE
  //    "%%-------------------------------------------------------------------------------------------\n");
  CFileOutputStream f;

  if (!f.open(filename, APPEND ? mrpt::io::OpenMode::TRUNCATE : mrpt::io::OpenMode::TRUNCATE))
    THROW_EXCEPTION(
        "[CFeature::saveToTextFile] ERROR: File could not be open for "
        "writing");

  f.printf(
      "%5u %2d %7.3f %7.3f %6.2f %2d %2d %6.3f ", (unsigned int)keypoint.ID, (int)get_type(),
      keypoint.pt.x, keypoint.pt.y, orientation, keypoint.octave, (int)keypoint.track_status,
      keypoint.response);

  f.printf("%2d ", int(descriptors.hasDescriptorSIFT() ? 1 : 0));
  if (descriptors.hasDescriptorSIFT())
  {
    f.printf("%4d ", int(descriptors.SIFT->size()));
    for (unsigned char k : *descriptors.SIFT) f.printf("%4d ", k);
  }

  f.printf("%2d ", int(descriptors.hasDescriptorSURF() ? 1 : 0));
  if (descriptors.hasDescriptorSURF())
  {
    f.printf("%4d ", int(descriptors.SURF->size()));
    for (float k : *descriptors.SURF) f.printf("%8.5f ", k);
  }

  f.printf("%2d ", int(descriptors.hasDescriptorORB() ? 1 : 0));
  if (descriptors.hasDescriptorORB())
    for (unsigned char k : *descriptors.ORB) f.printf("%d ", k);

  // # ADDED by Raghavender Sahdev
  f.printf("%2d ", int(descriptors.hasDescriptorBLD() ? 1 : 0));
  if (descriptors.hasDescriptorBLD())
  {
    f.printf("%4d ", int(descriptors.BLD->size()));
    for (unsigned char k : *descriptors.BLD) f.printf("%4d ", k);
  }

  f.printf("%2d ", int(descriptors.hasDescriptorLATCH() ? 1 : 0));
  if (descriptors.hasDescriptorLATCH())
  {
    f.printf("%4d ", int(descriptors.LATCH->size()));
    for (unsigned char k : *descriptors.LATCH) f.printf("%4d ", k);
  }

  f.printf("\n");
  f.close();

  MRPT_END
}  // end saveToTextFile

/****************************************************
         Class CFEATURELIST
*****************************************************/

CFeatureList::~CFeatureList() = default;

// --------------------------------------------------
// saveToTextFile
// --------------------------------------------------
// FORMAT: ID type x y orientation scale [descriptorSIFT] [descriptorSURF]
// track_status response
void CFeatureList::saveToTextFile(const std::string& filename, bool APPEND)
{
  MRPT_START

  CFileOutputStream f;

  if (!f.open(filename, APPEND ? mrpt::io::OpenMode::APPEND : mrpt::io::OpenMode::TRUNCATE))
    THROW_EXCEPTION(
        "[CFeatureList::saveToTextFile] ERROR: File could not be open for "
        "writing");

  f.printf(
      "%% Dump of mrpt::vision::CFeatureList. Each line format is:\n"
      "%% ID TYPE X Y ORIENTATION SCALE TRACK_STATUS RESPONSE HAS_SIFT "
      "[SIFT] HAS_SURF [SURF]\n"
      "%% \\---------------------- feature ------------------/ \\--------- "
      "descriptors -------/\n"
      "%% with:\n"
      "%%  TYPE  : The used detector: 0:KLT, 1: Harris, 2: BCD, 3: SIFT, 4: "
      "SURF, 5: Beacon, 6: FAST\n"
      "%%  HAS_* : 1 if a descriptor of that type is associated to the "
      "feature. \n"
      "%%  SIFT  : Present if HAS_SIFT=1: N DESC_0 ... DESC_N-1 \n"
      "%%  SURF  : Present if HAS_SURF=1: N DESC_0 ... DESC_N-1 \n"
      "%%--------------------------------------------------------------------"
      "-----------------------\n");

  for (auto& feat : *this)
  {
    f.printf(
        "%5u %2d %7.3f %7.3f %6.2f %2d %2d %6.3f ", (unsigned int)feat.keypoint.ID,
        (int)feat.get_type(), feat.keypoint.pt.x, feat.keypoint.pt.y, feat.orientation,
        feat.keypoint.octave, (int)feat.track_status, feat.response);

    f.printf("%2d ", int(feat.descriptors.hasDescriptorSIFT() ? 1 : 0));
    if (feat.descriptors.hasDescriptorSIFT())
    {
      f.printf("%4d ", int(feat.descriptors.SIFT->size()));
      for (unsigned int k = 0; k < feat.descriptors.SIFT->size(); k++)
        f.printf("%4d ", (*feat.descriptors.SIFT)[k]);
    }

    f.printf("%2d ", int(feat.descriptors.hasDescriptorSURF() ? 1 : 0));
    if (feat.descriptors.hasDescriptorSURF())
    {
      f.printf("%4d ", int(feat.descriptors.SURF->size()));
      for (unsigned int k = 0; k < feat.descriptors.SURF->size(); k++)
        f.printf("%8.5f ", (*feat.descriptors.SURF)[k]);
    }
    // # added by Raghavender Sahdev
    f.printf("%2d ", int(feat.descriptors.hasDescriptorBLD() ? 1 : 0));
    if (feat.descriptors.hasDescriptorBLD())
    {
      f.printf("%4d ", int(feat.descriptors.BLD->size()));
      for (unsigned int k = 0; k < feat.descriptors.BLD->size(); k++)
        f.printf("%4d ", (*feat.descriptors.BLD)[k]);
    }

    f.printf("%2d ", int(feat.descriptors.hasDescriptorLATCH() ? 1 : 0));
    if (feat.descriptors.hasDescriptorLATCH())
    {
      f.printf("%4d ", int(feat.descriptors.LATCH->size()));
      for (unsigned int k = 0; k < feat.descriptors.LATCH->size(); k++)
        f.printf("%4d ", (*feat.descriptors.LATCH)[k]);
    }

    f.printf("\n");
  }  // end for

  f.close();

  MRPT_END
}  // end saveToTextFile

// --------------------------------------------------
// loadFromTextFile
// --------------------------------------------------
void CFeatureList::loadFromTextFile(const std::string& filename)
{
  MRPT_START

  mrpt::io::CTextFileLinesParser parser(filename);
  std::istringstream line;

  while (parser.getNextLine(line))
  {
    try
    {
      CFeature feat;

      int _ID;
      if (!(line >> _ID)) throw std::string("ID");
      feat.keypoint.ID = TFeatureID(_ID);

      int _type;
      if (!(line >> _type)) throw std::string("type");
      feat.type = TKeyPointMethod(_type);

      if (!(line >> feat.keypoint.pt.x >> feat.keypoint.pt.y)) throw std::string("x,y");
      if (!(line >> feat.orientation)) throw std::string("orientation");
      if (!(line >> feat.keypoint.octave)) throw std::string("scale");

      int _track_st;
      if (!(line >> _track_st)) throw std::string("track_status");
      feat.track_status = TFeatureTrackStatus(_track_st);

      if (!(line >> feat.response)) throw std::string("response");

      int hasSIFT;
      if (!(line >> hasSIFT)) throw std::string("hasSIFT");
      if (hasSIFT)
      {
        size_t N;
        if (!(line >> N)) throw std::string("SIFT-len");
        feat.descriptors.SIFT->resize(N);
        for (size_t i = 0; i < N; i++)
        {
          int val;
          line >> val;
          // DON'T read directly SIFT[i] since it's a
          // uint8_t, interpreted as a cha
          (*feat.descriptors.SIFT)[i] = val;
        }

        if (!line) throw std::string("SIFT-data");
      }

      //# ADDED by Raghavender Sahdev
      int hasBLD;
      if (!(line >> hasBLD)) throw std::string("hasBLD");
      if (hasBLD)
      {
        size_t N;
        if (!(line >> N)) throw std::string("BLD-len");
        feat.descriptors.BLD->resize(N);
        for (size_t i = 0; i < N; i++)
        {
          int val;
          line >> val;
          (*feat.descriptors.BLD)[i] = val;
        }

        if (!line) throw std::string("BLD-data");
      }

      int hasLATCH;
      if (!(line >> hasLATCH)) throw std::string("hasLATCH");
      if (hasBLD)
      {
        size_t N;
        if (!(line >> N)) throw std::string("LATCH-len");
        feat.descriptors.LATCH->resize(N);
        for (size_t i = 0; i < N; i++)
        {
          int val;
          line >> val;
          (*feat.descriptors.LATCH)[i] = val;
        }

        if (!line) throw std::string("LATCH-data");
      }

      int hasSURF;
      if (!(line >> hasSURF)) throw std::string("hasSURF");
      if (hasSURF)
      {
        size_t N;
        if (!(line >> N)) throw std::string("SURF-len");
        feat.descriptors.SURF->resize(N);
        for (size_t i = 0; i < N; i++) line >> (*feat.descriptors.SURF)[i];
        if (!line) throw std::string("SURF-data");
      }

      emplace_back(std::move(feat));
    }
    catch (std::string& msg)
    {
      THROW_EXCEPTION(format(
          "%s:%d: Error parsing features text file (%s).", filename.c_str(),
          (int)parser.getCurrentLineNumber(), msg.c_str()));
    }
  }

  MRPT_END
}  // end loadFromTextFile

// --------------------------------------------------
// copyListFrom()
// --------------------------------------------------
void CFeatureList::copyListFrom(const CFeatureList& otherList)
{
  resize(otherList.size());
  CFeatureList::const_iterator it1;
  CFeatureList::iterator it2;
  for (it1 = otherList.begin(), it2 = begin(); it1 != otherList.end(); ++it1, ++it2)
  {
    *it2 = *it1;
  }
}  // end-copyListFrom

const CFeature* CFeatureList::getByID(const TFeatureID& ID) const
{
  for (const auto& f : *this)
    if (f.keypoint.ID == ID) return &f;

  return nullptr;
}

const CFeature* CFeatureList::getByID(const TFeatureID& ID, int& out_idx) const
{
  int k = 0;
  for (auto it = begin(); it != end(); ++it, ++k)
    if (it->keypoint.ID == ID)
    {
      out_idx = k;
      return &(*it);
    }
  out_idx = -1;
  return nullptr;
}

// --------------------------------------------------
// nearest(x,y)
// --------------------------------------------------
const CFeature* CFeatureList::nearest(const float x, const float y, double& dist_prev) const
{
  if (empty()) return nullptr;

  float closest_x, closest_y;
  float closest_sqDist;

  // Look for the closest feature using KD-tree look up:
  const size_t closest_idx = kdTreeClosestPoint2D(x, y, closest_x, closest_y, closest_sqDist);
  float closest_dist = std::sqrt(closest_sqDist);

  if (closest_dist <= dist_prev)
  {
    dist_prev = closest_dist;
    return &m_feats[closest_idx];
  }
  else
    return nullptr;
}  // end nearest

// --------------------------------------------------
// getMaxID()
// --------------------------------------------------
TFeatureID CFeatureList::getMaxID() const
{
  MRPT_START
  ASSERT_(!empty());
  vision::TFeatureID maxID = begin()->keypoint.ID;
  for (const auto& f : *this) mrpt::keep_max(maxID, f.keypoint.ID);
  return maxID;
  MRPT_END
}

/****************************************************
      Class CMATCHEDFEATUREKLT
*****************************************************/

void CMatchedFeatureList::saveToTextFile(const std::string& filename)
{
  // OUTPUT FORMAT: ID_1 x_1 y_1 ID_2 x_2 y_2

  FILE* f = os::fopen(filename.c_str(), "wt");
  if (!f) return;

  for (const auto& p : *this)
  {
    os::fprintf(
        f, "%d %.3f %.3f %d %.3f %.3f\n", (unsigned int)(p.first.keypoint.ID),
        p.first.keypoint.pt.x, p.first.keypoint.pt.y, (unsigned int)(p.second.keypoint.ID),
        p.second.keypoint.pt.x, p.second.keypoint.pt.y);

  }  // end for
  os::fclose(f);
}

// --------------------------------------------------
//			getBothFeatureLists
// --------------------------------------------------
const CFeature* CMatchedFeatureList::getByID(const TFeatureID& ID, const TListIdx& idx)
{
  CMatchedFeatureList::iterator it;
  for (it = begin(); it != end(); ++it)
  {
    const auto& feat = (idx == firstList) ? it->first : it->second;
    if (feat.keypoint.ID == ID) return &feat;
  }
  return nullptr;
}

// --------------------------------------------------
// updateMaxID()
// --------------------------------------------------
void CMatchedFeatureList::updateMaxID(const TListIdx& idx)
{
  MRPT_START
  TFeatureID maxID1 = begin()->first.keypoint.ID;
  TFeatureID maxID2 = begin()->second.keypoint.ID;
  for (auto itList = begin(); itList != end(); itList++)
  {
    if (idx == firstList || idx == bothLists) mrpt::keep_max(maxID1, itList->first.keypoint.ID);
    if (idx == secondList || idx == bothLists) mrpt::keep_max(maxID2, itList->second.keypoint.ID);
  }
  if (idx == firstList || idx == bothLists) m_leftMaxID = maxID1;
  if (idx == secondList || idx == bothLists) m_rightMaxID = maxID2;
  MRPT_END
}

// --------------------------------------------------
// getMaxID()
// --------------------------------------------------
void CMatchedFeatureList::getMaxID(
    const TListIdx& idx, TFeatureID& firstListID, TFeatureID& secondListID)
{
  MRPT_START
  ASSERT_(!empty());
  if (idx == firstList || idx == bothLists)
    if (m_leftMaxID == 0) updateMaxID(firstList);
  if (idx == secondList || idx == bothLists)
    if (m_rightMaxID == 0) updateMaxID(secondList);
  firstListID = m_leftMaxID;
  secondListID = m_rightMaxID;
  MRPT_END
}  // end getMaxID()
// --------------------------------------------------
//			getBothFeatureLists
// --------------------------------------------------
void CMatchedFeatureList::getBothFeatureLists(CFeatureList& list1, CFeatureList& list2)
{
  MRPT_START
  list1.resize(size());
  list2.resize(size());

  unsigned int k = 0;
  for (auto it = begin(); it != end(); ++it, ++k)
  {
    list1[k] = it->first;
    list2[k] = it->second;
  }  // end for
  MRPT_END
}

// --------------------------------------------------
//			getFirstDescriptorAsMatrix
// --------------------------------------------------
bool CFeature::getFirstDescriptorAsMatrix(mrpt::math::CMatrixFloat& desc) const
{
  if (descriptors.hasDescriptorSIFT())
  {
    desc.setSize(1, descriptors.SIFT->size());
    for (size_t i = 0; i < descriptors.SIFT->size(); i++) desc(0, i) = (*descriptors.SIFT)[i];
    return true;
  }
  else if (descriptors.hasDescriptorBLD())
  {
    desc.setSize(1, descriptors.BLD->size());
    for (size_t i = 0; i < descriptors.BLD->size(); i++) desc(0, i) = (*descriptors.BLD)[i];
    return true;
  }
  else if (descriptors.hasDescriptorLATCH())
  {
    desc.setSize(1, descriptors.LATCH->size());
    for (size_t i = 0; i < descriptors.LATCH->size(); i++) desc(0, i) = (*descriptors.LATCH)[i];
    return true;
  }
  else if (descriptors.hasDescriptorSURF())
  {
    desc.setSize(1, descriptors.SURF->size());
    for (size_t i = 0; i < descriptors.SURF->size(); i++) desc(0, i) = (*descriptors.SURF)[i];
    return true;
  }
  else if (descriptors.hasDescriptorSpinImg())
  {
    const size_t nR = descriptors.SpinImg_range_rows;
    const size_t nC = descriptors.SpinImg->size() / descriptors.SpinImg_range_rows;
    desc.resize(nR, nC);
    auto itD = descriptors.SpinImg->begin();
    for (size_t r = 0; r < nR; r++)
      for (size_t c = 0; c < nC; c++) desc.coeffRef(r, c) = *itD++;
    return true;
  }
  else if (descriptors.hasDescriptorPolarImg())
  {
    desc = *descriptors.PolarImg;
    return true;
  }
  else if (descriptors.hasDescriptorLogPolarImg())
  {
    desc = *descriptors.LogPolarImg;
    return true;
  }
  else
    return false;
}
