Merge pull request #164 from jlaura/gold (2acbb126) · Commits · aflab / astrogeology / Autocnet

autocnet/camera/camera.py

+41 −31

Original line number	Diff line number	Diff line
		import numpy as np
		from autocnet.camera.utils import crossform
		try:
		import cv2

		except:
		cv2 = None

		def compute_epipoles(f):
		"""
		@@ -21,9 +24,7 @@ def compute_epipoles(f):
		"""
		u, _, _ = np.linalg.svd(f)
		e = u[:, -1]
		e1 = np.array([[0, -e[2], e[1]],
		[e[2], 0, -e[0]],
		[-e[1], e[0], 0]])
		e1 = crossform(e)

		return e, e1

		@@ -102,24 +103,37 @@ def triangulate(pt, pt1, p, p1):
		pt = pt.T
		if pt1.shape[0] != 3:
		pt1 = pt1.T

		#if cv2:
		X = cv2.triangulatePoints(p, p1, pt[:2], pt1[:2])

		# Homogenize
		X /= X[3]

		X /= X[3] # Homogenize
		return X


		"""
		# Stubbed in for a ticket addressing making OpenCV an optional dependency
		else:
		npts = len(pt)
		a = np.zeros((4, 4))
		coords = np.empty((npts, 4))
		coords[:] = 1
		for i in range(npts):
		# Compute AX = 0
		a[0] = pt[i][0] * p[2] - p[0]
		a[1] = pt[i][1] * p[2] - p[1]
		a[2] = pt1[i][0] * p1[2] - p1[0]
		a[3] = pt1[i][1] * p1[2] - p1[1]
		# v.T is a least squares solution that minimizes the error residual
		u, s, vh = np.linalg.svd(a)
		v = vh.T
		coords[i] = v[:,3] / (v[:,3][-1])
		return coords.T
		"""
		def projection_error(p1, p, pt, pt1):
		"""
		Based on Hartley and Zisserman p.285 this function triangulates
		image correspondences and computes the reprojection error
		by back-projecting the points into the image.

		References
		----------
		.. [Hartley2003]
		This is the classic cost function (minimization problem) into
		the gold standard method for fundamental matrix estimation.

		Parameters
		-----------
		@@ -137,29 +151,25 @@ def projection_error(p1, p, pt, pt1):

		Returns
		-------
		residuals : ndarray
		(n, 1) residuals for each correspondence

		cumulative_error : float
		sum of the residuals
		reproj_error : ndarray
		(n, 1) vector of reprojection errors


		"""
		# SciPy least squares solver needs a vector, so reshape back to a 3x4 c
		# camera matrix at each iteration

		if p1.shape != (3,4):
		p1 = p1.reshape(3,4)

		# Triangulate the correspondences
		xw_est = triangulate(pt, pt1, p, p1)

		# Back project and homogenize
		xhat = np.dot(p, xw_est)
		xhat /= xhat[2]
		x2hat = np.dot(p1, xw_est)
		x2hat /= x2hat[2]
		xhat = triangulate(pt, pt1, p, p1)
		xhat1 = xhat[:3] / xhat[2]
		xhat2 = p1.dot(xhat)
		xhat2 /= xhat2[2]

		# Compute residuals
		dist = (pt.T - xhat)2 + (pt1.T - x2hat)2
		residuals = np.sum(dist, axis=0)
		reproj_error = np.sum(dist)
		# Compute error
		cost = (pt - xhat1)2 + (pt1 - xhat2)2
		cost = np.sqrt(np.sum(cost, axis=0))

		return residuals, reproj_error
		return cost

autocnet/camera/tests/test_camera.py

+2 −3

Original line number	Diff line number	Diff line
		@@ -60,7 +60,6 @@ class TestCamera(unittest.TestCase):
		c = camera.triangulate(coords1, coords2, p, p1)
		np.testing.assert_array_almost_equal(c, truth)

		truth = np.array([ 3.09866357e-02, 2.60295132e-01,
		8.12871690e-02, 5.57281224e-01, 4.72226586e-04])
		residuals, reproj_error = camera.projection_error(p1, p, coords1, coords2)
		truth = np.array([0.17603 , 0.510191, 0.285109, 0.746513, 0.021731])
		residuals = camera.projection_error(p1, p, coords1.T, coords2.T)
		np.testing.assert_array_almost_equal(residuals, truth)

autocnet/camera/utils.py

+8 −0

Original line number	Diff line number	Diff line
		import math
		import numpy as np

		def crossform(a):
		"""
		Convert a three element vector into a 3 x 3 skew matrix as per
		Hartley and Zisserman pg. 581
		"""
		return np.array([[0, -a[2], a[1]],
		[a[2], 0, -a[0]],
		[-a[1], a[0], 0]])

		def normalize(a):
		"""

autocnet/graph/edge.py

+16 −1

Original line number	Diff line number	Diff line
		@@ -442,7 +442,7 @@ class Edge(dict, MutableMapping):
		See Also
		--------
		autocnet.transformation.transformations.FundamentalMatrix
		:

		"""
		if not hasattr(self, 'matches'):
		raise AttributeError('Matches have not been computed for this edge')
		@@ -473,6 +473,21 @@ class Edge(dict, MutableMapping):
		# Set the initial state of the fundamental mask in the masks
		self.masks = ('fundamental', mask)

		def refine_fundamental_matrix_matches(self, **kwargs): # pragma: no cover
		"""
		Given an estimated fundamental matrix, refine the correspondences based
		on the reprojective error.

		See Also
		--------
		autocnet.transformation.transformations.FundamentalMatrix.refine_matches
		"""
		if not hasattr(self, 'fundamental_matrix'):
		raise AttributeError('No fundamental matrix exists for this edge.')
		return

		self.fundamental_matrix.refine_matches(**kwargs)

		def compute_homography(self, method='ransac', clean_keys=[], pid=None, **kwargs):
		"""
		For each edge in the (sub) graph, compute the homography

autocnet/graph/network.py

+10 −0

Original line number	Diff line number	Diff line
		@@ -413,6 +413,16 @@ class CandidateGraph(nx.Graph):
		'''
		self.apply_func_to_edges('compute_fundamental_matrix', args, *kwargs)

		def refine_fundamental_matrix_matches(self, args, *kwargs):
		"""
		Refine the fundamental matrix matches using reprojective error

		See Also
		--------
		autocnet.transformation.transformations.FundamentalMatrix.refine_matches
		"""
		self.apply_func_to_edges('refine_fundamental_matrix_matches', args, *kwargs)

		def subpixel_register(self, args, *kwargs):
		'''
		Compute subpixel offsets for all edges using identical parameters