FAST AND RESISTANT PROCRUSTEAN BUNDLE ADJUSTMENT

In a recent paper (Fusiello and Crosilla, 2015) a Procrustean formulation of the bundle block adjustment has been presented, with a solution based on alternating least squares. This paper improves on it in two respects: it introduces a faster iterative scheme that minimizes the same cost function, thereby achieving the same accuracy, and makes the method resistant to rogue measures through iteratively reweighted least-squares. Empirical results confirm the effectiveness of these enhancements.


INTRODUCTION
Orthogonal Procrustes Analysis (OPA) is a very useful tool to perform the direct least squares solution of similarity transformation problems in any dimensional space.At first, it was used for the multidimensional rotation and scaling of different matrix configuration pairs (Schönemann, 1966, Schönemann andCarroll, 1970).Successively, the solution was generalized (GPA) for the simultaneous least squares matching of more than two corresponding matrices (Gower, 1975, Ten Berge, 1977).Anisotropic OPA is referred instead to the case where the class of transformations is extended to anisotropic scaling of points or coordinates.For these problems (Gower and Dijksterhuis, 2004) suggest an iterative procedure where each variable is alternatively estimated while keeping the others fixed.This scheme is called block relaxation (de Leeuw, 1994) or alternating least squares (Young et al., 1976).While an iterative solution with guaranteed convergence was described by (Bennani Dosse and Ten Berge, 2010) for the cases of pre-and post-scaling of the columns, no analogous property is known in the literature for rows scaling.Different versions of anisotropic GPA are described in (Lingoes and Borg, 1978), (Gower and Dijksterhuis, 2004), (Commandeur, 1991) and (Bennani Dosse et al., 2011) (they differ on where the scaling is applied).
In the field of terrestrial laser scanning GPA has been already applied to the registration of multiple 3-D models (Beinat andCrosilla, 2001, Toldo et al., 2010).In the field of photogrammetry, (Crosilla and Beinat, 2002) applied the GPA to the solution of block adjustment by independent models, while (Garro et al., 2012) applied OPA (with anisotropic row-scaling) to solve the exterior orientation problem.In a recent paper (Fusiello and Crosilla, 2015) the Procrustean solution of the classical bundle block adjustment has been described.The method allows to obtain the same results of the classical least squares solution, without any linearization of the collinearity equations and without any a priori information about the exterior orientation parameters.The proposed method is based on the algorithms of the anisotropic Procrustean analysis and is implemented as a block relaxation procedure.
In this paper, an enhanced version of the above mentioned algorithm is presented.At first, a faster iterative solution of the same * Corresponding author minimization problem is developed, that allows to obtain a reduction of one order of magnitude w.r.t. the execution time required by the original algorithm.This new relaxation scheme solves an anisotropic GPA with row-scaling using the same framework of the GPA solution (Gower, 1975).Furthermore, a robust objective function is introduced that makes the method resistant to outliers.The new scheme, based on Iteratively Reweighted Least Squares (IRLS), achieves reliable results also in the presence of a percentage up to 10% of outliers.

PROBLEM FORMULATION
Let us now consider m images depicting the same n 3-D tiepoints s1 . . .sn.In the bundle block adjustment problem it is required to simultaneously find the image exterior orientation parameters and the tie-points 3-D coordinates that minimize a geometric error, without introducing intermediate models, as opposed to the block adjustment by independent models (Crosilla and Beinat, 2002).We review here the Procrustean solution to the free-network bundle block adjustment problem presented by (Fusiello and Crosilla, 2015).
We start from the collinearity equations for one camera: where sj is the coordinate vector of the j-th tie-point in the external system; c is the coordinate vector of the projection centre in the external system; ζj is a positive scalar proportional to the "depth" of the point, i.e., its distance to the plane containing the projection centre and parallel to the image plane; R is the rotation matrix transforming from the external system to the camera system; pj is the coordinate vector of the projected i-th tie-point in the camera system, where the third component is equal to −f , the principal distance or focal length.
Expressing (1) with respect to sj and transposing yields: and stacking the equations for n points s1 . . .sn, the following matrix formulation is obtained: or equivalently: where P is the matrix by rows of image tie-point coordinates defined in the camera system, S is the matrix by rows of tie-point coordinates defined in the external system, Z is the (positive) diagonal matrix containing the depth of each tie-point.
Thus, for each image i = 1 . . .m it is possible to write In this formula the image coordinates Pi are known, but all the other quantities are unknown.They can be recovered by minimizing the following error function: where each term of the sum represents the difference vector between 3-D tie-points (S) and the corresponding 2D points (Pi) back-projected according to their estimated depths (Zi) and estimated image orientation (Ri, ci) of image i.Let us call ∆ij each individual difference vector relative to exposure i = 1 . . .m and tie-point j = 1 . . .n (see Fig. 1).Overall, what is being minimized is the length of the residuals ∆ij for each exposure and each tie-point, in a least-squares sense.
The "legs" are line segments emanating from image points.The Procrustean bundle adjustment optimizes camera orientation and the length of each leg so as to bring the legs' endpoints as close as possible to each other, i.e., minimizing the length of the ∆ij.
Please note that if Zi were known, the problem would reduce to a GPA (see Appendix C), where the point sets to be aligned are the ZiPi i = 1 . . .m. On the other hand, computing Zi given all the other variables is independent in each camera, and the solution, originally developed by (Garro et al., 2012), is reported in App.B. This observation underlies both the solutions of (Fusiello and Crosilla, 2015) and the novel one proposed in this paper.

