Example Multiview Scene Reconstruction

From BoofCV
Jump to: navigation, search

Example showing how multiple views from a calibrated camera can be combined together to estimate the camera location and the 3D location of interest points. As discussed in the source code this is still a work in progress and the output is very noisy still.

Example Code:

Concepts:

Example Code

/**
 * Demonstration on how to do 3D reconstruction from a set of unordered photos with known intrinsic camera calibration.
 * The code below is still a work in process and is very basic, but still require a solid understanding of
 * structure from motion to understand.  In other words, this is not for beginners and requires good clean set of
 * images to work.
 *
 * @author Peter Abeles
 */
public class ExampleMultiviewSceneReconstruction {
	/**
	 * Process the images and reconstructor the scene as a point cloud using matching interest points between
	 * images.
	 */
	public void process(CameraPinholeRadial intrinsic , List<BufferedImage> colorImages ) {

		DetectDescribePoint detDesc = FactoryDetectDescribe.surfStable(null, null, null, GrayF32.class);
		ScoreAssociation scorer = FactoryAssociation.defaultScore(detDesc.getDescriptionType());
		AssociateDescription<TupleDesc> associate =
				FactoryAssociation.greedy(scorer, Double.MAX_VALUE, true);
		PairwiseImageMatching<GrayF32> imageMatching = new PairwiseImageMatching<>(detDesc,associate);
		imageMatching.setVerbose(System.out,0);

		String cameraName = "camera";
		imageMatching.addCamera(cameraName, LensDistortionFactory.narrow(intrinsic).undistort_F64(true,false),intrinsic);

		for (int i = 0; i < colorImages.size(); i++) {
			BufferedImage colorImage = colorImages.get(i);
			if( colorImage.getWidth() != intrinsic.width || colorImage.getHeight() != intrinsic.height )
				throw new RuntimeException("Looks like you tried to hack this example and run it on random images. Please RTFM");
			GrayF32 image = ConvertBufferedImage.convertFrom(colorImage, (GrayF32) null);
			imageMatching.addImage(image,cameraName);
		}

		if( !imageMatching.process() ) {
			throw new RuntimeException("Failed to match images! total="+colorImages.size());
		}

		EstimateSceneCalibrated estimateScene = new EstimateSceneCalibrated();
		estimateScene.setVerbose(System.out,0);

		if( !estimateScene.process(imageMatching.getGraph()))
			throw new RuntimeException("Scene estimation failed");

		// get the results
		SceneStructureMetric structure = estimateScene.getSceneStructure();
		SceneObservations observations = estimateScene.getObservations();

		// Configure bundle adjustment
		ConfigLevenbergMarquardt configLM = new ConfigLevenbergMarquardt();
		configLM.dampeningInitial = 1e-12;
		configLM.hessianScaling = true;
		ConfigBundleAdjustment configSBA = new ConfigBundleAdjustment();
		configSBA.configOptimizer = configLM;

		BundleAdjustment<SceneStructureMetric> sba = FactoryMultiView.bundleAdjustmentMetric(configSBA);
		sba.configure(1e-10,1e-10,100);
		sba.setVerbose(System.out,0);

		// Scale to improve numerical accuracy
		ScaleSceneStructure bundleScale = new ScaleSceneStructure();

		PruneStructureFromSceneMetric pruner = new PruneStructureFromSceneMetric(structure,observations);

		// Requiring 3 views per point reduces the number of outliers by a lot but also removes
		// many valid points
		pruner.prunePoints(3);

		// Optimize the results
		int pruneCycles=5;
		for (int i = 0; i < pruneCycles; i++) {
			System.out.println("BA + Prune iteration = "+i+"  points="+structure.points.length+"  obs="+observations.getObservationCount());
			bundleScale.applyScale(structure,observations);
			sba.setParameters(structure,observations);
			if( !sba.optimize(structure) ) {
				throw new RuntimeException("Bundle adjustment failed!");
			}

			bundleScale.undoScale(structure,observations);

			if( i == pruneCycles-1 )
				break;

			System.out.println("Pruning....");
			pruner.pruneObservationsByErrorRank(0.97);  // Prune outlier observations
			pruner.prunePoints(3,0.4);            // Prune stray points in 3D space
			pruner.prunePoints(2);                           // Prune invalid points
			pruner.pruneViews(10);                           // Prune views with too few observations
		}

		visualizeResults(structure,colorImages);
		System.out.println("Done!");
	}

	/**
	 * Opens a window showing the found point cloud. Points are colorized using the pixel value inside
	 * one of the input images
	 */
	private void visualizeResults( SceneStructureMetric structure,
								   List<BufferedImage> colorImages ) {

		List<Point3D_F64> cloudXyz = new ArrayList<>();
		GrowQueue_I32 cloudRgb = new GrowQueue_I32();
		Point3D_F64 world = new Point3D_F64();
		Point3D_F64 camera = new Point3D_F64();
		Point2D_F64 pixel = new Point2D_F64();
		for( int i = 0; i < structure.points.length; i++ ) {
			// Get 3D location
			SceneStructureMetric.Point p = structure.points[i];
			p.get(world);

			// Project point into an arbitrary view
			for (int j = 0; j < p.views.size; j++) {
				int viewIdx  = p.views.get(j);
				SePointOps_F64.transform(structure.views[viewIdx].worldToView,world,camera);
				int cameraIdx = structure.views[viewIdx].camera;
				structure.cameras[cameraIdx].model.project(camera.x,camera.y,camera.z,pixel);

				// Get the points color
				BufferedImage image = colorImages.get(viewIdx);
				int x = (int)pixel.x;
				int y = (int)pixel.y;

				// After optimization it might have been moved out of the camera's original FOV.
				// hopefully this isn't too common
				if( x < 0 || y < 0 || x >= image.getWidth() || y >= image.getHeight() )
					continue;
				cloudXyz.add( world.copy() );
				cloudRgb.add(image.getRGB((int)pixel.x,(int)pixel.y));
				break;
			}
		}

		PointCloudViewer viewer = VisualizeData.createPointCloudViewer();
		viewer.setTranslationStep(0.05);
		viewer.addCloud(cloudXyz,cloudRgb.data);
		viewer.setCameraHFov(UtilAngle.radian(60));

		SwingUtilities.invokeLater(()->{
			viewer.getComponent().setPreferredSize(new Dimension(500,500));
			ShowImages.showWindow(viewer.getComponent(), "Reconstruction Points", true);
		});

	}

	public static void main(String[] args) {

		String directory = UtilIO.pathExample("sfm/chair");

		CameraPinholeRadial intrinsic = CalibrationIO.load(
				new File(directory,"/intrinsic_DSC-HX5_3648x2736_to_640x480.yaml"));

		List<BufferedImage> images = UtilImageIO.loadImages(directory,".*jpg");

		int N = 8;
		while( images.size() > N ) {
			images.remove(N);
		}

		ExampleMultiviewSceneReconstruction example = new ExampleMultiviewSceneReconstruction();

		long before = System.currentTimeMillis();
		example.process(intrinsic,images);
		long after = System.currentTimeMillis();

		System.out.println("Elapsed time "+(after-before)/1000.0+" (s)");
	}
}