Példaprogram

import org.opencv.core.*;
import org.opencv.calib3d.*;
import org.opencv.highgui.*;
import org.opencv.features2d.*;
import org.opencv.utils.*;
import org.opencv.imgproc.*;
import java.util.*;
import java.io.*;

class EpipolarGeometryExample {

  static{ System.loadLibrary(Core.NATIVE_LIBRARY_NAME); }

  public static Comparator<DMatch> DMatchComparator = new Comparator<DMatch>() {
    public int compare(DMatch match1, DMatch match2) {
        if ( match1.distance < match2.distance) {
            return -1;
        }
        if ( match1.distance > match2.distance) {
            return 1;
        }
        return 0;
    }
      
  }; // END DMatchComparator
 
  public static Comparator<KeyPoint> KeypointComparator = new Comparator<KeyPoint>() {
    public int compare(KeyPoint keypoint1, KeyPoint keypoint2) {
        if ( keypoint1.response < keypoint2.response) {
            return -1;
        }
        if ( keypoint1.response > keypoint2.response) {
            return 1;
        }
        return 0;
    }
      
  }; // END KeypointComparator
 
  public static MatOfKeyPoint filterKeypoints(MatOfKeyPoint keypoints, int percent) {
      
    KeyPoint[] keypointArray = keypoints.toArray();
    Arrays.sort(keypointArray, EpipolarGeometryExample.KeypointComparator);
    int nRelevantKeypoints = Math.max(8, percent * keypointArray.length / 100 );
    KeyPoint[] relevantKeypointsArray = Arrays.copyOfRange(keypointArray, keypointArray.length - nRelevantKeypoints, keypointArray.length);
    MatOfKeyPoint relevantKeypoints = new MatOfKeyPoint();
    relevantKeypoints.fromArray(relevantKeypointsArray);
    
    return relevantKeypoints;
  } // END filterKeypoints()
 
  public static void findStableMatches(MatOfKeyPoint refKeypoints, MatOfKeyPoint queryKeypoints, MatOfDMatch matches, MatOfDMatch stableMatches) {
    int nRefKeypoints = refKeypoints.rows();
    int nQueryKeypoints = queryKeypoints.rows();
      
    double[][] matchingPairsTable = new double[nRefKeypoints][nQueryKeypoints];
    for ( int u = 0; u < nRefKeypoints; u++) {        /// initializing
        for (int v = 0; v < nQueryKeypoints; v++ ) {
            matchingPairsTable[u][v] = Double.MAX_VALUE;
        }
    }
    List<DMatch> matchesList = matches.toList();
    for (int j = 0; j < matchesList.size(); j++) {
        DMatch currentMatch = matchesList.get(j);
        
        int trainIdx = currentMatch.trainIdx;
        int queryIdx = currentMatch.queryIdx; // note: queryIdx == j
        
        double dist = currentMatch.distance;
        matchingPairsTable[trainIdx][queryIdx] = dist;
        
    }


    /// stabil párosítás (Gale - Shapley algoritmusa) http://www.cs.elte.hu/~kiraly/Grafalg.pdf
    int[] possibleQueriesToTrains = new int[nRefKeypoints];
    for ( int j=0; j < nRefKeypoints; j++ ) {
        possibleQueriesToTrains[j] = -1;
    }
    boolean newPairFound = true;
    do {    
        newPairFound = false;
        
        for ( int q = 0; q < nQueryKeypoints; q++ ) {
            //List<DMatch> currentMatches = kMatchesList.get(q).toList(); //  it is already increasing order
            DMatch currentMatch = matchesList.get(q);
         
                int trainIdx = currentMatch.trainIdx;
                double dist = currentMatch.distance;
                if (matchingPairsTable[trainIdx][q] == Double.MAX_VALUE) {
                    continue;
                }
                if ( possibleQueriesToTrains[trainIdx] == -1 ) {
                    possibleQueriesToTrains[trainIdx] = q;
                    newPairFound = true;
                } else {
                    if ( possibleQueriesToTrains[trainIdx] > dist ) {
                        matchingPairsTable[trainIdx][possibleQueriesToTrains[trainIdx]] = Double.MAX_VALUE;
                        possibleQueriesToTrains[trainIdx] = q;
                        newPairFound = true;
                        
                    }
                }
       
        }
        
    } while (newPairFound == true);
    
    List<DMatch> goodMatchesList = new ArrayList<DMatch>();
    for ( int t = 0; t < nRefKeypoints; t++ ) {
        if ( possibleQueriesToTrains[t] > -1 ) {
            goodMatchesList.add(new DMatch(possibleQueriesToTrains[t], t, (float) matchingPairsTable[t][possibleQueriesToTrains[t]] ));
        }
    }
    stableMatches.fromList(goodMatchesList);
  } // END findStableMatches()  --- note: not tested yet as method
 
