OpenCV Keypoint Detection and Matching

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(vfc)


find_package(OpenCV REQUIRED)
include_directories(
  ${OpenCV_INCLUDE_DIRS}
)

add_executable( keypoints keypoints.cpp )
add_executable( matcher matcher.cpp )
add_executable( robust_matcher vfc.cpp robust_matcher.cpp )


target_link_libraries( keypoints  ${OpenCV_LIBRARIES} )
target_link_libraries( matcher  ${OpenCV_LIBRARIES} )
target_link_libraries( robust_matcher  ${OpenCV_LIBRARIES} )

keypoints.cpp

/*
    Reads Images and Detect ORB keypoints
    
    Author : Manohar Kuse <mpkuse@connect.ust.hk>
    Released in Public Domain
*/

#include <iostream>
#include <vector>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

using namespace std;

int main()
{
  //
  // Load Images
  cv::Mat im = cv::imread( "../image/church1.jpg");
  cv::imshow( "win", im );
  cv::waitKey(0);


  //
  // Feature Detector - ORB
  cv::Ptr<cv::Feature2D> fdetector = cv::ORB::create();
  std::vector<cv::KeyPoint> keypoints;
  cv::Mat descriptors;
  fdetector->detectAndCompute(im, cv::Mat(), keypoints, descriptors);
  cout << "# of keypoints : "<< keypoints.size() << endl;
  cout << "descriptors shape : "<< descriptors.rows << "x" << descriptors.cols << endl;


  //
  // Draw Keypoints
  cv::Mat outImage;
  cv::drawKeypoints(im, keypoints, outImage );
  cv::imshow( "win-keys", outImage );
  cv::waitKey(0);


  return 0;
}

keypts.py

"""
    a) Loads Image
    b) Detect Keypoints & Descriptor
    c) Matches
"""

import cv2
cv2.ocl.setUseOpenCL(False)
import numpy as np
import time
#
# Load Images

im1 = cv2.imread( '../image/church1.jpg')
im2 = cv2.imread( '../image/church2.jpg')
startTime = time.time()
#
# ORB Feature Detector
#orb =  cv2.DescriptorExtractor_create("BRIEF") #cv2.AKAZE_create() #Need opencv3. some bug with python binding of orb detector.
orb = cv2.ORB_create()
kp1, des1 = orb.detectAndCompute(im1, None)
kp2, des2 = orb.detectAndCompute(im2, None)
print 'len(kp1) : ', len(kp1), '    des1.shape : ', des1.shape
print 'len(kp2) : ', len(kp2), '    des2.shape : ', des2.shape
print 'Time (ms) : ', (time.time() - startTime)*1000.

#
# Draw Keypoints
#im1_keypts = cv2.drawKeypoints(im1, kp1, None )
#im2_keypts = cv2.drawKeypoints(im2, kp2, None )
#cv2.imshow('keypts1', im1_keypts)
#cv2.imshow('keypts2', im2_keypts)


#
# Matcher
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=False)
matches_bf = bf.match(des1,des2)

#
# FLANN Matcher
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
search_params = dict(checks=50)   # or pass empty dictionary
flann = cv2.FlannBasedMatcher(index_params,search_params)
matches = flann.knnMatch(des1.astype('float32'),des2.astype('float32'),k=1)
matches = sum( matches, [] )



print 'Time (ms) : ', (time.time() - startTime)*1000.
#
# Draw Matches
im_matches = cv2.drawMatches(im1,kp1,  im2,kp2,  matches, None)
cv2.imshow( 'matches', im_matches)
cv2.waitKey(0)

matcher.cpp

/*
  Reads 2 images, detects keypoints, compute ORB descriptors at these keypoints, 
  matches the descriptors by nearest neighbour (brute force)
  
  Author : Manohar Kuse <mpkuse@connect.ust.hk>
  Released in Public Domain
*/

#include <iostream>
#include <vector>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

using namespace std;

