Max-Clique Approximation cv::Mat summation

edit

This code does just what you want and is fairly efficient. I welcome any feedback on making it more efficient and more general. To be clear, this finds the max-clique seeded by the node with the maximum degree. This also assumes that ptsA and ptsB have identical scale, otherwise, the distance threshold isn't meaningful (and yes, it could be an argument to the function).

template< typename T>
static std::vector<int> getMaxClique( const std::vector<T>& ptsA, const std::vector<T>& ptsB )
{
    Timer::Instance().start( "getMaxClique" );

    assert( ptsA.size() == ptsB.size() );

    const int32_t numMatches = ptsA.size();

    cv::Mat  consistencyMatrix    = cv::Mat::zeros( numMatches, numMatches, CV_8U );
    uint8_t* consistencyMatrixPtr = consistencyMatrix.ptr<uint8_t>();

    //TODO: Make this threshold adaptive to quadratic depth error
    static const double distanceThreshold = 5.0;

    std::vector<int> nodeDegrees( numMatches, 0 );

    for( int i = 0; i < numMatches; ++i )
    {
        for( int j = i+1, index = (i*numMatches)+j; j < numMatches; ++j, ++index )
        {
            const double absDistance = abs( cv::norm( ptsA[i] - ptsA[j] ) -
                                            cv::norm( ptsB[i] - ptsB[j] ) );

            if( absDistance <= distanceThreshold )
            {
                consistencyMatrixPtr[index] = 1;
                ++nodeDegrees[i];
                ++nodeDegrees[j];
            }
        }
    }

    // Fnd the largest set of mutually consistent matches:
    //
    // This is equivalent to finding the maximum clique on a graph with
    // adjacency matrix W. Since the maximum clique problem is known to
    // be NP-complete, we use the following sub-optimal algorithm:
    //
    // 1) Initialize the clique to contain the match with the largest
    //    number of consistent matches (i.e., choose the node with the
    //    maximum degree).
    // 2) Find the set of matches compatible with all the matches already
    //    in the clique.
    // 3) Add the match with the largest number consistent matches.
    //
    // Repeat (2) and (3) until the set of compatible matches is empty.

    const int maxNodeIndex = std::distance( nodeDegrees.begin(),
                                            std::max_element( nodeDegrees.begin(), nodeDegrees.end() ) );

    // We need to make this matrix not just upper triangular and so we
    // must 'complete' the Consistency Matrix:
    consistencyMatrix += consistencyMatrix.t();

    std::vector<int> candidates = consistencyMatrix.row( maxNodeIndex );

    std::vector<int> candidatesIndices;
    candidatesIndices.reserve( nodeDegrees[ maxNodeIndex ] );

    for( int i = 0; i < numMatches; ++i )
    {
        if( candidates[i] > 0 )
        {
            candidatesIndices.push_back( i );
        }
    }

    std::vector<int> maxClique;
    maxClique.reserve( nodeDegrees[ maxNodeIndex ] );
    maxClique.push_back( maxNodeIndex );

    while( !candidatesIndices.empty() )
    {
        // Find the candidate with largest 'consistent' degree:
        int maxIndex  = -1;
        int maxDegree = -1;
        for( int i = 0; i < candidatesIndices.size(); ++i )
        {
            const int degree = cv::countNonZero( consistencyMatrix.row( candidatesIndices[i] ) );

            if( degree > maxDegree )
            {
                maxIndex  = candidatesIndices[i];
                maxDegree = degree;
            }
        }

        maxClique.push_back( maxIndex );

        // New clique addition at consistencyMatrix(maxIndex,maxIndex) is now and
        // always zero, so it'll be erased:
        candidatesIndices.erase( std::remove_if( candidatesIndices.begin(), candidatesIndices.end(), 
                                                 [=]( const int index )
                                                 {
                                                    return consistencyMatrixPtr[ index*numMatches + maxIndex ] == 0;
                                                 } ),
                                 std::end( candidatesIndices ) );

    }

    Timer::Instance().end( "getMaxClique" );

    return maxClique;
}