/** * Computes the pixel depth from N views of the same object. Pixel depth in the first frame. * * @param obs List of observations on a single feature in normalized coordinates * @param motion List of camera motions. Each index 'i' is the motion from view 0 to view i+1. * @return depth of the pixels */ public double depthNView( List<Point2D_F64> obs , List<Se3_F64> motion ) { double top = 0, bottom = 0; Point2D_F64 a = obs.get(0); for( int i = 1; i < obs.size(); i++ ) { Se3_F64 se = motion.get(i-1); Point2D_F64 b = obs.get(i); GeometryMath_F64.multCrossA(b, se.getR(), temp0); GeometryMath_F64.mult(temp0,a,temp1); GeometryMath_F64.cross(b, se.getT(), temp2); top += temp2.x+temp2.y+temp2.z; bottom += temp1.x+temp1.y+temp1.z; } return -top/bottom; }
/** * Computes the pixel depth from N views of the same object. Pixel depth in the first frame. * * @param obs List of observations on a single feature in normalized coordinates * @param motion List of camera motions. Each index 'i' is the motion from view 0 to view i+1. * @return depth of the pixels */ public double depthNView( List<Point2D_F64> obs , List<Se3_F64> motion ) { double top = 0, bottom = 0; Point2D_F64 a = obs.get(0); for( int i = 1; i < obs.size(); i++ ) { Se3_F64 se = motion.get(i-1); Point2D_F64 b = obs.get(i); GeometryMath_F64.multCrossA(b, se.getR(), temp0); GeometryMath_F64.mult(temp0,a,temp1); GeometryMath_F64.cross(b, se.getT(), temp2); top += temp2.x+temp2.y+temp2.z; bottom += temp1.x+temp1.y+temp1.z; } return -top/bottom; }
/** * Specify the fundamental matrix and the camera 2 epipole. * * @param F Fundamental matrix. * @param e2 Epipole for camera 2. If null it will be computed internally. */ public void setFundamental( DMatrixRMaj F , Point3D_F64 e2 ) { if( e2 != null ) this.e2.set(e2); else { MultiViewOps.extractEpipoles(F,new Point3D_F64(),this.e2); } GeometryMath_F64.multCrossA(this.e2,F,A); }
/** * Specify the fundamental matrix and the camera 2 epipole. * * @param F Fundamental matrix. * @param e2 Epipole for camera 2. If null it will be computed internally. */ public void setFundamental( DenseMatrix64F F , Point3D_F64 e2 ) { if( e2 != null ) this.e2.set(e2); else { MultiViewOps.extractEpipoles(F,new Point3D_F64(),this.e2); } GeometryMath_F64.multCrossA(this.e2,F,A); }
/** * Specify the fundamental matrix and the camera 2 epipole. * * @param F Fundamental matrix. * @param e2 Epipole for camera 2. If null it will be computed internally. */ public void setFundamental( DenseMatrix64F F , Point3D_F64 e2 ) { if( e2 != null ) this.e2.set(e2); else { MultiViewOps.extractEpipoles(F,new Point3D_F64(),this.e2); } GeometryMath_F64.multCrossA(this.e2,F,A); }
/** * Specify the fundamental matrix and the camera 2 epipole. * * @param F Fundamental matrix. * @param e2 Epipole for camera 2. If null it will be computed internally. */ public void setFundamental( DMatrixRMaj F , Point3D_F64 e2 ) { if( e2 != null ) this.e2.set(e2); else { MultiViewOps.extractEpipoles(F,new Point3D_F64(),this.e2); } GeometryMath_F64.multCrossA(this.e2,F,A); }
/** * 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); }
/** * 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 ) { DMatrixRMaj 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); }
@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]; } }
GeometryMath_F64.multCrossA(e2, F, KR); CommonOps_DDRM.add(KR, outer, KR);
/** * 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); }
/** * 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); }