OdometryUtils

gistfile1.txt

template <typename T>
cv::Mat calculateDistanceAndErrorMatrix( const std::vector<T>& pts,
                                         const float focalLength,
                                         const float baseline,
                                         const float pixelError )
{
  const float& f   = focalLength;  // focal length in pixels
  const float& t   = baseline;     // baseline in same units as points
  const float& d_e = pixelError;   // delta e = image error (in pixels)

  const size_t numMatches = pts.size();

  //
  // de = distance and error
  //
  cv::Mat de = cv::Mat::zeros( numMatches, numMatches, CV_32FC2 );

  auto dePtr = de.ptr<cv::Vec2f>();

  //
  // Calculate distance and error for point set:
  //
  for( int i = 0; i < numMatches; ++i )
  {
    const float x1 = pts[i].x;
    const float y1 = pts[i].y;
    const float z1 = pts[i].z;

    for( int j = i+1, index = (i*numMatches)+j; j < numMatches; ++j, ++index )
    {
      const float x2 = pts[j].x;
      const float y2 = pts[j].y;
      const float z2 = pts[j].z;

      const float x12 = x1-x2;
      const float y12 = y1-y2;
      const float z12 = z1-z2;

      // L = Length (distance between points 1 and 2)
      const float L = sqrt( (x12*x12) + (y12*y12) +(z12*z12) );

      const float a = (x12*(t-x1)) - (y12*y1) - (z12*z1);
      const float A = a*a;

      const float b = (x12*x1) + (y12*y1) + (z12*z1);
      const float B = b*b;

      const float C = 0.5 * (t*y12) * (t*y12);

      const float d = (x12*(t-x2)) - (y12*y2) - (z12*z2);
      const float D = d*d;

      const float e = (x12*x2) + (y12*y2) + (z12*z2);
      const float E = e*e;

      const float F = C;

      const float L_e = ( d_e / (L*f*t) ) * sqrt( (z1*z1)*(A+B+C) +
                                                  (z2*z2)*(D+E+F) );

      dePtr[index][0] = L;
      dePtr[index][1] = L_e;
    }
  }

  return de;
}


//
// This algorithm calculates an approximation of a max-clique.
//
// TODO:
//
// 1) Implement angle contraint (see notes below)
//
// Fast, Unconstrained Camera Motion Estimation from Stereo without Tracking
// and Robust Statistics by Heiko Hirschmuller
//
// A property of rigid transformations in Euclidean space is that the relative
// distances between transformed points are maintained. Thus,
// |Pi-Pk| = |Ci-Ck| with i,k=1..n (2)
//
// Another constraint can be derived if the rotation R is restricted. Every
// rotation in 3-D can be represented by a rotation axis r and a rotation
// angle φ. If the vector Pi-Pk is orthogonal to r, then the angle between
// Pi-Pk and Ci-Ck is exactly φ. If Pi-Pk is parallel to r, then the angle
// between Pi-Pk and Ci-Ck is always zero. In all other cases, the angle is
// in between 0 and φ.
// The rotation axis r is unknown as well as the angle φ. However, imposing
// the restriction φ < θ results in the in-equality,
//
// (Pi-Pk)(Ci-Ck) > cos θ with i,k=1..n (3)
//
// For this study, θ was set to π/4 , which is a very low restriction on
// camera movement, since the camera may rotate by up to 45 degrees between
// two consecutive images. Still, this additional constraint is useful to
// identify outliers.
//
template <typename T>
std::vector<int> getMaxClique( const std::vector<T>& pts1,
                               const std::vector<T>& pts2,
                               const float focalLength,
                               const float baseline,
                               const float pixelError,
                               const double numStdDev )
{
  assert( pts1.size() == pts2.size() );

  const int32_t numMatches = pts1.size();

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

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

  cv::Mat de1 = calculateDistanceAndErrorMatrix( pts1, focalLength, baseline, pixelError );
  cv::Mat de2 = calculateDistanceAndErrorMatrix( pts2, focalLength, baseline, pixelError );

  auto de1Ptr = de1.ptr<cv::Vec2f>();
  auto de2Ptr = de2.ptr<cv::Vec2f>();

  //
  // Upper triangular:
  //
  for( int i = 0; i < numMatches; ++i )
  {
    for( int j = i+1, index = (i*numMatches)+j; j < numMatches; ++j, ++index )
    {
      const float L1   = de1Ptr[index][0];
      const float L1_e = de1Ptr[index][1];
      const float L2   = de2Ptr[index][0];
      const float L2_e = de2Ptr[index][1];

      //
      // sigma gating test
      //
      if( std::fabs( L1 - L2 ) <= numStdDev * sqrt( (L1_e*L1_e) + (L2_e*L2_e) ) )
      {
        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( const auto& candidateIndex : candidatesIndices )
    {
      const int degree = cv::countNonZero( consistencyMatrix.row( candidateIndex ) );

      if( degree > maxDegree )
      {
        maxIndex  = candidateIndex;
        maxDegree = degree;
      }
    }

    maxClique.push_back( maxIndex );

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

  return maxClique;
}