/** * 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 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; }
/** * 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); }
/** * 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; }
/** * U=[a,b,hat(a)*b] */ private void setU( DenseMatrix64F 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); }
/** * 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); }
@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); }
/** * 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 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); }
/** * 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); }
/** * <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, @Nullable 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; }
/** * 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()); }
/** * R = W*U^T * N = v2 cross u * (1/d)*T = (H-R)*N */ private void createSolution( DMatrixRMaj W , DMatrixRMaj U , Vector3D_F64 u , DMatrixRMaj H , Se3_F64 se , Vector3D_F64 N ) { CommonOps_DDRM.multTransB(W,U,se.getR()); GeometryMath_F64.cross(v2,u,N); CommonOps_DDRM.subtract(H, se.getR(), tempM); GeometryMath_F64.mult(tempM,N,se.getT()); }
private void createRotation( PlaneNormal3D_F64 left , PlaneNormal3D_F64 right , DenseMatrix64F R ) { v0.set(left.n); v1.set(right.n); GeometryMath_F64.cross(v0, v1, v2); GeometryMath_F64.cross(v2, v0, v1); // columns represent the coordinates in the rotated space of unit vectors along the axes of the original space. R.unsafe_set(0,0,v0.x); R.unsafe_set(1,0,v0.y); R.unsafe_set(2,0,v0.z); R.unsafe_set(0,1,v1.x); R.unsafe_set(1,1,v1.y); R.unsafe_set(2,1,v1.z); R.unsafe_set(0,2,v2.x); R.unsafe_set(1,2,v2.y); R.unsafe_set(2,2,v2.z); }
/** * <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; }
/** * Selects axises of new coordinate system */ private void selectAxises(SimpleMatrix R, SimpleMatrix c1, SimpleMatrix c2) { // --------- Compute the new x-axis v1.set(c2.get(0) - c1.get(0), c2.get(1) - c1.get(1), c2.get(2) - c1.get(2)); v1.normalize(); // --------- Compute the new y-axis // cross product of old z axis and new x axis // According to the paper [1] this choice is arbitrary, however it is not. By selecting // the original axis the similarity with the first view is maximized. The other extreme // would be to make it perpendicular, resulting in an unusable rectification. // extract old z-axis from rotation matrix Vector3D_F64 oldZ = new Vector3D_F64(R.get(2,0), R.get(2,1), R.get(2,2)); GeometryMath_F64.cross(oldZ, v1, v2); v2.normalize(); // ---------- Compute the new z-axis // simply the process product of the first two GeometryMath_F64.cross(v1,v2,v3); v3.normalize(); }
/** * <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> * 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(); DMatrixRMaj sum = new DMatrixRMaj(3,3); CommonOps_DDRM.add(p1.x,tensor.T1,sum,sum); CommonOps_DDRM.add(p1.y,tensor.T2,sum,sum); CommonOps_DDRM.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; }
/** * Selects axises of new coordinate system */ private void selectAxises(SimpleMatrix R1, SimpleMatrix R2, SimpleMatrix c1, SimpleMatrix c2) { // --------- Compute the new x-axis v1.set(c2.get(0) - c1.get(0), c2.get(1) - c1.get(1), c2.get(2) - c1.get(2)); v1.normalize(); // --------- Compute the new y-axis // cross product of old z axis and new x axis // According to the paper [1] this choice is arbitrary, however it is not. By selecting // the original axis the similarity with the first view is maximized. The other extreme // would be to make it perpendicular, resulting in an unusable rectification. // extract old z-axis from rotation matrix Vector3D_F64 oldZ = new Vector3D_F64( R1.get(2,0)+R2.get(2,0), R1.get(2,1)+R2.get(2,1), R1.get(2,2)+R2.get(2,2)); GeometryMath_F64.cross(oldZ, v1, v2); v2.normalize(); // ---------- Compute the new z-axis // simply the process product of the first two GeometryMath_F64.cross(v1,v2,v3); v3.normalize(); }