Updates to the homography tests and cleanup of a bug in the homography for...

import numpy as np

import pandas as pd

from autocnet.utils.utils import make_homogeneous

try:

    import cv2

    cv2_avail = True

def compute_error(H, x, x1):

    """

    Give this homography, compute the planar reprojection error

    between points a and b.

    between points a and b.  x and x1 can be n, 2 or n,3 homogeneous

    coordinates.  If only x, y coordinates, assume that z=1, e.g. x, y, 1 for

    all coordiantes.

    Parameters

    ----------

    if x1.shape[1] == 2:

        x1 = make_homogeneous(x1)

    z = np.empty(x.shape)

    # ToDo: Vectorize for performance

    for i, j in enumerate(x):

        x[i] = H.dot(j)

        x[i] /= x[i][-1]

        z[i] = H.dot(j)

        z[i] /= z[i][-1]

    data = np.empty((x.shape[0], 4))

    data[:, 0] = x_res = x1[:, 0] - x[:, 0]

    data[:, 1] = y_res = x1[:, 1] - x[:, 1]

    data[:, 0] = x_res = x1[:, 0] - z[:, 0]

    data[:, 1] = y_res = x1[:, 1] - z[:, 1]

    if data[:,:1].all() == 0:

        data[:] = 0.0

        total_rms = x_rms = y_rms = 0

    else:

        data[:, 2] = rms = np.sqrt(x_res**2 + y_res**2)

        total_rms = np.sqrt(np.mean(x_res**2 + y_res**2))

        x_rms = np.sqrt(np.mean(x_res**2))

        y_rms = np.sqrt(np.mean(y_res**2))

        data[:, 3] = rms / total_rms

    df = pd.DataFrame(data,

                               'y_residuals',

                               'rmse',

                               'error_contribution'])

    df.total_rms = total_rms

    df.x_rms = x_rms

    df.y_rms = y_rms

class TestHomography(unittest.TestCase):

    def test_Homography(self):

    @classmethod

    def setUpClass(cls):

        np.random.seed(12345)

        nbr_inliers = 20

        fp = np.array(np.random.standard_normal((nbr_inliers, 2)))  # inliers

        # Make homogeneous

        fph = np.hstack((fp, np.ones((nbr_inliers, 1))))

        tp = static_H.dot(fph.T)

        # normalize hom. coordinates

        tp /= tp[-1, :np.newaxis]

        H, mask = hm.compute_homography(fph, tp.T)

        np.testing.assert_array_almost_equal(H, static_H)

        tp = np.empty(fph.shape)

        for i in range(nbr_inliers):

            proj = fph[i].dot(static_H.T)

            proj /= proj[-1]

            tp[i] = proj

        cls.H = static_H

        cls.fph = fph

        cls.tp = tp

    def test_Homography(self):

        H, mask = hm.compute_homography(self.fph, self.tp, method='lmeds')

        np.testing.assert_array_almost_equal(H, self.H)

        H, mask = hm.compute_homography(self.fph, self.tp, method='normal')

        np.testing.assert_array_almost_equal(H, self.H)

        H, mask = hm.compute_homography(self.fph, self.tp, method='ransac')

        np.testing.assert_array_almost_equal(H, self.H)

        error = hm.compute_error(H, fph, tp.T)

    def test_compute_error(self):

        error = hm.compute_error(self.H, self.fph, self.tp)

        np.testing.assert_array_almost_equal(error['rmse'], np.zeros(20))

        self.assertAlmostEqual(error.total_rms, 0.0, 1)

        self.assertAlmostEqual(error.x_rms, 0.0, 1)

        self.assertAlmostEqual(error.y_rms, 0.0, 1)

    def test_compute_error_nonhomogeneous(self):

        error = hm.compute_error(self.H, self.fph[:,:2], self.tp[:,:2])

        np.testing.assert_array_almost_equal(error['rmse'], np.zeros(20))

        self.assertAlmostEqual(error.total_rms, 0.0, 1)

        self.assertAlmostEqual(error.x_rms, 0.0, 1)

        self.assertAlmostEqual(error.y_rms, 0.0, 1)

    def test_compute_error_not_perfect(self):

        eps = np.random.normal(0,0.25, size=(3,3))

        print(eps)

        h = self.H + eps

        print(h)

        error = hm.compute_error(h, self.fph, self.tp)

        truth_means = np.array([ 0.5607765, 0.0027841, 1.64353546, 0.78280023])

        means = error.describe().loc['mean'].values

        np.testing.assert_array_almost_equal(truth_means, means)