int main()
{
  //
  // Load Images
  cv::Mat im1 = cv::imread( "../image/church1.jpg");
  cv::Mat im2 = cv::imread( "../image/church2.jpg");
  cv::imshow( "im1", im1 );
  cv::imshow( "im2", im2 );
  cv::waitKey(0);


  //
  // Feature Detector
  cv::Ptr<cv::Feature2D> fdetector = cv::ORB::create();
  std::vector<cv::KeyPoint> keypoints1, keypoints2;
  cv::Mat descriptors1, descriptors2;
  fdetector->detectAndCompute(im1, cv::Mat(), keypoints1, descriptors1);
  fdetector->detectAndCompute(im2, cv::Mat(), keypoints2, descriptors2);
  cout << "# of keypoints : "<< keypoints1.size() << endl;
  cout << "# of keypoints : "<< keypoints2.size() << endl;
  cout << "descriptors shape : "<< descriptors1.rows << "x" << descriptors1.cols << endl;
  cout << "descriptors shape : "<< descriptors2.rows << "x" << descriptors2.cols << endl;


  //
  // Draw Keypoints
  cv::Mat outImage1, outImage2;
  cv::drawKeypoints(im1, keypoints1, outImage1 );
  cv::drawKeypoints(im2, keypoints2, outImage2 );
  cv::imshow( "win-keys1", outImage1 );
  cv::imshow( "win-keys2", outImage2 );
  cv::waitKey(0);

  //
  // Matcher - Brute Force
  cv::BFMatcher matcher(cv::NORM_L2);
  std::vector< cv::DMatch > matches;
  matcher.match(descriptors1, descriptors2, matches);

  /* - Consider using an FLANN based matches for speed
  // Matcher - FLANN (Approx NN)
  if(descriptors1.type()!=CV_32F)
  {
    descriptors1.convertTo(descriptors1, CV_32F);
    descriptors2.convertTo(descriptors2, CV_32F);
  }
  cv::FlannBasedMatcher matcher;
  std::vector< cv::DMatch > matches;
  matcher.match( descriptors1, descriptors2, matches );
  */

  //
  // Draw Matches
  cv::Mat outImg;
  cv::drawMatches(im1, keypoints1, im2, keypoints2, matches, outImg );
  cv::imshow( "matches", outImg );
  cv::waitKey(0);

  return 0;
}

robust_matcher.cpp

/*
  a) Load 2 Images
  b) Detect ORB Keypoints for both images
  c) Compute ORB Descriptors
  d) FLANN Matcher
  e) Vector Field Consensus (VFC) for filtering false matches
  
  Note: 
    Depends on vfc.h and vfc.cpp
   
  Reference
    [1] Jiayi Ma, Ji Zhao, Jinwen Tian, Alan Yuille, and Zhuowen Tu.
        Robust Point Matching via Vector Field Consensus,
        IEEE Transactions on Image Processing, 23(4), pp. 1706-1721, 2014
    [2] Jiayi Ma, Ji Zhao, Jinwen Tian, Xiang Bai, and Zhuowen Tu.
        Regularized Vector Field Learning with Sparse Approximation for Mismatch Removal,
        Pattern Recognition, 46(12), pp. 3519-3532, 2013
        
    Author : Manohar Kuse <mpkuse@connect.ust.hk>
    Released in Public Domain
*/        
        
#include <iostream>
#include <vector>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "vfc.h"

using namespace std;

int main()
{
  //
  // Load Images
  cv::Mat im1 = cv::imread( "../image/church1.jpg");
  cv::Mat im2 = cv::imread( "../image/church2.jpg");

  //
  // Feature Detector
  cv::Ptr<cv::Feature2D> fdetector = cv::ORB::create();
  std::vector<cv::KeyPoint> keypoints1, keypoints2;
  cv::Mat descriptors1, descriptors2;
  fdetector->detectAndCompute(im1, cv::Mat(), keypoints1, descriptors1);
  fdetector->detectAndCompute(im2, cv::Mat(), keypoints2, descriptors2);
  cout << "# of keypoints : "<< keypoints1.size() << endl;
  cout << "# of keypoints : "<< keypoints2.size() << endl;
  cout << "descriptors shape : "<< descriptors1.rows << "x" << descriptors1.cols << endl;
  cout << "descriptors shape : "<< descriptors2.rows << "x" << descriptors2.cols << endl;


  // Matcher - FLAN (Approx NN)
  if(descriptors1.type()!=CV_32F)
  {
    descriptors1.convertTo(descriptors1, CV_32F);
    descriptors2.convertTo(descriptors2, CV_32F);
  }
  cv::FlannBasedMatcher matcher;
  std::vector< cv::DMatch > matches;
  matcher.match( descriptors1, descriptors2, matches );

  //
  // Draw Matches
  cv::Mat outImg;
  cv::drawMatches(im1, keypoints1, im2, keypoints2, matches, outImg );
  cv::imshow( "Raw Matches", outImg );

  //
  // Filter Matches with Vector Field consensus (VFC)
  // a - preprocess data format
	vector<Point2f> X;
	vector<Point2f> Y;
	X.clear();
	Y.clear();
	for (unsigned int i = 0; i < matches.size(); i++) {
		int idx1 = matches[i].queryIdx;
		int idx2 = matches[i].trainIdx;
		X.push_back(keypoints1[idx1].pt);
		Y.push_back(keypoints2[idx2].pt);
	}


  // b - main - vfc
  double t = (double)getTickCount();
	VFC myvfc;
	myvfc.setData(X, Y);
	myvfc.optimize();
	vector<int> matchIdx = myvfc.obtainCorrectMatch();
	t = 1000 * ((double)getTickCount() - t) / getTickFrequency();
	cout << "Times (ms): " << t << endl;

  // c - post process
  std::vector< DMatch > correctMatches;
  std::vector<KeyPoint> correctKeypoints1, correctKeypoints2;
  correctMatches.clear();
  for (unsigned int i = 0; i < matchIdx.size(); i++) {
    int idx = matchIdx[i];
    correctMatches.push_back(matches[idx]);
    correctKeypoints1.push_back(keypoints1[idx]);
    correctKeypoints2.push_back(keypoints2[idx]);
  }

  //
  // Draw Corrected Matches
  Mat img_correctMatches;
	drawMatches(im1, keypoints1, im2, keypoints2, correctMatches, img_correctMatches);
  imshow("Detected Correct Matches", img_correctMatches);


  cv::waitKey(0);
  return 0;
}

        

