descriptor_pairing.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 |
   +------------------------------------------------------------------------+ */
 
#ifndef mrpt_vision_descriptor_pairing_H
#define mrpt_vision_descriptor_pairing_H
 
#include <mrpt/vision/types.h>
 
namespace mrpt
{
namespace vision
{
/** \addtogroup  mrptvision_features
        @{ */
 
/** Search for pairings between two sets of visual descriptors (for now, only
 * SURF
 *   and SIFT features are considered).
 *  Pairings are returned in one of two formats (or both of them
 * simultaneously),
 *   depending on the non-NULL output containers present in the call.
 *
 * \code
 *  CFeatureList  feats1, feats2;
 *  // Populate feature lists [...]
 *
 *  // Create kd-tree for SIFT features of "feats2":
 *  TSIFTDescriptorsKDTreeIndex<double>  feats2_kdtree(feats2);
 *
 *  // Search correspondences:
 *  std::vector<std::vector<size_t>>             pairings_1_to_multi_2;
 *  std::vector<std::pair<size_t,size_t> > pairings_1_to_2;
 *  mrpt::vision::find_descriptor_pairings(
 *     &pairings_1_to_multi_2,   // Can be set to nullptr if not needed
 *     &pairings_1_to_2,         // Can be set to nullptr if not needed
 *     feats1, feats2_kdtree,    // The two sets of features
 *     mrpt::vision::descSIFT    // Select descriptor to use
 *     // [further optional params]
 *     );
 * \endcode
 *
 * \sa TSIFTDescriptorsKDTreeIndex, TSURFDescriptorsKDTreeIndex
 */
template <class DESCRIPTOR_KDTREE>
size_t find_descriptor_pairings(
        std::vector<std::vector<size_t>>* pairings_1_to_multi_2,
        std::vector<std::pair<size_t, size_t>>* pairings_1_to_2,
        const CFeatureList& feats_img1, const DESCRIPTOR_KDTREE& feats_img2_kdtree,
        const mrpt::vision::TDescriptorType descriptor = descSIFT,
        const size_t max_neighbors = 4, const double max_relative_distance = 1.2,
        const typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType max_distance =
                std::numeric_limits<
                        typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType>::max())
{
        MRPT_START
        ASSERT_ABOVEEQ_(max_neighbors, 1);
        ASSERT_(pairings_1_to_multi_2 != nullptr || pairings_1_to_2 != nullptr);
 
        /** The expected data type of elements for the kd-tree*/
        using KDTreeElementType = typename DESCRIPTOR_KDTREE::kdtree_t::ElementType;
        using KDTreeDistanceType =
                typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType;
 
        const size_t N = feats_img1.size();
        if (pairings_1_to_multi_2)
                pairings_1_to_multi_2->assign(
                        N, std::vector<size_t>());  // Reset output container
        if (pairings_1_to_2)
        {
                pairings_1_to_2->clear();
                pairings_1_to_2->reserve(N);
        }
 
        size_t overall_pairs = 0;
 
        if (!N) return overall_pairs;  // No features -> nothing to do
 
        if (descriptor == descSIFT)
        {
                ASSERTMSG_(
                        feats_img1[0]->descriptors.hasDescriptorSIFT(),
                        "Request to match SIFT features but feats_img1 has no SIFT "
                        "descriptors!");
                ASSERTMSG_(
                        sizeof(KDTreeElementType) ==
                                sizeof(feats_img1[0]->descriptors.SIFT[0]),
                        "Incorrect data type kd_tree::ElementType for SIFT (should be "
                        "uint8_t)");
        }
        else if (descriptor == descSURF)
        {
                ASSERTMSG_(
                        feats_img1[0]->descriptors.hasDescriptorSURF(),
                        "Request to match SURF features but feats_img1 has no SURF "
                        "descriptors!");
                ASSERTMSG_(
                        sizeof(KDTreeElementType) ==
                                sizeof(feats_img1[0]->descriptors.SURF[0]),
                        "Incorrect data type kd_tree::ElementType for SURF (should be "
                        "float)");
        }
        else
        {
                THROW_EXCEPTION(
                        "This function only supports SIFT or SURFT descriptors");
        }
 
        std::vector<size_t> indices(max_neighbors);
        std::vector<double> distances(max_neighbors);
 
        for (size_t i = 0; i < N; i++)
        {
                const CFeature::TDescriptors& descs = feats_img1[i]->descriptors;
 
                const void* ptr_query;
                if (descriptor == descSIFT)
                        ptr_query = &descs.SIFT[0];
                else if (descriptor == descSURF)
                        ptr_query = &descs.SURF[0];
 
                feats_img2_kdtree.get_kdtree().knnSearch(
                        static_cast<const KDTreeElementType*>(ptr_query),  // Query point
                        max_neighbors,  // Number of neigbors
                        &indices[0], &distances[0]  // Output
                );
 
                // Include all correspondences below the absolute and the relative
                // threshold (indices comes ordered by distances):
                const KDTreeDistanceType this_thresh =
                        std::min(max_relative_distance * distances[0], max_distance);
                for (size_t j = 0; j < max_neighbors; j++)
                {
                        if (distances[j] <= this_thresh)
                        {
                                overall_pairs++;
                                if (pairings_1_to_multi_2)
                                        (*pairings_1_to_multi_2)[i].push_back(indices[j]);
                                if (pairings_1_to_2)
                                        pairings_1_to_2->push_back(std::make_pair(i, indices[j]));
                        }
                        else
                                break;
                }
        }
        return overall_pairs;
        MRPT_END
}
 
/** @} */
}  // namespace vision
}  // namespace mrpt
#endif