boofcv.abst.geo.bundle.BundleAdjustmentSchur Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of boofcv-geo Show documentation
BoofCV is an open source Java library for real-time computer vision and robotics applications.
There is a newer version: 1.1.7
/*
 * Copyright (c) 2011-2020, Peter Abeles. All Rights Reserved.
 *
 * This file is part of BoofCV (http://boofcv.org).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package boofcv.abst.geo.bundle;

import org.ddogleg.optimization.UnconstrainedLeastSquaresSchur;
import org.ddogleg.optimization.functions.FunctionNtoM;
import org.ddogleg.optimization.functions.SchurJacobian;
import org.ejml.data.DMatrix;
import org.jetbrains.annotations.Nullable;

import java.io.PrintStream;
import java.util.Set;

/**
 * Implementation of bundle adjustment using Shur Complement and generic sparse matrices.
 *
 * @author Peter Abeles
 */
public class BundleAdjustmentSchur
		implements BundleAdjustment {
	// minimization algorithm
	private final UnconstrainedLeastSquaresSchur minimizer;

	private final FunctionResiduals function;
	private final Jacobian jacobian;

	private int maxIterations;
	private double[] parameters = new double[0];

	private volatile boolean stopRequested = false;

	private double ftol, gtol;

	private final Codec codec;

	public BundleAdjustmentSchur( UnconstrainedLeastSquaresSchur minimizer,
								  FunctionResiduals function,
								  Jacobian jacobian,
								  Codec codec ) {
		this.minimizer = minimizer;
		this.function = function;
		this.jacobian = jacobian;
		this.codec = codec;
	}

	@Override
	public void configure( double ftol, double gtol, int maxIterations ) {
		this.ftol = ftol;
		this.gtol = gtol;
		this.maxIterations = maxIterations;
	}

	@Override
	public void setParameters( Structure structure, SceneObservations observations ) {
		this.function.configure(structure, observations);
		this.jacobian.configure(structure, observations);
		this.minimizer.setFunction(function, jacobian);

		int N = structure.getParameterCount();
		if (parameters.length < N) {
			parameters = new double[N];
		}
		codec.encode(structure, parameters);
		this.minimizer.initialize(parameters, ftol, gtol);
	}

	@Override
	public boolean optimize( Structure output ) {
		stopRequested = false;

		double before = minimizer.getFunctionValue();
		for (int i = 0; i < maxIterations && !stopRequested; i++) {
//			DMatrixRMaj residual = ((UnconLeastSqLevenbergMarquardtSchur_F64)minimizer).residuals;
//			for (int j = 0; j < residual.numRows; j++) {
//				double r = residual.data[j];
//				if( Math.abs(r) > 1 ) {
//					System.out.println(j+" large residual "+r);
//				}
//			}
			if (minimizer.iterate())
				break;
		}

		codec.decode(minimizer.getParameters(), output);
		return minimizer.getFunctionValue() < before;
	}

	@Override
	public double getFitScore() {
		return minimizer.getFunctionValue();
	}

	@Override
	public void requestStop() {
		stopRequested = true;
	}

	@Override
	public boolean isStopRequested() {
		return stopRequested;
	}

	@Override
	public void setVerbose( @Nullable PrintStream out, @Nullable Set configuration ) {
		this.minimizer.setVerbose(out, 0);
	}

	public interface Codec {
		void decode( double[] input, Structure structure );

		void encode( Structure structure, double[] output );
	}

	public interface FunctionResiduals extends FunctionNtoM {
		void configure( Structure structure, SceneObservations observations );
	}

	public interface Jacobian extends SchurJacobian {
		void configure( Structure structure, SceneObservations observations );
	}
}