GeneratePnPObservation { void simulate(double distance, double angle, int numTrials, List<Se3_F64> BodyToCameras, List<List<Point2D_F64>> observations, boolean randomTilt , boolean centerImage ) { Se3_F64 BodyToUp = new Se3_F64(); Se3_F64 UpToTilt = new Se3_F64(); Se3_F64 TiltToZ = new Se3_F64(); Se3_F64 ZToCamera = new Se3_F64(); Se3_F64 BodyToCamera = new Se3_F64(); ConvertRotation3D_F64.eulerToMatrix(EulerType.XYZ, 0, 0, 0, BodyToUp.R); TiltToZ.T.set(0, 0, distance); InvertibleTransformSequence<Se3_F64> sequence = new InvertibleTransformSequence<>(); sequence.addTransform(true, BodyToUp); sequence.addTransform(true, UpToTilt); sequence.addTransform(true, TiltToZ); sequence.addTransform(true, ZToCamera); Point2D_F64 centerPixel = new Point2D_F64(); for (int trial = 0; trial < numTrials; trial++) { List<Point2D_F64> pixels = new ArrayList<>(); for (int i = 0; i < marker.size(); i++) { pixels.add(new Point2D_F64()); } boolean success = false; for (int attempt = 0; attempt < 10000; attempt++) { double tiltAngle = randomTilt ? 2.0 * (random.nextDouble() - 0.5) * angle : angle; randomTilt(tiltAngle, UpToTilt); if( !centerImage ) { centerPixel.x = random.nextDouble() * (imageWidth - 1); centerPixel.y = random.nextDouble() * (imageHeight - 1); rotateToPixel(centerPixel, ZToCamera); } sequence.computeTransform(BodyToCamera); if (renderMarker(marker, BodyToCamera, pixels)) { sanityCheckObservations(pixels); success = true; observations.add(pixels); BodyToCameras.add(BodyToCamera.copy()); break; } } if (!success) { throw new RuntimeException("Failed to find a configuration which was visible"); } } } GeneratePnPObservation(double hfov, int width, int height); void initialize( double stdevPixel, String path ); void targetSquare(double length); void generateUniformImageDiscreteDistances(DiscreteRange range, double maxTilt, int trialsPerSet); void generateUniformImageDiscreteAngles(DiscreteRange angles, double distance, int trialsPerSet); void saveSimulation(List<Point3D_F64> corners,
                               List<Se3_F64> BodyToCameras, List<List<Point2D_F64>> observations,
                               long randomSeed,
                               PrintStream out); double getHfov(); int getImageWidth(); int getImageHeight(); double getStdevPixel(); static void main(String[] args);  }
@Test public void simulate() { int width = 640; int height = 480; GeneratePnPObservation generator = new GeneratePnPObservation(60,width,height); generator.random = new Random(1234); generator.stdevPixel = 0; generator.targetSquare(0.3); List<Se3_F64> BodyToCameras = new ArrayList<>(); List<List<Point2D_F64>> observations = new ArrayList<>(); double distance = 2; int N = 150; generator.simulate(distance,0.01,N,BodyToCameras,observations,true,false); assertEquals(N,BodyToCameras.size()); assertEquals(N,observations.size()); for (int i = 0; i < N; i++) { assertEquals(distance, BodyToCameras.get(i).T.norm(), 1e-4); for( Point2D_F64 p : observations.get(i) ) { assertTrue( p.x >= 0 && p.x <= width-1 ); assertTrue( p.y >= 0 && p.y <= height-1 ); } } }