CFeature.cpp

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+
   */
 
#include "vision-precomp.h"  // Precompiled headers
 
#include <mrpt/utils/CTextFileLinesParser.h>
#include <mrpt/utils/CFileOutputStream.h>
#include <mrpt/utils/CFileInputStream.h>
#include <mrpt/utils/CStdOutStream.h>
#include <mrpt/utils/stl_serialization.h>
#include <mrpt/vision/CFeature.h>
#include <mrpt/vision/types.h>
#include <mrpt/utils/stl_serialization.h>
#include <mrpt/math/data_utils.h>
#include <mrpt/system/os.h>
 
using namespace mrpt;
using namespace mrpt::vision;
using namespace mrpt::math;
using namespace mrpt::system;
using namespace mrpt::utils;
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::utils::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_double(section, "searchAreaSize", 20, false);
}
 
// --------------------------------------------------
//                      saveToConfigFile
// --------------------------------------------------
void TMultiResDescMatchOptions::saveToConfigFile(
        mrpt::utils::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(
        mrpt::utils::CStream& out) const
{
        out.printf(
                "\n----------- [vision::TMultiResDescMatchOptions] ------------ \n");
        out.printf("Use orientation filter?:        ");
        if (useOriFilter)
        {
                out.printf("Yes\n");
                out.printf(
                        "· Orientation threshold:        %.1f deg\n",
                        RAD2DEG(oriThreshold));
        }
        else
                out.printf("No\n");
        out.printf("Use depth filter?:              ");
        if (useDepthFilter)
                out.printf("Yes\n");
        else
        {
                out.printf("No\n");
                out.printf("Lowest scale in list1:          %d\n", lowScl1);
                out.printf("Highest scale in list1:         %d\n", highScl1);
                out.printf("Lowest scale in list2:          %d\n", lowScl2);
                out.printf("Highest scale in list2:         %d\n", highScl2);
        }
        out.printf("#frames last seen threshold:    %d\n", lastSeenThreshold);
        out.printf("#frames to be stable threshold: %d\n", timesSeenThreshold);
        out.printf("min. # features in system:      %d\n", minFeaturesToFind);
        out.printf("min. # features to be lost:     %d\n", minFeaturesToBeLost);
        out.printf("Matching threshold:             %.2f\n", matchingThreshold);
        out.printf(
                "Matching ratio threshold:       %.2f\n", matchingRatioThreshold);
        out.printf("Size of the search window:      %d px\n", searchAreaSize);
        out.printf("-------------------------------------------------------- \n");
}  // end-dumpToTextStream
 
// --------------------------------------------------
//                      loadFromConfigFile
// --------------------------------------------------
/**  Load all the params from a config source, in the format described in
 * saveToConfigFile()
  */
void TMultiResDescOptions::loadFromConfigFile(
        const mrpt::utils::CConfigFileBase& cfg, const std::string& section)
{
        basePSize = cfg.read_double(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::utils::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 (unsigned int k = 0; k < scales.size(); ++k)
                mrpt::system::os::sprintf(buf, 300, "%.2f ", scales[k]);
        cfg.write(section, "scales", buf);
}  // end-saveToConfigFile
 
// --------------------------------------------------
//                      dumpToTextStream
// --------------------------------------------------
void TMultiResDescOptions::dumpToTextStream(mrpt::utils::CStream& out) const
{
        out.printf("\n----------- [vision::TMultiResDescOptions] ------------ \n");
        out.printf("Base patch size:                %d px\n", basePSize);
        out.printf("Lowest scale to compute:        %d\n", comLScl);
        out.printf("Highest scale to compute:       %d\n", comHScl);
        out.printf("Image smoothing sigma:          %.2f px\n", sg1);
        out.printf("Orientation histogram sigma:    %.2f\n", sg2);
        out.printf("Descriptor histogram sigma:     %.2f\n", sg3);
        out.printf("Compute depth:                  ");
        if (computeDepth)
        {
                out.printf("Yes\n");
                out.printf("Focal length:                   %.2f px\n", fx);
                out.printf("Principal point (cx):           %.2f px\n", cx);
                out.printf("Principal point (cy):           %.2f px\n", cy);
                out.printf("Baseline:                       %.2f m\n", baseline);
        }
        else
                out.printf("No\n");
 
        out.printf("Compute Hash Coeffs:            ");
        if (computeHashCoeffs)
                out.printf("Yes\n");
        else
                out.printf("No\n");
 
        out.printf("Blur image previously:          ");
        if (blurImage)
                out.printf("Yes\n");
        else
                out.printf("No\n");
 
        out.printf("Scales:                         ");
        for (unsigned int k = 0; k < scales.size(); ++k)
                out.printf("%.2f ", scales[k]);
        out.printf("\n");
        out.printf("-------------------------------------------------------- \n");
}  // end-dumpToTextStream
 
void CFeature::dumpToTextStream(mrpt::utils::CStream& out) const
{
        out.printf("\n----------- [vision::CFeature] ------------ \n");
        out.printf("Feature ID:                     %d\n", (int)ID);
        out.printf("Coordinates:                    (%.2f,%.2f) px\n", x, y);
        out.printf("PatchSize:                      %d\n", patchSize);
        out.printf("Type:                           ");
        switch (type)
        {
                case -1:
                        out.printf("Not defined\n");
                        break;
                case 0:
                        out.printf("KLT\n");
                        break;
                case 1:
                        out.printf("Harris\n");
                        break;
                case 2:
                        out.printf("BCD\n");
                        break;
                case 3:
                        out.printf("SIFT\n");
                        break;
                case 4:
                        out.printf("SURF\n");
                        break;
                case 5:
                        out.printf("Beacon\n");
                        break;
                case 6:
                        out.printf("FAST\n");
                        break;
                case 7:
                        out.printf("FASTER-9\n");
                        break;
                case 8:
                        out.printf("FASTER-10\n");
                        break;
                case 9:
                        out.printf("FASTER-12\n");
                        break;
                case 10:
                        out.printf("ORB\n");
                        break;
                case 11:
                        out.printf("AKAZE\n");
                        break;
                case 12:
                        out.printf("LSD");
                        break;
        }
        out.printf("Status:                         ");
        switch (track_status)
        {
                case 0:
                        out.printf("Idle\n");
                        break;
                case 1:
                        out.printf("[KLT] Out of bounds [KLT]\n");
                        break;
                case 5:
                        out.printf("[KLT] Tracked\n");
                        break;
                case 10:
                        out.printf("[KLT] Lost\n");
                        break;
        }
 
        out.printf("Response:                       %.2f\n", response);
        out.printf("Main orientation:               %.2f\n", orientation);
        out.printf("Main scale:                     %.2f\n", scale);
        out.printf("# frames seen:                  %d\n", nTimesSeen);
        out.printf("# frames not seen:              %d\n", nTimesNotSeen);
        out.printf("# frames since last seen:       %d\n", nTimesLastSeen);
        out.printf("Initial Depth:                  %.2f m\n", initialDepth);
        out.printf("Depth:                          %.2f m\n", depth);
        out.printf(
                "3D point:                       (%.2f,%.2f,%.2f) m\n", p3D.x, p3D.y,
                p3D.z);
        out.printf("Is point feature?:              ");
        isPointFeature() ? out.printf("Yes\n") : out.printf("No\n");
 
        out.printf("Has SIFT descriptor?:           ");
        descriptors.hasDescriptorSIFT() ? out.printf("Yes\n") : out.printf("No\n");
        out.printf("Has SURF descriptor?:           ");
        descriptors.hasDescriptorSURF() ? out.printf("Yes\n") : out.printf("No\n");
        out.printf("Has Spin image descriptor?:     ");
        descriptors.hasDescriptorSpinImg() ? out.printf("Yes\n")
                                                                           : out.printf("No\n");
        out.printf("Has Polar descriptor?:          ");
        descriptors.hasDescriptorPolarImg() ? out.printf("Yes\n")
                                                                                : out.printf("No\n");
        out.printf("Has Log Polar descriptor?:      ");
        descriptors.hasDescriptorLogPolarImg() ? out.printf("Yes\n")
                                                                                   : out.printf("No\n");
        out.printf("Has ORB descriptor?:                        ");
        descriptors.hasDescriptorORB() ? out.printf("Yes\n") : out.printf("No\n");
        //# added by Raghavender Sahdev
        out.printf("Has BLD descriptor?:                        ");
        descriptors.hasDescriptorBLD() ? out.printf("Yes\n") : out.printf("No\n");
        out.printf("Has LATCH descriptor?:                      ");
        descriptors.hasDescriptorLATCH() ? out.printf("Yes\n") : out.printf("No\n");
 
        out.printf("Has multiscale?:                ");
        if (!descriptors.hasDescriptorMultiSIFT())
                out.printf("No\n");
        else
        {
                out.printf("Yes [%d]\n", (int)multiScales.size());
                for (int k = 0; k < (int)multiScales.size(); ++k)
                {
                        out.printf(" · Scale %d: %.2f\n", k, multiScales[k]);
                        for (int m = 0; m < (int)multiOrientations[k].size(); ++m)
                        {
                                out.printf(
                                        " ·· Orientation %d: %.2f\n", m, multiOrientations[k][m]);
                                out.printf(" ·· [D] ");
                                for (int n = 0;
                                         n < (int)descriptors.multiSIFTDescriptors[k][m].size();
                                         ++n)
                                        out.printf(
                                                "%d ", descriptors.multiSIFTDescriptors[k][m][n]);
                                out.printf("\n");
                                if (multiHashCoeffs.size() > 0)
                                        out.printf(
                                                " ·· HASH coefficients %d,%d,%d\n",
                                                multiHashCoeffs[k][m][0], multiHashCoeffs[k][m][1],
                                                multiHashCoeffs[k][m][2]);
                        }  // end-for-m
                }  // end-for-k
        }  // end else
}  // end dumpToTextStream
 
void CFeature::dumpToConsole() const
{
        CStdOutStream myOut;
        dumpToTextStream(myOut);
}
 
void CFeature::writeToStream(mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 2;
        else
        {
                // The coordinates:
                out << x << y << ID << patch << patchSize << (uint32_t)type
                        << (uint32_t)track_status << response << orientation << scale
                        << user_flags << nTimesSeen << nTimesNotSeen << nTimesLastSeen
                        << depth << initialDepth << p3D << multiScales << multiOrientations
                        << multiHashCoeffs << descriptors.SIFT << descriptors.SURF
                        << descriptors.SpinImg << descriptors.SpinImg_range_rows
                        << descriptors.PolarImg << descriptors.LogPolarImg
                        << descriptors.polarImgsNoRotation
                        << descriptors.multiSIFTDescriptors << descriptors.ORB
                        //# ADDED by Raghavender Sahdev
                        << descriptors.BLD << descriptors.LATCH;
        }
}
 
void CFeature::readFromStream(mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:
                case 1:
                case 2:
                {
                        // The coordinates:
                        uint32_t aux_type, aux_KLTS;
                        in >> x >> y >> ID >> patch >> patchSize >> aux_type >> aux_KLTS >>
                                response >> orientation >> scale >> user_flags;
                        if (version > 0)
                        {
                                in >> nTimesSeen >> nTimesNotSeen >> nTimesLastSeen >> depth >>
                                        initialDepth >> p3D >> multiScales >> multiOrientations >>
                                        multiHashCoeffs;
                        }
                        in >> descriptors.SIFT >> descriptors.SURF >> descriptors.SpinImg >>
                                descriptors.SpinImg_range_rows >> descriptors.PolarImg >>
                                descriptors.LogPolarImg >> descriptors.polarImgsNoRotation
                                // # added by Raghavender Sahdev
                                >> descriptors.BLD >> descriptors.LATCH;
                        if (version > 0) in >> descriptors.multiSIFTDescriptors;
                        if (version > 1) in >> descriptors.ORB;
 
                        type = (TFeatureType)aux_type;
                        track_status = (TFeatureTrackStatus)aux_KLTS;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}
 
/****************************************************
                                Class CFEATURE
*****************************************************/
// CONSTRUCTOR
CFeature::CFeature()
        : x(0.0f),
          y(0.0f),
          ID(0),
          patchSize(21),
          type(featNotDefined),
          track_status(status_IDLE),
          response(0.0),
          orientation(0.0),
          scale(0.0),
          user_flags(0),
          nTimesSeen(1),
          nTimesNotSeen(0),
          nTimesLastSeen(0),
          depth(0),
          initialDepth(0),
          p3D(),
          multiScales(),
          multiOrientations(),
          multiHashCoeffs(),
          descriptors()
{
}
 
// Ctor
CFeature::TDescriptors::TDescriptors()
        : SIFT(),
          SURF(),
          SpinImg(),
          SpinImg_range_rows(0),
          PolarImg(0, 0),
          LogPolarImg(0, 0),
          polarImgsNoRotation(false),
          ORB(),
          BLD(),
          LATCH()
{
}
 
// 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.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;
        patch.cross_correlation(oFeature.patch, x_max, y_max, max_val);
 
        return 0.5 -
                   0.5 * max_val;  // Value as "distance" in the range [0,1], best = 0
 
        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_(this->descriptors.SIFT.size() == oFeature.descriptors.SIFT.size());
        ASSERT_(
                this->descriptors.hasDescriptorSIFT() &&
                oFeature.descriptors.hasDescriptorSIFT())
 
        float dist = 0.0f;
        std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
        for (itDesc1 = this->descriptors.SIFT.begin(),
                itDesc2 = oFeature.descriptors.SIFT.begin();
                 itDesc1 != this->descriptors.SIFT.end(); itDesc1++, itDesc2++)
        {
                dist += square(*itDesc1 - *itDesc2);
        }
        if (normalize_distances) dist /= this->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_(this->descriptors.SURF.size() == oFeature.descriptors.SURF.size());
        ASSERT_(
                this->descriptors.hasDescriptorSURF() &&
                oFeature.descriptors.hasDescriptorSURF())
 
        float dist = 0.0f;
        std::vector<float>::const_iterator itDesc1, itDesc2;
        for (itDesc1 = this->descriptors.SURF.begin(),
                itDesc2 = oFeature.descriptors.SURF.begin();
                 itDesc1 != this->descriptors.SURF.end(); itDesc1++, itDesc2++)
        {
                dist += square(*itDesc1 - *itDesc2);
        }
        if (normalize_distances) dist /= this->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_(
                this->descriptors.SpinImg.size() ==
                oFeature.descriptors.SpinImg.size());
        ASSERT_(
                this->descriptors.hasDescriptorSpinImg() &&
                oFeature.descriptors.hasDescriptorSpinImg())
        ASSERT_(!this->descriptors.SpinImg.empty())
 
        float dist = 0.0f;
        std::vector<float>::const_iterator itDesc1, itDesc2;
        for (itDesc1 = this->descriptors.SpinImg.begin(),
                itDesc2 = oFeature.descriptors.SpinImg.begin();
                 itDesc1 != this->descriptors.SpinImg.end(); itDesc1++, itDesc2++)
        {
                dist += square(*itDesc1 - *itDesc2);
        }
 
        if (normalize_by_vector_length) dist /= 0.25 * descriptors.SpinImg.size();
 
        return sqrt(dist);
}  // end descriptorSpinImgDistanceTo
 
// --------------------------------------------------
//        descriptorPolarImgDistanceTo
// --------------------------------------------------
float CFeature::internal_distanceBetweenPolarImages(
        const CMatrix& desc1, const CMatrix& desc2, float& minDistAngle,
        bool normalize_distances, bool dont_shift_angle)
{
        MRPT_START
 
        // Find the smallest distance:
        unsigned int delta, i, j, ii, height = desc1.getRowCount(),
                                                                  width = desc1.getColCount();
        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() / static_cast<float>(width * height);
        const float desc2_mean = desc2.sum() / static_cast<float>(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.get_unsafe(i, j) - desc1_mean -
                                        desc2.get_unsafe(ii, j) + desc2_mean);
#else
                                dist +=
                                        square(desc1.get_unsafe(i, j) - desc2.get_unsafe(ii, j));
#endif
#elif defined(LM_CORR_METHOD_MANHATTAN)
#ifdef LM_CORR_BIAS_MEAN
                                dist +=
                                        abs(desc1.get_unsafe(i, j) - desc1_mean -
                                                desc2.get_unsafe(ii, j) + desc2_mean);
#else
                                dist += abs(desc1.get_unsafe(i, j) - desc2.get_unsafe(ii, j));
#endif
#elif defined(LM_CORR_METHOD_CORRELATION)
                                float d1 = desc1.get_unsafe(i, j) - desc1_mean;
                                float d2 = desc2.get_unsafe(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 /= static_cast<float>(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.minimum(&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 * M_2PI / static_cast<float>(width);
        return minDist;
 
        MRPT_END
}
 
// --------------------------------------------------
//        descriptorPolarImgDistanceTo
// --------------------------------------------------
float CFeature::descriptorPolarImgDistanceTo(
        const CFeature& oFeature, float& minDistAngle,
        bool normalize_distances) const
{
        MRPT_START
 
        ASSERT_(
                size(descriptors.PolarImg, 1) == size(oFeature.descriptors.PolarImg, 1))
        ASSERT_(
                size(descriptors.PolarImg, 2) == size(oFeature.descriptors.PolarImg, 2))
        ASSERT_(
                this->descriptors.hasDescriptorPolarImg() &&
                oFeature.descriptors.hasDescriptorPolarImg())
        ASSERT_(
                size(descriptors.PolarImg, 1) > 1 && size(descriptors.PolarImg, 2) > 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_(
                size(descriptors.LogPolarImg, 1) ==
                size(oFeature.descriptors.LogPolarImg, 1))
        ASSERT_(
                size(descriptors.LogPolarImg, 2) ==
                size(oFeature.descriptors.LogPolarImg, 2))
        ASSERT_(
                this->descriptors.hasDescriptorLogPolarImg() &&
                oFeature.descriptors.hasDescriptorLogPolarImg())
        ASSERT_(
                size(descriptors.LogPolarImg, 1) > 1 &&
                size(descriptors.LogPolarImg, 2) > 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_(
                this->descriptors.hasDescriptorORB() &&
                oFeature.descriptors.hasDescriptorORB())
        ASSERT_(this->descriptors.ORB.size() == oFeature.descriptors.ORB.size())
 
        const std::vector<uint8_t>& t_desc = this->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)
        {
                uint8_t x_or = t_desc[k] ^ o_desc[k];
                uint8_t count;  // 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
                for (count = 0; x_or; count++)  // ...
                        x_or &= x_or - 1;  // ...
                distance += count;
        }
 
        return float(distance);
}  // end-descriptorORBDistanceTo
 
// # added by Raghavender Sahdev
// --------------------------------------------------
// descriptorBLDDistanceTo
// --------------------------------------------------
float CFeature::descriptorBLDDistanceTo(
        const CFeature& oFeature, bool normalize_distances) const
{
        ASSERT_(this->descriptors.BLD.size() == oFeature.descriptors.BLD.size());
        ASSERT_(
                this->descriptors.hasDescriptorBLD() &&
                oFeature.descriptors.hasDescriptorBLD())
 
        float dist = 0.0f;
        std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
        for (itDesc1 = this->descriptors.BLD.begin(),
                itDesc2 = oFeature.descriptors.BLD.begin();
                 itDesc1 != this->descriptors.BLD.end(); itDesc1++, itDesc2++)
        {
                dist += square(*itDesc1 - *itDesc2);
        }
        if (normalize_distances) dist /= this->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_(
                this->descriptors.LATCH.size() == oFeature.descriptors.LATCH.size());
        ASSERT_(
                this->descriptors.hasDescriptorLATCH() &&
                oFeature.descriptors.hasDescriptorLATCH())
 
        float dist = 0.0f;
        std::vector<unsigned char>::const_iterator itDesc1, itDesc2;
        for (itDesc1 = this->descriptors.LATCH.begin(),
                itDesc2 = oFeature.descriptors.LATCH.begin();
                 itDesc1 != this->descriptors.LATCH.end(); itDesc1++, itDesc2++)
        {
                dist += square(*itDesc1 - *itDesc2);
        }
        if (normalize_distances) dist /= this->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))
                THROW_EXCEPTION(
                        "[CFeature::saveToTextFile] ERROR: File could not be open for "
                        "writing");
 
        f.printf(
                "%5u %2d %7.3f %7.3f %6.2f %6.2f %2d %6.3f ", (unsigned int)this->ID,
                (int)this->get_type(), this->x, this->y, this->orientation, this->scale,
                (int)this->track_status, this->response);
 
        f.printf("%2d ", int(this->descriptors.hasDescriptorSIFT() ? 1 : 0));
        if (this->descriptors.hasDescriptorSIFT())
        {
                f.printf("%4d ", int(this->descriptors.SIFT.size()));
                for (unsigned int k = 0; k < this->descriptors.SIFT.size(); k++)
                        f.printf("%4d ", this->descriptors.SIFT[k]);
        }
 
        f.printf("%2d ", int(this->descriptors.hasDescriptorSURF() ? 1 : 0));
        if (this->descriptors.hasDescriptorSURF())
        {
                f.printf("%4d ", int(this->descriptors.SURF.size()));
                for (unsigned int k = 0; k < this->descriptors.SURF.size(); k++)
                        f.printf("%8.5f ", this->descriptors.SURF[k]);
        }
 
        f.printf("%2d ", int(this->descriptors.hasDescriptorMultiSIFT() ? 1 : 0));
        if (this->descriptors.hasDescriptorMultiSIFT())
        {
                for (int k = 0; k < int(this->multiScales.size()); ++k)
                {
                        for (int m = 0; m < int(this->multiOrientations[k].size()); ++m)
                        {
                                f.printf(
                                        "%.2f %6.2f ", this->multiScales[k],
                                        this->multiOrientations[k][m]);
                                f.printf(
                                        "%4d ",
                                        int(this->descriptors.multiSIFTDescriptors[k][m].size()));
                                for (unsigned int n = 0;
                                         n < this->descriptors.multiSIFTDescriptors[k][m].size();
                                         ++n)
                                        f.printf(
                                                "%4d ",
                                                this->descriptors.multiSIFTDescriptors[k][m][n]);
                        }
                }  // end-for
        }  // end-if
 
        f.printf("%2d ", int(this->descriptors.hasDescriptorORB() ? 1 : 0));
        if (this->descriptors.hasDescriptorORB())
                for (size_t k = 0; k < this->descriptors.ORB.size(); ++k)
                        f.printf("%d ", this->descriptors.ORB[k]);
 
        // # ADDED by Raghavender Sahdev
        f.printf("%2d ", int(this->descriptors.hasDescriptorBLD() ? 1 : 0));
        if (this->descriptors.hasDescriptorBLD())
        {
                f.printf("%4d ", int(this->descriptors.BLD.size()));
                for (unsigned int k = 0; k < this->descriptors.BLD.size(); k++)
                        f.printf("%4d ", this->descriptors.BLD[k]);
        }
 
        f.printf("%2d ", int(this->descriptors.hasDescriptorLATCH() ? 1 : 0));
        if (this->descriptors.hasDescriptorLATCH())
        {
                f.printf("%4d ", int(this->descriptors.LATCH.size()));
                for (unsigned int k = 0; k < this->descriptors.LATCH.size(); k++)
                        f.printf("%4d ", this->descriptors.LATCH[k]);
        }
 
        f.printf("\n");
        f.close();
 
        MRPT_END
}  // end saveToTextFile
 
/****************************************************
                           Class CFEATURELIST
*****************************************************/
// --------------------------------------------------
// CONSTRUCTOR
// --------------------------------------------------
CFeatureList::CFeatureList() {}  // end constructor
// --------------------------------------------------
// DESTRUCTOR
// --------------------------------------------------
CFeatureList::~CFeatureList() {}  // end destructor
// --------------------------------------------------
// 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))
                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 (CFeatureList::iterator it = this->begin(); it != this->end(); ++it)
        {
                f.printf(
                        "%5u %2d %7.3f %7.3f %6.2f %6.2f %2d %6.3f ",
                        (unsigned int)(*it)->ID, (int)(*it)->get_type(), (*it)->x, (*it)->y,
                        (*it)->orientation, (*it)->scale, (int)(*it)->track_status,
                        (*it)->response);
 
                f.printf("%2d ", int((*it)->descriptors.hasDescriptorSIFT() ? 1 : 0));
                if ((*it)->descriptors.hasDescriptorSIFT())
                {
                        f.printf("%4d ", int((*it)->descriptors.SIFT.size()));
                        for (unsigned int k = 0; k < (*it)->descriptors.SIFT.size(); k++)
                                f.printf("%4d ", (*it)->descriptors.SIFT[k]);
                }
 
                f.printf("%2d ", int((*it)->descriptors.hasDescriptorSURF() ? 1 : 0));
                if ((*it)->descriptors.hasDescriptorSURF())
                {
                        f.printf("%4d ", int((*it)->descriptors.SURF.size()));
                        for (unsigned int k = 0; k < (*it)->descriptors.SURF.size(); k++)
                                f.printf("%8.5f ", (*it)->descriptors.SURF[k]);
                }
                // # added by Raghavender Sahdev
                f.printf("%2d ", int((*it)->descriptors.hasDescriptorBLD() ? 1 : 0));
                if ((*it)->descriptors.hasDescriptorBLD())
                {
                        f.printf("%4d ", int((*it)->descriptors.BLD.size()));
                        for (unsigned int k = 0; k < (*it)->descriptors.BLD.size(); k++)
                                f.printf("%4d ", (*it)->descriptors.BLD[k]);
                }
 
                f.printf("%2d ", int((*it)->descriptors.hasDescriptorLATCH() ? 1 : 0));
                if ((*it)->descriptors.hasDescriptorLATCH())
                {
                        f.printf("%4d ", int((*it)->descriptors.LATCH.size()));
                        for (unsigned int k = 0; k < (*it)->descriptors.LATCH.size(); k++)
                                f.printf("%4d ", (*it)->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::utils::CTextFileLinesParser parser(filename);
        std::istringstream line;
 
        while (parser.getNextLine(line))
        {
                try
                {
                        CFeature::Ptr feat_ptr = std::make_shared<CFeature>();
                        CFeature* feat = feat_ptr.get();  // for faster access
 
                        int _ID;
                        if (!(line >> _ID)) throw std::string("ID");
                        feat->ID = TFeatureID(_ID);
 
                        int _type;
                        if (!(line >> _type)) throw std::string("type");
                        feat->type = TFeatureType(_type);
 
                        if (!(line >> feat->x >> feat->y)) throw std::string("x,y");
                        if (!(line >> feat->orientation)) throw std::string("orientation");
                        if (!(line >> feat->scale)) 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;
                                        feat->descriptors.SIFT[i] =
                                                val;  // DON'T read directly SIFT[i] since it's a
                                        // uint8_t, interpreted as a cha
                                }
 
                                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;  // comment copied from SIFT, DON'T read directly
                                        // SIFT[i] since it's a uint8_t, interpreted as a
                                        // cha
                                }
 
                                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;  // comment copied from SIFT, DON'T read directly
                                        // SIFT[i] since it's a uint8_t, interpreted as a
                                        // cha
                                }
 
                                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");
                        }
 
                        push_back(feat_ptr);
                }
                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)
{
        this->resize(otherList.size());
        CFeatureList::const_iterator it1;
        CFeatureList::iterator it2;
        for (it1 = otherList.begin(), it2 = this->begin(); it1 != otherList.end();
                 ++it1, ++it2)
        {
                *it2 = *it1;
                (*it2).reset(dynamic_cast<CFeature*>((*it2)->clone()));
        }
}  // end-copyListFrom
 
// --------------------------------------------------
// getByID()
// --------------------------------------------------
CFeature::Ptr CFeatureList::getByID(const TFeatureID& ID) const
{
        for (CFeatureList::const_iterator it = begin(); it != end(); ++it)
                if ((*it)->ID == ID) return (*it);
 
        return CFeature::Ptr();
}  // end getByID
 
// --------------------------------------------------
// getByID()
// --------------------------------------------------
CFeature::Ptr CFeatureList::getByID(const TFeatureID& ID, int& out_idx) const
{
        int k = 0;
        for (CFeatureList::const_iterator it = begin(); it != end(); ++it, ++k)
                if ((*it)->ID == ID)
                {
                        out_idx = k;
                        return (*it);
                }
        out_idx = -1;
        return CFeature::Ptr();
}  // end getByID
 
// --------------------------------------------------
// getByID()
// --------------------------------------------------
void CFeatureList::getByMultiIDs(
        const vector<TFeatureID>& IDs, vector<CFeature::Ptr>& out,
        vector<int>& outIndex) const
{
        out.clear();
        outIndex.clear();
        out.reserve(IDs.size());
        outIndex.reserve(IDs.size());
 
        for (int k = 0; k < int(IDs.size()); ++k)
        {
                int idx;
                CFeature::Ptr f = getByID(IDs[k], idx);
                out.push_back(f);
                outIndex.push_back(idx);
        }
}  // end getByID
 
// --------------------------------------------------
// nearest(x,y)
// --------------------------------------------------
CFeature::Ptr CFeatureList::nearest(
        const float x, const float y, double& dist_prev) const
{
        if (this->empty()) return CFeature::Ptr();
 
        float closest_x, closest_y;
        float closest_sqDist;
 
        // Look for the closest feature using KD-tree look up:
        const size_t closest_idx =
                this->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 CFeature::Ptr();
}  // end nearest
 
// --------------------------------------------------
// getMaxID()
// --------------------------------------------------
TFeatureID CFeatureList::getMaxID() const
{
        MRPT_START
        ASSERT_(!empty())
        vision::TFeatureID maxID = (*begin())->ID;
        for (CFeatureList::const_iterator itList = begin(); itList != end();
                 itList++)
                mrpt::utils::keep_max(maxID, (*itList)->ID);
        return maxID;
        MRPT_END
 
}  // end getMaxID()
 
/****************************************************
                  Class CMATCHEDFEATUREKLT
*****************************************************/
// --------------------------------------------------
// CONSTRUCTOR
// --------------------------------------------------
CMatchedFeatureList::CMatchedFeatureList() : m_leftMaxID(0), m_rightMaxID(0) {}
// --------------------------------------------------
// DESTRUCTOR
// --------------------------------------------------
CMatchedFeatureList::~CMatchedFeatureList() {}  // end destructor
// --------------------------------------------------
// saveToTextFile
// --------------------------------------------------
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;
 
        CMatchedFeatureList::iterator it;
        for (it = this->begin(); it != this->end(); it++)
        {
                os::fprintf(
                        f, "%d %.3f %.3f %d %.3f %.3f\n", (unsigned int)(*it->first).ID,
                        (*it->first).x, (*it->first).y, (unsigned int)(*it->second).ID,
                        (*it->second).x, (*it->second).y);
 
        }  // end for
        os::fclose(f);
}
 
// --------------------------------------------------
//                      getBothFeatureLists
// --------------------------------------------------
CFeature::Ptr CMatchedFeatureList::getByID(
        const TFeatureID& ID, const TListIdx& idx)
{
        CMatchedFeatureList::iterator it;
        for (it = begin(); it != end(); ++it)
        {
                CFeature::Ptr feat = (idx == firstList) ? it->first : it->second;
                if (feat->ID == ID) return feat;
        }
        return CFeature::Ptr();
}
 
// --------------------------------------------------
// updateMaxID()
// --------------------------------------------------
void CMatchedFeatureList::updateMaxID(const TListIdx& idx)
{
        MRPT_START
        TFeatureID maxID1 = begin()->first->ID;
        TFeatureID maxID2 = begin()->second->ID;
        for (CMatchedFeatureList::const_iterator itList = begin(); itList != end();
                 itList++)
        {
                if (idx == firstList || idx == bothLists)
                        mrpt::utils::keep_max(maxID1, itList->first->ID);
                if (idx == secondList || idx == bothLists)
                        mrpt::utils::keep_max(maxID2, itList->second->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(this->size());
        list2.resize(this->size());
 
        unsigned int k = 0;
        for (CMatchedFeatureList::iterator it = this->begin(); it != this->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);
                std::vector<float>::const_iterator 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;
}