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))