PREVIOUS SOLUTION
In (Fusiello and Crosilla, 2015) a solution to ( 6) is proposed, iterating between the following stages: • assuming all the Zi known, compute Ri, ci by solving a GPA problem (see App. C); • set the putative 3-D points S to the centroids • solve for Zi independently in each image (see App. B): The same formulation as (6) has been described at pg. 129 of (Commandeur, 1991), under the name STIMIDIO.The iterative solution proposed there is different from ours, though.

THE NEW SOLUTION: AGPA
The previous solution consists of two nested cycles, with a GPA as the inner one.This paper proposes a new iterative scheme without inner cycles, that has the same structure as the GPA, and therefore will prove to be faster.
Let us introduce the Anisotropic Generalized Procrustes Analysis (AGPA), which extends GPA (described in App.C) with anisotropic row-scaling for each individual model, and minimizes the following least squares objective function: where P1, P2, . . ., Pm are m model matrices containing (by rows) the same set of n points in m different coordinate systems.
Let P i = ZiPiRi + 1c i , by the same token as in the GPA (Commandeur, 1991), the problem is equivalent to minimize where S is the centroid: This is the same objective function of (6), hence AGPA solves the bundle adjustment problem as formulated in Sec. 2.
The objective function of AGPA ( 10) is very similar to the objective function of GPA ( 27), modulo the substitution of the scalar z with the corresponding weight matrix Z.As a consequence, the solution to AGPA, reported in Algorithm 1, is very similar to the GPA one, with the difference that the isotropic scale factor computation (step (b) of Algorithm 3) is substituted with the anisotropic scale formula (step (c) of Algorithm 1), borrowed from the AEOPA (App.B).
The solution corresponds to a free adjustment, as it does not involve any object space constraint.In fact, the global scale of the solution is discretionary, and only the ratios of the Zi are relevant.Therefore, at each iteration the Zi are normalized to unit average, in order to avoid degeneracy.Moreover, negative values are clipped to zero in step (c).
Ground control points can be incorporated as in (Crosilla and Beinat, 2002).
As it will be shown in Sec.6., this method has the same accuracy and convergence properties of the one reported by (Fusiello and Crosilla, 2015), but it is considerably faster.

RESISTANT AGPA
In real-world applications two issues must be considered, in order for any bundle adjustment method to be applicable.First, not all points are visible in all the images, so the Pi matrices have some unspecified entries.
Second, when tie-points have been obtained automatically -a very common trend (e.g.(Hartmann et al., 2015)) borrowed from the structure-from-motion literature -some of them are rogue points that would skew the Procrustean least square estimate, if proper countermeasures are not taken.This is particularly dangerous since the Procrustes procedure is fully automatic and does not require any a-priori information about the unknowns.
The first issue is handled as proposed in (Crosilla and Beinat, 2002), where for each matrix Pi, a diagonal matrix Mi can be inserted, containing unit values along the main diagonal in case the corresponding row of Pi is specified and zero on the contrary.As a result, the cost function is rewritten as: where the centroid S, is now defined as: Resistance to outliers can be obtained, following (Verboon and Heiser, 1992), by substituting the least squares cost function of the classical Procrustes method with specific robust cost functions, and solving the resulting minimization problem with Iteratively Reweighted Least Squares (IRLS) (Holland and Welsch, 1977).
This technique iteratively solves weighted least squares problems where the weights are estimated at each iteration by analysing the residuals of the current solution.The weights are assigned by a specific weight function in such a way to penalize outliers and promote inliers.
In our case, the weighted cost function is: where W is a diagonal matrix containing the positive weights W (j, j) for each point.
The solution to the weighted version of the problem is the same as in AGPA, with the following substitutions: Please note that the matrix W is not defined for each exposure, meaning that we conservatively attribute outlyingness to tie-points, as opposed to their image projections.
Several weight functions have been proposed in the literature; the ones offering greatest resistance to outliers are the so-called hard redescenders, that assign zero weight to points with residual higher than a threshold.Among them we picked the popular bisquare (a.k.a.biweight) function (Mosteller and Tukey, 1977): where the tuning constant k is chosen, as customary, so as to yield a reasonably high efficiency in the normal case, and still offers protection against outliers.In particular, k = 4.685σ produces 95-percent efficiency when the errors are normal with standard deviation σ.The latter is estimated robustly from the median absolute deviation (MAD) of the residuals as σ = M AD/0.6845.
In our case the residuals are defined as: Mi(j, j) S(j, :) − P i (j, :) where S(j, :) (resp.P i (j, :)) denotes the j−th row of S (resp.P i ).