vfc.cpp

#include "vfc.h"
// Mismatch removal by vector field consensus (VFC)
// Author: Ji Zhao
// Date:   01/25/2015
// Email:  zhaoji84@gmail.com
//
// Reference
// [1] Jiayi Ma, Ji Zhao, Jinwen Tian, Alan Yuille, and Zhuowen Tu.
//     Robust Point Matching via Vector Field Consensus,
//     IEEE Transactions on Image Processing, 23(4), pp. 1706-1721, 2014
// [2] Jiayi Ma, Ji Zhao, Jinwen Tian, Xiang Bai, and Zhuowen Tu.
//     Regularized Vector Field Learning with Sparse Approximation for Mismatch Removal,
//     Pattern Recognition, 46(12), pp. 3519-3532, 2013

VFC::VFC() {
	_lX.clear();
	_rX.clear();
	_X.clear();
	_Y.clear();
	_V.clear();
	_C.clear();
	_P.clear();
	_ctrlPts.clear();

	_sumP = 0;
	_sigma2 = 0;
	_E = 1;
	_traceCKC = 0;

	_numPt = 0;
	_numDim = 2;
	_numElement = 0;

	// set the default method
	//_method = NORMAL_VFC;
	//_method = FAST_VFC;
	_method = SPARSE_VFC;

	_maxIter = 50;
	_gamma = 0.9f;
	_beta = 0.1f;
	_lambda = 3.0f;
	_theta = 0.75f;
	_a = 10.0f;
	_ecr = 1e-5f;
	_minP = 1e-5f;

	_numEig = 1;
	_traceCQSQC = 0;

	_numCtrlPts = 16;
}

VFC::~VFC() {

}

bool VFC::setData(vector<Point2f> X1, vector<Point2f> X2) {
	if (X1.size() < MIN_POINT_NUMBER || X2.size() < MIN_POINT_NUMBER || X1.size() != X2.size())
		return 0;
	_numPt = X1.size();
	_numElement = _numPt * _numDim;
	_matchIdx.clear();
	for (int i = 0; i < _numPt; i++) {
		_lX.push_back(X1[i]);
		_rX.push_back(X2[i]);
		_matchIdx.push_back(i);
	}
	return 1;
}

vector<int> VFC::obtainCorrectMatch() {
	return _matchIdx;
}

void VFC::optimize() {
	if (!normalize())
		return;

	if (_method == NORMAL_VFC)
		optimizeVFC();
	else if (_method == FAST_VFC)
		optimizeFastVFC();
	else if (_method == SPARSE_VFC)
		optimizeSparseVFC();

#ifdef PRINT_RESULTS
	cout << "Removing outliers succesfully completed." << endl
		<< "number of detected matches: " << setw(3) << _matchIdx.size() << endl;
#endif
}

