@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; }
void originalCoordinates( DMatrixRMaj T , DMatrixRMaj R , Point3D_F64 p ) { GeometryMath_F64.multTran(R,p,p); GeometryMath_F64.mult(T,p,p); }
/** * 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); }
@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; }
@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 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 compute(double x, double y, Point2D_F64 out) { out.set(x,y); GeometryMath_F64.mult(K_inv, out, out); } }
@Override public double computeResidual(AssociatedPair observation) { double bottom = 0; GeometryMath_F64.mult(K2E, observation.p1, temp); bottom += temp.x*temp.x + temp.y*temp.y; GeometryMath_F64.multTran(EK1, observation.p2, temp); bottom += temp.x*temp.x + temp.y*temp.y; if( bottom == 0) { return Double.MAX_VALUE; } else { GeometryMath_F64.multTran(E,observation.p2,temp); return (temp.x*observation.p1.x + temp.y*observation.p1.y + temp.z)/bottom; } } }
/** * Input is in pinhole camera pixel coordinates. Output is in equirectangular coordinates * * @param x Pixel x-coordinate in rendered pinhole camera * @param y Pixel y-coordinate in rendered pinhole camera */ @Override public void compute(int x, int y) { // grab precomputed normalized image coordinate at canonical location Point3D_F64 v = vectors[y*outWidth+x]; // move to requested orientation GeometryMath_F64.mult(R,v,n); // TODO make faster by not using an array based matrix // compute pixel coordinate tools.normToEquiFV(n.x,n.y,n.z,out); distX = out.x; distY = out.y; }
@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; bottom = Math.sqrt(bottom); if( bottom <= UtilEjml.EPS) { 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; } } }
/** * 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); }
private void applyScaleTranslation3D(SceneStructureMetric structure) { double scale = desiredDistancePoint / medianDistancePoint; Point3D_F64 c = new Point3D_F64(); for (int i = 0; i < structure.views.length; i++) { SceneStructureMetric.View view = structure.views[i]; // X_w = R'*(X_c - T) let X_c = 0 then X_w = -R'*T is center of camera in world GeometryMath_F64.multTran(view.worldToView.R,view.worldToView.T,c); // Apply transform c.x = -scale*(c.x + medianPoint.x); c.y = -scale*(c.y + medianPoint.y); c.z = -scale*(c.z + medianPoint.z); // -R*T GeometryMath_F64.mult(view.worldToView.R,c,view.worldToView.T); view.worldToView.T.scale(-1); } }
/** * W=[H*a,H*b,hat(H*a)*H*b] */ private void setW( DMatrixRMaj W, DMatrixRMaj 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_DDRM.mult(Hv2,H,tempM); GeometryMath_F64.mult(tempM,b, tempV); setColumn(W,2, tempV); }
public static Point2D_F64 renderPixel( Se3_F64 worldToCamera , DMatrixRMaj K , Point3D_F64 X ) { Point3D_F64 X_cam = new Point3D_F64(); SePointOps_F64.transform(worldToCamera, X, X_cam); // see if it's behind the camera if( X_cam.z <= 0 ) return null; Point2D_F64 norm = new Point2D_F64(X_cam.x/X_cam.z,X_cam.y/X_cam.z); if( K == null ) return norm; // convert into pixel coordinates return GeometryMath_F64.mult(K, norm, norm); }
/** * 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; }
public void computeMotion( PlaneNormal3D_F64 dstLeft , PlaneNormal3D_F64 dstRight , PlaneNormal3D_F64 dstTop ) { intersect(dstLeft, dstRight, dstTop, centerB); createRotation(dstLeft,dstRight,R1); CommonOps.multTransB(R1,R0,found.getR()); GeometryMath_F64.mult(found.getR(),centerA,centerA); found.T.x = centerB.x - centerA.x; found.T.y = centerB.y - centerA.y; found.T.z = centerB.z - centerA.z; }
public void setCameraToWorld(Se3_F64 cameraToWorld) { Vector3D_F64 v = new Vector3D_F64(0,0,1); GeometryMath_F64.mult(cameraToWorld.getR(), v, v); orientation.setVector(v); orientation.repaint(); displayOrigin.setText(String.format("%6.1f",cameraToWorld.getT().norm())); if( prevToWorld == null ) { prevToWorld = cameraToWorld.copy(); } else { Se3_F64 worldToPrev = prevToWorld.invert(null); cameraToWorld.concat(worldToPrev, prevToWorld); integral += prevToWorld.getT().norm(); prevToWorld.set(cameraToWorld); } displayIntegral.setText(String.format("%6.1f",integral)); }
/** * 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()); }