ConvertRotation3D_F64.matrixToRodrigues(R, rod); errorsR[i] = rod.theta;
public void print() { double euler[] = ConvertRotation3D_F64.matrixToEuler(rightToLeft.getR(), EulerType.XYZ,(double[])null); Vector3D_F64 t = rightToLeft.getT(); System.out.println(); System.out.println("Left Camera"); left.print(); System.out.println(); System.out.println("Right Camera"); right.print(); System.out.println("Right to Left"); System.out.printf(" Euler XYZ [ %8.3f , %8.3f , %8.3f ]\n",euler[0],euler[1],euler[2]); System.out.printf(" Translation [ %8.3f , %8.3f , %8.3f ]\n",t.x,t.y,t.z); } }
/** * Specifies the rotation offset from the canonical location using yaw and pitch. * @param yaw Radian from -pi to pi * @param pitch Radian from -pi/2 to pi/2 * @param roll Radian from -pi to pi */ public void setDirection(double yaw, double pitch, double roll ) { ConvertRotation3D_F64.eulerToMatrix(EulerType.YZX,pitch,yaw,roll,R); }
@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]; }
public void log( BufferedImage image , long timeStamp ) { DenseMatrix64F R = CommonOps.identity(3, 3); Vector3D T = new Vector3D(); // Matrix3d Rm = new Matrix3d(); // rosTransformFromHeadBaseToCamera.get(Rm); // MatrixTools.matrix3DToDenseMatrix(Rm,R,0,0); Quaternion_F64 quad = ConvertRotation3D_F64.matrixToQuaternion(R, null); // rosTransformFromHeadBaseToCamera.get(T); logPose.printf("%d %15f %15f %15f %15f %15f %15f %15f\n", timeStamp, quad.x, quad.y, quad.z, quad.w, T.getX(), T.getY(), T.getZ()); logPose.flush(); UtilImageIO.saveImage(image, String.format("%s/image%06d.png", outputDir.getAbsolutePath(), tick)); tick++; }
ConvertRotation3D_F64.approximateRotationMatrix(R,ret.getR()); ret.getT().set(t.data[0],t.data[1],t.data[2]);
@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]; }
public void log( BufferedImage image , long timeStamp ) { DenseMatrix64F R = CommonOps.identity(3, 3); Vector3d T = new Vector3d(); // Matrix3d Rm = new Matrix3d(); // rosTransformFromHeadBaseToCamera.get(Rm); // MatrixTools.matrix3DToDenseMatrix(Rm,R,0,0); Quaternion_F64 quad = ConvertRotation3D_F64.matrixToQuaternion(R, null); // rosTransformFromHeadBaseToCamera.get(T); logPose.printf("%d %15f %15f %15f %15f %15f %15f %15f\n", timeStamp, quad.x, quad.y, quad.z, quad.w, T.getX(), T.getY(), T.getZ()); logPose.flush(); UtilImageIO.saveImage(image, String.format("%s/image%06d.png", outputDir.getAbsolutePath(), tick)); tick++; }
ConvertRotation3D_F64.approximateRotationMatrix(R,ret.getR()); ret.getT().set(t.data[0],t.data[1],t.data[2]);
/** * Converts results fond in the linear algorithms into {@link Zhang99AllParam} */ public void convertIntoZhangParam(List<Se3_F64> motions, DMatrixRMaj K, double[] distort , Zhang99AllParam param ) { param.getIntrinsic().initialize(K,distort); param.setNumberOfViews(motions.size()); for( int i = 0; i < param.views.length; i++ ) { Se3_F64 m = motions.get(i); Zhang99AllParam.View v = new Zhang99AllParam.View(); v.T = m.getT(); ConvertRotation3D_F64.matrixToRodrigues(m.getR(), v.rotation); param.views[i] = v; } }
public void print() { double euler[] = ConvertRotation3D_F64.matrixToEuler(rightToLeft.getR(), EulerType.XYZ,(double[])null); Vector3D_F64 t = rightToLeft.getT(); System.out.println(); System.out.println("Left Camera"); left.print(); System.out.println(); System.out.println("Right Camera"); right.print(); System.out.println("Right to Left"); System.out.printf(" Euler XYZ [ %8.3f , %8.3f , %8.3f ]\n",euler[0],euler[1],euler[2]); System.out.printf(" Translation [ %8.3f , %8.3f , %8.3f ]\n",t.x,t.y,t.z); } }
/** * 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; }
/** * Specifies the rotation offset from the canonical location using yaw and pitch. * @param yaw Radian from -pi to pi * @param pitch Radian from -pi/2 to pi/2 * @param roll Radian from -pi to pi */ public void setDirection(double yaw, double pitch, double roll ) { ConvertRotation3D_F64.eulerToMatrix(EulerType.YZX,pitch,yaw,roll,R); }
public static Transform convert( Se3_F64 input , Transform output ) { if( output == null ) output = new Transform(); Quaternion_F64 quatGR = new Quaternion_F64(); ConvertRotation3D_F64.matrixToQuaternion(input.getR(), quatGR); Quaternion quatJME = new Quaternion(); quatJME.set((float)quatGR.x,(float)quatGR.y,(float)quatGR.z,(float)quatGR.w); output.setRotation(quatJME); output.setTranslation((float)input.T.x,(float)input.T.y,(float)input.T.z); return output; }
private void computeDisturbance(int which, Point2D_F64 pixel, Point2D3D p23) { pixelToNorm.compute(pixel.x,pixel.y,p23.observation); if( estimatePose(which, detected2D3D, targetToCameraSample) ) { referenceCameraToTarget.concat(targetToCameraSample, difference); double d = difference.getT().norm(); ConvertRotation3D_F64.matrixToRodrigues(difference.getR(), rodrigues); double theta = Math.abs(rodrigues.theta); if (theta > maxOrientation) { maxOrientation = theta; } if (d > maxLocation) { maxLocation = d; } } }
public Se2_F64 getCurrToWorld() { output.T.x = groundToWorld.T.z; output.T.y = -groundToWorld.T.x; double[] euler = ConvertRotation3D_F64.matrixToEuler(groundToWorld.getR(), EulerType.XYZ, null); output.setYaw(-euler[1]); return output; } }
/** * 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 Se3_F64 createWorldToCamera() { // pick an appropriate amount of motion for the scene double z = baseline*focalLengthX/(minDisparity+rangeDisparity); double adjust = baseline/20.0; Vector3D_F64 rotPt = new Vector3D_F64(offsetX*adjust,offsetY*adjust,z* range); double radians = tiltAngle*Math.PI/180.0; DenseMatrix64F R = ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ,radians,0,0,null); Se3_F64 a = new Se3_F64(R,rotPt); return a; }
Quaternion_F64 q = ConvertRotation3D_F64.matrixToQuaternion(alg.getMotionSrcToDst().getR(), null); System.out.println(q);
@Override public void encode(Se3_F64 input, double[] output) { // force the "rotation matrix" to be an exact rotation matrix // otherwise Rodrigues will have issues with the noise if( !svd.decompose(input.getR()) ) throw new RuntimeException("SVD failed"); DenseMatrix64F U = svd.getU(null,false); DenseMatrix64F V = svd.getV(null,false); CommonOps.multTransB(U, V, R); // extract Rodrigues coordinates ConvertRotation3D_F64.matrixToRodrigues(R,rotation); output[0] = rotation.unitAxisRotation.x*rotation.theta; output[1] = rotation.unitAxisRotation.y*rotation.theta; output[2] = rotation.unitAxisRotation.z*rotation.theta; Vector3D_F64 T = input.getT(); output[3] = T.x; output[4] = T.y; output[5] = T.z; }