def CooksD_query_test(J,p,x, f, query,fquery_ref, CooksD_nox, k_dima, k_cook, i=0): r'''Test the concept of CooksD for querying hypothetical data fquery_test = f(q) isn't true here. If it WERE true, the x of the query point would be 0 (we fit the model exactly), so the outlierness factor would be 0 also ''' # current solve Nmeasurements,Nstate = J.shape query = query [i] fquery_ref = fquery_ref[i] # I add a new point, and reoptimize fquery = func_hypothesis(query,p) xquery = fquery - fquery_ref jquery = model_matrix(query, len(p)) J1 = nps.glue(J, jquery, axis=-2) f1 = nps.glue(f, fquery_ref, axis=-1) p1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1))) x1 = nps.matmult(p1, nps.transpose(J1)) - f1 dx = x1[:-1] - x dx_both = x1 - nps.glue(x,xquery, axis=-1) report_mismatch_relerr( nps.inner(dx_both,dx_both)*k_cook, CooksD_nox['self_others'][i]*xquery*xquery, "self_others query-CooksD computed analytically, explicitly") report_mismatch_relerr( nps.inner(dx,dx)*k_cook, CooksD_nox['others'][i]*xquery*xquery, "others query-CooksD computed analytically, explicitly")
def callback_linf_angle(p, v0, v1, t01): costh0 = nps.inner(p, v0) / np.sqrt(nps.norm2(p) * nps.norm2(v0)) costh1 = nps.inner(p - t01, v1) / np.sqrt( nps.norm2(p - t01) * nps.norm2(v1)) # Simpler function that has the same min return (1 - min(costh0, costh1)) * 1e9
def CooksD_test(J, x, f, CooksD, k_dima, k_cook, i=0): r'''Test the computation of Cook's D I have an analytical expression for this computed in compute_outliernesses(). This explicitly computes the quantity represented by compute_outliernesses() to make sure that that analytical expression is correct ''' # I reoptimize without measurement i Nmeasurements,Nstate = J.shape J1 = nps.glue(J[:i,:], J[(i+1):,:], axis=-2) f1 = nps.glue(f[:i ], f[(i+1): ], axis=-1) p1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1))) x1 = nps.matmult(p1, nps.transpose(J)) - f dx = x1-x report_mismatch_relerr( nps.inner(dx,dx) * k_cook, CooksD['self_others'][i], "self_others CooksD computed analytically, explicitly") report_mismatch_relerr( (nps.inner(dx,dx) - dx[i]*dx[i]) * k_cook, CooksD['others'][i], "others CooksD computed analytically, explicitly")
def outlierness_test(J, x, f, outlierness, k_dima, k_cook, i=0): r'''Test the computation of outlierness I have an analytical expression for this computed in compute_outliernesses(). This explicitly computes the quantity represented by compute_outliernesses() to make sure that that analytical expression is correct ''' # I reoptimize without measurement i E0 = nps.inner(x,x) J1 = nps.glue(J[:i,:], J[(i+1):,:], axis=-2) f1 = nps.glue(f[:i ], f[(i+1): ], axis=-1) p1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1))) x1 = nps.matmult(p1, nps.transpose(J1)) - f1 E1 = nps.inner(x1,x1) report_mismatch_relerr( (E0-E1) * k_dima, outlierness['self_others'][i], "self_others outlierness computed analytically, explicitly") report_mismatch_relerr( (E0-x[i]*x[i] - E1) * k_dima, outlierness['others'][i], "others outlierness computed analytically, explicitly")
def callback_l1_angle(p, v0, v1, t01): costh0 = nps.inner(p, v0) / np.sqrt(nps.norm2(p) * nps.norm2(v0)) costh1 = nps.inner(p - t01, v1) / np.sqrt( nps.norm2(p - t01) * nps.norm2(v1)) th0 = np.arccos(min(costh0, 1.0)) th1 = np.arccos(min(costh1, 1.0)) return np.abs(th0) + np.abs(th1)
def _HVs_HVc_HVp(cahvor): r'''Given a cahvor dict returns a tuple containing (Hs,Vs,Hc,Vc,Hp,Vp)''' Hc = nps.inner(cahvor['H'], cahvor['A']) hshp = cahvor['H'] - Hc * cahvor['A'] Hs = np.sqrt(nps.inner(hshp,hshp)) Vc = nps.inner(cahvor['V'], cahvor['A']) vsvp = cahvor['V'] - Vc * cahvor['A'] Vs = np.sqrt(nps.inner(vsvp,vsvp)) Hp = hshp / Hs Vp = vsvp / Vs return Hs,Vs,Hc,Vc,Hp,Vp
def rotation_any_v_to_z(v): r'''Return any rotation matrix that maps the given unit vector v to [0,0,1]''' z = v if np.abs(v[0]) < .9: x = np.array((1,0,0)) else: x = np.array((0,1,0)) x -= nps.inner(x,v)*v x /= nps.mag(x) y = np.cross(z,x) return nps.cat(x,y,z)
def test_matmult(self): r'''Testing the broadcasted matrix multiplication''' self.assertValueShape( None, (4,2,3,5), nps.matmult, arr(2,3,7), arr(4,1,7,5) ) ref = np.array([[[[ 42, 48, 54], [ 114, 136, 158]], [[ 114, 120, 126], [ 378, 400, 422]]], [[[ 186, 224, 262], [ 258, 312, 366]], [[ 642, 680, 718], [ 906, 960, 1014]]]]) self._check_output_modes( ref, nps.matmult2, arr(2,1,2,4), arr(2,4,3), dtype=float ) ref2 = np.array([[[[ 156.], [ 452.]], [[ 372.], [ 1244.]]], [[[ 748.], [ 1044.]], [[ 2116.], [ 2988.]]]]) self._check_output_modes(ref2, nps.matmult2, arr(2,1,2,4), nps.matmult2(arr(2,4,3), arr(3,1))) # not doing _check_output_modes() because matmult() doesn't take an # 'out' kwarg self.assertNumpyAlmostEqual(ref2, nps.matmult(arr(2,1,2,4), arr(2,4,3), arr(3,1))) # checking the null-dimensionality logic A = arr(2,3) self._check_output_modes( nps.inner(nps.transpose(A), np.arange(2)), nps.matmult2, np.arange(2), A ) A = arr(3) self._check_output_modes( A*2, nps.matmult2, np.array([2]), A ) A = arr(3) self._check_output_modes( A*2, nps.matmult2, np.array(2), A )
def outlierness_query_test(J,p,x, f, query,fquery_ref, outlierness_nox, k_dima, k_cook, i=0): r'''Test the concept of outlierness for querying hypothetical data fquery_test = f(q) isn't true here. If it WERE true, the x of the query point would be 0 (we fit the model exactly), so the outlierness factor would be 0 also ''' # current solve E0 = nps.inner(x,x) query = query [i] fquery_ref = fquery_ref[i] # I add a new point, and reoptimize fquery = func_hypothesis(query,p) xquery = fquery - fquery_ref jquery = model_matrix(query, len(p)) J1 = nps.glue(J, jquery, axis=-2) f1 = nps.glue(f, fquery_ref, axis=-1) p1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1))) x1 = nps.matmult(p1, nps.transpose(J1)) - f1 E1 = nps.inner(x1,x1) report_mismatch_relerr( (x1[-1]*x1[-1]) * k_dima, outlierness_nox['self'][i]*xquery*xquery, "self query-outlierness computed analytically, explicitly") report_mismatch_relerr( (E1-x1[-1]*x1[-1] - E0) * k_dima, outlierness_nox['others'][i]*xquery*xquery, "others query-outlierness computed analytically, explicitly") report_mismatch_relerr( (E1 - E0) * k_dima, outlierness_nox['self_others'][i]*xquery*xquery, "self_others query-outlierness computed analytically, explicitly")
def check(intrinsics, p_ref, q_ref, unproject=True): q_projected = mrcal.project(p_ref, *intrinsics) testutils.confirm_equal(q_projected, q_ref, msg=f"Projecting {intrinsics[0]}", eps=1e-2) if not unproject: return v_unprojected = mrcal.unproject(q_projected, *intrinsics, normalize=True) testutils.confirm_equal(nps.norm2(v_unprojected), np.ones((p_ref.shape[0], ), dtype=float), msg=f"Unprojected v are normalized", eps=1e-6) cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref) cos = np.clip(cos, -1, 1) testutils.confirm_equal(np.arccos(cos), np.zeros((p_ref.shape[0], ), dtype=float), msg=f"Unprojecting {intrinsics[0]}", eps=1e-6)
def Var_df(J, squery, stdev): r'''Propagates noise in input to noise in f noise in input -> noise in params -> noise in f dp ~ M dm where M = inv(JtJ)Jt df = df/dp dp df/dp = squery Var(dm) = stdev^2 I -> Var(df) = stdev^2 squery inv(JtJ) Jt J inv(JtJ) squeryt = = stdev^2 squery inv(JtJ) squeryt This function broadcasts over squery ''' return \ nps.inner(squery, nps.transpose(np.linalg.solve(nps.matmult(nps.transpose(J),J), nps.transpose(squery)))) *stdev*stdev
def test_order(q,f, query, order): # I look at linear and quadratic models: a0 + a1 q + a2 q^2, with a2=0 for the # linear case. I use plain least squares. The parameter vector is [a0 a1 a2]t. S # = [1 q q^2], so the measurement vector x = S p - f. E = norm2(x). J = dx/dp = # S. # # Note the problem "order" is the number of parameters, so a linear model has # order==2 p,J,x = fit(q,f,order) Nmeasurements,Nstate = J.shape k_dima = 1.0/Nmeasurements k_cook = 1.0/((Nstate + 1.0) * nps.inner(x,x)/(Nmeasurements - Nstate - 1.0)) report_mismatch_abserr(np.linalg.norm(nps.matmult(x,J)), 0, "Jtx") squery = model_matrix(query, order) fquery = func_hypothesis(query, p) metrics = compute_outliernesses(J,x, squery, k_dima, k_cook) outlierness_test(J, x, f, metrics['dima']['outliers'], k_dima, k_cook, i=10) CooksD_test (J, x, f, metrics['cook']['outliers'], k_dima, k_cook, i=10) outlierness_query_test(J,p,x,f, query, fquery + 1.2e-3, metrics['dima']['query'], k_dima, k_cook, i=10 ) CooksD_query_test (J,p,x,f, query, fquery + 1.2e-3, metrics['cook']['query'], k_dima, k_cook, i=10 ) Vquery = Var_df(J, squery, noise_stdev) return \ dict( p = p, J = J, x = x, Vquery = Vquery, squery = squery, fquery = fquery, metrics = metrics, k_dima = k_dima, k_cook = k_cook )
def check(intrinsics, p_ref, q_ref): ########## project q_projected = mrcal.project(p_ref, *intrinsics) testutils.confirm_equal(q_projected, q_ref, msg = f"Projecting {intrinsics[0]}", eps = 1e-2) q_projected *= 0 mrcal.project(p_ref, *intrinsics, out = q_projected) testutils.confirm_equal(q_projected, q_ref, msg = f"Projecting {intrinsics[0]} in-place", eps = 1e-2) meta = mrcal.lensmodel_metadata_and_config(intrinsics[0]) if meta['has_gradients']: @nps.broadcast_define( ((3,),('N',)) ) def grad_broadcasted(p_ref, i_ref): return grad(lambda pi: mrcal.project(pi[:3], intrinsics[0], pi[3:]), nps.glue(p_ref,i_ref, axis=-1)) dq_dpi_ref = grad_broadcasted(p_ref,intrinsics[1]) q_projected,dq_dp,dq_di = mrcal.project(p_ref, *intrinsics, get_gradients=True) testutils.confirm_equal(q_projected, q_ref, msg = f"Projecting {intrinsics[0]} with grad", eps = 1e-2) testutils.confirm_equal(dq_dp, dq_dpi_ref[...,:3], msg = f"dq_dp {intrinsics[0]}", eps = 1e-2) testutils.confirm_equal(dq_di, dq_dpi_ref[...,3:], msg = f"dq_di {intrinsics[0]}", eps = 1e-2) out=[q_projected,dq_dp,dq_di] out[0] *= 0 out[1] *= 0 out[2] *= 0 mrcal.project(p_ref, *intrinsics, get_gradients=True, out=out) testutils.confirm_equal(q_projected, q_ref, msg = f"Projecting {intrinsics[0]} with grad in-place", eps = 1e-2) testutils.confirm_equal(dq_dp, dq_dpi_ref[...,:3], msg = f"dq_dp in-place", eps = 1e-2) testutils.confirm_equal(dq_di, dq_dpi_ref[...,3:], msg = f"dq_di in-place", eps = 1e-2) ########## unproject if 1: ##### Un-normalized v_unprojected = mrcal.unproject(q_projected, *intrinsics, normalize = False) cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref) cos = np.clip(cos, -1, 1) testutils.confirm_equal( np.arccos(cos), np.zeros((p_ref.shape[0],), dtype=float), msg = f"Unprojecting {intrinsics[0]}", eps = 1e-6) if 1: ##### Normalized v_unprojected_nograd = mrcal.unproject(q_projected, *intrinsics, normalize = True) testutils.confirm_equal( nps.norm2(v_unprojected_nograd), 1, msg = f"Unprojected v are normalized", eps = 1e-6) cos = nps.inner(v_unprojected_nograd, p_ref) / nps.mag(p_ref) cos = np.clip(cos, -1, 1) testutils.confirm_equal( np.arccos(cos), np.zeros((p_ref.shape[0],), dtype=float), msg = f"Unprojecting {intrinsics[0]} (normalized)", eps = 1e-6) if not meta['has_gradients']: # no in-place output for the no-gradients unproject() path return v_unprojected *= 0 mrcal.unproject(q_projected, *intrinsics, normalize = True, out = v_unprojected) testutils.confirm_equal( nps.norm2(v_unprojected), 1, msg = f"Unprojected in-place v are normalized", eps = 1e-6) cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref) cos = np.clip(cos, -1, 1) testutils.confirm_equal( np.arccos(cos), np.zeros((p_ref.shape[0],), dtype=float), msg = f"Unprojecting in-place {intrinsics[0]}", eps = 1e-6) ### unproject gradients v_unprojected,dv_dq,dv_di = mrcal.unproject(q_projected, *intrinsics, get_gradients=True) # I'd like to turn this on, but unproject() doesn't behave the way it # should, so this test always fails currently # # testutils.confirm_equal( v_unprojected, # v_unprojected_nograd, # msg = f"Unproject() should return the same thing whether get_gradients or not", # eps = 1e-6) # Two different gradient computations, to match the two different ways the # internal computation is performed if intrinsics[0] == 'LENSMODEL_PINHOLE' or \ intrinsics[0] == 'LENSMODEL_STEREOGRAPHIC' or \ intrinsics[0] == 'LENSMODEL_LATLON' or \ intrinsics[0] == 'LENSMODEL_LONLAT': @nps.broadcast_define( ((2,),('N',)) ) def grad_broadcasted(q_ref, i_ref): return grad(lambda qi: mrcal.unproject(qi[:2], intrinsics[0], qi[2:]), nps.glue(q_ref,i_ref, axis=-1)) dv_dqi_ref = grad_broadcasted(q_projected,intrinsics[1]) else: @nps.broadcast_define( ((2,),('N',)) ) def grad_broadcasted(q_ref, i_ref): return grad(lambda qi: \ mrcal.unproject_stereographic( \ mrcal.project_stereographic( mrcal.unproject(qi[:2], intrinsics[0], qi[2:]))), nps.glue(q_ref,i_ref, axis=-1)) dv_dqi_ref = grad_broadcasted(q_projected,intrinsics[1]) testutils.confirm_equal(mrcal.project(v_unprojected, *intrinsics), q_projected, msg = f"Unprojecting {intrinsics[0]} with grad", eps = 1e-2) testutils.confirm_equal(dv_dq, dv_dqi_ref[...,:2], msg = f"dv_dq: {intrinsics[0]}", worstcase = True, relative = True, eps = 0.01) testutils.confirm_equal(dv_di, dv_dqi_ref[...,2:], msg = f"dv_di {intrinsics[0]}", worstcase = True, relative = True, eps = 0.01) # Normalized unprojected gradients v_unprojected,dv_dq,dv_di = mrcal.unproject(q_projected, *intrinsics, normalize = True, get_gradients = True) testutils.confirm_equal( nps.norm2(v_unprojected), 1, msg = f"Unprojected v (with gradients) are normalized", eps = 1e-6) cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref) cos = np.clip(cos, -1, 1) testutils.confirm_equal( np.arccos(cos), np.zeros((p_ref.shape[0],), dtype=float), msg = f"Unprojecting (normalized, with gradients) {intrinsics[0]}", eps = 1e-6) @nps.broadcast_define( ((2,),('N',)) ) def grad_normalized_broadcasted(q_ref, i_ref): return grad(lambda qi: \ mrcal.unproject(qi[:2], intrinsics[0], qi[2:], normalize=True), nps.glue(q_ref,i_ref, axis=-1)) dvnormalized_dqi_ref = grad_normalized_broadcasted(q_projected,intrinsics[1]) testutils.confirm_equal(dv_dq, dvnormalized_dqi_ref[...,:2], msg = f"dv_dq (normalized v): {intrinsics[0]}", worstcase = True, relative = True, eps = 0.01) testutils.confirm_equal(dv_di, dvnormalized_dqi_ref[...,2:], msg = f"dv_di (normalized v): {intrinsics[0]}", worstcase = True, relative = True, eps = 0.01) # unproject() with gradients, in-place if 1: # Normalized output out=[v_unprojected,dv_dq,dv_di] out[0] *= 0 out[1] *= 0 out[2] *= 0 mrcal.unproject(q_projected, *intrinsics, normalize = True, get_gradients = True, out = out) testutils.confirm_equal( nps.norm2(v_unprojected), 1, msg = f"Unprojected v (with gradients, in-place) are normalized", eps = 1e-6) cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref) cos = np.clip(cos, -1, 1) testutils.confirm_equal( np.arccos(cos), np.zeros((p_ref.shape[0],), dtype=float), msg = f"Unprojecting (normalized, with gradients, in-place) {intrinsics[0]}", eps = 1e-6) testutils.confirm_equal(dv_dq, dvnormalized_dqi_ref[...,:2], msg = f"dv_dq (normalized v, in-place): {intrinsics[0]}", worstcase = True, relative = True, eps = 0.01) testutils.confirm_equal(dv_di, dvnormalized_dqi_ref[...,2:], msg = f"dv_di (normalized v, in-place): {intrinsics[0]}", worstcase = True, relative = True, eps = 0.01) if 1: # un-normalized output out=[v_unprojected,dv_dq,dv_di] out[0] *= 0 out[1] *= 0 out[2] *= 0 mrcal.unproject(q_projected, *intrinsics, normalize = False, get_gradients = True, out = out) cos = nps.inner(v_unprojected, p_ref) / nps.mag(p_ref) cos = np.clip(cos, -1, 1) testutils.confirm_equal( np.arccos(cos), np.zeros((p_ref.shape[0],), dtype=float), msg = f"Unprojecting (non-normalized, with gradients, in-place) {intrinsics[0]}", eps = 1e-6) testutils.confirm_equal(dv_dq, dv_dqi_ref[...,:2], msg = f"dv_dq (unnormalized v, in-place): {intrinsics[0]}", worstcase = True, relative = True, eps = 0.01) testutils.confirm_equal(dv_di, dv_dqi_ref[...,2:], msg = f"dv_di (unnormalized v, in-place): {intrinsics[0]}", worstcase = True, relative = True, eps = 0.01)
########################################################################### # First a very basic gradient check. Looking at an arbitrary camera's # intrinsics. The test-gradients tool does this much more thoroughly optimization_inputs = copy.deepcopy(baseline) dp_packed = np.random.randn(len(p0)) * 1e-9 mrcal.ingest_packed_state(p0 + dp_packed, **optimization_inputs) x1 = mrcal.optimizer_callback(no_factorization=True, no_jacobian=True, **optimization_inputs)[1] dx_observed = x1 - x0 dx_predicted = nps.inner(J0, dp_packed) testutils.confirm_equal(dx_predicted, dx_observed, eps=1e-1, worstcase=True, relative=True, msg="Trivial, sanity-checking gradient check") if 0: import gnuplotlib as gp gp.plot( nps.cat( dx_predicted, dx_observed, ), _with='lines',
def remove_projection(a, proj_base): r'''Returns the normalized component of a orthogonal to proj_base proj_base assumed normalized''' v = a - nps.inner(a,proj_base)*proj_base return normalize(v)
def _compose_r(r0, r1, assume_r0_tiny=False, get_gradients=False): r'''Axis/angle rotation composition THIS IS TEMPORARY. WILL BE REDONE IN C, WITH DOCS AND TESTS Described here: Altmann, Simon L. "Hamilton, Rodrigues, and the Quaternion Scandal." Mathematics Magazine, vol. 62, no. 5, 1989, pp. 291–308 Available here: https://www.jstor.org/stable/2689481 I use Equations (19) and (20) on page 302 of this paper. These equations say that R(angle=gamma, axis=n) = compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) ) I need to compute gamma*n, and these are given as solutions to: cos(gamma/2) = cos(alpha/2)*cos(beta/2) - sin(alpha/2)*sin(beta/2) * inner(l,m) sin(gamma/2) n = sin(alpha/2)*cos(beta/2)*l + cos(alpha/2)*sin(beta/2)*m + sin(alpha/2)*sin(beta/2) * cross(l,m) For nicer notation, I define A = alpha/2 B = beta /2 C = gamma/2 l = r0 /(2A) m = r1 /(2B) n = r01/(2C) I rewrite: cos(C) = cos(A)*cos(B) - sin(A)*sin(B) * inner(r0,r1) / 4AB sin(C) r01 / 2C = sin(A)*cos(B)*r0 / 2A + cos(A)*sin(B)*r1 / 2B + sin(A)*sin(B) * cross(r0,r1) / 4AB If alpha ~ 0, I have A ~ 0, and I can simplify: cos(C) ~ cos(B) - A*sin(B) * inner(r0,r1) / 4AB sin(C) r01 / 2C ~ A*cos(B)* r0 / 2A + sin(B) * r1 / 2B + A*sin(B) * cross(r0,r1) / 4AB I have C = B + dB where dB ~ 0, so cos(C) ~ cos(B + dB) ~ cos(B) - dB sin(B) -> dB = A * inner(r0,r1) / 4AB = inner(r0,r1) / 4B -> C = B + inner(r0,r1) / 4B Now let's look at the axis expression. Assuming A ~ 0 sin(C) r01 / 2C ~ A*cos(B) r0 / 2A + sin(B) r1 / 2B + A*sin(B) * cross(r0,r1) / 4AB -> sin(C)/C * r01 ~ cos(B) r0 + sin(B) r1 / B + sin(B) * cross(r0,r1) / 2B I linearize the left-hand side: sin(C)/C = sin(B+dB)/(B+dB) ~ ~ sin(B)/B + d( sin(B)/B )/de dB = = sin(B)/B + dB (B cos(B) - sin(B)) / B^2 So (sin(B)/B + dB (B cos(B) - sin(B)) / B^2) r01 ~ cos(B) r0 + sin(B) r1 / B + sin(B) * cross(r0,r1) / 2B -> (sin(B) + dB (B cos(B) - sin(B)) / B) r01 ~ sin(B) r1 + cos(B)*B r0 + sin(B) * cross(r0,r1) / 2 I want to find the perturbation to give me r01 ~ r1 + deltar -> ( dB (B cos(B) - sin(B)) / B) r1 + (sin(B) + dB (B cos(B) - sin(B)) / B) deltar ~ cos(B)*B r0 + sin(B) * cross(r0,r1) / 2 All terms here are linear or quadratic in r0. For tiny r0, I can ignore the quadratic terms: ( dB (B cos(B) - sin(B)) / B) r1 + sin(B) deltar ~ cos(B)*B r0 + sin(B) * cross(r0,r1) / 2 I solve for deltar: deltar ~ cos(B)/sin(B)*B r0 + cross(r0,r1) / 2 - ( dB (B cos(B)/sin(B) - 1) / B) r1 I substitute in the dB from above, and I simplify: deltar ~ B/tan(B) r0 + cross(r0,r1) / 2 - ( inner(r0,r1) / 4B * (1/tan(B) - 1/B)) r1 And I differentiate: dr01/dr0 = ddeltar/dr0 = B/tan(B) I + -skew_symmetric(r1) / 2 - outer(r1,r1) / 4B * (1/tan(B) - 1/B) ''' if not assume_r0_tiny: if get_gradients: raise Exception("Not implemented") A = nps.mag(r0) / 2 B = nps.mag(r1) / 2 cos_C = np.cos(A) * np.cos(B) - np.sin(A) * np.sin(B) * nps.inner( r0, r1) / (4 * A * B) sin_C = np.sqrt(1. - cos_C * cos_C) th01 = np.arctan2(sin_C, cos_C) * 2. sin_C__a01 = np.sin(A) * np.cos(B) * r0 / ( 2 * A) + np.cos(A) * np.sin(B) * r1 / ( 2 * B) + np.sin(A) * np.sin(B) * np.cross(r0, r1) / (4 * A * B) a01 = sin_C__a01 / sin_C return a01 * th01 B = nps.mag(r1) / 2 if nps.norm2(r0) != 0: # for broadcasting if isinstance(B, np.ndarray): Bs = nps.dummy(B, -1) else: Bs = B r01 = r1 + \ Bs/np.tan(Bs) * r0 + \ np.cross(r0,r1) / 2 - \ ( nps.inner(r0,r1) / (4*Bs) * (1/np.tan(Bs) - 1/Bs))* r1 else: r01 = r1 if not get_gradients: return r01 # for broadcasting if isinstance(B, np.ndarray): Bs = nps.dummy(B, -1, -1) else: Bs = B dr01_dr0 = \ Bs/np.tan(Bs) * np.eye(3) + \ -mrcal.utils._skew_symmetric(r1) / 2 - \ nps.outer(r1,r1) / (4*Bs) * (1/np.tan(Bs) - 1/Bs) return r01, dr01_dr0
def stereo_rectify_prepare(models, az_fov_deg, el_fov_deg, az0_deg = None, el0_deg = 0, pixels_per_deg_az = None, pixels_per_deg_el = None): r'''Precompute everything needed for stereo rectification and matching SYNOPSIS import sys import mrcal import cv2 import numpy as np # Read commandline arguments: model0 model1 image0 image1 models = [ mrcal.cameramodel(sys.argv[1]), mrcal.cameramodel(sys.argv[2]), ] images = [ cv2.imread(sys.argv[i]) \ for i in (3,4) ] # Prepare the stereo system rectification_maps,cookie = \ mrcal.stereo_rectify_prepare(models, az_fov_deg = 120, el_fov_deg = 100) # Visualize the geometry of the two cameras and of the rotated stereo # coordinate system Rt_cam0_ref = models[0].extrinsics_Rt_fromref() Rt_cam0_stereo = cookie['Rt_cam0_stereo'] Rt_stereo_ref = mrcal.compose_Rt( mrcal.invert_Rt(Rt_cam0_stereo), Rt_cam0_ref ) rt_stereo_ref = mrcal.rt_from_Rt(Rt_stereo_ref) mrcal.show_geometry( models + [ rt_stereo_ref ], ( "camera0", "camera1", "stereo" ), show_calobjects = False, wait = True ) # Rectify the images images_rectified = \ [ mrcal.transform_image(images[i], rectification_maps[i]) \ for i in range(2) ] cv2.imwrite('/tmp/rectified0.jpg', images_rectified[0]) cv2.imwrite('/tmp/rectified1.jpg', images_rectified[1]) # Find stereo correspondences using OpenCV block_size = 3 max_disp = 160 # in pixels stereo = \ cv2.StereoSGBM_create(minDisparity = 0, numDisparities = max_disp, blockSize = block_size, P1 = 8 *3*block_size*block_size, P2 = 32*3*block_size*block_size, uniquenessRatio = 5, disp12MaxDiff = 1, speckleWindowSize = 50, speckleRange = 1) disparity16 = stereo.compute(*images_rectified) # in pixels*16 cv2.imwrite('/tmp/disparity.png', mrcal.apply_color_map(disparity16, 0, max_disp*16.)) # Convert the disparities to range to camera0 r = mrcal.stereo_range( disparity16.astype(np.float32) / 16., **cookie ) cv2.imwrite('/tmp/range.png', mrcal.apply_color_map(r, 5, 1000)) This function does the initial computation required to perform stereo matching, and to get ranges from a stereo pair. It computes - the pose of the rectified stereo coordinate system - the azimuth/elevation grid used in the rectified images - the rectification maps used to transform images into the rectified space Using the results of one call to this function we can compute the stereo disparities of many pairs of synchronized images. This function is generic: the two cameras may have any lens models, any resolution and any geometry. They don't even have to match. As long as there's some non-zero baseline and some overlapping views, we can set up stereo matching using this function. The input images are tranformed into a "rectified" space. Geometrically, the rectified coordinate system sits at the origin of camera0, with a rotation. The axes of the rectified coordinate system: - x: from the origin of camera0 to the origin of camera1 (the baseline direction) - y: completes the system from x,z - z: the "forward" direction of the two cameras, with the component parallel to the baseline subtracted off In a nominal geometry (the two cameras are square with each other, camera1 strictly to the right of camera0), the rectified coordinate system exactly matches the coordinate system of camera0. The above formulation supports any geometry, however, including vertical and/or forward/backward shifts. Vertical stereo is supported. Rectified images represent 3D planes intersecting the origins of the two cameras. The tilt of each plane is the "elevation". While the left/right direction inside each plane is the "azimuth". We generate rectified images where each pixel coordinate represents (x = azimuth, y = elevation). Thus each row scans the azimuths in a particular elevation, and thus each row in the two rectified images represents the same plane in 3D, and matching features in each row can produce a stereo disparity and a range. In the rectified system, elevation is a rotation along the x axis, while azimuth is a rotation normal to the resulting tilted plane. We produce rectified images whose pixel coordinates are linear with azimuths and elevations. This means that the azimuth angular resolution is constant everywhere, even at the edges of a wide-angle image. We return a set of transformation maps and a cookie. The maps can be used to generate rectified images. These rectified images can be processed by any stereo-matching routine to generate a disparity image. To interpret the disparity image, call stereo_unproject() or stereo_range() with the cookie returned here. The cookie is a Python dict that describes the rectified space. It is guaranteed to have the following keys: - Rt_cam0_stereo: an Rt transformation to map a representation of points in the rectified coordinate system to a representation in the camera0 coordinate system - baseline: the distance between the two cameras - az_row: an array of shape (Naz,) describing the azimuths in each row of the disparity image - el_col: an array of shape (Nel,1) describing the elevations in each column of the disparity image ARGUMENTS - models: an iterable of two mrcal.cameramodel objects representing the cameras in the stereo system. Any sane combination of lens models and resolutions and geometries is valid - az_fov_deg: required value for the azimuth (along-the-baseline) field-of-view of the desired rectified view, in pixels - el_fov_deg: required value for the elevation (across-the-baseline) field-of-view of the desired rectified view, in pixels - az0_deg: optional value for the azimuth center of the rectified images. This is especially significant in a camera system with some forward/backward shift. That causes the baseline to no longer be perpendicular with the view axis of the cameras, and thus azimuth = 0 is no longer at the center of the input images. If omitted, we compute the center azimuth that aligns with the center of the cameras' view - el0_deg: optional value for the elevation center of the rectified system. Defaults to 0. - pixels_per_deg_az: optional value for the azimuth resolution of the rectified image. If omitted (or None), we use the resolution of the input image at (azimuth, elevation) = 0. If a resolution of <0 is requested, we use this as a scale factor on the resolution of the input image. For instance, to downsample by a factor of 2, pass pixels_per_deg_az = -0.5 - pixels_per_deg_el: same as pixels_per_deg_az but in the elevation direction RETURNED VALUES We return a tuple - transformation maps: a tuple of length-2 containing transformation maps for each camera. Each map can be used to mrcal.transform_image() images to the rectified space - cookie: a dict describing the rectified space. Intended as input to stereo_unproject() and stereo_range(). See the description above for more detail ''' if len(models) != 2: raise Exception("I need exactly 2 camera models") def normalize(v): v /= nps.mag(v) return v def remove_projection(a, proj_base): r'''Returns the normalized component of a orthogonal to proj_base proj_base assumed normalized''' v = a - nps.inner(a,proj_base)*proj_base return normalize(v) ######## Compute the geometry of the rectified stereo system. This is a ######## rotation, centered at camera0. More or less we have axes: ######## ######## x: from camera0 to camera1 ######## y: completes the system from x,z ######## z: component of the cameras' viewing direction ######## normal to the baseline Rt_cam0_ref = models[0].extrinsics_Rt_fromref() Rt01 = mrcal.compose_Rt( Rt_cam0_ref, models[1].extrinsics_Rt_toref()) Rt10 = mrcal.invert_Rt(Rt01) # Rotation relating camera0 coords to the rectified camera coords. I fill in # each row separately R_stereo_cam0 = np.zeros((3,3), dtype=float) right = R_stereo_cam0[0,:] down = R_stereo_cam0[1,:] forward = R_stereo_cam0[2,:] # "right" of the rectified coord system: towards the origin of camera1 from # camera0, in camera0 coords right[:] = Rt01[3,:] baseline = nps.mag(right) right /= baseline # "forward" for each of the two cameras, in the cam0 coord system forward0 = np.array((0,0,1.)) forward1 = Rt01[:3,2] # "forward" of the rectified coord system, in camera0 coords. The mean of # the two non-right "forward" directions forward[:] = normalize( ( remove_projection(forward0,right) + remove_projection(forward1,right) ) / 2. ) # "down" of the rectified coord system, in camera0 coords. Completes the # right,down,forward coordinate system down[:] = np.cross(forward,right) R_cam0_stereo = nps.transpose(R_stereo_cam0) ######## Done with the geometry! Now to get the az/el grid. I need to figure ######## out the resolution and the extents if az0_deg is not None: az0 = az0_deg * np.pi/180. else: # In the rectified system az=0 sits perpendicular to the baseline. # Normally the cameras are looking out perpendicular to the baseline # also, so I center my azimuth samples around 0 to match the cameras' # field of view. But what if the geometry isn't square, and one camera # is behind the other? Like this: # # camera # view # ^ # | # \ | / # \_/ # . / # . /az=0 # ./ # . # baseline . # . # \ / # \_/ # # Here the center-of-view axis of each camera is not at all # perpendicular to the baseline. Thus I compute the mean "forward" # direction of the cameras in the rectified system, and set that as the # center azimuth az0. v0 = nps.matmult( forward0, R_cam0_stereo ).ravel() v1 = nps.matmult( forward1, R_cam0_stereo ).ravel() v0[1] = 0.0 v1[1] = 0.0 normalize(v0) normalize(v1) v = v0 + v1 az0 = np.arctan2(v[0],v[2]) el0 = el0_deg * np.pi/180. ####### Rectified image resolution if pixels_per_deg_az is None or pixels_per_deg_az < 0 or \ pixels_per_deg_el is None or pixels_per_deg_el < 0: # I need to compute the resolution of the rectified images. I try to # match the resolution of the cameras. I just look at camera0. If you # have different cameras, pass in pixels_per_deg yourself :) # # I look at the center of the stereo field of view. There I have q = # project(v) where v is a unit projection vector. I compute dq/dth where # th is an angular perturbation applied to v. def rotation_any_v_to_z(v): r'''Return any rotation matrix that maps the given unit vector v to [0,0,1]''' z = v if np.abs(v[0]) < .9: x = np.array((1,0,0)) else: x = np.array((0,1,0)) x -= nps.inner(x,v)*v x /= nps.mag(x) y = np.cross(z,x) return nps.cat(x,y,z) v, dv_dazel = stereo_unproject(az0, el0, get_gradients = True) v0 = mrcal.rotate_point_R(R_cam0_stereo, v) dv0_dazel = nps.matmult(R_cam0_stereo, dv_dazel) _,dq_dv0,_ = mrcal.project(v0, *models[0].intrinsics(), get_gradients = True) # I rotate my v to a coordinate system where u = rotate(v) is [0,0,1]. # Then u = [a,b,0] are all orthogonal to v. So du/dth = [cos, sin, 0]. # I then have dq/dth = dq/dv dv/du [cos, sin, 0]t # ---> dq/dth = dq/dv dv/du[:,:2] [cos, sin]t = M [cos,sin]t # # norm2(dq/dth) = [cos,sin] MtM [cos,sin]t is then an ellipse with the # eigenvalues of MtM giving me the best and worst sensitivities. I can # use mrcal.worst_direction_stdev() to find the densest direction. But I # actually know the directions I care about, so I evaluate them # independently for the az and el directions # Ruv = rotation_any_v_to_z(v0) # M = nps.matmult(dq_dv0, nps.transpose(Ruv[:2,:])) # # I pick the densest direction: highest |dq/dth| # pixels_per_rad = mrcal.worst_direction_stdev( nps.matmult( nps.transpose(M),M) ) if pixels_per_deg_az is None or pixels_per_deg_az < 0: dq_daz = nps.inner( dq_dv0, dv0_dazel[:,0] ) pixels_per_rad_az_have = nps.mag(dq_daz) if pixels_per_deg_az is not None: # negative pixels_per_deg_az requested means I use the requested # value as a scaling pixels_per_deg_az = -pixels_per_deg_az * pixels_per_rad_az_have*np.pi/180. else: pixels_per_deg_az = pixels_per_rad_az_have*np.pi/180. if pixels_per_deg_el is None or pixels_per_deg_el < 0: dq_del = nps.inner( dq_dv0, dv0_dazel[:,1] ) pixels_per_rad_el_have = nps.mag(dq_del) if pixels_per_deg_el is not None: # negative pixels_per_deg_el requested means I use the requested # value as a scaling pixels_per_deg_el = -pixels_per_deg_el * pixels_per_rad_el_have*np.pi/180. else: pixels_per_deg_el = pixels_per_rad_el_have*np.pi/180. Naz = round(az_fov_deg*pixels_per_deg_az) Nel = round(el_fov_deg*pixels_per_deg_el) # Adjust the fov to keep the requested resolution and pixel counts az_fov_radius_deg = Naz / (2.*pixels_per_deg_az) el_fov_radius_deg = Nel / (2.*pixels_per_deg_el) # shape (Naz,) az = np.linspace(az0 - az_fov_radius_deg*np.pi/180., az0 + az_fov_radius_deg*np.pi/180., Naz) # shape (Nel,1) el = nps.dummy( np.linspace(el0 - el_fov_radius_deg*np.pi/180., el0 + el_fov_radius_deg*np.pi/180., Nel), -1 ) # v has shape (Nel,Naz,3) v = stereo_unproject(az, el) v0 = nps.matmult( nps.dummy(v, -2), R_stereo_cam0 )[...,0,:] v1 = nps.matmult( nps.dummy(v0, -2), Rt01[:3,:] )[...,0,:] cookie = \ dict( Rt_cam0_stereo = nps.glue(R_cam0_stereo, np.zeros((3,)), axis=-2), baseline = baseline, az_row = az, el_col = el, # The caller should NOT assume these are available in the cookie: # some other rectification scheme may not produce linear az/el # maps pixels_per_deg_az = pixels_per_deg_az, pixels_per_deg_el = pixels_per_deg_el, ) return \ (mrcal.project( v0, *models[0].intrinsics()).astype(np.float32), \ mrcal.project( v1, *models[1].intrinsics()).astype(np.float32)), \ cookie
def callback_l2_geometric(p, v0, v1, t01): if p[2] < 0: return 1e6 distance_p_v0 = nps.mag(p - nps.inner(p, v0) / nps.norm2(v0) * v0) distance_p_v1 = nps.mag(p - t01 - nps.inner(p - t01, v1) / nps.norm2(v1) * v1) return np.abs(distance_p_v0) + np.abs(distance_p_v1)
def compute_outliernesses(J, x, jq, k_dima, k_cook): '''Computes all the outlierness/Cook's D metrics I have 8 things I can compute coming from 3 yes/no choices. These are all very similar, with two pairs actually coming out identical. I choose: - Are we detecting outliers, or looking at effects of a new query point? - Dima's outlierness factor or Cook's D - Look ONLY at the effect on the other variables, or on the other variables AND self? If we're detecting outliers, we REMOVE measurements from the dataset, and see what happens to the fit. If we're looking at effects of a new query point, we see what happend if we ADD measurements Dima's outlierness factor metric looks at what happens to the cost function E = norm2(x). Specifically I look at (norm2(x_before) - norm(x_after))/Nmeasurements Cook's D instead looks at (norm2(x_before - x_after)) * k for some constant k. Finally, we can decide whether to include the effects on the measurements we're adding/removing, or not. Note that here I only look at adding/removing SCALAR measurements ============= This is similar-to, but not exactly-the-same-as Cook's D. I assume the least squares fit optimizes a cost function E = norm2(x). The outlierness factor I return is f = 1/Nmeasurements (E(outliers and inliers) - E(inliers only)) For a scalar measurement, this solves to k = xo^2 / Nmeasurements B = 1.0/(jt inv(JtJ) j - 1) f = -k * B (see the comment in dogleg_getOutliernessFactors() for a description) Note that my metric is proportional to norm2(x_io) - norm2(x_i). This is NOT the same as Cook's distance, which is proportional to norm2(x_io - x_i). It's not yet obvious to me which is better There're several slightly-different definitions of Cook's D and of a rule-of-thumb threshold floating around on the internet. Wikipedia says: D = norm2(x_io - x_i)^2 / (Nstate * norm2(x_io)/(Nmeasurements - Nstate)) D_threshold = 1 An article https://www.nature.com/articles/nmeth.3812 says D = norm2(x_io - x_i)^2 / ((Nstate+1) * norm2(x_io)/(Nmeasurements - Nstate -1)) D_threshold = 4/Nmeasurements Here I use the second definition. That definition expands to k = xo^2 / ((Nstate+1) * norm2(x_io)/(Nmeasurements - Nstate -1)) B = 1.0/(jt inv(JtJ) j - 1) f = k * (B + B*B) ''' Nmeasurements,Nstate = J.shape # The A values for each measurement Aoutliers = nps.inner(J, nps.transpose(np.linalg.pinv(J))) Aquery = nps.inner(jq, nps.transpose(np.linalg.solve(nps.matmult(nps.transpose(J),J), nps.transpose(jq)))) def dima(): k = k_dima k = 1 # Here the metrics are linear, so self + others = self_others def outliers(): B = 1.0 / (Aoutliers - 1.0) return dict( self = k * x*x, others = k * x*x*(-B-1), self_others = k * x*x*(-B )) def query(): B = 1.0 / (Aquery + 1.0) return dict( self = k * ( B*B), others = k * (B-B*B), self_others = k * (B)) return dict(outliers = outliers(), query = query()) def cook(): k = k_cook k = 1 # Here the metrics maybe aren't linear (I need to think about it), so # maybe self + others != self_others. I thus am not returning the "self" # metric def outliers(): B = 1.0 / (Aoutliers - 1.0) return dict( self_others = k * x*x*(B+B*B ) , others = k * x*x*(-B-1)) def query(): B = 1.0 / (Aquery + 1.0) return dict( self_others = k * (1-B) , others = k * (B-B*B)) return dict(outliers = outliers(), query = query()) return dict(cook = cook(), dima = dima())
v0_rect = mrcal.unproject_latlon(np.array((az0, el0))) # already normalized testutils.confirm_equal( v0_rect, v0, msg=f'vanilla stereo: az0,el0 represent the same point ({lensmodel})') else: v0_rect = mrcal.unproject_pinhole(np.array((np.tan(az0), np.tan(el0)))) v0_rect /= nps.mag(v0_rect) testutils.confirm_equal( v0_rect, v0, msg=f'vanilla stereo: az0,el0 represent the same point ({lensmodel})', eps = 1e-3) dq0x = np.array((1e-1, 0)) dq0y = np.array((0, 1e-1)) v0x = mrcal.unproject(q0+dq0x, *models_rectified[0].intrinsics()) v0y = mrcal.unproject(q0+dq0y, *models_rectified[0].intrinsics()) dthx = np.arccos(nps.inner(v0x,v0)/np.sqrt(nps.norm2(v0x)*nps.norm2(v0))) dthy = np.arccos(nps.inner(v0y,v0)/np.sqrt(nps.norm2(v0y)*nps.norm2(v0))) pixels_per_rad_az_rect = nps.mag(dq0x)/dthx pixels_per_rad_el_rect = nps.mag(dq0y)/dthy q0_cam0 = mrcal.project(mrcal.rotate_point_R(Rt_cam0_rect[:3,:], v0), *model0.intrinsics()) q0x_cam0 = mrcal.project(mrcal.rotate_point_R(Rt_cam0_rect[:3,:], v0x), *model0.intrinsics()) q0y_cam0 = mrcal.project(mrcal.rotate_point_R(Rt_cam0_rect[:3,:], v0y), *model0.intrinsics()) pixels_per_rad_az_cam0 = nps.mag(q0x_cam0 - q0_cam0)/dthx pixels_per_rad_el_cam0 = nps.mag(q0y_cam0 - q0_cam0)/dthy testutils.confirm_equal(pixels_per_rad_az_rect * 8., pixels_per_rad_az_cam0,
# a few points, some wide, some not. Some behind the camera p = np.array(((1.0, 2.0, 10.0), (-1.1, 0.3, -1.0), (-0.9, -1.5, -1.0))) q_projected_ref = np.array([[649.35582325, 552.6874014], [-5939.33490417, 1624.58376866], [-2181.52681292, -2953.8803086]]) q_projected = mrcal.project_stereographic(p, fx, fy, cx, cy) testutils.confirm_equal(q_projected, q_projected_ref, msg=f"Projecting", eps=1e-3) p_unprojected = mrcal.unproject_stereographic(q_projected, fx, fy, cx, cy) cos = nps.inner(p_unprojected, p) / (nps.mag(p) * nps.mag(p_unprojected)) cos = np.clip(cos, -1, 1) testutils.confirm_equal(np.arccos(cos), np.zeros((p.shape[0], ), dtype=float), msg="Unprojecting", eps=1e-6) # Now gradients for project() delta = 1e-6 q_projected, dq_dp_reported = mrcal.project_stereographic(p, fx, fy, cx, cy, get_gradients=True) testutils.confirm_equal(q_projected,
Nw = 40 Nh = 30 # shape (Nh,Nw,2) xy = \ nps.mv(nps.cat(*np.meshgrid( np.linspace(0,W-1,Nw), np.linspace(0,H-1,Nh) )), 0,-1) fxy = m.intrinsics()[1][0:2] cxy = m.intrinsics()[1][2:4] # shape (Nh,Nw,2) v = mrcal.unproject(np.ascontiguousarray(xy), *m.intrinsics()) v0 = mrcal.unproject(cxy, *m.intrinsics()) # shape (Nh,Nw) costh = nps.inner(v, v0) / (nps.mag(v) * nps.mag(v0)) th = np.arccos(costh) # shape (Nh,Nw,2) xy_rel = xy - cxy # shape (Nh,Nw) az = np.arctan2(xy_rel[..., 1], xy_rel[..., 0]) if args.scheme == 'stereographic': r = np.tan(th / 2.) * 2. elif args.scheme == 'equidistant': r = th elif args.scheme == 'equisolidangle': r = np.sin(th / 2.) * 2. elif args.scheme == 'orthographic': r = np.sin(th) elif args.scheme == 'pinhole': r = np.tan(th) else: print( "Unknown scheme {args.scheme}. Shouldn't happen. argparse should have taken care of it"