@Override public void decode(double[] input, Se3_F64 outputModel) { rotation.setParamVector(input[0],input[1],input[2]); ConvertRotation3D_F64.rodriguesToMatrix(rotation, outputModel.getR()); Vector3D_F64 T = outputModel.getT(); T.x = input[3]; T.y = input[4]; T.z = input[5]; }
@Override public void decode(double[] input, Se3_F64 outputModel) { rotation.setParamVector(input[0],input[1],input[2]); ConvertRotation3D_F64.rodriguesToMatrix(rotation, outputModel.getR()); Vector3D_F64 T = outputModel.getT(); T.x = input[3]; T.y = input[4]; T.z = input[5]; }
/** * Compute intrinsic calibration for one of the cameras */ private CameraPinholeRadial calibrateMono(CalibrateMonoPlanar calib , List<Se3_F64> location ) { CameraPinholeRadial intrinsic = calib.process(); Zhang99ParamAll zhangParam = calib.getZhangParam(); for( Zhang99ParamAll.View v : zhangParam.views ) { Se3_F64 pose = new Se3_F64(); ConvertRotation3D_F64.rodriguesToMatrix(v.rotation,pose.getR()); pose.getT().set(v.T); location.add(pose); } return intrinsic; }
/** * Compute intrinsic calibration for one of the cameras */ private CameraPinholeRadial calibrateMono(CalibrateMonoPlanar calib , List<Se3_F64> location ) { CameraPinholeRadial intrinsic = calib.process(); Zhang99AllParam zhangParam = calib.getZhangParam(); for( Zhang99AllParam.View v : zhangParam.views ) { Se3_F64 pose = new Se3_F64(); ConvertRotation3D_F64.rodriguesToMatrix(v.rotation,pose.getR()); pose.getT().set(v.T); location.add(pose); } return intrinsic; }
public void process(Zhang99AllParam param , double []residuals ) { int index = 0; for( int indexView = 0; indexView < param.views.length; indexView++ ) { Zhang99AllParam.View v = param.views[indexView]; ConvertRotation3D_F64.rodriguesToMatrix(v.rotation,se.getR()); se.T = v.T; CalibrationObservation viewSet = observations.get(indexView); for( int i = 0; i < viewSet.size(); i++ ) { int gridIndex = viewSet.get(i).index; Point2D_F64 obs = viewSet.get(i); // Put the point in the camera's reference frame SePointOps_F64.transform(se,grid.get(gridIndex), cameraPt); param.getIntrinsic().project(cameraPt,pixelPt); residuals[index++] = pixelPt.x-obs.x; residuals[index++] = pixelPt.y-obs.y; } } } }
ConvertRotation3D_F64.rodriguesToMatrix(v.rotation,se.getR()); se.T = v.T;
rodJacobian.process(rodX,rodY,rodZ); ConvertRotation3D_F64.rodriguesToMatrix(rodrigues,R); ViewPointObservations obs = observations.get(i); gradientViewMotionAndPoint(input, paramIndex-6, obs);
DenseMatrix64F R = ConvertRotation3D_F64.rodriguesToMatrix(rod, null);
ConvertRotation3D_F64.rodriguesToMatrix(rodrigues,view.worldToView.R);
@Override public void process(double[] input, double[] output) { this.output = output; // initialize data structures rodrigues.setParamVector(input[0],input[1],input[2]); rodJacobian.process(input[0], input[1], input[2]); worldToCamera.T.x = input[3]; worldToCamera.T.y = input[4]; worldToCamera.T.z = input[5]; ConvertRotation3D_F64.rodriguesToMatrix(rodrigues, worldToCamera.getR()); // compute the gradient for each observation for( int i = 0; i < observations.size(); i++ ) { Point2D3D o = observations.get(i); SePointOps_F64.transform(worldToCamera,o.location, cameraPt); indexX = 2*6*i; indexY = indexX + 6; // add gradient from rotation addRodriguesJacobian(rodJacobian.Rx,o.location,cameraPt); addRodriguesJacobian(rodJacobian.Ry,o.location,cameraPt); addRodriguesJacobian(rodJacobian.Rz,o.location,cameraPt); // add gradient from translation addTranslationJacobian(cameraPt); } }
@Override public void process(double[] input, DMatrixRMaj J) { this.output = J.data; // initialize data structures rodrigues.setParamVector(input[0],input[1],input[2]); rodJacobian.process(input[0], input[1], input[2]); worldToCamera.T.x = input[3]; worldToCamera.T.y = input[4]; worldToCamera.T.z = input[5]; ConvertRotation3D_F64.rodriguesToMatrix(rodrigues, worldToCamera.getR()); // compute the gradient for each observation for( int i = 0; i < observations.size(); i++ ) { Point2D3D o = observations.get(i); SePointOps_F64.transform(worldToCamera,o.location, cameraPt); indexX = 2*6*i; indexY = indexX + 6; // add gradient from rotation addRodriguesJacobian(rodJacobian.Rx,o.location,cameraPt); addRodriguesJacobian(rodJacobian.Ry,o.location,cameraPt); addRodriguesJacobian(rodJacobian.Rz,o.location,cameraPt); // add gradient from translation addTranslationJacobian(cameraPt); } }
rodJacobian.process(rodX,rodY,rodZ); ConvertRotation3D_F64.rodriguesToMatrix(rodrigues, se.getR()); se.T.set(tranX, tranY, tranZ);
/** * Creates a 3D point on the surface of the provided sphere at the specified coordinate * * @param sphere Description of the sphere * @param phi angular coordinate in radians * @param theta angular coordinate in radians * @return Point on the sphere */ public static Point3D_F64 createPt(Sphere3D_F64 sphere, double phi, double theta) { Point3D_F64 p = new Point3D_F64(); p.set(0, 0, sphere.radius); Rodrigues_F64 rodX = new Rodrigues_F64(phi, new Vector3D_F64(1, 0, 0)); DenseMatrix64F rotX = ConvertRotation3D_F64.rodriguesToMatrix(rodX, null); Rodrigues_F64 rodZ = new Rodrigues_F64(theta, new Vector3D_F64(0, 0, 1)); DenseMatrix64F rotZ = ConvertRotation3D_F64.rodriguesToMatrix(rodZ, null); GeometryMath_F64.mult(rotX, p, p); GeometryMath_F64.mult(rotZ, p, p); p.x += sphere.center.x; p.y += sphere.center.y; p.z += sphere.center.z; return p; }
@Override public void decode(double[] input, CalibratedPoseAndPoint outputModel) { int paramIndex = 0; // first decode the transformation for( int i = 0; i < numViews; i++ ) { // don't decode if it is already known if( knownView[i] ) continue; Se3_F64 se = outputModel.getWorldToCamera(i); rotation.setParamVector(input[paramIndex++], input[paramIndex++], input[paramIndex++]); ConvertRotation3D_F64.rodriguesToMatrix(rotation,se.getR()); Vector3D_F64 T = se.getT(); T.x = input[paramIndex++]; T.y = input[paramIndex++]; T.z = input[paramIndex++]; } // now decode the points for( int i = 0; i < numPoints; i++ ) { Point3D_F64 p = outputModel.getPoint(i); p.x = input[paramIndex++]; p.y = input[paramIndex++]; p.z = input[paramIndex++]; } }
rodJacobian.process(rodX,rodY,rodZ); ConvertRotation3D_F64.rodriguesToMatrix(rodrigues, se.getR()); se.T.set(tranX, tranY, tranZ);
/** * Creates a 3D point on the surface of the provided plane at the specified coordinate. The * 2D coordinate on the plane is arbitrarily selected. * * @param plane Description of the plane * @param x 2D coordinate on the plane, x-axis * @param y 2D coordinate on the plane, y-axis * @return Point on the plane */ public static Point3D_F64 createPt(PlaneNormal3D_F64 plane, double x, double y) { Point3D_F64 p = new Point3D_F64(); p.set(x, y, 0); Vector3D_F64 v = new Vector3D_F64(0, 0, 1); Vector3D_F64 cross = v.cross(plane.n); if (Math.abs(cross.norm()) < 1e-8) { cross.set(0, 0, 1); } else { cross.normalize(); } double angle = v.dot(plane.n); angle = Math.acos(angle / (plane.n.norm())); Rodrigues_F64 rod = new Rodrigues_F64(angle, cross); DenseMatrix64F R = ConvertRotation3D_F64.rodriguesToMatrix(rod, null); GeometryMath_F64.mult(R, p, p); p.x += plane.p.x; p.y += plane.p.y; p.z += plane.p.z; return p; }
rodJacobian.process(rodX,rodY,rodZ); ConvertRotation3D_F64.rodriguesToMatrix(rodrigues,worldToView.R); } else { worldToView.set(view.worldToView);