void VFC::optimizeSparseVFC() {
	_numCtrlPts = min(_numCtrlPts, _numPt);
	selectSubset();
	_K = constructIntraKernel(_ctrlPts);
	_U = constructInterKernel(_ctrlPts, _X);

	initialize();
	int iter = 0;
	float tecr = 1;
	float E_old = float(inf);

	while (iter < _maxIter && tecr > _ecr && _sigma2 > 1e-8) {
		// E-step
		E_old = _E;
		getP();
		calculateTraceCKC();
		_E += _lambda / 2 * _traceCKC;
		tecr = abs((_E - E_old) / _E);
#ifdef PRINT_RESULTS
		cout << "# " << setw(3) << iter
			<< ", gamma: " << setw(5) << _gamma
			<< ", E change rate: " << setw(5) << tecr
			<< ", sigma2: " << setw(5) << _sigma2 << endl;
#endif
		// M-step. Solve linear system for C.
		calculateC_SparseVFC();
		// Update V and sigma^2
		calculateV();
		calculateSigmaSquare();
		// Update gamma
		calculateGamma();
		iter++;
	}
}

bool VFC::selectSubset() {
	_numCtrlPts = min(_numCtrlPts, _numPt);
	_ctrlPts.clear();
	int cnt = 0;
	int iter = 0;
	while (cnt < _numCtrlPts && iter < _numCtrlPts*3){
		int idx = (rand() % _numPt);
		float dist = float(inf);
		for (unsigned int i = 0; i < _ctrlPts.size(); i++){
			float tmp = fabs(_ctrlPts[i].x - _X[idx].x) + fabs(_ctrlPts[i].y - _X[idx].y);
			dist = min(tmp, dist);
		}
		if (dist>1e-3) {
			_ctrlPts.push_back(_X[idx]);
			cnt++;
		}
		iter++;
	}
	_numCtrlPts = cnt;
	return (_numCtrlPts > MIN_POINT_NUMBER);
}

void VFC::calculateC_SparseVFC() {
	Mat K(_numCtrlPts, _numCtrlPts, CV_32FC1);
	for (int i = 0; i < _numCtrlPts; i++) {
		for (int j = i; j < _numCtrlPts; j++) {
			float tmp = 0;
			float* p = _U.ptr<float>(i);
			float* q = _U.ptr<float>(j);
			for (int k = 0; k < _numPt; k++) {
				tmp += _P[k] * p[k] * q[k];
			}
			tmp += _lambda * _sigma2 * _K.at<float>(i, j);
			K.at<float>(i, j) = tmp;
			K.at<float>(j, i) = tmp;
		}
	}
	Mat Y(_numCtrlPts, 2, CV_32FC1);
	for (int i = 0; i < _numCtrlPts; i++) {
		float tmp1 = 0;
		float tmp2 = 0;
		float* p = _U.ptr<float>(i);
		for (int k = 0; k < _numPt; k++) {
			float t = _P[k] * p[k];
			tmp1 += t * _Y[k].x;
			tmp2 += t * _Y[k].y;
		}
		Y.at<float>(i, 0) = tmp1;
		Y.at<float>(i, 1) = tmp2;
	}
	Mat C;
	solve(K, Y, C, DECOMP_LU);
	_C.clear();
	for (int i = 0; i < _numCtrlPts; i++) {
		float t1 = C.at<float>(i, 0);
		float t2 = C.at<float>(i, 1);
		_C.push_back(Point2f(t1, t2));
	}
}


void VFC::optimizeFastVFC() {
	_K = constructIntraKernel(_X);
	_numEig = static_cast<int>( sqrt(_numPt) + 0.5f );
	//eigen(_K, _S, _Q, -1, -1); //the last two parameters are ignored in the current opencv
	eigen(_K, _S, _Q );

	initialize();
	int iter = 0;
	float tecr = 1;
	float E_old = float(inf);

	while (iter < _maxIter && tecr > _ecr && _sigma2 > 1e-8) {
		// E-step
		E_old = _E;
		getP();
		calculateTraceCQSQC();
		_E += _lambda / 2 * _traceCQSQC;
		tecr = abs((_E - E_old) / _E);
#ifdef PRINT_RESULTS
		cout << "# " << setw(3) << iter
			<< ", gamma: " << setw(5) << _gamma
			<< ", E change rate: " << setw(5) << tecr
			<< ", sigma2: " << setw(5) << _sigma2 << endl;
#endif
		// M-step. Solve linear system for C.
		calculateCFastVFC();
		// Update V and sigma^2
		calculateV();
		calculateSigmaSquare();
		// Update gamma
		calculateGamma();
		iter++;
	}
}

