EPnPPointCorrespondencePinholeCameraEstimator.java
/*
* Copyright (C) 2017 Alberto Irurueta Carro (alberto@irurueta.com)
*
* 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 com.irurueta.geometry.estimators;
import com.irurueta.algebra.*;
import com.irurueta.algebra.Utils;
import com.irurueta.geometry.*;
import java.util.ArrayList;
import java.util.List;
/**
* EPnP (Efficient Perspective-n-Point) implementation to estimate pinhole
* cameras from 2D/3D point correspondences.
* This class is an implementation following the one proposed by Vincent Lepetit
* on "EPnP: An Accurate O(n) Solution to the PnP Problem" with some minor
* changes and improvements.
* Paper and source code can be found at:
* <a href="http://cvlabwww.epfl.ch/~lepetit/papers/lepetit_ijcv08.pdf">
* http://cvlabwww.epfl.ch/~lepetit/papers/lepetit_ijcv08.pdf
* </a>
* <a href="http://cvlab.epfl.ch/EPnP/index.php">http://cvlab.epfl.ch/EPnP/index.php</a>
*/
@SuppressWarnings("DuplicatedCode")
public class EPnPPointCorrespondencePinholeCameraEstimator extends PointCorrespondencePinholeCameraEstimator {
/**
* Indicates that by default planar configuration is checked to determine
* whether point correspondences are in such configuration and find a
* specific solution for such case.
*/
public static final boolean DEFAULT_PLANAR_CONFIGURATION_ALLOWED = true;
/**
* Indicates that by default a dimension 2 null-space is allowed.
*/
public static final boolean DEFAULT_NULLSPACE_DIMENSION2_ALLOWED = true;
/**
* Indicates that by default a dimension 3 null-space is allowed.
*/
public static final boolean DEFAULT_NULLSPACE_DIMENSION3_ALLOWED = true;
/**
* Default threshold to determine whether 3D matched points are in a
* planar configuration.
* Points are considered to be laying in a plane when the smallest singular
* value of their covariance matrix has a value much smaller than the
* second smallest as many times as this value.
*/
public static final double DEFAULT_PLANAR_THRESHOLD = 1e13;
/**
* Number of control points used in a general configuration.
*/
private static final int GENERAL_NUM_CONTROL_POINTS = 4;
/**
* Number of control points used in a planar configuration.
*/
private static final int PLANAR_NUM_CONTROL_POINTS = 3;
/**
* Indicates whether planar configuration is checked to determine whether
* point correspondences are in such configuration and find a specific
* solution for such case.
*/
private boolean planarConfigurationAllowed = DEFAULT_PLANAR_CONFIGURATION_ALLOWED;
/**
* Indicates whether the case where a dimension 2 null-space is allowed.
* When allowed, additional constraints are taken into account to ensure
* equality of scales so that less point correspondences are required.
* Enabling this parameter is usually ok.
*/
private boolean nullspaceDimension2Allowed = DEFAULT_NULLSPACE_DIMENSION2_ALLOWED;
/**
* Indicates whether the case where a dimension 3 null-space is allowed.
* When allowed, additional constraints are taken into account to ensure
* equality of scales so that less point correspondences are required.
* Enabling this parameter is usually ok although less precise than
* when a null-space of dimension 2 is used.
*/
private boolean nullspaceDimension3Allowed = DEFAULT_NULLSPACE_DIMENSION3_ALLOWED;
/**
* Threshold to determine whether 3D matched points are in a planar
* configuration.
* Points are considered to be laying in a plane when the smallest singular
* value of their covariance matrix has a value much smaller than the
* largest one as many times as this value.
*/
private double planarThreshold = DEFAULT_PLANAR_THRESHOLD;
/**
* Intrinsic parameters of camera to be estimated.
*/
private PinholeCameraIntrinsicParameters intrinsic;
/**
* Indicates whether provided correspondences were found to be laying in a
* planar configuration during the estimation.
*/
private boolean isPlanar;
/**
* Computed control points in world coordinates.
*/
private List<Point3D> controlWorldPoints;
/**
* Contains barycentric coordinates to express 3D world point in terms of
* control points.
* For general configuration, each row contains 4 coordinates and alphas
* has size nx4, where n is the number of provided 3D world points.
* For planar configuration, each row contains 3 coordinates and alphas
* has size nx3, where n is the number of provided 3D world points.
* both reference frames are centered in the centroid, alphas can be used
* in both world and camera coordinates.
*/
private Matrix alphas;
/**
* M matrix to find control points in camera coordinates.
* M has size 2*n x 12 (general configuration) or 2*n x 9
* (planar configuration), where n is the number of provided 2D observed
* points.
*/
private Matrix m;
/**
* List containing columns of null-space of M. Linear combinations of these
* columns contain possible solutions for control points coordinates in
* camera reference (up to scale).
* First item of the list contains last column of v, which corresponds to
* the smallest singular value.
* Last item of the list contains (column - number of control points) column
* of v.
*/
private List<double[]> nullspace;
/**
* Possible solutions for the estimation.
*/
private List<Solution> solutions;
/**
* Constructor.
*/
public EPnPPointCorrespondencePinholeCameraEstimator() {
super();
}
/**
* Constructor with listener.
*
* @param listener listener to be notified of events such as when estimation
* starts, ends or estimation progress changes.
*/
public EPnPPointCorrespondencePinholeCameraEstimator(final PinholeCameraEstimatorListener listener) {
super(listener);
}
/**
* Constructor.
*
* @param points3D list of corresponding 3D points.
* @param points2D list of corresponding 2D points.
* @throws IllegalArgumentException if any of the lists are null.
* @throws WrongListSizesException if provided lists of points don't have
* the same size and enough points.
*/
public EPnPPointCorrespondencePinholeCameraEstimator(
final List<Point3D> points3D, final List<Point2D> points2D) throws WrongListSizesException {
super();
internalSetListsEpnP(points3D, points2D);
}
/**
* Constructor.
*
* @param points3D list of corresponding 3D points.
* @param points2D list of corresponding 2D points.
* @param listener listener to be notified of events such as when estimation
* starts, ends or estimation progress changes.
* @throws IllegalArgumentException if any of the lists are null.
* @throws WrongListSizesException if provided lists of points don't have
* the same size and enough points.
*/
public EPnPPointCorrespondencePinholeCameraEstimator(
final List<Point3D> points3D, final List<Point2D> points2D, final PinholeCameraEstimatorListener listener)
throws WrongListSizesException {
super(listener);
internalSetListsEpnP(points3D, points2D);
}
/**
* Constructor.
*
* @param intrinsic intrinsic parameters of camera to be estimated.
*/
public EPnPPointCorrespondencePinholeCameraEstimator(final PinholeCameraIntrinsicParameters intrinsic) {
this();
this.intrinsic = intrinsic;
}
/**
* Constructor with listener.
*
* @param intrinsic intrinsic parameters of camera to be estimated.
* @param listener listener to be notified of events such as when estimation
* starts, ends or estimation progress changes.
* @throws IllegalArgumentException if absolute values of focal lengths are
* too small.
*/
public EPnPPointCorrespondencePinholeCameraEstimator(
final PinholeCameraIntrinsicParameters intrinsic, final PinholeCameraEstimatorListener listener) {
this(listener);
this.intrinsic = intrinsic;
}
/**
* Constructor.
*
* @param intrinsic intrinsic parameters of camera to be estimated.
* @param points3D list of corresponding 3D points.
* @param points2D list of corresponding 2D points.
* @throws IllegalArgumentException if any of the lists are null or if
* absolute values of focal lengths are too small.
* @throws WrongListSizesException if provided lists of points don't have
* the same size and enough points.
*/
public EPnPPointCorrespondencePinholeCameraEstimator(
final PinholeCameraIntrinsicParameters intrinsic, final List<Point3D> points3D,
final List<Point2D> points2D) throws WrongListSizesException {
this(points3D, points2D);
this.intrinsic = intrinsic;
}
/**
* Constructor.
*
* @param intrinsic intrinsic parameters of camera to be estimated.
* @param points3D list of corresponding 3D points.
* @param points2D list of corresponding 2D points.
* @param listener listener to be notified of events such as when estimation
* starts, ends or estimation progress changes.
* @throws IllegalArgumentException if any of the lists are null or if
* absolute values of focal lengths are too small.
* @throws WrongListSizesException if provided lists of points don't have
* the same size and enough points.
*/
public EPnPPointCorrespondencePinholeCameraEstimator(
final PinholeCameraIntrinsicParameters intrinsic, final List<Point3D> points3D,
final List<Point2D> points2D, final PinholeCameraEstimatorListener listener)
throws WrongListSizesException {
this(points3D, points2D, listener);
this.intrinsic = intrinsic;
}
/**
* Sets list of corresponding points.
*
* @param points3D list of corresponding 3D points.
* @param points2D list of corresponding 2D points.
* @throws LockedException if estimator is locked.
* @throws IllegalArgumentException if any of the lists are null.
* @throws WrongListSizesException if provided lists of points don't have
* the same size and enough points.
*/
@Override
public void setLists(final List<Point3D> points3D, final List<Point2D> points2D) throws LockedException,
WrongListSizesException {
if (isLocked()) {
throw new LockedException();
}
internalSetListsEpnP(points3D, points2D);
}
/**
* Indicates whether planar configuration is checked to determine whether
* point correspondences are in such configuration and find a specific
* solution for such case.
*
* @return true to allow specific solutions for planar configurations,
* false to always find a solution assuming the general case.
*/
public boolean isPlanarConfigurationAllowed() {
return planarConfigurationAllowed;
}
/**
* Specifies whether planar configuration is checked to determine whether
* point correspondences are in such configuration and find a specific
* solution for such case.
*
* @param planarConfigurationAllowed true to allow specific solutions for
* planar configurations, false to always find a solution assuming the
* general case.
* @throws LockedException if estimator is locked.
*/
public void setPlanarConfigurationAllowed(final boolean planarConfigurationAllowed) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.planarConfigurationAllowed = planarConfigurationAllowed;
}
/**
* Indicates whether the case where a dimension 2 null-space is allowed.
* When allowed, additional constraints are taken into account to ensure
* equality of scales so that less point correspondences are required.
* Enabling this parameter is usually ok.
*
* @return true to allow 2-dimensional null-space, false otherwise.
*/
public boolean isNullspaceDimension2Allowed() {
return nullspaceDimension2Allowed;
}
/**
* Specifies whether the case where a dimension 2 null-space is allowed.
* When allowed, additional constraints are taken into account to ensure
* equality of scales so that less point correspondences are required.
* Enabling this parameter is usually ok.
*
* @param nullspaceDimension2Allowed true to allow 2-dimensional null-space,
* false otherwise.
* @throws LockedException if estimator is locked.
*/
public void setNullspaceDimension2Allowed(final boolean nullspaceDimension2Allowed) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.nullspaceDimension2Allowed = nullspaceDimension2Allowed;
}
/**
* Indicates whether the case where a dimension 3 null-space is allowed.
* When allowed, additional constraints are taken into account to ensure
* equality of scales so that less point correspondences are required.
* Enabling this parameter is usually ok although less precise than
* when a null-space of dimension 2 is used.
*
* @return true to allow 3-dimensional null-space, false otherwise.
*/
public boolean isNullspaceDimension3Allowed() {
return nullspaceDimension3Allowed;
}
/**
* Specifies whether the case where a dimension 3 null-space is allowed.
* When allowed, additional constraints are taken into account to ensure
* equality of scales so that less point correspondences are required.
* Enabling this parameter is usually ok although less precise than
* when a null-space of dimension 2 is used.
*
* @param nullspaceDimension3Allowed true to allow 3-dimensional null-space,
* false otherwise.
* @throws LockedException if estimator is locked.
*/
public void setNullspaceDimension3Allowed(final boolean nullspaceDimension3Allowed) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.nullspaceDimension3Allowed = nullspaceDimension3Allowed;
}
/**
* Gets threshold to determine whether 3D matched points are in a planar
* configuration.
* Points are considered to be laying in a plane when the smallest singular
* value of their covariance matrix has a value much smaller than the
* largest one as many times as this value.
*
* @return threshold to determine whether 3D matched points are in a planar
* configuration.
*/
public double getPlanarThreshold() {
return planarThreshold;
}
/**
* Sets threshold to determine whether 3D matched points are in a planar
* configuration.
* Points are considered to be laying in a plane when the smallest singular
* value of their covariance matrix has a value much smaller than the
* largest one as many times as this value.
*
* @param planarThreshold threshold to determine whether 3D matched points
* are in a planar configuration.
* @throws IllegalArgumentException if provided threshold is negative.
* @throws LockedException if estimator is locked.
*/
public void setPlanarThreshold(final double planarThreshold) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
if (planarThreshold < 0.0) {
throw new IllegalArgumentException();
}
this.planarThreshold = planarThreshold;
}
/**
* Gets intrinsic parameters of camera to be estimated.
*
* @return intrinsic parameters of camera to be estimated.
*/
public PinholeCameraIntrinsicParameters getIntrinsic() {
return intrinsic;
}
/**
* Sets intrinsic parameters of camera to be estimated.
*
* @param intrinsic intrinsic parameters of camera to be estimated.
* @throws LockedException if estimator is locked.
*/
public void setIntrinsic(final PinholeCameraIntrinsicParameters intrinsic) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
this.intrinsic = intrinsic;
}
/**
* Indicates if this estimator is ready to start the estimation.
*
* @return true if estimator is ready, false otherwise.
*/
@Override
public boolean isReady() {
return areListsAvailable() && areValidLists(points3D, points2D) && intrinsic != null;
}
/**
* Returns type of pinhole camera estimator.
*
* @return type of pinhole camera estimator.
*/
@Override
public PinholeCameraEstimatorType getType() {
return PinholeCameraEstimatorType.EPNP_PINHOLE_CAMERA_ESTIMATOR;
}
/**
* Indicates if provided point correspondences are normalized to increase
* the accuracy of the estimation.
*
* @return true if input point correspondences will be normalized, false
* otherwise.
*/
@Override
public boolean arePointCorrespondencesNormalized() {
return false;
}
/**
* Specifies whether provided point correspondences are normalized to
* increase the accuracy of the estimation.
*
* @param normalize true if input point correspondences will be normalized,
* false otherwise.
* @throws LockedException if estimator is locked.
*/
@Override
public void setPointCorrespondencesNormalized(final boolean normalize) throws LockedException {
if (isLocked()) {
throw new LockedException();
}
}
/**
* Estimates a pinhole camera.
*
* @return estimated pinhole camera.
* @throws LockedException if estimator is locked.
* @throws NotReadyException if input has not yet been provided.
* @throws PinholeCameraEstimatorException if an error occurs during
* estimation, usually because input data is not valid.
*/
@Override
public PinholeCamera estimate() throws LockedException, NotReadyException, PinholeCameraEstimatorException {
if (isLocked()) {
throw new LockedException();
}
if (!isReady()) {
throw new NotReadyException();
}
try {
locked = true;
if (listener != null) {
listener.onEstimateStart(this);
}
computeWorldControlPointsAndPointConfiguration();
computeBarycentricCoordinates();
buildM();
solveNullspace();
} catch (final AlgebraException e) {
locked = false;
throw new PinholeCameraEstimatorException(e);
}
solutions = new ArrayList<>();
// general case
try {
generalSolution1();
} catch (final GeometryException ignore) {
// continue attempting 2nd solution if 1st one fails
}
if (nullspaceDimension2Allowed) {
try {
generalSolution2();
} catch (final GeometryException | AlgebraException ignore) {
// continue attempting 3rd solution if 2nd one fails
}
}
if (nullspaceDimension3Allowed && !isPlanar) {
try {
generalSolution3();
} catch (final GeometryException | AlgebraException ignore) {
// 3rd solution could not be found
}
}
// pick best solution
final var bestSolution = pickBestSolution();
if (listener != null) {
listener.onEstimateEnd(this);
}
if (bestSolution == null) {
throw new PinholeCameraEstimatorException();
}
locked = false;
return attemptRefine(bestSolution.camera);
}
/**
* Indicates whether provided correspondences were found to be laying in a
* planar configuration during the estimation.
*
* @return true if point correspondences are in a planar configuration,
* false otherwise.
*/
public boolean isPlanar() {
return isPlanar;
}
/**
* Internal method that actually computes the normalized pinhole camera
* internal matrix.
* This implementation makes no action.
*
* @param points3D list of 3D points. Points might or might not be
* normalized.
* @param points2D list of 2D points. Points might or might not be
* normalized.
* @return matrix of estimated pinhole camera.
*/
@Override
protected Matrix internalEstimate(final List<Point3D> points3D, final List<Point2D> points2D) {
return null;
}
/**
* Internal method to set list of corresponding points (it does not check
* if estimator is locked).
*
* @param points3D list of corresponding 3D points.
* @param points2D list of corresponding 2D points.
* @throws IllegalArgumentException if any of the lists are null
* @throws WrongListSizesException if provided lists of points don't have
* the same size and enough points.
*/
private void internalSetListsEpnP(final List<Point3D> points3D, final List<Point2D> points2D)
throws WrongListSizesException {
if (points3D == null || points2D == null) {
throw new IllegalArgumentException();
}
if (!areValidLists(points3D, points2D)) {
throw new WrongListSizesException();
}
this.points3D = points3D;
this.points2D = points2D;
}
/**
* Picks best solution (the one having the smallest re-projection error).
*
* @return best solution.
*/
private Solution pickBestSolution() {
Solution bestSolution = null;
var bestError = Double.MAX_VALUE;
for (final var s : solutions) {
if (s.reprojectionError < bestError) {
bestError = s.reprojectionError;
bestSolution = s;
}
}
return bestSolution;
}
/**
* Tests solution 3 for general point configuration.
* Because solution is up to scale, 8 different solutions for different
* beta1, beta2 and beta3 signs are tried.
*
* @throws AlgebraException if a numerical degeneracy occurs.
* @throws LockedException never happens.
* @throws NotReadyException never happens.
* @throws CoincidentPointsException if a point degeneracy has occurred.
*/
private void generalSolution3() throws AlgebraException, LockedException, NotReadyException,
CoincidentPointsException {
if (isPlanar) {
return;
}
// we have the distance constraints between control world points (c) and
// control camera points (v):
// ||(beta1*vai + beta2*vbi + beta3*vci) - (beta1*vaj + beta2*vbj + beta3*vcj)||^2 = ||ci - cj||^2, i,j 1...4
// ((beta1*vaix + beta2*vbix + beta3*vcix) - (beta1*vajx + beta2*vbjx + beta3*vcjx))^2 +
// ((beta1*vaiy + beta2*vbiy + beta3*vciy) - (beta1*vajy + beta2*vbjy + beta3*vcjy))^2 +
// ((beta1*vaiz + beta2*vbiz + beta3*vciz) - (beta1*vajz + beta2*vbjz + beta3*vcjz))^2 =
// (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// (beta1*(vaix - vajx) + beta2*(vbix - vbjx) + beta3*(vcix - vcjx))^2 +
// (beta1*(vaiy - vajy) + beta2*(vbiy - vbjy) + beta3*(vciy - vcjy))^2 +
// (beta1*(vaiz - vajz) + beta2*(vbiz - vbjz) + beta3*(vciz - vcjz))^2 =
// (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// beta1^2*(vaix - vajx)^2 + 2*beta1*(vaix - vajx)*(beta2*(vbix - vbjx) + beta3*(vcix - vcjx)) + (beta2*(vbix - vbjx) + beta3*(vcix - vcjx))^2 +
// beta1^2*(vaiy - vajy)^2 + 2*beta1*(vaiy - vajy)*(beta2*(vbiy - vbjy) + beta3*(vciy - vcjy)) + (beta2*(vbiy - vbjy) + beta3*(vciy - vcjy))^2 +
// beta1^2*(vaiz - vajz)^2 + 2*beta1*(vaiz - vajz)*(beta2*(vbiz - vbjz) + beta3*(vciz - vcjz)) + (beta2*(vbiz - vbjz) + beta3*(vciz - vcjz))^2 =
// (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// beta1^2*(vaix - vajx)^2 + beta1*beta2*2*(vaix - vajx)*(vbix - vbjx) + beta1*beta3*2*(vaix - vajx)*(vcix - vcjx) + beta2^2*(vbix - vbjx)^2 + beta2*beta3*2*(vbix - vbjx)*(vcix - vcjx) + beta3^2*(vcix - vcjx)^2 +
// beta1^2*(vaiy - vajy)^2 + beta1*beta2*2*(vaiy - vajy)*(vbiy - vbjy) + beta1*beta3*2*(vaiy - vajy)*(vciy - vcjy) + beta2^2*(vbiy - vbjy)^2 + beta2*beta3*2*(vbiy - vbjy)*(vciy - vcjy) + beta3^2*(vciy - vcjy)^2 +
// beta1^2*(vaiz - vajz)^2 + beta1*beta2*2*(vaiz - vajz)*(vbiz - vbjz) + beta1*beta3*2*(vaiz - vajz)*(vciz - vcjz) + beta2^2*(vbiz - vbjz)^2 + beta2*beta3*2*(vbiz - vbjz)*(vciz - vcjz) + beta3^2*(vciz - vcjz)^2 =
// (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// We linearize the equation assuming:
// alpha1 = beta1^2
// alpha2 = beta1*beta2
// alpha3 = beta1*beta3
// alpha4 = beta2^2
// alpha5 = beta2*beta3
// alpha6 = beta3^2
// alpha1*(vaix - vajx)^2 + alpha2*2*(vaix - vajx)*(vbix - vbjx) + alpha3*2*(vaix - vajx)*(vcix - vcjx) + alpha4*(vbix - vbjx)^2 + alpha5*2*(vbix - vbjx)*(vcix - vcjx) + alpha6*(vcix - vcjx)^2 +
// alpha1*(vaiy - vajy)^2 + alpha2*2*(vaiy - vajy)*(vbiy - vbjy) + alpha3*2*(vaiy - vajy)*(vciy - vcjy) + alpha4*(vbiy - vbjy)^2 + alpha5*2*(vbiy - vbjy)*(vciy - vcjy) + alpha6*(vciy - vcjy)^2 +
// alpha1*(vaiz - vajz)^2 + alpha2*2*(vaiz - vajz)*(vbiz - vbjz) + alpha3*2*(vaiz - vajz)*(vciz - vcjz) + alpha4*(vbiz - vbjz)^2 + alpha5*2*(vbiz - vbjz)*(vciz - vcjz) + alpha6*(vciz - vcjz)^2 =
// (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// Reorder
// alpha1*((vaix - vajx)^2 + (vaiy - vajy)^2 + (vaiz - vajz)^2) +
// alpha2*2*((vaix - vajx)*(vbix - vbjx) + (vaiy - vajy)*(vbiy - vbjy) + (vaiz - vajz)*(vbiz - vbjz)) +
// alpha3*2*((vaix - vajx)*(vcix - vcjx) + (vaiy - vajy)*(vciy - vcjy) + (vaiz - vajz)*(vciz - vcjz)) +
// alpha4*((vbix - vbjx)^2 + (vbiy - vbjy)^2 + (vbiz - vbjz)^2) +
// alpha5*2*((vbix - vbjx)*(vcix - vcjx) + (vbiy - vbjy)*(vciy - vcjy) + (vbiz - vbjz)*(vciz - vcjz)) +
// alpha6*((vcix - vcjx)^2 + (vciy - vcjy)^2 + (vciz - vcjz)^2) =
// (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// this also produces 6 equations as in case 2.
final var va = nullspace.get(0);
final var vb = nullspace.get(1);
final var vc = nullspace.get(2);
final var controlCameraPointsA = controlPointsFromV(va);
final var controlCameraPointsB = controlPointsFromV(vb);
final var controlCameraPointsC = controlPointsFromV(vc);
final var c = constraintMatrixSolution3(controlCameraPointsA, controlCameraPointsB, controlCameraPointsC);
final var rhos = rhos(controlWorldPoints);
final var a = Utils.solve(c, rhos);
double beta1;
double beta2;
double beta3;
if (a[0] < 0.0) {
beta1 = Math.sqrt(-a[0]);
beta2 = a[3] < 0.0 ? Math.sqrt(-a[3]) : 0.0;
} else {
beta1 = Math.sqrt(a[0]);
beta2 = a[3] > 0.0 ? Math.sqrt(a[3]) : 0.0;
}
// fix sign of betas
if (a[1] < 0.0) {
beta1 = -beta1;
}
beta3 = a[2] / beta1;
// We linearize the equation assuming:
// alpha1 = beta1^2
// alpha2 = beta1*beta2
// alpha3 = beta1*beta3
// alpha4 = beta2^2
// alpha5 = beta*beta3
// alpha6 = beta3^2
final var initialBeta1 = beta1;
final var initialBeta2 = beta2;
final var initialBeta3 = beta3;
// compute linear combination of va and vb as
// v = beta1*va + beta2*vb + beta3*vc
final var tmp1 = ArrayUtils.multiplyByScalarAndReturnNew(va, beta1);
final var tmp2 = ArrayUtils.multiplyByScalarAndReturnNew(vb, beta2);
final var tmp3 = ArrayUtils.multiplyByScalarAndReturnNew(vc, beta3);
ArrayUtils.sum(tmp1, tmp2, tmp1);
ArrayUtils.sum(tmp1, tmp3, tmp1);
var controlCameraPoints = controlPointsFromV(tmp1);
var solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
// because solutions are square roots, beta1, beta2 and beta3 can have
// different signs, so we add solutions for each combination so that the
// one with the smallest re-projection error will be picked
beta1 = -initialBeta1;
// no need to set: beta2 = initialBeta2 and beta3 = initialBeta3 because they already have
// those values
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.multiplyByScalar(vc, beta3, tmp3);
ArrayUtils.sum(tmp1, tmp2, tmp1);
ArrayUtils.sum(tmp1, tmp3, tmp1);
// no need to set v = tmp1, because it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
beta1 = initialBeta1;
beta2 = -initialBeta2;
// no need to set: beta3 = initialBeta3 as it already has that value
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.multiplyByScalar(vc, beta3, tmp3);
ArrayUtils.sum(tmp1, tmp2, tmp1);
ArrayUtils.sum(tmp1, tmp3, tmp1);
// no need to set v = tmp1, because it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
beta1 = -initialBeta1;
beta2 = -initialBeta2;
// no need to set beta3 = initialBeta3, as it already has that value
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.multiplyByScalar(vc, beta3, tmp3);
ArrayUtils.sum(tmp1, tmp2, tmp1);
ArrayUtils.sum(tmp1, tmp3, tmp1);
// no need to set v = tmp1, because it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
beta1 = initialBeta1;
beta2 = initialBeta2;
beta3 = -initialBeta3;
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.multiplyByScalar(vc, beta3, tmp3);
ArrayUtils.sum(tmp1, tmp2, tmp1);
ArrayUtils.sum(tmp1, tmp3, tmp1);
// no need to set v = tmp1, because it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
beta1 = -initialBeta1;
// no need to set beta2 = initialBeta2, because it already has this value
beta3 = -initialBeta3;
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.multiplyByScalar(vc, beta3, tmp3);
ArrayUtils.sum(tmp1, tmp2, tmp1);
ArrayUtils.sum(tmp1, tmp3, tmp1);
// no need to set v = tmp1, because it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
beta1 = initialBeta1;
beta2 = -initialBeta2;
beta3 = -initialBeta3;
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.multiplyByScalar(vc, beta3, tmp3);
ArrayUtils.sum(tmp1, tmp2, tmp1);
ArrayUtils.sum(tmp1, tmp3, tmp1);
// no need to set v = tmp1, because it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
beta1 = -initialBeta1;
beta2 = -initialBeta2;
beta3 = -initialBeta3;
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.multiplyByScalar(vc, beta3, tmp3);
ArrayUtils.sum(tmp1, tmp2, tmp1);
ArrayUtils.sum(tmp1, tmp3, tmp1);
// no need to set v = tmp1, because it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
}
/**
* Tests solution 2 for general point configuration.
* Because solution is up to scale, 4 different solutions for different
* beta1 and beta2 signs are tried.
*
* @throws AlgebraException if a numerical degeneracy occurs.
* @throws LockedException never happens.
* @throws NotReadyException never happens.
* @throws CoincidentPointsException if a point degeneracy has occurred.
*/
private void generalSolution2() throws AlgebraException, LockedException, NotReadyException,
CoincidentPointsException {
// we have the distance constraints between control world points (c) and
// control camera points (v):
// ||beta*vi - beta*vj||^2 = ||ci - cj||^2, i,j 1...4
// we need to find beta to scale control camera points
// in the case we pick 2 columns of the null-space v, then v is a linear
// combination v = beta1*vA + beta2*vB and previous equation becomes
// ||(beta1*vAi + beta2*vBi) - (beta1*vAj + beta2*vBj)||^2 = ||ci - cj||^2, i,j 1...4
// This equation can be expanded in x,y,z coordinates as follows:
// ((beta1*vAix + beta2*vBix) - (beta1*vAjx + beta2*vBjx))^2 + ((beta1*vAiy + beta2*vBiy) - (beta1*vAjy + beta2*vBjy))^2 + ((beta1*vAiz + beta2*vBiz) - (beta1*vAjz + beta2*vBjz))^2 = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2), i,j 1...4
// (beta1*(vAix - vAjx) + beta2*(vBix - vBjx))^2 + (beta1*(vAiy - vAjy) + beta2*(vBiy - vBjy))^2 + (beta1*(vAiz - vAjz) + beta2*(vBiz - vBjz))^2 = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2), i,j 1...4
// beta1^2*(vAix - vAjx)^2 + beta1*beta2*2*(vAix - vAjx)*(vBix - vBjx) + beta2^2*(vBix - vBjx)^2 + beta1^2*(vAiy - vAjy)^2 + beta1*beta2*2*(vAiy - vAjy)*(vBiy - vBjy) + beta2^2*(vBiy - vBjy)^2 + beta1^2*(vAiz - vAjz)^2 + beta1*beta2*2*(vAiz - vAjz)*(vBiz - vBjz) + beta2^2*(vBiz - vBjz)^2 = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2), i,j 1...4
// Since beta1 and beta2 are the unknowns, we can reorganize equation as:
// beta1^2*((vAix - vAjx)^2 + (vAiy - vAjy)^2 + (vAiz - vAjz)^2) +
// beta1*beta2*2*((vAix - vAjx)*(vBix - vBjx) + (vAiy - vAjy)*(vBiy - vBjy) + (vAiz - vAjz)*(vBiz - vBjz)) +
// beta2^2*((vBix - vBjx)^2 + (vBiy - vBjy)^2 + (vBiz - vBjz)^2) =
// ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2), i,j 1...4
// We linearize the equation assuming:
// alpha1 = beta1^2
// alpha2 = beta1*beta2
// alpha3 = beta2^2
// alpha1*((vAix - vAjx)^2 + (vAiy - vAjy)^2 + (vAiz - vAjz)^2) + alpha2*2*((vAix - vAjx)*(vBix - vBjx) + (vAiy - vAjy)*(vBiy - vBjy) + (vAiz - vAjz)*(vBiz - vBjz)) + alpha3*((vBix - vBjx)^2 + (vBiy - vBjy)^2 + (vBiz - vBjz)^2) = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2), i,j 1...4
// finally we evaluate the equation for all 6 possible combinations of
// i,j 1...4 when we have 4 control points
// Obtaining the following equations:
// alpha1*((vA1x - vA2x)^2 + (vA1y - vA2y)^2 + (vA1z - vA2z)^2) + alpha2*2*((vA1x - vA2x)*(vB1x - vB2x) + (vA1y - vA2y)*(vB1y - vB2y) + (vA1z - vA2z)*(vB1z - vB2z)) + alpha3*((vB1x - vB2x)^2 + (vB1y - vB2y)^2 + (vB1z - vB2z)^2) = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2)
// alpha1*((vA1x - vA3x)^2 + (vA1y - vA3y)^2 + (vA1z - vA3z)^2) + alpha2*2*((vA1x - vA3x)*(vB1x - vB3x) + (vA1y - vA3y)*(vB1y - vB3y) + (vA1z - vA3z)*(vB1z - vB3z)) + alpha3*((vB1x - vB3x)^2 + (vB1y - vB3y)^2 + (vB1z - vB3z)^2) = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2)
// alpha1*((vA1x - vA4x)^2 + (vA1y - vA4y)^2 + (vA1z - vA4z)^2) + alpha2*2*((vA1x - vA4x)*(vB1x - vB4x) + (vA1y - vA4y)*(vB1y - vB4y) + (vA1z - vA4z)*(vB1z - vB4z)) + alpha3*((vB1x - vB4x)^2 + (vB1y - vB4y)^2 + (vB1z - vB4z)^2) = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2)
// alpha1*((vA2x - vA3x)^2 + (vA2y - vA3y)^2 + (vA2z - vA3z)^2) + alpha2*2*((vA2x - vA3x)*(vB2x - vB3x) + (vA2y - vA3y)*(vB2y - vB3y) + (vA2z - vA3z)*(vB2z - vB3z)) + alpha3*((vB2x - vB3x)^2 + (vB2y - vB3y)^2 + (vB2z - vB3z)^2) = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2)
// alpha1*((vA2x - vA4x)^2 + (vA2y - vA4y)^2 + (vA2z - vA4z)^2) + alpha2*2*((vA2x - vA4x)*(vB2x - vB4x) + (vA2y - vA4y)*(vB2y - vB4y) + (vA2z - vA4z)*(vB2z - vB4z)) + alpha3*((vB2x - vB4x)^2 + (vB2y - vB4y)^2 + (vB2z - vB4z)^2) = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2)
// alpha1*((vA3x - vA4x)^2 + (vA3y - vA4y)^2 + (vA3z - vA4z)^2) + alpha2*2*((vA3x - vA4x)*(vB3x - vB4x) + (vA3y - vA4y)*(vB3y - vB4y) + (vA3z - vA4z)*(vB3z - vB4z)) + alpha3*((vB3x - vB4x)^2 + (vB3y - vB4y)^2 + (vB3z - vB4z)^2) = ((cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2)
// where alpha1, alpha2 and alpha3 are the unknowns of linear system of
// equations whose matrix C has size 6,3 (as seen below), and the right
// terms of the equation can be built by calling method
// #rhos(List<Point3D>) in this class
final var va = nullspace.get(0);
final var vb = nullspace.get(1);
final var controlCameraPointsA = controlPointsFromV(va);
final var controlCameraPointsB = controlPointsFromV(vb);
final var c = constraintMatrixSolution2(controlCameraPointsA, controlCameraPointsB);
final var rhos = rhos(controlWorldPoints);
final var a = Utils.solve(c, rhos);
// obtained a values are related to betas with the following expressions
// due to linearization:
// alpha1 = beta1^2
// alpha2 = beta1*beta2
// alpha3 = beta2^2
double beta1;
double beta2;
if (a[0] < 0.0) {
beta1 = Math.sqrt(-a[0]);
beta2 = a[2] < 0.0 ? Math.sqrt(-a[2]) : 0.0;
} else {
beta1 = Math.sqrt(a[0]);
beta2 = a[2] > 0.0 ? Math.sqrt(a[2]) : 0.0;
}
// fix sign of betas
if (a[1] < 0.0) {
beta1 = -beta1;
}
final var initialBeta1 = beta1;
final var initialBeta2 = beta2;
// compute linear combination of va and vb as v = beta1*va + beta2*vb
final var tmp1 = ArrayUtils.multiplyByScalarAndReturnNew(va, beta1);
final var tmp2 = ArrayUtils.multiplyByScalarAndReturnNew(vb, beta2);
ArrayUtils.sum(tmp1, tmp2, tmp1);
var controlCameraPoints = controlPointsFromV(tmp1);
var solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
// because solutions are square roots, beta1 and beta2 can have different
// signs, so we add solutions for each combination so that the one with
// the smallest re-projection error will be picked
beta1 = -initialBeta1;
beta2 = -initialBeta2;
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.sum(tmp1, tmp2, tmp1);
// no need to set v = tmp1, as it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
beta1 = initialBeta1;
beta2 = -initialBeta2;
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.sum(tmp1, tmp2, tmp1);
// no need to set v = tmp1, as it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
beta1 = -initialBeta1;
beta2 = initialBeta2;
ArrayUtils.multiplyByScalar(va, beta1, tmp1);
ArrayUtils.multiplyByScalar(vb, beta2, tmp2);
ArrayUtils.sum(tmp1, tmp2, tmp1);
// no need to set v = tmp1, as it already has this value
controlCameraPoints = controlPointsFromV(tmp1);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
}
/**
* Fills constraint matrix to solve betas when using control points from the
* last 3 columns of v (the null-space).
* The solution will be the linear combination of control points from the
* last 3 columns using estimated betas. This solution will be control
* points in camera coordinates.
*
* @param controlCameraPointsA control points of last column of v.
* @param controlCameraPointsB control points of second last column of v.
* @param controlCameraPointsC control points of third last column of v.
* @return constraint matrix to solve a linear system of equations.
* @throws AlgebraException never happens.
*/
private static Matrix constraintMatrixSolution3(
final List<Point3D> controlCameraPointsA, final List<Point3D> controlCameraPointsB,
final List<Point3D> controlCameraPointsC) throws AlgebraException {
final var numControl = controlCameraPointsA.size();
final var numEquations = numEquations(numControl);
final var c = new Matrix(numEquations, 6);
int row = 0;
for (var i = 0; i < numControl; i++) {
final var vai = controlCameraPointsA.get(i);
final var vbi = controlCameraPointsB.get(i);
final var vci = controlCameraPointsC.get(i);
for (var j = i + 1; j < numControl; j++) {
final var vaj = controlCameraPointsA.get(j);
final var vbj = controlCameraPointsB.get(j);
final var vcj = controlCameraPointsC.get(j);
fillRowConstraintMatrixSolution3(row, c, vai, vaj, vbi, vbj, vci, vcj);
row++;
}
}
return c;
}
/**
* Fills constraint matrix to solve betas when using control points from the
* last 2 columns of v (the null-space).
* The solution will be the linear combination of control points from the
* last 2 columns using estimated betas. This solution will be control
* points in camera coordinates.
*
* @param controlCameraPointsA control points of last column of v.
* @param controlCameraPointsB control points of second last column of v.
* @return constraint matrix to solve a linear system of equations.
* @throws AlgebraException never happens.
*/
private static Matrix constraintMatrixSolution2(
final List<Point3D> controlCameraPointsA, final List<Point3D> controlCameraPointsB)
throws AlgebraException {
final var numControl = controlCameraPointsA.size();
final var numEquations = numEquations(numControl);
final var c = new Matrix(numEquations, 3);
var row = 0;
for (var i = 0; i < numControl; i++) {
final var vai = controlCameraPointsA.get(i);
final var vbi = controlCameraPointsB.get(i);
for (var j = i + 1; j < numControl; j++) {
final var vaj = controlCameraPointsA.get(j);
final var vbj = controlCameraPointsB.get(j);
fillRowConstraintMatrixSolution2(row, c, vai, vaj, vbi, vbj);
row++;
}
}
return c;
}
/**
* Fills a row of constraint matrix for solution 3.
*
* @param row row to be filled.
* @param c matrix to be filled.
* @param vai i-th control point in camera coordinates of last column of v
* (i.e. the null-space).
* @param vaj j-th control point in camera coordinates of last column of v
* (i.e. the null-space).
* @param vbi i-th control point in camera coordinates of second last column
* of v (i.e. the null-space).
* @param vbj j-th control point in camera coordinates of second last column
* of v (i.e. the null-space).
* @param vci i-th control point in camera coordinates of third last column
* of v (i.e. the null-space).
* @param vcj j-th control point in camera coordinates of third last column
* of v (i.e. the null-space).
*/
private static void fillRowConstraintMatrixSolution3(
final int row, final Matrix c, final Point3D vai, final Point3D vaj, final Point3D vbi, final Point3D vbj,
final Point3D vci, final Point3D vcj) {
final var vaix = vai.getInhomX();
final var vaiy = vai.getInhomY();
final var vaiz = vai.getInhomZ();
final var vajx = vaj.getInhomX();
final var vajy = vaj.getInhomY();
final var vajz = vaj.getInhomZ();
final var vbix = vbi.getInhomX();
final var vbiy = vbi.getInhomY();
final var vbiz = vbi.getInhomZ();
final var vbjx = vbj.getInhomX();
final var vbjy = vbj.getInhomY();
final var vbjz = vbj.getInhomZ();
final var vcix = vci.getInhomX();
final var vciy = vci.getInhomY();
final var vciz = vci.getInhomZ();
final var vcjx = vcj.getInhomX();
final var vcjy = vcj.getInhomY();
final var vcjz = vcj.getInhomZ();
// 1st column
c.setElementAt(row, 0, Math.pow(vaix - vajx, 2.0) + Math.pow(vaiy - vajy, 2.0)
+ Math.pow(vaiz - vajz, 2.0));
// 2nd column
c.setElementAt(row, 1, 2.0 * ((vaix - vajx) * (vbix - vbjx) + (vaiy - vajy) * (vbiy - vbjy)
+ (vaiz - vajz) * (vbiz - vbjz)));
// 3rd column
c.setElementAt(row, 2, 2.0 * ((vaix - vajx) * (vcix - vcjx) + (vaiy - vajy) * (vciy - vcjy)
+ (vaiz - vajz) * (vciz - vcjz)));
// 4th column
c.setElementAt(row, 3, Math.pow(vbix - vbjx, 2.0) + Math.pow(vbiy - vbjy, 2.0)
+ Math.pow(vbiz - vbjz, 2.0));
// 5th column
c.setElementAt(row, 4, 2.0 * ((vbix - vbjx) * (vcix - vcjx) + (vbiy - vbjy) * (vciy - vcjy)
+ (vbiz - vbjz) * (vciz - vcjz)));
// 6th column
c.setElementAt(row, 5, Math.pow(vcix - vcjx, 2.0) + Math.pow(vciy - vcjy, 2.0)
+ Math.pow(vciz - vcjz, 2.0));
}
/**
* Fills a row of constraint matrix for solution 2.
*
* @param row row to be filled.
* @param c matrix to be filled.
* @param vai i-th control point in camera coordinates of last column of v
* (i.e. the null-space).
* @param vaj j-th control point in camera coordinates of last column of v
* (i.e. the null-space).
* @param vbi i-th control point in camera coordinates of second last column
* of v (i.e. the null-space).
* @param vbj j-th control point in camera coordinates of second last column
* of v (i.e. the null-space).
*/
private static void fillRowConstraintMatrixSolution2(
final int row, final Matrix c, final Point3D vai, final Point3D vaj, final Point3D vbi, final Point3D vbj) {
final var vaix = vai.getInhomX();
final var vaiy = vai.getInhomY();
final var vaiz = vai.getInhomZ();
final var vajx = vaj.getInhomX();
final var vajy = vaj.getInhomY();
final var vajz = vaj.getInhomZ();
final var vbix = vbi.getInhomX();
final var vbiy = vbi.getInhomY();
final var vbiz = vbi.getInhomZ();
final var vbjx = vbj.getInhomX();
final var vbjy = vbj.getInhomY();
final var vbjz = vbj.getInhomZ();
// 1st column
c.setElementAt(row, 0, Math.pow(vaix - vajx, 2.0) + Math.pow(vaiy - vajy, 2.0)
+ Math.pow(vaiz - vajz, 2.0));
// 2nd column
c.setElementAt(row, 1, 2.0 * ((vaix - vajx) * (vbix - vbjx) + (vaiy - vajy) * (vbiy - vbjy)
+ (vaiz - vajz) * (vbiz - vbjz)));
// 3rd column
c.setElementAt(row, 2, Math.pow(vbix - vbjx, 2.0) + Math.pow(vbiy - vbjy, 2.0)
+ Math.pow(vbiz - vbjz, 2.0));
}
/**
* Tests solution 1 for general point configuration.
* Because solution is up to scale. Two possible solutions must be evaluated
* (positive or negative scale). The one with the smallest re-projection
* error will be picked.
*
* @throws LockedException never happens.
* @throws NotReadyException never happens.
* @throws CoincidentPointsException if a point degeneracy has occurred.
*/
private void generalSolution1() throws LockedException, NotReadyException, CoincidentPointsException {
// pick last column of null-space, contains control points in camera
// coordinates up to scale (including sign change)
var v = nullspace.get(0);
var controlCameraPoints = controlPointsFromV(v);
// similarly to solution2 and solution3, we could find the scale by
// imposing the restriction: ||beta*vi - beta*vj||^2 = ||ci - cj||^2, i,j 1...4
// This results in a linear system of 6 equations (when we have 4 control
// points)
// The previous constraint can be expanded as follows:
// (beta*vi - beta*vj)^2 = (ci - cj)^2
// (beta*vix - beta*vjx)^2 + (beta*viy - beta*vjy)^2 + (beta*viz - beta*vjz)^2 = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// beta^2*(vix - vjx)^2 + beta^2*(viy - vjy)^2 + beta^2*(viz - vjz)^2 = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// beta^2*((vix - vjx)^2 + (viy - vjy)^2 + (viz - vjz)^2) = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
//
// And the system is linearized by assuming
// alpha = beta^2
//
// alpha * ((vix - vjx)^2 + (viy - vjy)^2 + (viz - vjz)^2) = (cix - cjx)^2 + (ciy - cjy)^2 + (ciz - cjz)^2, i,j 1...4
// However, in order to find a solution a MetricTransformation3D estimator
// is used, which is capable to determine the scale relating input and
// output points, and thus, solving the linear system of equations is not
// required in this case.
var solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
// because v is a solution up to scale, we provide a solution with
// opposite sign
v = ArrayUtils.multiplyByScalarAndReturnNew(v, -1.0);
controlCameraPoints = controlPointsFromV(v);
solution = computePossibleSolutionWithPoseAndReprojectionError(controlCameraPoints);
solutions.add(solution);
}
/**
* Computes a possible solution with camera, transformation, re-projection
* error and control points in camera coordinates.
*
* @param controlCameraPoints control points in camera coordinates.
* @return a possible solution.
* @throws LockedException never happens.
* @throws NotReadyException never happens.
* @throws CoincidentPointsException if a point degeneracy has occurred.
*/
private Solution computePossibleSolutionWithPoseAndReprojectionError(
final List<Point3D> controlCameraPoints) throws LockedException, NotReadyException,
CoincidentPointsException {
final var worldToCameraTransformation = worldToCameraTransformationMetric(controlCameraPoints);
final var rotation = worldToCameraTransformation.getRotation();
final var t = worldToCameraTransformation.getTranslation();
final var scale = worldToCameraTransformation.getScale();
// Camera center is C = -1/s*R'*t
final var center = new InhomogeneousPoint3D(-t[0] / scale, -t[1] / scale, -t[2] / scale);
final var invRotation = rotation.inverseRotationAndReturnNew();
invRotation.rotate(center, center);
final var camera = new PinholeCamera(intrinsic, rotation, center);
final var solution = new Solution();
solution.controlCameraPoints = controlCameraPoints;
solution.worldToCameraTransformation = worldToCameraTransformation;
solution.camera = camera;
// compute projection error
solution.reprojectionError = reprojectionError(camera);
return solution;
}
/**
* Estimates world to camera transformation using estimated control points
* in world and camera coordinates as a metric transformation.
*
* @param controlCameraPoints control points in camera coordinates.
* @return metric transformation relating control points from world to
* camera coordinates.
* @throws LockedException never happens.
* @throws NotReadyException never happens.
* @throws CoincidentPointsException if a point degeneracy has occurred.
*/
private MetricTransformation3D worldToCameraTransformationMetric(final List<Point3D> controlCameraPoints)
throws LockedException, NotReadyException, CoincidentPointsException {
final var estimator = new MetricTransformation3DEstimator(controlWorldPoints, controlCameraPoints, isPlanar);
return estimator.estimate();
}
/**
* Number of equations required to solve constraints for case 1 to 4.
*
* @param numControl number of control points.
* @return number of constraint equations.
*/
private static int numEquations(final int numControl) {
var numEquations = 0;
for (var i = 1; i < numControl; i++) {
numEquations += i;
}
return numEquations;
}
/**
* Right term of linearized system of equations to solve betas.
*
* @param controlWorldPoints control points in world coordinates.
* @return right term.
*/
private static double[] rhos(final List<Point3D> controlWorldPoints) {
final var numControl = controlWorldPoints.size();
final var numEquations = numEquations(numControl);
final var rhos = new double[numEquations];
// squared distance from control world i to control world j
double dcijSqr;
var pos = 0;
for (var i = 0; i < numControl; i++) {
final var ci = controlWorldPoints.get(i);
for (var j = i + 1; j < numControl; j++) {
final var cj = controlWorldPoints.get(j);
dcijSqr = Math.pow(ci.distanceTo(cj), 2.0);
rhos[pos] = dcijSqr;
pos++;
}
}
return rhos;
}
/**
* Total re-projection error for provided camera.
*
* @param camera camera to estimate re-projection error.
* @return reprojection error.
*/
private double reprojectionError(final PinholeCamera camera) {
final var n = points2D.size();
Point3D point3D;
final var projected = Point2D.create();
Point2D point2D;
var error = 0.0;
for (var i = 0; i < n; i++) {
point3D = points3D.get(i);
point2D = points2D.get(i);
camera.project(point3D, projected);
error += projected.distanceTo(point2D);
}
return error;
}
/**
* Computes list of control points from provided array containing one column
* of the null-space of M or a linear combination of columns of the
* null-space.
*
* @param v one column of the null-space of M or a linear combination of
* columns of the null-space.
* @return control points.
*/
private List<Point3D> controlPointsFromV(final double[] v) {
final var numControl = controlWorldPoints.size();
final var points = new ArrayList<Point3D>();
for (var j = 0; j < numControl; j++) {
final var k = j * 3;
final var p = new InhomogeneousPoint3D(v[k], v[k + 1], v[k + 2]);
points.add(p);
}
return points;
}
/**
* Solves null-space of matrix M containing possible solutions of camera
* coordinates of control points.
*
* @throws AlgebraException if something fails due to numerical
* instabilities.
*/
private void solveNullspace() throws AlgebraException {
final var rows = m.getRows();
final var cols = m.getColumns();
final var numControl = cols / Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH;
// normalize rows of m to increase numerical accuracy
for (var i = 0; i < rows; i++) {
normalizeRow(m, i);
}
final var decomposer = new SingularValueDecomposer(m);
decomposer.decompose();
// Singular values are always in descending order, hence null space is in
// the last columns of v.
// V is 12x12 (general configuration) or 9x9 (planar configuration).
// Each column of v contains coordinates of control points in camera
// coordinates.
// A solution for the linear system M*x = 0 is obtained as a linear
// combination of the columns of v forming the null-space.
final var v = decomposer.getV();
// although nullity of M could be determined after SVD, it is assumed
// instead that null-space could be located in any of the latter columns
// of v up to the number of control points.
// Hence, for general configuration we pick the last 4 columns of v and
// for planar configuration we pick the last 3.
// extract null points from the null space
nullspace = new ArrayList<>();
final var colsMinusOne = cols - 1;
for (var i = 0; i < numControl; i++) {
final var column = colsMinusOne - i;
// each picked column of v contains a possible solution
final var vCol = v.getSubmatrixAsArray(0, column, colsMinusOne, column);
nullspace.add(vCol);
}
}
/**
* Normalizes provided row of m.
*
* @param m matrix to be normalized.
* @param row row to be normalized.
*/
private static void normalizeRow(final Matrix m, final int row) {
final var cols = m.getColumns();
var norm = 0.0;
for (int i = 0; i < cols; i++) {
norm += Math.pow(m.getElementAt(row, i), 2.0);
}
norm = Math.sqrt(norm);
for (var i = 0; i < cols; i++) {
m.setElementAt(row, i, m.getElementAt(row, i) / norm);
}
}
/**
* In order to find control points in camera coordinates, an homogeneous
* linear system of equations must be solved having the form M*x = 0, where
* x contains the coordinates of all control points in the form [x1, y1, z1,
* x2, y2, z2, ... ].
* For general configuration there are 4 control points, hence x has length
* 12 (3 coordinates * 4 control points).
* For a planar configuration there are 3 control points, hence x has length
* 9 (3 coordinates * 3 control points).
* This method builds M matrix required to solve such linear system of
* equations, where M has size 2*n x 12 (general configuration) or 2*n x 9
* (planar configuration), where n is the number of provided 2D observed
* points.
*
* @throws AlgebraException if numerical instabilities occur.
*/
private void buildM() throws AlgebraException {
final var n = points2D.size();
final var numControlPoints = alphas.getColumns();
m = new Matrix(2 * n, 3 * numControlPoints);
int row;
int col;
double alpha;
final var horizontalFocalLength = intrinsic.getHorizontalFocalLength();
final var verticalFocalLength = intrinsic.getVerticalFocalLength();
final var skewness = intrinsic.getSkewness();
final var horizontalPrincipalPoint = intrinsic.getHorizontalPrincipalPoint();
final var verticalPrincipalPoint = intrinsic.getVerticalPrincipalPoint();
Point2D p;
double pX;
double pY;
for (var i = 0; i < n; i++) {
p = points2D.get(i);
pX = p.getInhomX();
pY = p.getInhomY();
row = i * 2;
for (var j = 0; j < numControlPoints; j++) {
col = j * 3;
alpha = alphas.getElementAt(i, j);
m.setElementAt(row, col, alpha * horizontalFocalLength);
m.setElementAt(row, col + 1, alpha * skewness);
m.setElementAt(row, col + 2, alpha * (horizontalPrincipalPoint - pX));
m.setElementAt(row + 1, col, 0.0);
m.setElementAt(row + 1, col + 1, alpha * verticalFocalLength);
m.setElementAt(row + 1, col + 2, alpha * (verticalPrincipalPoint - pY));
}
}
}
/**
* Computes the coordinates of each provided world point in terms of
* estimated control points in world coordinates.
* Such coordinates (i.e. barycentric coordinates) are stored in alphas
* matrix, where each row contains the coordinates of each world point in
* terms of control points.
* For general configuration, each row contains 4 coordinates and alphas
* has size nx4, where n is the number of provided 3D world points.
* For planar configuration, each row contains 3 coordinates and alphas
* has size nx3, where n is the number of provided 3D world points.
* Because world and camera coordinates are related by a rotation (since
* both reference frames are centered in the centroid), alphas can be used
* in both world and camera coordinates.
*
* @throws AlgebraException if there are numerical instabilities.
*/
private void computeBarycentricCoordinates() throws AlgebraException {
// we need to express world points in terms of control points in world
// coordinates
// In the general configuration case:
// For a point p1 in world inhomogeneous coordinates
// p1 = alpha1 + c1 + alpha2 * c2 + alpha3 * c3 + alpha4 * c4
// where alpha1, alpha2, alpha3, alpha4 are scalars and
// c1, c2, c3 are the control points in the principal axes and
// centroid is the last control point c4, all 4 expressed in world
// inhomogeneous coordinates as 3-column vectors.
// Assuming a matrix form:
// [p1] = [c1 c2 c3 c4]*[alpha1]
// [alpha2]
// [alpha3]
// [alpha4]
// or in simpler for p = C * alpha, where p is a 3-column vector, C is a
// 3x4 matrix and alpha is a 4-1 vector.
// This can be repeated for each i-th point so that:
// pi = C * alphai --> alphai = inv(C)*pi
// However, in this form C is not invertible because it is rank deficient
// To avoid this deficiency we add the constraint that the sum of alphas
// for a point must be 1, so we can use the reduced form:
// [p1 - c4] = [(c1 - c4) (c2 - c4) (c3 - c4)]*[alpha1]
// [alpha2]
// [alpha3]
// and set alpha4 = 1 - alpha1 - alpha2 - alpha3
// This way the equation still holds:
// p1 - c4 = (c1 - c4) * alpha1 + (c2 - c4) * alpha2 + (c3 - c4) * alpha3 =
// = c1 * alpha1 + c2 * alpha2 + c3 * alpha3 - c4 * (alpha1 + alpha2 + alpha3)
// p1 = c1 * alpha1 + c2 * alpha2 * c3 * alpha3 + c4 * (1 - alpha1 - alpha2 - alpha3)
// This way, we create reduced matrix C as having 3 rows (one for each
// inhomogeneous coordinate) and 3 columns in the general case.
// In the planar case we have only 3 control points, and the last one
// (c3) is the centroid.
final var numControl = controlWorldPoints.size();
final var numDimensions = numControl - 1;
final var numControlMinusTwo = numControl - 2;
final var c = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, numDimensions);
// the last control point is the centroid (or mean point)
final var mean = controlWorldPoints.get(numDimensions);
final var meanX = mean.getInhomX();
final var meanY = mean.getInhomY();
final var meanZ = mean.getInhomZ();
for (var i = 0; i < numDimensions; i++) {
final var controlPoint = controlWorldPoints.get(i);
c.setElementAt(0, i, controlPoint.getInhomX() - meanX);
c.setElementAt(1, i, controlPoint.getInhomY() - meanY);
c.setElementAt(2, i, controlPoint.getInhomZ() - meanZ);
}
// to find reduced alphas, we need to inverse the reduced C matrix and
// multiply it by [p - centroid], where centroid can be c4 or c3 in
// planar case.
final var invC = Utils.inverse(c);
// x is p - centroid, where p is each 3D world point
final var n = points3D.size();
final var reducedPoint = new Matrix(Point3D.POINT3D_INHOMOGENEOUS_COORDINATES_LENGTH, 1);
final var reducedAlpha = new Matrix(numDimensions, 1);
double[] buffer;
Point3D worldPoint;
alphas = new Matrix(n, numControl);
for (var i = 0; i < n; i++) {
worldPoint = points3D.get(i);
reducedPoint.setElementAtIndex(0, worldPoint.getInhomX() - meanX);
reducedPoint.setElementAtIndex(1, worldPoint.getInhomY() - meanY);
reducedPoint.setElementAtIndex(2, worldPoint.getInhomZ() - meanZ);
invC.multiply(reducedPoint, reducedAlpha);
buffer = reducedAlpha.getBuffer();
// copy reducedAlpha into the former components of i-th row of alphas
alphas.setSubmatrix(i, 0, i, numControlMinusTwo, buffer);
// The last component of each alpha for each point is computed so
// that their sum is equal to one
if (numControl == GENERAL_NUM_CONTROL_POINTS) {
// general configuration
alphas.setElementAt(i, numDimensions, 1.0 - buffer[0] - buffer[1] - buffer[2]);
} else {
// planar configuration
alphas.setElementAt(i, numDimensions, 1.0 - buffer[0] - buffer[1]);
}
}
}
/**
* Computes control points in world coordinates and determines whether
* they are located in a planar configuration or not.
* This method computes the centroid of provided 3D points and their
* covariance.
* Uses PCA by means of SVD decomposition of their covariance matrix in
* order to find the principal directions of the cloud formed by the
* collection of points and sets control points as the computed centroid
* and points along the principal axes so that they form a basis that
* can be used to express any 3D points into.
* If the smallest singular value is close to zero in comparison to the
* largest one, then it is assumed that 3D points are in a planar
* configuration.
* If a planar configuration is allowed, then only 3 control points are
* computed along the plane using the centroid and two points on the
* principal directions of such plane.
* Otherwise, in general configuration, 4 control points are computed as
* the centroid and 3 points along the principal axes of the cloud of 3D
* points.
*
* @throws AlgebraException if something fails because of numerical
* instabilities.
*/
private void computeWorldControlPointsAndPointConfiguration() throws AlgebraException {
final var centroid = Point3D.centroid(points3D);
// covariance matrix elements, summed up here for speed
var c11 = 0.0;
var c12 = 0.0;
var c13 = 0.0;
var c22 = 0.0;
var c23 = 0.0;
var c33 = 0.0;
double dx;
double dy;
double dz;
final var n = points3D.size();
for (final var point : points3D) {
dx = point.getInhomX() - centroid.getInhomX();
dy = point.getInhomY() - centroid.getInhomY();
dz = point.getInhomZ() - centroid.getInhomZ();
c11 += dx * dx;
c12 += dx * dy;
c13 += dx * dz;
c22 += dy * dy;
c23 += dy * dz;
c33 += dz * dz;
}
c11 /= n;
c12 /= n;
c13 /= n;
c22 /= n;
c23 /= n;
c33 /= n;
final var covar = new Matrix(3, 3);
covar.setElementAt(0, 0, c11);
covar.setElementAt(1, 0, c12);
covar.setElementAt(2, 0, c13);
covar.setElementAt(0, 1, c12);
covar.setElementAt(1, 1, c22);
covar.setElementAt(2, 1, c23);
covar.setElementAt(0, 2, c13);
covar.setElementAt(1, 2, c23);
covar.setElementAt(2, 2, c33);
final var decomposer = new SingularValueDecomposer(covar);
decomposer.decompose();
final var singularValues = decomposer.getSingularValues();
final var v = decomposer.getV();
// planar check
int numControl;
if (!planarConfigurationAllowed
|| Math.abs(singularValues[0]) < Math.abs(singularValues[2]) * planarThreshold) {
// general configuration
numControl = GENERAL_NUM_CONTROL_POINTS;
isPlanar = false;
} else {
// planar configuration (only if allowed)
numControl = PLANAR_NUM_CONTROL_POINTS;
isPlanar = true;
}
controlWorldPoints = new ArrayList<>();
final var centroidX = centroid.getInhomX();
final var centroidY = centroid.getInhomY();
final var centroidZ = centroid.getInhomZ();
final var numDimensions = numControl - 1;
final var k = Math.sqrt(singularValues[0] / n);
double vx;
double vy;
double vz;
for (var i = 0; i < numDimensions; i++) {
vx = v.getElementAt(0, i) * k;
vy = v.getElementAt(1, i) * k;
vz = v.getElementAt(2, i) * k;
controlWorldPoints.add(new InhomogeneousPoint3D(centroidX + vx, centroidY + vy, centroidZ + vz));
}
// add centroid (it will be used for the metric transformation
// estimation)
controlWorldPoints.add(centroid);
}
/**
* A possible solution.
*/
private static class Solution {
/**
* Control points in camera coordinates.
*/
List<Point3D> controlCameraPoints;
/**
* Transformation from world to camera coordinates.
* Point projection is expressed by x = P * Xw, where P is a pinhole
* camera and Xw is a point in world coordinates.
* Points in camera coordinates are expressed as:
* Xc = Tw-<c * Xw, where Tw-<c is the transformation from world to
* camera.
* The Euclidean transformation Tw-<c is expressed as:
* Tw-<c = [R t]
* [0' 1]
* Projection of a point in camera coordinates can also be expressed
* as x = Pc * Xc = K * [I 0] * Xc
* where Pc is a camera and has the form Pc = K *[I 0], so that
* x = Pc * Xc = K * [I 0] * Xc = K * [I 0] * Tw-<c * Xw
* x = K * [I 0] * [R t] * Xw = K * [I*R + 0, I*t + 0] * Xw =
* [0' 1]
* x = K * [R t] * Xw = K * [R - R*C] * Xw = x = P * Xw,
* where R is a rotation and C is the camera center in world
* coordinates.
* Assuming that control points are obtained up to scale, then instead
* of an Euclidean transformation we will assume that Tw-<c is a metric
* transformation, hence:
* Tw-<c = [s*R t2]
* [0' 1 ]
* To obtain the previous equation, then point in camera coordinates
* must be 1/s*Xc so that:
* x = Pc * 1/s * Xc = K * [I 0] * 1/s * Xc
* x = K * [I 0] * 1/s * Tw-<c * Xw
* x = K * [I 0] * 1/s *[s*R t2] * Xw = K * 1/s * [I*s*R + 0, I*t2 + 0]
* [0' 1 ]
* x = K * 1 / s * [s*R t2] * Xw = K * [R 1/s*t2] * Xw
* where t = 1/s*t2 = -R*C and so again
* x = K * [R t] * Xw
* and camera center is C = -1/s*R'*t2
*/
MetricTransformation3D worldToCameraTransformation;
/**
* Pinhole camera using provided intrinsic parameters and estimated
* transformation for this solution.
*/
PinholeCamera camera;
/**
* Re-projection error.
*/
double reprojectionError;
}
}