/** * b = [(x cross (A*x))^T ( x cross e2 )] / || x cross e2 ||^2 */ private double computeB( Point2D_F64 x ) { GeometryMath_F64.mult(A,x,Ax); GeometryMath_F64.cross(x,Ax,t0); GeometryMath_F64.cross(x,e2,t1); double top = GeometryMath_F64.dot(t0,t1); double bottom = t1.normSq(); return top/bottom; }
/** * b = [(x cross (A*x))^T ( x cross e2 )] / || x cross e2 ||^2 */ private double computeB( Point2D_F64 x ) { GeometryMath_F64.mult(A,x,Ax); GeometryMath_F64.cross(x,Ax,t0); GeometryMath_F64.cross(x,e2,t1); double top = GeometryMath_F64.dot(t0,t1); double bottom = t1.normSq(); return top/bottom; }
/** * Computes the homography based on a line and point on the plane * @param line Line on the plane * @param point Point on the plane */ public void process(PairLineNorm line, AssociatedPair point) { // t0 = (F*x) cross l' GeometryMath_F64.mult(F,point.p1,Fx); GeometryMath_F64.cross(Fx,line.getL2(),t0); // t1 = x' cross ((f*x) cross l') GeometryMath_F64.cross(point.p2, t0, t1); // t0 = x' cross e' GeometryMath_F64.cross(point.p2,e2,t0); double top = GeometryMath_F64.dot(t0,t1); double bottom = t0.normSq()*(line.l1.x*point.p1.x + line.l1.y*point.p1.y + line.l1.z); // e' * l^T GeometryMath_F64.outerProd(e2, line.l1, el); // cross(l')*F GeometryMath_F64.multCrossA(line.l2, F, lf); CommonOps_DDRM.add(lf,top/bottom,el,H); // pick a good scale and sign for H adjust.adjust(H, point); }
/** * Computes the homography based on a line and point on the plane * @param line Line on the plane * @param point Point on the plane */ public void process(PairLineNorm line, AssociatedPair point) { // t0 = (F*x) cross l' GeometryMath_F64.mult(F,point.p1,Fx); GeometryMath_F64.cross(Fx,line.getL2(),t0); // t1 = x' cross ((f*x) cross l') GeometryMath_F64.cross(point.p2, t0, t1); // t0 = x' cross e' GeometryMath_F64.cross(point.p2,e2,t0); double top = GeometryMath_F64.dot(t0,t1); double bottom = t0.normSq()*(line.l1.x*point.p1.x + line.l1.y*point.p1.y + line.l1.z); // e' * l^T GeometryMath_F64.outerProd(e2, line.l1, el); // cross(l')*F GeometryMath_F64.multCrossA(line.l2, F, lf); CommonOps.add(lf,top/bottom,el,H); // pick a good scale and sign for H adjust.adjust(H, point); }