void VFC::calculateTraceCQSQC() {
	_traceCQSQC = 0;
	for (int i = 0; i < _numEig; i++) {
		float t1 = 0;
		float t2 = 0;
		float* t = _Q.ptr<float>(i);
		for (int j = 0; j < _numPt; j++) {
			t1 += t[j] * _C[j].x;
			t2 += t[j] * _C[j].y;
		}
		_traceCQSQC += t1*t1*_S.at<float>(i, 0);
		_traceCQSQC += t2*t2*_S.at<float>(i, 0);
	}
}

void VFC::calculateCFastVFC() {
	Mat dPQ(_numEig, _numPt, CV_32FC1);
	Mat F(2, _numPt, CV_32FC1);
	// dP = spdiags(P,0,N,N); dPQ = dP*Q;
	for (int i = 0; i < _numEig; i++) {
		float* p = dPQ.ptr<float>(i);
		float* q = _Q.ptr<float>(i);
		for (int j = 0; j < _numPt; j++) {
			p[j] = _P[j] * q[j];
		}
	}
	// F = dP*Y;
	float* p1 = F.ptr<float>(0);
	float* p2 = F.ptr<float>(1);
	for (int j = 0; j < _numPt; j++) {
		p1[j] = _P[j] * _Y[j].x;
		p2[j] = _P[j] * _Y[j].y;
	}

	Mat QF(_numEig, 2, CV_32FC1);
	for (int i = 0; i < _numEig; i++) {
		float* q = _Q.ptr<float>(i);
		float t1 = 0;
		float t2 = 0;
		for (int j = 0; j < _numPt; j++) {
			t1 += q[j] * p1[j];
			t2 += q[j] * p2[j];
		}
		QF.at<float>(i, 0) = t1;
		QF.at<float>(i, 1) = t2;
	}

	// Q'*dPQ
	Mat K(_numEig, _numEig, CV_32FC1);
	for (int i = 0; i < _numEig; i++) {
		for (int j = i; j < _numEig; j++) {
			float tmp = 0;
			float* p = dPQ.ptr<float>(i);
			float* q = _Q.ptr<float>(j);
			for (int k = 0; k < _numPt; k++) {
				tmp += p[k] * q[k];
			}
			if (i != j) {
				K.at<float>(i, j) = tmp;
				K.at<float>(j, i) = tmp;
			}
			else {
				K.at<float>(i, i) = tmp + _lambda * _sigma2 / _S.at<float>(i, 0);
			}
		}
	}

	Mat T;
	solve(K, QF, T, DECOMP_LU);
	//Mat C(2, _numPt, CV_32FC1);
	Mat C = F - (T.t() * dPQ);
	C = C / (_lambda*_sigma2);

	_C.clear();
	for (int i = 0; i < _numPt; i++) {
		float t1 = C.at<float>(0, i);
		float t2 = C.at<float>(1, i);
		_C.push_back(Point2f(t1, t2));
	}
}


void VFC::optimizeVFC() {
	_K = constructIntraKernel(_X);
	initialize();
	int iter = 0;
	float tecr = 1;
	float E_old = float(inf);

	while (iter < _maxIter && tecr > _ecr && _sigma2 > 1e-8) {
		// E-step
		E_old = _E;
		getP();
		calculateTraceCKC();
		_E += _lambda / 2 * _traceCKC;
		tecr = abs((_E - E_old) / _E);
#ifdef PRINT_RESULTS
		cout << "# " << setw(3) << iter
			<< ", gamma: " << setw(5) << _gamma
			<< ", E change rate: " << setw(5) << tecr
			<< ", sigma2: " << setw(5) << _sigma2 << endl;
#endif
		// M-step. Solve linear system for C.
		calculateC();
		// Update V and sigma^2
		calculateV();
		calculateSigmaSquare();
		// Update gamma
		calculateGamma();
		iter++;
	}

	/////////////////////////////////
	// Fix gamma, redo the EM process.
	initialize();
	iter = 0;
	tecr = 1;
	E_old = float(inf);
	_E = 1;

	while (iter < _maxIter && tecr > _ecr && _sigma2 > 1e-8) {
		// E-step
		E_old = _E;
		getP();
		calculateTraceCKC();
		_E += _lambda / 2 * _traceCKC;
		tecr = abs((_E - E_old) / _E);
#ifdef PRINT_RESULTS
		cout << "# " << setw(3) << iter
			<< ", gamma: " << setw(5) << _gamma
			<< ", E change rate: " << setw(5) << tecr
			<< ", sigma2: " << setw(5) << _sigma2 << endl;
#endif
		// M-step. Solve linear system for C.
		calculateC();
		// Update V and sigma^2
		calculateV();
		calculateSigmaSquare();
		// Update gamma
		//calculateGamma();
		iter++;
	}
}