Iterate from step 2. until convergence of weights
It is worth noting that the computation of the centroid S does not involve the IRLS weights W but only Mi, and that Zi does not depend on the weights at all, as all computations, although in matrix form, are essentially point-wise, so any unspecified value in the j −th row of Pi only reflects to the j −th diagonal value of Zi that should be ignored.For the same reason it does not make sense to down-weight outliers in the computation of Zi.

EXPERIMENTS
In this set of experiments, we tested (R)AGPA on repeated trials with simulated data.The aim was to assess i) its resistance to outliers, ii) the increase in speed w.r.t.(Fusiello and Crosilla, 2015) and iii) the accuracy and failure rate.The comparison with photogrammetric bundle adjustment would have been pointless here because AGPA minimizes the same cost function as (Fusiello and Crosilla, 2015), which in turn has been already compared to photogrammetric bundle adjustment for accuracy.
We followed the same validation protocol as in (Fusiello and Crosilla, 2015).For each trial, n 3-D tie-points have been randomly generated in a sphere of unit radius 1 centred on the origin.Sixteen random cameras (m = 16) looking toward the origin have been positioned in a 60 • sector of the sphere, at an average distance of d units from the origin.The focal length has been chosen so as to yield a view angle of {60 • , 120 • } with an image size of 1000 × 1000 pixels.Random noise with unit variance has been added to the image coordinates.Missing points have been simulated by zeroing random elements in the visibility matrix.
Outliers have been generated with uniform random coordinates in [−500, 500].A sample simulated scene is shown in Fig. 2.
The first experiment was designed for characterizing the failure rate, i.e., counting how many times our algorithm failed to reach the correct solution in 100 random trials, starting with the usual 1 In these experiments on simulated data, measures are in arbitrary "units".uninformed initialization (Z = 1).A random noise with 1 pixel standard deviation has been added to image coordinates.The parameters considered in this simulation are the distance d of the camera from the origin and the number p of tie-points visible in each image.
For each value of d = {2, 10, 20} 3-D tie-points have been stretched in the XY plane so as to fit in the view frustum, while the Z range is kept constant (to the original two units), so as to give rise to increasing values for the distance to Z-range ratio, namely {1, 5, 10} respectively.
The number of visible tie-points per image has been set to p = {18, 36, 54}.When the number of images and the number of visible tie-points per image are fixed, total number of points and the ray multiplicity become dependent.In one case the total number of points has been kept fixed to n = 96 and the ray multiplicity took the values {3, 6, 9}.In another case ray multiplicity has been fixed to 3 and the total number of points took values n = {96, 192, 288}.
Figure 3 reports the results, that are aligned with those published in (Fusiello and Crosilla, 2015).The digest is that when the ray multiplicity is greater than 3 (top row, p = {36, 54}) the convergence rate is 100%.The case of ray multiplicity= 3 is shown in the bottom row.
Figure 4 reports the median RMS error on 3-D points position for this experiment.The RMS error is in percentage with respect to the radius of the point cloud (i.e., 1% is 0.01 units on 3-D points in a sphere with radius 1).This error is always below 2% in the case of 120 • field of view (right column), and always below 1% in the 60 • case (left column).These results are in line with those reported in (Fusiello and Crosilla, 2015).[96,192,288], where the ray multiplicity is fixed to 3. Left/right column correspond to 60 • /120 • field of view, respectively.The error is in percentage with reference to points distributed on a unit sphere.Top row: Corresponding ray multiplicity values are [3,6,9], where n = 96.Bottom row: Corresponding total number of points are [96,192,288], where the ray multiplicity is fixed to 3. Left/right column correspond to 60 • /120 • field of view, respectively.
In summary, AGPA has the same convergence rate and accuracy than (Fusiello and Crosilla, 2015).
In the experiment aimed at assessing the resistance to outliers of RAGPA the parameters were set to: m = 16, n = 96, d = 10, p = 36.An increasing number of outliers has been added and 100 trials for each contamination level have been run.The results, reported in Fig. 5, indicates a breakdown point at 10%.In the experiment aimed at assessing the speed-up of AGPA (MAT-LAB implementation) with respect to (Fusiello and Crosilla, 2015) (original MATLAB implementation by the authors) the parameters were set to: m = 16, d = 10 and the ray multiplicity was set to 6.The number of points m has been increased from 32 to 144, and the average running time over 20 independent trials has been recorded.The results are reported in Fig. 6, which shows that AGPA is at least of one order of magnitude faster than (Fusiello and Crosilla, 2015).(Fusiello and Crosilla, 2015) and AGPA.
The tests on the same real datasets used in (Fusiello and Crosilla, 2015) confirm the outcome of the simulations.On the Herz-Jesu-P25 image-set (Strecha et al., 2008) the running time goes from 130s to 30s, whereas on the Hessigheim data (Cramer, 2013) it decreases from 460s to 100s at equal accuracy.

