def esserrf_tq(prm,qray,dray,pr,wRd,domidx): # set variables dRq = np.dot(tp(wRd),geom.RfromYPR(prm[2],pr[0],pr[1])) td = np.dot(tp(wRd),geom.normalrows(np.insert(prm[:2],domidx,1))) E = np.dot(tp(dRq),geom.xprodmat(td)) # Compute homography error return np.sum( qray * tp(np.dot(E,tp(dray))) , 1 )
def esserrf_tq(prm, qray, dray, pr, wRd, domidx): # set variables dRq = np.dot(tp(wRd), geom.RfromYPR(prm[2], pr[0], pr[1])) td = np.dot(tp(wRd), geom.normalrows(np.insert(prm[:2], domidx, 1))) E = np.dot(tp(dRq), geom.xprodmat(td)) # Compute homography error return np.sum(qray * tp(np.dot(E, tp(dray))), 1)
def errE_t(prm,qray,dray,constants,domidx): # set variables wRq, wRd, qYaw = constants dRq = np.dot(tp(wRd),wRd) td = np.dot(tp(wRd),prm) E = np.dot(tp(dRq),geom.xprodmat(td)) # Compute homography error return np.abs( np.sum( qray * tp(np.dot(E,tp(dray))) , 1 ) )
def errE_t(prm, qray, dray, constants, domidx): # set variables wRq, wRd, qYaw = constants dRq = np.dot(tp(wRd), wRd) td = np.dot(tp(wRd), prm) E = np.dot(tp(dRq), geom.xprodmat(td)) # Compute homography error return np.abs(np.sum(qray * tp(np.dot(E, tp(dray))), 1))