void VFC::calculateGamma() {
	int numcorr = 0;
	_matchIdx.clear();
	for (int i = 0; i < _numPt; i++) {
		if (_P[i] > _theta) {
			numcorr++;
			_matchIdx.push_back(i);
		}
	}
	_gamma = float(numcorr) / _numPt;
	_gamma = min(_gamma, 0.95f);
	_gamma = max(_gamma, 0.05f);
}

void VFC::calculateSigmaSquare() {
	_sigma2 = 0;
	_sumP = 0;
	for (int i = 0; i < _numPt; i++) {
		float t = pow(_Y[i].x - _V[i].x, 2) + pow(_Y[i].y - _V[i].y, 2);
		_sigma2 += _P[i] * t;
		_sumP += _P[i];
	}
	_sigma2 /= (_sumP * _numDim);
}

void VFC::calculateV() {
	// calculate V=K*C
	_V.clear();
	if (_method == NORMAL_VFC) {
		for (int i = 0; i < _numPt; i++) {
			float *p = _K.ptr<float>(i);
			float t1 = 0;
			float t2 = 0;
			for (int j = 0; j < _numPt; j++) {
				t1 += p[j] * _C[j].x;
				t2 += p[j] * _C[j].y;
			}
			_V.push_back(Point2f(t1, t2));
		}
	}
	else if (_method == FAST_VFC) {
		Mat T(2, _numEig, CV_32FC1);
		for (int i = 0; i < _numEig; i++) {
			float* q = _Q.ptr<float>(i);
			float t1 = 0;
			float t2 = 0;
			for (int j = 0; j < _numPt; j++) {
				t1 += q[j] * _C[j].x;
				t2 += q[j] * _C[j].y;
			}
			T.at<float>(0, i) = _S.at<float>(i,0)*t1;
			T.at<float>(1, i) = _S.at<float>(i,0)*t2;
		}
		for (int i = 0; i < _numPt; i++) {
			float t1 = 0;
			float t2 = 0;
			for (int j = 0; j < _numEig; j++) {
				float tmp = _Q.at<float>(j, i);
				t1 += tmp * T.at<float>(0, j);
				t2 += tmp * T.at<float>(1, j);
			}
			_V.push_back(Point2f(t1, t2));
		}
	}
	else if (_method == SPARSE_VFC) {
		for (int i = 0; i < _numPt; i++) {
			float t1 = 0;
			float t2 = 0;
			for (int j = 0; j < _numCtrlPts; j++) {
				float t = _U.at<float>(j, i);
				t1 += t * _C[j].x;
				t2 += t * _C[j].y;
			}
			_V.push_back(Point2f(t1, t2));
		}
	}
}

void VFC::calculateC() {
	Mat K;
	_K.copyTo(K);
	for (int i = 0; i < _numPt; i++) {
		K.at<float>(i, i) += _lambda*_sigma2 / _P[i];
	}
	Mat Y(_numPt, 2, CV_32FC1);
	for (int i = 0; i < _numPt; i++) {
		Y.at<float>(i, 0) = _Y[i].x;
		Y.at<float>(i, 1) = _Y[i].y;
	}
	Mat C;
	solve(K, Y, C, DECOMP_LU);
	_C.clear();
	for (int i = 0; i < _numPt; i++) {
		float t1 = C.at<float>(i, 0);
		float t2 = C.at<float>(i, 1);
		_C.push_back( Point2f(t1, t2) );
	}
}

void VFC::calculateTraceCKC() {
	// calculate K*C
	int n = _K.rows;
	vector<Point2f> KC;
	KC.clear();
	for (int i = 0; i < n; i++) {
		float *p = _K.ptr<float>(i);
		float t1 = 0;
		float t2 = 0;
		for (int j = 0; j < n; j++) {
			t1 += p[j] * _C[j].x;
			t2 += p[j] * _C[j].y;
		}
		KC.push_back( Point2f(t1, t2) );
	}
	// calculate C_transpose*(K*C)
	_traceCKC = 0;
	for (int i = 0; i < n; i++) {
		_traceCKC += _C[i].x * KC[i].x + _C[i].y * KC[i].y;
	}
}