CONCLUSIONS
We have presented an improvement over a recent approach to bundle block adjustment based on anisotropic OPA (Fusiello and Crosilla, 2015).This new formulation solves the same minimization problem of its precursor but it is consistently faster.As the previous version, the method achieves the same accuracy of the classical photogrammetric bundle adjustment without any linearization of the collinearity equations and without any a priori information about the exterior orientation parameters.The robust cost function allows to tolerate up to 10% of outliers, by reducing their influence with a suitable weight function.
Future work will aim at characterizing the convergence of the method and possibly improving the breakdown point value.

APPENDICES A EXTENDED ORTHOGONAL PROCRUSTES ANALYSIS
The terms Procrustes Analysis is referred to a set of least squares mathematical methods used to compute transformations among corresponding points (or models) belonging to a generic k-dimensional space, in order to achieve their maximum agreement (e.g.(Gower and Dijksterhuis, 2004)).In particular, the Extended Orthogonal Procrustes Analysis (EOPA) allows to recover the least squares similarity transformation between two models.
Let us consider two matrices A and B containing two sets of numerical data, e.g., the coordinates of n points of R k by rows.EOPA allows to directly estimate the unknown rotation matrix R, a translation vector c and a global scale factor z that solves: The minimization proceeds by defining a Lagrangian function and setting the derivatives to zero (details can be found in (Schönemann and Carroll, 1970)).In the following we report only the results.
Algorithm 1 AGPA Input: a set of 3-D models Pi i = 1 . . .m Output: translation ci, attitude Ri and scale Zi of each model 1.Initialize Zi = I and P i = Pi ∀i 2. Compute centroid S = each model Pi to S: i − S) Mi(P i − S) .The Robust (or Resistant) AGPA is summarized in Algorithm 2. Algorithm 2 RAGPA Input: a set of 3-D models Pi, Mi i = 1 . . .m Output: translation ci, attitude Ri and scale Zi of each model 1.Initialize W = I and Zi = I, P i = Pi ∀i each model Pi to S: c) Iterate from step (a) until convergence of residual n j=1 rj 3. Compute weights W (j, j) = (|rj/k| ≤ 1)• 1 − (rj/k) 2 2

Figure 2 .
Figure 2. Top: A depiction of a simulated scene (cameras and points) used in the experiment (m = 16, n = 192, d = 10, p = 36); Bottom: the simulated visibility matrix, showing which point (x-axis) is visible in which image (y-axis).The outliers (10) are shown in red.

Figure 3 .
Figure 3. Failure rate of AGPA as a function of camera distance d and number of tie-points visible in each image.Top row: Corresponding ray multiplicity values are [3, 6, 9], where n = 96.Bottom row: Corresponding total number of points are[96, 192, 288], where the ray multiplicity is fixed to 3. Left/right column correspond to 60 • /120 • field of view, respectively.

Figure 4 .
Figure 4. Median RMS error of AGPA as a function of camera distance d and number of tie-points visible in each image p.The error is in percentage with reference to points distributed on a unit sphere.Top row: Corresponding ray multiplicity values are[3, 6, 9], where n = 96.Bottom row: Corresponding total number of points are[96, 192, 288], where the ray multiplicity is fixed to 3. Left/right column correspond to 60 • /120 • field of view, respectively.

Figure 5 .
Figure 5. Mean and median RMS error vs percentage of outliers (m = 16, n = 96, d = 10, p = 36).The RMS error is in percentage with reference to points distributed on a unit sphere.