  public static void drawMatchingPairs(MatOfPoint2f points1, MatOfPoint2f points2, Mat img1, Mat img2, Mat resultImg) {
    resultImg.create(Math.max(img1.rows(), img2.rows()), img1.cols()+img2.cols(), CvType.CV_8U );
    
    /** copy images side-by-side into one images **/  
    img1.copyTo(resultImg.submat(new Range(0,img1.rows()), new Range(0,img1.cols())));
    img2.copyTo(resultImg.submat(new Range(0,img2.rows()), new Range(img1.cols(), img1.cols()+ img2.cols())));
    
    List<Point> p1List = points1.toList();
    List<Point> p2List = points2.toList();
    Imgproc.cvtColor(resultImg, resultImg, Imgproc.COLOR_GRAY2BGR);
    
    for (int i = 0; i < points1.rows(); i++) {
        double r = Math.random()*255;
        double g = Math.random()*255;
        double b = Math.random()*255;
        Point p1 = p1List.get(i);
        Point p2 = p2List.get(i);
        //p2.y = p2.y + img1.cols();
        Core.line(resultImg, new Point(p1.x, p1.y), new Point(p2.x+img1.cols(), p2.y), new Scalar(r,g,b), 2);
    }
  } // END drawMatchingPairs()
 
 
  public static void main(String[] args) {
 
    String photosDir = "jatek/";
   
    String queryFile = photosDir + "jatek4.jpg";
    String referenceFile = photosDir + "jatek3.jpg";
    
    /**
     * loading images
     **/

    Mat queryImage = Highgui.imread(queryFile, Highgui.CV_LOAD_IMAGE_GRAYSCALE);
    Mat referenceImage = Highgui.imread(referenceFile, Highgui.CV_LOAD_IMAGE_GRAYSCALE);
    
    /**
     *  extract SIFT feature points
     **/
    FeatureDetector siftDetector = FeatureDetector.create(FeatureDetector.SIFT);
    DescriptorExtractor siftDescriptor = DescriptorExtractor.create(DescriptorExtractor.SIFT);
    
    // -- we preserve only a percent of the detected keypoints ---
    int percentOfKeypoints = 80;
    
    MatOfKeyPoint referenceKeypoints = new MatOfKeyPoint();
    siftDetector.detect(referenceImage, referenceKeypoints);
    referenceKeypoints = EpipolarGeometryExample.filterKeypoints(referenceKeypoints, percentOfKeypoints);
    Mat referenceDescriptor = new Mat();
    siftDescriptor.compute(referenceImage, referenceKeypoints, referenceDescriptor);
   
    MatOfKeyPoint queryKeypoints = new MatOfKeyPoint();
    siftDetector.detect(queryImage, queryKeypoints);
    queryKeypoints = EpipolarGeometryExample.filterKeypoints(queryKeypoints, percentOfKeypoints);
    Mat queryDescriptor = new Mat();
    siftDescriptor.compute(queryImage, queryKeypoints, queryDescriptor);

    int nRefKeypoints = referenceDescriptor.rows(); // = referenceSiftKeypoints.toList().size();
    int nQueryKeypoints = queryDescriptor.rows();


    /**
     * Matches feature points
     **/
    DescriptorMatcher matcher = DescriptorMatcher.create(DescriptorMatcher.FLANNBASED);
    
    List<MatOfDMatch> kMatchesList = new ArrayList<MatOfDMatch>();
    matcher.knnMatch(queryDescriptor, referenceDescriptor, kMatchesList, 1);
   
    List<DMatch> matchesList = new ArrayList<DMatch>();
    List<DMatch> goodMatchesList = new ArrayList<DMatch>();
    for (int i = 0; i < kMatchesList.size(); i++) {
        MatOfDMatch match_i = kMatchesList.get(i);
        List<DMatch> matches_i = match_i.toList();
        matchesList.addAll(matches_i);
    }
    
    DMatch[] matchesArray = matchesList.toArray(new DMatch[0]);

    Arrays.sort(matchesArray, EpipolarGeometryExample.DMatchComparator);
    
    for (int j = 0; j < (int)(40 * matchesArray.length / 100); j++) {
        goodMatchesList.add(matchesArray[j]);
        System.out.println("distance[" + j +"]: " + matchesArray[j].distance);
    }
    
    MatOfDMatch goodMatches = new MatOfDMatch();
    goodMatches.fromList(goodMatchesList);
    Mat img_with_matches = new Mat();
    Features2d.drawMatches(
                           queryImage, queryKeypoints,
                           referenceImage, referenceKeypoints,
                           goodMatches, img_with_matches);
    Highgui.imwrite("reference_match_to_"+ new File(queryFile).getName(), img_with_matches);
    
    
    /*** Compute fundamental matrix ***/
    Mat F = new Mat();
    MatOfPoint2f quepoints = new MatOfPoint2f();
    MatOfPoint2f refpoints = new MatOfPoint2f();
    List<Point> quePointList = new ArrayList<Point>();
    List<Point> refPointList = new ArrayList<Point>();
    List<Point3> quePoint3fList = new ArrayList<Point3>();
    List<Point3> refPoint3fList = new ArrayList<Point3>();
    List<KeyPoint> refKeypointList = referenceKeypoints.toList();
    List<KeyPoint> queKeypointList = queryKeypoints.toList();
    for (int t = 0; t < goodMatchesList.size(); t++) {
        DMatch m = goodMatchesList.get(t);
        int trainIdx = m.trainIdx;
        int queryIdx = m.queryIdx;
        Point refPt = refKeypointList.get(trainIdx).pt;
        Point quePt = queKeypointList.get(queryIdx).pt;
        refPointList.add(refPt);
        quePointList.add(quePt);
        refPoint3fList.add(new Point3(refPt.x, refPt.y, 1.0));
        quePoint3fList.add(new Point3(quePt.x, quePt.y, 1.0));
    }
    quepoints.fromList(quePointList);
    refpoints.fromList(refPointList);
    F = Calib3d.findFundamentalMat(refpoints, quepoints, Calib3d.FM_8POINT,3,0.99);
    
    Mat newRefPointMat = new Mat();
    Mat newQuePointMat = new Mat();
    Mat quePointMat =  Converters.vector_Point2f_to_Mat(quePointList);
    Mat refPointMat =  Converters.vector_Point2f_to_Mat(refPointList);
    
    // -- geometrical correction of matching point pairs with the help of fundamental matrix ----
    Calib3d.correctMatches(F, refPointMat.t(), quePointMat.t(), newRefPointMat, newQuePointMat);
    
    MatOfPoint2f newRefpoints = new MatOfPoint2f(newRefPointMat.t());
    MatOfPoint2f newQuepoints = new MatOfPoint2f(newQuePointMat.t());
    
    F = Calib3d.findFundamentalMat(newRefpoints, newQuepoints, Calib3d.FM_8POINT,3,0.99);
    
   
    quePointMat = newQuePointMat;
    refPointMat = newRefPointMat;
    
    refpoints = newRefpoints;
    quepoints = newQuepoints;
    
    System.out.println(F.dump());
    
    Mat quePoint3fMat = new Mat();
    Mat refPoint3fMat = new Mat();
    Calib3d.convertPointsToHomogeneous(quePointMat, quePoint3fMat);
    Calib3d.convertPointsToHomogeneous(refPointMat, refPoint3fMat);
    
    
    Mat queLines = new Mat();
    Mat refLines = new Mat();
    Calib3d.computeCorrespondEpilines(refPoint3fMat, 1, F, queLines);
    Calib3d.computeCorrespondEpilines(quePoint3fMat, 2, F, refLines);
    System.out.println(queLines.dump()); // queLines contains (a,b,c) in ax+by+c = 0;
  
    System.out.println("pontok mentése tömbbe");
   
    Mat img2WithEpilines = queryImage.clone();
    Imgproc.cvtColor(img2WithEpilines, img2WithEpilines, Imgproc.COLOR_GRAY2BGR);
    
    System.out.println("kép mentése epilines_1");
    Highgui.imwrite("epilines_"+ (new File(queryFile)).getName(), img2WithEpilines);
    
    for (int r = 0; r < queLines.rows()-1; r++) {
        
        double[] queLinesCoeff = queLines.get(r,0) ;
        double a = queLinesCoeff[0];
        double b = queLinesCoeff[1];
        double c = queLinesCoeff[2];
        
        Point p0 = new Point(0, -c/b);
        Point p1 = new Point(img2WithEpilines.cols(), -(c+a*img2WithEpilines.cols()) / b);
        Core.line(img2WithEpilines, p0,p1, new Scalar(255.0,0.0, 255.0));
    }
    System.out.println("kép mentése epilines_1");
    Highgui.imwrite("epilines_"+ (new File(queryFile)).getName(), img2WithEpilines);
    
    Mat img1WithEpilines = referenceImage.clone();
    Imgproc.cvtColor(img1WithEpilines, img1WithEpilines, Imgproc.COLOR_GRAY2BGR);
    
    for (int r = 0; r < refLines.rows()-1; r++) {

        double[] refLinesCoeff = refLines.get(r,0) ;
        double a = refLinesCoeff[0];
        double b = refLinesCoeff[1];
        double c = refLinesCoeff[2];
        
        Point p0 = new Point(0, -c/b);
        Point p1 = new Point(img1WithEpilines.cols(), -(c+a*img1WithEpilines.cols()) / b);
        Core.line(img1WithEpilines, p0,p1, new Scalar(255.0,0.0, 255.0));
    }
    System.out.println("kép mentése epilines_1");
    Highgui.imwrite("epilines_"+ (new File(referenceFile)).getName(), img1WithEpilines);
    
    
    /// ------ rectification ------
    Mat H1 = new Mat();
    Mat H2 = new Mat();
    Mat rectifiedImg1 = img1WithEpilines.clone();
    
    Mat rectifiedImg2 = img2WithEpilines.clone();
    Calib3d.stereoRectifyUncalibrated(refpoints, quepoints, F, queryImage.size(), H1, H2, 3);
    
    //Imgproc.warpPerspective(queryImage, rectifiedImg2, H2, img2WithEpilines.size() );
    Imgproc.warpPerspective(img2WithEpilines, rectifiedImg2, H2, img2WithEpilines.size() );
    //Imgproc.warpPerspective(referenceImage, rectifiedImg1, H1, img1WithEpilines.size() );
    Imgproc.warpPerspective(img1WithEpilines, rectifiedImg1, H1, img1WithEpilines.size() );
    Highgui.imwrite("rectified_"+ (new File(queryFile)).getName(), rectifiedImg2);
    Highgui.imwrite("rectified_ref_to_"+ (new File(queryFile)).getName(), rectifiedImg1);
       
  } // END main
 
}