@Override public int computeResiduals(AssociatedPair p, double[] residuals, int index) { GeometryMath_F64.mult(H, p.p1, temp); residuals[index++] = temp.x-p.p2.x; residuals[index++] = temp.y-p.p2.y; return index; }
/** * Computes pixel depth in image 'a' from two observations. * * @param a Observation in first frame. In calibrated coordinates. Not modified. * @param b Observation in second frame. In calibrated coordinates. Not modified. * @param fromAtoB Transform from frame a to frame b. * @return Pixel depth in first frame. In same units as T inside of fromAtoB. */ public double depth2View( Point2D_F64 a , Point2D_F64 b , Se3_F64 fromAtoB ) { DenseMatrix64F R = fromAtoB.getR(); Vector3D_F64 T = fromAtoB.getT(); GeometryMath_F64.multCrossA(b, R, temp0); GeometryMath_F64.mult(temp0,a,temp1); GeometryMath_F64.cross(b, T, temp2); return -(temp2.x+temp2.y+temp2.z)/(temp1.x+temp1.y+temp1.z); }
/** * W=[H*a,H*b,hat(H*a)*H*b] */ private void setW( DenseMatrix64F W, DenseMatrix64F H , Vector3D_F64 a , Vector3D_F64 b ) { GeometryMath_F64.mult(H,b, tempV); setColumn(W,1, tempV); GeometryMath_F64.mult(H,a, tempV); setColumn(W,0, tempV); GeometryMath_F64.crossMatrix(tempV,Hv2); CommonOps.mult(Hv2,H,tempM); GeometryMath_F64.mult(tempM,b, tempV); setColumn(W,2, tempV); }
/** * 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.add(lf,top/bottom,el,H); // pick a good scale and sign for H adjust.adjust(H, point); }
/** * R = W*U^T * N = v2 cross u * (1/d)*T = (H-R)*N */ private void createSolution( DenseMatrix64F W , DenseMatrix64F U , Vector3D_F64 u , DenseMatrix64F H , Se3_F64 se , Vector3D_F64 N ) { CommonOps.multTransB(W,U,se.getR()); GeometryMath_F64.cross(v2,u,N); CommonOps.subtract(H, se.getR(), tempM); GeometryMath_F64.mult(tempM,N,se.getT()); }
@Override public void process(double[] input, double[] output) { // given the current value of v, compute P3 // P3 = [cross(T31)'*F31 + T31*v' | T31 ] computeP3(input); // [R,T] = [P3*P2 | P3*u] CommonOps_DDRM.mult(P3, P2inv, R); GeometryMath_F64.mult(P3, u,a); // Compute the new fundamental matrix GeometryMath_F64.multCrossA(a, R, F32_est); // TODO sign ambuguity? // Put into a common scale so that it can be compared against F32 double n = NormOps_DDRM.normF(F32_est); CommonOps_DDRM.scale(1.0/n,F32_est); for (int i = 0; i < 9; i++) { output[i] = F32_est.data[i] - F32.data[i]; } }
double a0 = GeometryMath_F64.dot(e2,line0.l2); double a1 = GeometryMath_F64.dot(e2,line1.l2); GeometryMath_F64.multTran(A,line0.l2,Al0); GeometryMath_F64.multTran(A,line1.l2,Al1); GeometryMath_F64.cross(intersect0.slope,from0to1,pi.n); pi.p.set(intersect0.p); GeometryMath_F64.outerProd(e2,v,av); CommonOps.subtract(A, av, H);
/** * U=[a,b,hat(a)*b] */ private void setU( DMatrixRMaj U, Vector3D_F64 a , Vector3D_F64 b ) { setColumn(U, 0, a); setColumn(U, 1, b); GeometryMath_F64.cross(a, b, tempV); setColumn(U, 2, tempV); }
/** * Estimates the homography from view 1 to view 2 induced by a plane from 3 point associations. * Each pair must pass the epipolar constraint. This can fail if the points are colinear. * * @param p1 Associated point observation * @param p2 Associated point observation * @param p3 Associated point observation * @return True if successful or false if it failed */ public boolean process(AssociatedPair p1, AssociatedPair p2, AssociatedPair p3) { // Fill rows of M with observations from image 1 fillM(p1.p1,p2.p1,p3.p1); // Compute 'b' vector b.x = computeB(p1.p2); b.y = computeB(p2.p2); b.z = computeB(p3.p2); // A_inv_b = inv(A)*b if( !solver.setA(M) ) return false; GeometryMath_F64.toMatrix(b,temp0); solver.solve(temp0,temp1); GeometryMath_F64.toTuple3D(temp1, A_inv_b); GeometryMath_F64.addOuterProd(A, -1, e2, A_inv_b, H); // pick a good scale and sign for H adjust.adjust(H, p1); return true; }
/** * <p> * Trifocal tensor with line-line-line correspondence:<br> * (l2<sup>T</sup>*[T1,T2,T3]*L2)*[l1]<sub>x</sub> = 0 * </p> * * @param tensor Trifocal tensor * @param l1 A line in the first view. * @param l2 A line in the second view. * @param l3 A line in the third view. * @param ret Storage for output. If null a new instance will be declared. * @return Result of applying the constraint. With perfect inputs will be zero. */ public static Vector3D_F64 constraint(TrifocalTensor tensor, Vector3D_F64 l1, Vector3D_F64 l2, Vector3D_F64 l3, Vector3D_F64 ret) { if( ret == null ) ret = new Vector3D_F64(); double x = GeometryMath_F64.innerProd(l2, tensor.T1, l3); double y = GeometryMath_F64.innerProd(l2, tensor.T2, l3); double z = GeometryMath_F64.innerProd(l2, tensor.T3, l3); GeometryMath_F64.cross(new Vector3D_F64(x, y, z), l1, ret); return ret; }
/** * <p> * Trifocal tensor with point-line-point correspondence:<br> * (l2<sup>T</sup>(sum p1<sup>i</sup>*T<sub>i</sub>)[p3]<sub>x</sub> = 0 * </p> * * @param tensor Trifocal tensor * @param p1 A point in the first view. * @param l2 A line in the second view. * @param p3 A point in the third view. * @return Result of applying the constraint. With perfect inputs will be zero. */ public static Vector3D_F64 constraint(TrifocalTensor tensor, Point2D_F64 p1, Vector3D_F64 l2, Point2D_F64 p3, Vector3D_F64 ret) { if( ret == null ) ret = new Vector3D_F64(); DenseMatrix64F sum = new DenseMatrix64F(3,3); CommonOps.add(p1.x,tensor.T1,sum,sum); CommonOps.add(p1.y,tensor.T2,sum,sum); CommonOps.add(tensor.T3,sum,sum); Vector3D_F64 tempV = new Vector3D_F64(); GeometryMath_F64.multTran(sum, l2, tempV); GeometryMath_F64.cross(tempV, new Vector3D_F64(p3.x, p3.y, 1), ret); return ret; }
/** * <p> * Applies the epipolar relationship constraint to an essential or fundamental matrix:<br> * 0 = p2<sup>T</sup>*F*p1<br> * Input points are in normalized image coordinates for an essential matrix and pixels for * fundamental. * </p> * * @param F 3x3 essential or fundamental matrix. * @param p1 Point in view 1. * @param p2 Point in view 2. * @return Constraint value. */ public static double constraint( DMatrixRMaj F , Point2D_F64 p1, Point2D_F64 p2 ) { return GeometryMath_F64.innerProd(p2,F,p1); }
@Override public boolean generate(List<PointVectorNN> dataSet, PlaneGeneral3D_F64 output) { PointVectorNN pa = dataSet.get(0); PointVectorNN pb = dataSet.get(1); PointVectorNN pc = dataSet.get(2); // find the plane's normal vector GeometryMath_F64.sub(pa.p, pb.p, a); GeometryMath_F64.sub(pa.p, pc.p, b); GeometryMath_F64.cross(a, b, n); n.normalize(); if (!checkModel(pa, pb, pc)) return false; // asume the first point is one the plane planeNormal.n = n; planeNormal.p = pa.p; UtilPlane3D_F64.convert(planeNormal, output); return check.valid(output); }
GeometryMath_F64.crossMatrix(e2, crossMatrix); GeometryMath_F64.outerProd(e2,v,outer);
/** * <p> * Computes an essential matrix from a rotation and translation. This motion * is the motion from the first camera frame into the second camera frame. The essential * matrix 'E' is defined as:<br> * E = hat(T)*R<br> * where hat(T) is the skew symmetric cross product matrix for vector T. * </p> * * @param R Rotation matrix. * @param T Translation vector. * @param E (Output) Storage for essential matrix. 3x3 matrix * @return Essential matrix */ public static DMatrixRMaj createEssential(DMatrixRMaj R, Vector3D_F64 T, @Nullable DMatrixRMaj E) { if( E == null ) E = new DMatrixRMaj(3,3); DMatrixRMaj T_hat = GeometryMath_F64.crossMatrix(T, null); CommonOps_DDRM.mult(T_hat, R, E); return E; }
@Override public double computeResidual(AssociatedPair observation) { double bottom = 0; GeometryMath_F64.mult(F, observation.p1, temp); bottom += temp.x*temp.x + temp.y*temp.y; GeometryMath_F64.multTran(F, observation.p2, temp); bottom += temp.x*temp.x + temp.y*temp.y; if( bottom == 0) { return Double.MAX_VALUE; } else { GeometryMath_F64.multTran(F,observation.p2,temp); return (temp.x*observation.p1.x + temp.y*observation.p1.y + temp.z)/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); }