descriptor_pairing.h

Go to the documentation of this file.

/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2014, 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
        {
                using mrpt::math::TPose3D;
                using mrpt::math::TPoint3D;
                using mrpt::utils::TCamera;

                /** \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<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 NULL if not needed
                  *     &pairings_1_to_2,         // Can be set to NULL 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<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!=NULL || pairings_1_to_2!=NULL)

                        typedef typename DESCRIPTOR_KDTREE::kdtree_t::ElementType   KDTreeElementType; // The expected data type of elements for the kd-tree
                        typedef typename DESCRIPTOR_KDTREE::kdtree_t::DistanceType  KDTreeDistanceType; 

                        const size_t N=feats_img1.size();
                        if (pairings_1_to_multi_2) pairings_1_to_multi_2->assign(N, 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
                }

                /** @} */

        }
}
#endif