void VFC::getP() {
	_E = 0;
	_sumP = 0;
	float temp2;
	temp2 = pow(DOUBLE_PI*_sigma2, _numDim / 2.0f) * (1 - _gamma) / (_gamma*_a);
	for (int i = 0; i < _numPt; i++) {
		float t = pow(_Y[i].x - _V[i].x, 2) + pow(_Y[i].y - _V[i].y, 2);
		float temp1 = expf(-t/(2*_sigma2));
		float p = temp1 / (temp1 + temp2);
		_P[i] = max(_minP, p);
		_sumP += p;
		_E += p * t;
	}
	_E /= (2 * _sigma2);
	_E += _sumP * log(_sigma2) * _numDim / 2;
}

void VFC::initialize() {
	_V.clear();
	_C.clear();
	_P.clear();
	for (int i = 0; i < _numPt; i++) {
		_V.push_back( Point2f(0.0f, 0.0f) );
		_P.push_back(1.0f);
	}
	if (_method == NORMAL_VFC || _method == FAST_VFC) {
		for (int i = 0; i < _numPt; i++) {
			_C.push_back(Point2f(0.0f, 0.0f));
		}
	}
	else if (_method == SPARSE_VFC) {
		for (int i = 0; i < _numCtrlPts; i++) {
			_C.push_back(Point2f(0.0f, 0.0f));
		}
	}
	calculateSigmaSquare();
}

Mat VFC::constructIntraKernel(vector<Point2f> X) {
	int n = X.size();
	Mat K;
	K.create(n, n, CV_32FC1);
	for (int i = 0; i < n; i++) {
		K.at<float>(i, i) = 1;
		for (int j = i+1; j < n; j++) {
			float t = pow(X[i].x - X[j].x, 2) + pow(X[i].y - X[j].y, 2);
			t *= -_beta;
			t = expf(t);
			K.at<float>(i, j) = t;
			K.at<float>(j, i) = t;
		}
	}
	return K;
}

Mat VFC::constructInterKernel(vector<Point2f> X, vector<Point2f> Y) {
	int m = X.size();
	int n = Y.size();
	Mat K;
	K.create(m, n, CV_32FC1);
	for (int i = 0; i < m; i++) {
		float* p = K.ptr<float>(i);
		for (int j = 0; j < n; j++) {
			float t = pow(X[i].x - Y[j].x, 2) + pow(X[i].y - Y[j].y, 2);
			p[j] = expf(-_beta * t);
		}
	}
	return K;
}

bool VFC::normalize() {
	// calculate mean
	float x1, y1, x2, y2;
	x1 = 0; y1 = 0;
	x2 = 0; y2 = 0;
	for (int i = 0; i < _numPt; i++) {
		x1 += _lX[i].x;
		y1 += _lX[i].y;
		x2 += _rX[i].x;
		y2 += _rX[i].y;
	}
	x1 /= _numPt;
	y1 /= _numPt;
	x2 /= _numPt;
	y2 /= _numPt;

	_meanLeftX.x = x1;
	_meanLeftX.y = y1;
	_meanRightX.x = x2;
	_meanRightX.y = y2;

	// minus mean
	for (int i = 0; i < _numPt; i++) {
		_lX[i].x -= x1;
		_lX[i].y -= y1;
		_rX[i].x -= x2;
		_rX[i].y -= y2;
	}

	// calculate scale
	double s1 = 0;
	double s2 = 0;
	for (int i = 0; i < _numPt; i++) {
		s1 += pow(_lX[i].x, 2);
		s1 += pow(_lX[i].y, 2);
		s2 += pow(_rX[i].x, 2);
		s2 += pow(_rX[i].y, 2);
	}
	s1 /= _numPt;
	s2 /= _numPt;
	s1 = sqrt(s1);
	s2 = sqrt(s2);
	_scaleLeftX = static_cast<float> (s1);
	_scaleRightX = static_cast<float> (s2);

	if (_scaleLeftX < MIN_STANDARD_DEVIATION || _scaleRightX < MIN_STANDARD_DEVIATION)
		return 0;

	// divide by scale, and prepare vector field samples
	_X.clear();
	_Y.clear();
	for (int i = 0; i < _numPt; i++) {
		_lX[i].x /= static_cast<float> (s1);
		_lX[i].y /= static_cast<float> (s1);
		_rX[i].x /= static_cast<float> (s2);
		_rX[i].y /= static_cast<float> (s2);
		Point2f tmp(_rX[i].x-_lX[i].x, _rX[i].y-_lX[i].y);
		_X.push_back(_lX[i]);
		_Y.push_back(tmp);
	}
	return 1;
}

vfc.h

