SampsonCorrector.java

/*
 * Copyright (C) 2015 Alberto Irurueta Carro (alberto@irurueta.com)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *         http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package com.irurueta.ar.epipolar;

import com.irurueta.geometry.CoordinatesType;
import com.irurueta.geometry.Line2D;
import com.irurueta.geometry.Point2D;
import com.irurueta.geometry.estimators.LockedException;
import com.irurueta.geometry.estimators.NotReadyException;

import java.util.ArrayList;
import java.util.List;

/**
 * Fixes matched pairs of points so that they perfectly follow a given epipolar
 * geometry.
 * When matching points typically the matching precision is about 1 pixel,
 * however this makes that matched points under a given epipolar geometry (i.e.
 * fundamental or essential matrix), do not lie perfectly on the corresponding
 * epipolar plane or epipolar lines.
 * The consequence is that triangularization of these matches will fail or
 * produce inaccurate results.
 * By fixing matched points using a corrector following a given epipolar
 * geometry, this effect is alleviated.
 * This corrector uses the Sampson approximation which is capable to remove
 * small errors when matches are close to their real epipolar lines. This method
 * is faster than the Gold Standard method, but can only correct small errors
 * (1 or 2 pixels).
 */
public class SampsonCorrector extends Corrector {

    /**
     * Constructor.
     */
    public SampsonCorrector() {
        super();
    }

    /**
     * Constructor.
     *
     * @param fundamentalMatrix fundamental matrix to be set.
     */
    public SampsonCorrector(final FundamentalMatrix fundamentalMatrix) {
        super(fundamentalMatrix);
    }

    /**
     * Constructor.
     *
     * @param leftPoints  points to be corrected on left view.
     * @param rightPoints points to be corrected on right view.
     * @throws IllegalArgumentException if provided lists of points don't have
     *                                  the same size.
     */
    public SampsonCorrector(final List<Point2D> leftPoints, final List<Point2D> rightPoints) {
        super(leftPoints, rightPoints);
    }

    /**
     * Constructor.
     *
     * @param leftPoints        points to be corrected on left view.
     * @param rightPoints       points to be corrected on right view.
     * @param fundamentalMatrix fundamental matrix to be set.
     * @throws IllegalArgumentException if provided lists of points don't have
     *                                  the same size.
     */
    public SampsonCorrector(final List<Point2D> leftPoints, final List<Point2D> rightPoints,
                            final FundamentalMatrix fundamentalMatrix) {
        super(leftPoints, rightPoints, fundamentalMatrix);
    }

    /**
     * Constructor.
     *
     * @param listener listener to handle events generated by this class.
     */
    public SampsonCorrector(final CorrectorListener listener) {
        super(listener);
    }

    /**
     * Constructor.
     *
     * @param fundamentalMatrix fundamental matrix to be set.
     * @param listener          listener to handle events generated by this class.
     */
    public SampsonCorrector(final FundamentalMatrix fundamentalMatrix, final CorrectorListener listener) {
        super(fundamentalMatrix, listener);
    }

    /**
     * Constructor.
     *
     * @param leftPoints  points to be corrected on left view.
     * @param rightPoints points to be corrected on right view.
     * @param listener    listener to handle events generated by this class.
     * @throws IllegalArgumentException if provided lists of points don't have
     *                                  the same size.
     */
    public SampsonCorrector(final List<Point2D> leftPoints, final List<Point2D> rightPoints,
                            final CorrectorListener listener) {
        super(leftPoints, rightPoints, listener);
    }

    /**
     * Constructor.
     *
     * @param leftPoints        points to be corrected on left view.
     * @param rightPoints       points to be corrected on right view.
     * @param fundamentalMatrix fundamental matrix to be set.
     * @param listener          listener to handle events generated by this class.
     * @throws IllegalArgumentException if provided lists of points don't have
     *                                  the same size.
     */
    public SampsonCorrector(final List<Point2D> leftPoints, final List<Point2D> rightPoints,
                            final FundamentalMatrix fundamentalMatrix, final CorrectorListener listener) {
        super(leftPoints, rightPoints, fundamentalMatrix, listener);
    }

    /**
     * Corrects the lists of provided matched points to be corrected.
     *
     * @throws NotReadyException if this instance is not ready (either points or
     *                           fundamental matrix has not been provided yet).
     * @throws LockedException   if this instance is locked doing computations.
     */
    @SuppressWarnings("DuplicatedCode")
    @Override
    public void correct() throws NotReadyException, LockedException {
        if (isLocked()) {
            throw new LockedException();
        }
        if (!isReady()) {
            throw new NotReadyException();
        }

        locked = true;

        if (listener != null) {
            listener.onCorrectStart(this);
        }

        // epipolar lines to be reused for memory efficiency
        final var leftEpipolarLine = new Line2D();
        final var rightEpipolarLine = new Line2D();

        leftCorrectedPoints = new ArrayList<>();
        rightCorrectedPoints = new ArrayList<>();

        final var size = leftPoints.size();
        float progress;
        var previousProgress = 0.0f;
        for (var i = 0; i < size; i++) {
            final var leftPoint = leftPoints.get(i);
            final var rightPoint = rightPoints.get(i);

            final var leftCorrectedPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);
            final var rightCorrectedPoint = Point2D.create(CoordinatesType.HOMOGENEOUS_COORDINATES);

            // correct single pair
            SampsonSingleCorrector.correct(leftPoint, rightPoint, fundamentalMatrix, leftCorrectedPoint,
                    rightCorrectedPoint, leftEpipolarLine, rightEpipolarLine);

            leftCorrectedPoints.add(leftCorrectedPoint);
            rightCorrectedPoints.add(rightCorrectedPoint);

            if (listener != null) {
                progress = i / (float) size;
                if (progress - previousProgress > progressDelta) {
                    // progress has changed significantly
                    previousProgress = progress;
                    listener.onCorrectProgressChange(this, progress);
                }
            }
        }

        if (listener != null) {
            listener.onCorrectEnd(this);
        }

        locked = false;
    }

    /**
     * Gets type of correction being used.
     *
     * @return type of correction.
     */
    @Override
    public CorrectorType getType() {
        return CorrectorType.SAMPSON_CORRECTOR;
    }
}