#ifndef _VECTOR_FIELD_CONSENSUS_H
#define _VECTOR_FIELD_CONSENSUS_H
// Mismatch removal by vector field consensus (VFC)
// Author: Ji Zhao
// Date:   01/25/2015
// Email:  zhaoji84@gmail.com
//
// Parameters:
//   gamma: Percentage of inliers in the samples. This is an inital value
//       for EM iteration, and it is not important. Default value is 0.9.
//   beta: Paramerter of Gaussian Kernel, k(x, y) = exp(-beta* || x - y || ^ 2).
//       Default value is 0.1.
//   lambda: Represents the trade - off between the goodness of data fit
//       and smoothness of the field.Default value is 3.
//   theta: If the posterior probability of a sample being an inlier is
//       larger than theta, then it will be regarded as an inlier.
//       Default value is 0.75.
//   a: Paramerter of the uniform distribution. We assume that the outliers
//       obey a uniform distribution 1 / a. Default Value is 10.
//   MaxIter : Maximum iterition times.Defualt value is 500.
//   ecr: The minimum limitation of the energy change rate in the iteration
//       process. Default value is 1e-5.
//   minP: The posterior probability Matrix P may be singular for matrix
//       inversion.We set the minimum value of P as minP. Default value is
//       1e-5.
//   method: Choose the method for outlier removal.There are three optional
//       methods : NORMAL_VFC, FAST_VFC, SPARSE_VFC. Default value is NORMAL_VFC.
//
//
// Reference
// [1] Jiayi Ma, Ji Zhao, Jinwen Tian, Alan Yuille, and Zhuowen Tu.
//     Robust Point Matching via Vector Field Consensus, 
//     IEEE Transactions on Image Processing, 23(4), pp. 1706-1721, 2014
// [2] Jiayi Ma, Ji Zhao, Jinwen Tian, Xiang Bai, and Zhuowen Tu.
//     Regularized Vector Field Learning with Sparse Approximation for Mismatch Removal, 
//     Pattern Recognition, 46(12), pp. 3519-3532, 2013


#include "opencv2/core/core.hpp"
#include <iomanip>
#include <iostream>

//#define PRINT_RESULTS
#define DOUBLE_PI (6.283185f)
#define inf (0x3f3f3f3f)
#define NORMAL_VFC 1
#define FAST_VFC   2
#define SPARSE_VFC 3
#define MIN_POINT_NUMBER 5
#define MIN_STANDARD_DEVIATION 0.1

using namespace cv;
using namespace std;

class VFC{
public:
	VFC();
	~VFC();
	bool setData(vector<Point2f> X1, vector<Point2f> X2);
	bool normalize();
	Mat constructIntraKernel(vector<Point2f> X);
	Mat constructInterKernel(vector<Point2f> X, vector<Point2f> Y);
	void initialize();
	void getP();
	void calculateTraceCKC();
	void calculateC();
	void calculateV();
	void calculateSigmaSquare();
	void calculateGamma();
	void optimize();
	void optimizeVFC();
	void optimizeFastVFC();
	void optimizeSparseVFC();
	vector<int> obtainCorrectMatch();
	// for FastVFC only
	void calculateTraceCQSQC();
	void calculateCFastVFC();
	// for SparseVFC only
	bool selectSubset();
	void calculateC_SparseVFC();
private:
	vector<int> _matchIdx;
	int _numPt; // number of matches
	int _numDim; // dimensions, fixed as 2 in current version
	int _numElement; // _numPt * _numElement
	vector<Point2f> _lX; // keypoint position in left image
	vector<Point2f> _rX; // keypoint position in right image
	vector<Point2f> _X; // start point of vector field
	vector<Point2f> _Y; // end point of vector field
	Mat _K; // kernel matrix
	vector<Point2f> _V; // vector field
	vector<Point2f> _C; // coefficient
	vector<float> _P; // probability
	float _sumP; // sum of _P
	float _sigma2; // deviation of Gaussian noise
	float _E; // energy
	float _traceCKC;
	
	// parameters
	int _method;
	int _maxIter;
	float _gamma;
	float _beta;
	float _lambda;
	float _theta;
	float _a;
	float _ecr;
	float _minP;

	// parameters for FastVFC
	int _numEig;
	Mat _S; // eigenvalues
	Mat _Q; // eigenvectors
	float _traceCQSQC;

	// parameters for SparseVFC
	int _numCtrlPts;
	vector<Point2f> _ctrlPts;
	Mat _U;

	// intermediate variables
	Point2f _meanLeftX;
	Point2f _meanRightX;
	float _scaleLeftX;
	float _scaleRightX;
};

#endif