708 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 2, APRIL 2019

Iteratively Reweighted Midpoint Method for FastMultiple View Triangulation

Kui Yang , Wei Fang, Yan Zhao, and Nianmao Deng

Abstract—The classic midpoint method for triangulation is ex-tremely fast, but usually labelled as inaccurate. We investigatethe cost function that the midpoint method tries to minimize,and the result shows that the midpoint method is prone to un-derestimate the accuracy of the measurement acquired relativelyfar from the three-dimensional (3-D) point. Accordingly, the costfunction used in this letter is enhanced by assigning a weight toeach measurement, which is inversely proportional to the distancebetween the 3-D point and the corresponding camera center. Afteranalyzing the gradient of the modified cost function, we propose todo minimization by applying fixed-point iterations to find the rootsof the gradient. Thus, the proposed method is called the iterativelyreweighted midpoint method. In addition, a theoretical study is pre-sented to reveal that the proposed method is an approximation tothe Newton’s method near the optimal point and, hence, inheritsthe quadratic convergence rate. Finally, the comparisons of the ex-perimental results on both synthetic and real datasets demonstratethat the proposed method is more efficient than the state-of-the-artwhile achieves the same level of accuracy.

Index Terms—Localization, SLAM, mapping, visual-basednavigation.

I. INTRODUCTION

TRIANGULATION is the task of estimating the 3D co-ordinate of a physical point, given its observations in

multiple camera views. And the task can be solved by findingthe intersection of multiple known rays originating from eachcamera. In the absence of noises, this problem is trivial andcan be dealt with many linear methods. In practice, however,those rays will not intersect in space due to various sources ofnoises. The goal becomes the search of the point that best fits themeasurements.

Triangulation is a fundamental building block for many ap-plications like Structure from Motion (SfM) and SimultaneousLocalization And Mapping (SLAM), on which there has been alarge number of literatures during the past few years. For thespecific case of two views, the most well known triangulationmethod may be the midpoint method [1]. Given two views of

Manuscript received September 10, 2018; accepted December 27, 2018. Dateof publication January 14, 2019; date of current version February 4, 2019. Thisletter was recommended for publication by Associate Editor J. Civera andEditor C. Stachniss upon evaluation of the reviewers’ comments. This letter wassupported by the National Natural Science Foundation of China under Grant61803035. (Corresponding author: Kui Yang.)

K. Yang, Y. Zhao, and N. Deng are with the School of Instrumentation Scienceand Opto-electronics Engineering, Beihang University, Beijing 100191, China(e-mail:, yang3kui@gmail.com; zhaoyanbh@126.com; dengnianmaobh@126.com).

W. Fang is with the School of Automation, Beijing University of Posts andTelecommunications, Beijing 100876, China (e-mail:,fangweichn@163.com).

Digital Object Identifier 10.1109/LRA.2019.2893022

the 3D point, the midpoint method chooses the midpoint of thecommon perpendicular to the two rays as the best estimation.Although the midpoint method is straightforward, it has beenknown to be suboptimal and may result in a bad estimation [1].The optimal closed form solutions for two-views triangulationhave already been achieved by polynomial root finding in [2],and latter accelerated in [3]. These methods solve the triangu-lation problem by explicitly computing the complete set of thestationary points of the cost function. They have been extendedto the case of three views in [4], where a polynomial of de-gree 47 must be solved. Nevertheless, they are not applicable tothe more general cases with more than three views since thedegree of the resulting polynomial grows quadratically with thenumber of views. Besides, there is another traditional methodfor solving multiple views triangulation called the direct lineartransform, which constructs a set of linear equations for eachobservation, and then solves the linear equations by applyingsingular value decomposition. Due to the ignorance of the errorminimized by the obtained solution, the results provided by thedirect linear transform may be far from the optimal when theinput is noisy [1].

Starting from an inaccurate initial point, the triangulation re-sult can be refined by minimizing the �2 norm, i.e., the meansquares, of the reprojection errors using gradient descent meth-ods [5]. Unfortunately, it is known that this cost function isnot convex, and the gradient descent methods may get stuck atsuboptimal local minima [6]. To overcome this drawback, re-searchers have proposed to utilize the �∞ norm, in other wordthe maximum, of the reprojection errors as the cost function[7]–[11]. The �∞ norm of the reprojection errors is quasiconvex andthus contains a single global minimum. Although these �∞ normmethods avoid the problem of local minima, their computationalcosts are considerable, since the convex programs have to bedone at each iteration to determine the update direction [11].In practice, as addressed in [6], it is scarce for �2 norm meth-ods to be trapped at local minima, while the �2 norm methodsare significantly less time-consuming than �∞ norm methods.Therefore, the �2 norm methods are better balanced betweenspeed and accuracy, and they are popularly used in recent state-of-the-art SLAM systems [12]–[15]. Nevertheless, the �2 normmethods are still too slow for large-scale reconstruction prob-lems or SLAM systems where there are a significant number ofobservations for each 3D point to be handled, and hence thereis room for further improvement.

Although the basic principles in triangulation have alreadybeen extensively studied, efficient and accurate methods forlarge-scale problem are strongly needed. In this letter, were-examine this problem and propose an extended midpointmethod for fast multiple view triangulation. The classic mid-point method is inaccurate in some cases because its linear

2377-3766 © 2019 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.See http://www.ieee.org/publications standards/publications/rights/index.html for more information.

YANG et al.: ITERATIVELY REWEIGHTED MIDPOINT METHOD FOR FAST MULTIPLE VIEW TRIANGULATION 709

cost function is inclined to underestimate the accuracies of themeasurements acquired relatively far from the 3D points. In or-der to get rid of the bias in different measurements, we proposeto assign each term a weight that decreases with the correspond-ing distance. Since the distances between the 3D point and thecameras are unknown before the location of the 3D point is de-termined, this new cost function is not linear any more. And forthis nonlinear cost function, the method of fixed point iterationsis applied to find the root of the gradient which is needed by min-imization. We thus name our method the Iteratively ReweightedMidPoint (IRMP) method. Further study reveals the connec-tion between the IRMP method and minimizing the proposedcost function by Newton’s method [5]. Experimental resultsvalidate that the proposed IRMP method can improve the accu-racy of the classic midpoint method. When compared with thestate-of-the-art, the IRMP method is several times faster withoutcompromising the accuracy.

In Section II, the theoretical aspect of the IRMP method willbe introduced in detail. In Section III, the experimental resultson synthetic and real data will be presented and discussed. Atlast, the conclusions will be drawn in Section IV.

II. ITERATIVE REWEIGHTED MIDPOINT METHOD

Throughout this letter, matrices, vectors, and scalars are de-noted by bold capital letters, bold lowercase letters and plainlowercase letters, respectively. Points in 3D space are rep-resented using column vectors. I represents the identity ma-trix, and (·)T denotes the transpose of a matrix. And all thequantities are expressed in a uniform world coordinate systemunless stated otherwise. Besides, we consider a calibrated cen-tric camera model, where an image point can be represented bya unit column vector originated from the corresponding cameracenter.

A. Multiple View Midpoint Method

Given a 3D point p and a set of its unit measurement vectors{bi} by cameras centered at {oi}, the triangulation problem isto find the best estimation of the 3D point p. In the absence ofnoises, we should have bi = (p − oi)/‖p − oi‖, where ‖ · ‖denotes the �2 norm, or equally speaking the length, of a vector.Meanwhile, the rays with directions {bi} and original points{oi} should intersect at p. However, with noises in the mea-surements, these back-projected rays do not exactly intersect inspace. In the specific case of two views, the midpoint of thecommon perpendicular to the two rays is chosen as the bestestimation. When extended to the more general cases of mul-tiple views [16], the midpoint method determines the optimalpoint by minimizing the sum of squares of the distances fromthis point to all the rays. In a mathematical way, the midpointmethod minimizes

e(p) =N∑

i=1

∥∥(I − bibTi ) (p − oi)

∥∥2. (1)

Here, bibTi is the orthogonal projection matrix from the point

p to the i-th ray. Let Bi = I − bibTi . Bi always has the

eigenvalues of {1, 1, 0} indicating its positive semidefinite at-tribute. Besides, there are two useful relations, i.e. BT

i = Bi

and BiTBi = Bi , which will be exploited in latter discussion.

Fig. 1. The geometrical relationship between different variables used in thisletter. oi is the center of the i-th camera. bi is the unit measurement vectorof the 3D point p in the i-th camera. ei is the error vector from p to themeasurement bi . Mathematically, ei = Bi (p − oi ) = (I − bibT

i )(p − oi ),and also ei⊥bi . θi is the error angle. Obviously, sin(θi ) = ‖ei‖/‖p − oi‖.

With Bi , the cost function can be rewritten as

e(p) =N∑

i=1

‖Bi (p − oi)‖2 . (2)

Although the formulation of the cost function used here isslightly different from the original one in [16], they are equiva-lent to each other in the sense of geometry. In [17], the authorsexplored the possibility of using �q (1 ≤ q < 2) norm to replacethe �2 norm in the cost function. In this letter, we concentrate onthe �2 norm formulation, since it is the optimal choice under theGaussian noise model. Now with {bi} and {oi} already known,the midpoint method regards solving a least square problem,which is equivalent to solving the linear equations

(N∑

i=1

Bi

)p =

(N∑

i=1

Bioi

), (3)

where∑N

i=1 Bi is a 3 × 3 matrix and∑N

i=1 Bioi is a 3 ×1 vector. Only if not all the observation vectors are parallel,∑N

i=1 Bi is full-rank and thus a unique solution can be obtainedwithout much effort. To summarize, the midpoint method isextremely efficient, since it only requires to construct and solvea set of 3 linear equations by one time.

B. Unbiased Cost Function

The midpoint method is fast by sacrificing the accuracy. Theerrors in midpoint method are described in a Euclidean waywhile the camera projection constitutes a projective space. Asshown in Fig. 1, when p moves further away from the cameracenter oi along the line oip, the error computed by (2), i.e.,‖Bi(p − oi)‖2 , grows larger. However, all the points lying onoip project to the same image point in the i-th camera, andthus the measure bi should have the same error for all the pointson oip. Therefore, the error formulation (2) is biased, and isprone to overestimate the error level for 3D points relatively farfrom the camera center. As a result, the naive midpoint methodbecomes inaccurate when the distances from the 3D point todifferent cameras vary considerably.

With this observation in mind, we propose to add a weight,which is inversely proportional to the distance between the 3Dpoint and the camera center, for each term in (2) to rebalancethe errors. The cost function is thus reformed as,

e (p) =N∑

i=1

‖wi(p)Bi (p − oi)‖2 , (4)

with

wi(p) = 1/‖p − oi‖.

710 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 2, APRIL 2019

For convenience, we denote the cost term with respect to thei-th camera as

ei (p) = ‖wi (p) ei (p)‖2 =‖Bi (p − oi)‖2

‖p − oi‖2 . (5)

The geometric relationship between different variables used inthis letter is illustrated in Fig. 1.

The weight term wi has a twofold influence on the cost func-tion. On one hand, as shown in Fig. 1, we have sin(θi) =‖Bi(p − oi)‖/‖p − oi‖, and consequently the cost functionrepresents a summation of squares of the sine values of differ-ent error angles θi . In this way, the cost function is unbiasedfor all the points lying on the same projection line. Moreover,when the error is small, the difference between sin(θi) and θi

is insignificant, so the proposed cost function provides a veryclose approximation of the exact angular error. On the otherhand, however, minimizing the cost function (4) is no longer alinear least square problem, and hence adds complexity to theprocedure of solution.

It should be noticed that the cost function formulated byequation (4) does not guarantee the solution to meet the chiral-ity rules. In other word, the solution may locate behind someof the observing cameras. However, this only happens whenthe initial guess falls behind the camera or the observing cam-eras are singularly configured, which is rarely happened in realapplications and can be easily handled by preprocessing.

C. Fixed Point Iterations

In order to minimize (4), its gradient is derived in closed formas

∇e(p) = 2N∑

i=1

(w2

i (p)(p − oi)T (Bi − ei(p)I)

). (6)

Let p∗ be the optimal solution, by writing explicitly the opti-mality condition ∇e(p∗) = 0, we have

(N∑

i=1

Ai(p∗)

)p∗ =

N∑

i=1

(Ai(p∗)oi + ri(p∗)), (7)

with Ai(p) = w2i (p)Bi and ri(p) = w2

i (p)ei(p) (p − oi).Equation (7) can be solved by fixed point iterations as the

following algorithm. In the first step, an initial guess of thesolution p0 can be obtained by solving (3). Then at each step,the estimation of the optimal point pn is updated by solving

(N∑

i=1

Ai(pn−1)

)pn =

N∑

i=1

(Ai(pn−1)oi + ri(pn−1)). (8)

Since pn−1 is already known, (8) is a set of linear equations sim-ilar to (3) and the solution is trivial. This step is repeated until‖pn − pn−1‖ is smaller than a threshold. Then the convergedpn is a root of (7), and thus is the solution of the triangula-tion problem only if the starting point p0 is close enough tothe global optimal. At present, we assume that this algorithmalways converges, and will present a detailed discussion on theconvergence property later. The proposed algorithm is called theIteratively Reweighted MidPoint (IRMP) method.

It should be noticed that minimizing (4) is a typical nonlinearleast square problem, and thus can be solved using traditionalgradient descent methods, such as the Gauss-Newton method,the Levenberg–Marquardt method or the Dogleg method [5].

When compared with them, the proposed algorithm consumesless computational resources at each iteration. This is becausethe proposed algorithm only updates the weight wi(p) as wellas an additional term ri(p) at each iteration (both Bi and Bioi

remain unchanged during the iterations and thus can be precom-puted to reduce runtime), while traditional methods require therecomputation of the jacobian and the hessian matrix or even de-mand to solve an additional optimization problem to determinethe update direction or the step length.

D. Approximation to Newton’s Method

The proposed algorithm is actually an approximation to theclassic Newton’s method at the neighborhood of the optimalsolution. To make it clear, (6) and (8) are combined to produce

(2

N∑

i=1

Ai(pn−1)

)(pn − pn−1) = −∇e(pn−1)T . (9)

Meanwhile, when minimizing (4) using Newton’s method, theiteration direction is determined by

∇2e(pn−1) (pn − pn−1) = −∇e(pn−1)T . (10)

Here ∇2e(pn−1) is the hessian matrix of e(pn−1), which canbe derived in closed form as

∇2e(p) =N∑

i=1

(2Ai(p) + Di(p) + Ei(p)), (11)

where

Di(p) = −4 (Ai(p)Ci(p) + Ci(p)Ai(p)) , (12)

Ei(p) = 2ei(p)w2i (p) (4Ci(p) − I) , (13)

and

Ci(p) =(p − oi) (p − oi)

T

‖p − oi‖2 . (14)

Whenpmoves toward the global optimal, both sin(θi) and ei(p)approach to zero. Consequently, Ei(p) is negligible comparedto Ai(p) and can be dropped in (11) at the neighborhood of theglobal optimal. Another important observation is that Di(p)is also constituted by negligible tiny values near the globaloptimal, which can be clarified by investigating the norm ofAi(p)Ci(p), namely,

‖Ai(p)Ci(p)‖ =

∥∥∥Ai(p) (p − o) (p − o)T∥∥∥

‖p − o‖2 (15)

≤ ‖Ai(p) (p − o)‖‖p − o‖

‖p − o‖‖p − o‖ (16)

= w2i (p) sin (θi) → 0. (17)

The same result also holds for Ci(p)Ai(p), and as a conse-quence ‖Di(p)‖ is close to zero. Now, we can conclude that

∇2e(p) ≈ 2n∑

i=1

Ai(p) (18)

near the optimal point. Recalling (9) and (10), it indicates thatthe proposed IRMP method is an approximation to minimizethe cost function (4) using Newton’s method when the startingpoint is close enough to the global optimal.

YANG et al.: ITERATIVELY REWEIGHTED MIDPOINT METHOD FOR FAST MULTIPLE VIEW TRIANGULATION 711

Fig. 2. Several 2D toy examples on the convergence property of the IRMP method. The black curve is the contour of (4) near its global minimum. The greencircle is the solution of the classic midpoint method, which is the global minimun of (2) and usually at the neighborhood of the global minimum of (4). The bluesquares represent the trace of the IRMP method while the red crosses represent the trace of minimizing (4) using naive Newton’s method.

E. Convergency

Recalling Ai(p) = w2i (p)Bi and the attributes of Bi dis-

cussed before, Ai is symmetric and its eigenvalues are{w2

i (p), w2i (p), 0

}, while the eigenvector corresponding to the

eigenvalue 0 is bi .∑N

i=1 Ai(pn ) would most likely to be apositive definite matrix because of the dominant places of thepositive eigenvalues in each Ai , with the only exception thatall the measurements bi are parallel (suggesting the triangula-tion problem itself is ill-conditioned). Therefore, as long as thetriangulation problem is well defined, (9) produces a descent di-rection and the proposed IRMP method can at least reach a localminimum. In practice, the ratio between the minimum and themaximum of the eigenvalues of

∑Ni=1 Ai(pn ) can be utilized

as a criterion to determine whether the triangulation problem isill-conditioned.

When p is out of the scope of the global mininum, the contri-butions of Di(p) and Ei(p) may bring negative eigenvalues tothe hessian matrix (11), and therefore the cost function used inthe IRMP method may not be convex. When the starting pointis far from the global minimum, the iteration characterized by(8) may get suck at local minima. As addressed in [6], this is acommon drawback of using the �2 norm based cost functions inmultiple view geometry.

Fortunately, the IRMP method starts from the solution of theclassic midpoint method, which is the unique global minimumof (2) and should be close to the global minimum of (4) whenthe triangulation problem is well defined. As we have discussedbefore, near the global minimum, the proposed IRMP methodis an approximation of the Newton’s method, and thus inheritsthe quadratic convergence rate as the Newton’s method has. Atthe meantime, the IRMP method requires less computation ateach iteration, and thus is faster than using the naive Newton’smethod to minimize (4). Several 2D toy examples are presentedin Fig. 2, which validate the analysis above.

III. EXPERIMENTS

In this section, experimental results on both synthetic andreal data are presented to validate the efficiency and the ac-curacy of the proposed IRMP method. The proposed IRMPmethod is compared with three benchmark algorithms. The firstone is the naive Multiple View Middle Point (MVMP) method.To the best of our knowledge, though usually labelled as inac-curate, the MVMP method is currently the fastest method formultiple view triangulation. Besides, as the proposed IRMPmethod is derived from the MVMP method, it would be inter-esting to compare their behaviors under different conditions.

The second algorithm chosen as a benchmark is minimizingthe cost function (4) using the NN (Naive Newton’s) method.As illustrated in the previous section, the IRMP method canbe viewed as an approximation of the NN method by droppingsome insignificant terms in the hessian matrix. In this section,some experimental results will be presented to show the influ-ence of the approximation on the accuracy and efficiency ofthe triangulation results. The third benchmark algorithm is thegradient-based minimization of the �2 norm based reprojectionerrors, which is abbreviated as the GMRE (Gradient Minimiza-tion of the Reprojection Errors) method. This method is wellbalanced between the efficiency and the accuracy and thus pop-ularly used in recent state-of-the-art SLAM systems, including[12]–[15]. The result from the MVMP method is used to ini-tialize the other three methods. The reprojection error, so-calledthe golden standard [1], is used as a criterion to compare theaccuracies of different algorithms. The same camera intrinsicsare used in all the synthetic experiments, with an image sizeof 1024 × 1024 pixels and a focus length of 400 pixels. In theGMRE implementation, the reprojection error is minimized us-ing Gauss-Newton iterations. All the experiments were doneon an Intel 3.70 GHz i7-8700 K CPU with 16 GB memory. Inthe first three experiments, we implemented all the algorithmsby ourselves using MATLAB on a single thread without anyspecial optimization. In the last experiment, we compared theC++ implementation of the IRMP method with the C++ imple-mentation of the GRME method in SVO [12]. The process oftriangulation using the GRME method is traditionally called thestructure-only bundle adjustment in SVO.

A. Monte Carlo Simulation

We set up a Monte Carlo simulation to validate the accuracyand the efficiency of the IRMP method. In the simulation, theobserved 3D point was fixed at (0, 0, 0), and 100 cameras werelocated at random aspects to observe the 3D point. The dis-tances between the 3D point and the cameras were randomlygenerated with the continuous uniform distribution in the rangeof [ds , dm]. The ratio γ = dm/ds was used to describe the dis-persion of the distances between the 3D point and the cameras.The space point was then projected on the camera planes, andthe results were corrupted using Guassian noise with a 10-pixelscovariance. After that, the 3D point was reconstructed using thecorrupted measurements by all the four triangulation methods,respectively. At last, the triangulation results were projectedback to the image planes to compute the average reprojectionerrors. In this experiment, γ is altered from 1 to 100 to investi-gate its influence on the details of triangulation. For each γ, the

712 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 2, APRIL 2019

Fig. 3. The Monte Carlo simulation on the accuracy and the efficiency of the IRMP method.

Fig. 4. The synthetic data configurations. The 3D points are drawn as blue dots. Their bounding boxes are drawn as black cubes. The cameras has red x axis,green y axis as well as blue z axis.

simulation was repeated for 10,000 times and the average resultwas reported.

As shown in Fig. 3, when γ = 1, all the four methods pro-duced similar results. However, the accuracy of the MVMPmethod degrades significantly when γ increases, which vali-dates the theoretical analysis presented in the previous section.Moreover, the experimental results reveal an almost linear re-lationship between γ and the reprojection errors of the MVMPmethod. In the presented simulation, the IRMP method canalways refine the accuracy to an acceptable level. And the dif-ferences between the accuracies of the IRMP method, the NNmethod, and the GMRE method are insignificant. Besides, Theiterations of the IRMP method needed for convergence increaseswhen γ grows larger. The average iterations the IRMP method,the NN method, and the GRME method take before converge arealmost identical while the IRMP method has higher efficiencybecause it takes less computation at each iteration.

If we continue to increase γ in the simulation, the MVMPmethod may give extremely bad initializations, starting fromwhich the rest three iterative methods may diverge. A two-phaseinitialization is suggested to handle these failure cases. Afterthe first initialization p0 was computed by the MVMP method,γ can be computed using p0 . If γ > 100 , those observationslocated relatively in large distance will be eliminated to makeγ ≤ 100. And then a second initialization p1 is computed usingthe rest observations by the MVMP method. After that all theobservations are used to compute the final solution by the IRMPmethod starting from p1 . We found that γ > 100 rarely happensin real applications.

B. Evaluation in Synthetic Scenarios

We synthesized four different scenarios:� A: The camera moves along a curve trajectory towards

the 3D points, as shown in Fig. 4(a). This simulates the

TABLE ITHE COMPARISONS OF TOTAL RUNTIME IN SECONDS USING SYNTHETIC DATA

scenario where the robot moves toward the object it ob-serves or the flying robot lands [18].

� B: The camera moves along a curve trajectory through the3D points, as shown in Fig. 4(b). This simulates that therobot freely explores a space with randomly distributedlandmarks, which is the target of most SLAM systems[19].

� C: The camera moves on a circle around the 3D points,as shown in Fig. 4(c). This simulates 3D modelling with arotating platform [20].

� D: Both the cameras and the 3D points are randomly dis-tributed, as shown in Fig. 4(d). This simulates large-scale3D reconstruction with the images from various sources[21].

As shown in Fig. 4, 5000 uniformly distributed random 3Dpoints were generated within the black box and projected back to100 cameras. Then those projections within the correspondingcamera’s scope and passed the chirality check were corrupted us-ing Guassian noise with a 10-pixels covariance. After that, those3D points with more than 2 valid projections were triangulatedfor four times by MVMP, IRMP, NN, GMRE, respectively. Atlast, the triangulation results were projected back to the imageplanes again to compute the average reprojection errors.

Table (I) shows the comparisons of total runtime used to trian-gulate the 3D points by different algorithms. It is not surprising

YANG et al.: ITERATIVELY REWEIGHTED MIDPOINT METHOD FOR FAST MULTIPLE VIEW TRIANGULATION 713

Fig. 5. The comparisons of reprojection errors in pixels in synthetic scenarios.

TABLE IITHE COMPARISONS OF RESULTS ON LARGE SCALE REAL DATASETS. MER IS ABBREVIATED FOR THE MAX ERROR REDUCTION AGAINST

THE MVMP METHOD FOR A SPECIFIC INSTANCE

Full names of the Datasets [22], [23]: A → Aos Hus ; B → Lund Cathedral; C → San Marco; D → Orebro Castle; E → Park gate, Clermont-Ferrand; F → Buddah Statue; G →Skansen Lejonet, Gothenburg;

that the MVMP method is always the fastest algorithm in allfour configurations. The proposed IRMP method is about 2 to3 times slower than the MVMP method, but is still about 2 to 3times faster than the GMRE method and the NN method. Fig. 5shows the comparisons of the reprojection errors. It can be seenthat the proposed IRMP method is almost as accurate as theGMRE method and the NN method. The MVMP method, how-ever, behaves much worse in A, B and D. As we have discussedbefore, the MVMP method is inaccurate because it has a biasedcost function. In A, B and D, some of the 3D points are observedby cameras from tremendously different distances, thus are notcorrectly triangulated by the MVMP method. In contrast, theIRMP method can refine those 3D points with bad initiations toan acceptable accuracy level. Another interesting phenomenonis that the MVMP method is as accurate as the other three meth-ods in C. This is because the distances between a 3D point andall the observing cameras have smaller differences in C, alle-viating the bias of the cost function (2). In the most extremecase where a 3D point has the same distances to all the camerasobserving it, the MVMP method is equivalent to the proposedIRMP method, and hence has the same accuracy.

C. Evaluation in Real Scenarios

The algorithms were tested on publicly available datasets[22]–[25] for large scale 3D reconstruction. The camera ma-trixes and the feature correspondences supplied with thesedatasets were used in the experiments. As demonstrated inTable II, the MVMP method produces the fastest as well as

the most inaccurate results on all the datasets. The proposedIRMP method brings about 0.01 average pixel error reductionagainst the MVMP method, which is insignificant at the firstsight. But if we take a close look at every specific point inthe datasets, the accuracy improvement brought by the IRMPcan be as large as 6.312 pixels (see columns with the headerMER in Table II, which record the max error reductions againstthe MVMP method). When compared with the state-of-the-artGMRE methods, the proposed IRMP method is about 2-3 timesfaster while achieves fairly close accuracy. Some of the triangu-lation results by the IRMP method are demonstrated in Fig. 6.

D. Application to SLAM

We have integrated the C++ implementation of the proposedIRMP method1 into our own SLAM system, which consists ofa stereo SVO based front-end [12] and an ORB-SLAM based[13] loop closure mechanism. In the original SVO implementa-tion, the procedure of multiple view triangulation is done by theGMRE method. In this experiment, the IRMP method was usedto substitute the GMRE method. The SLAM systems were eval-uated on public available dataset KITTI [26], and the results areillustrated on Fig. 7. All the accuracy comparison results weregenerated using the toolbox published along with a recent tu-torial on quantitative trajectory evaluation for visual odometry[27]. As illustrated in Fig. 7, the accuracies of the IRMP-basedSLAM and the GMRE-based SLAM are very close. Meanwhile,

1https://github.com/yang3kui/triangulation_IRMP

714 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 2, APRIL 2019

Fig. 6. The images and the triangulation results of the proposed IRMP method. The images on the top row are from the corresponding datasets and the bluepoints clouds on the bottom row are the triangulation results of the IRMP method.

Fig. 7. Comparison of SLAM results on KITTI [26].

the IRMP-based SLAM took about 0.533 millisecond on aver-age to compute multiple view triangulation for each frame whilethe GMRE method took 1.217 milliseconds.

IV. CONCLUSION

In this letter, we clarified that the classic midpoint method isinaccurate due to its biased cost function. Therefore, in order toimprove the accuracy, we proposed to assign a weight to eachmeasurement to rebalance the cost function. The proposed unbi-ased cost function can be minimized using fixed point iterationsat a quadratic convergence rate near the optimal point. Besides,at each iteration, the proposed method consumes less computa-tional resources than traditional gradient descent methods, andthus is faster than the state-of-the-art. Experimental results onsynthetic and real data are also presented, which validate that theproposed method improves the accuracy when compared withthe classic midpoint method and is more efficient than the state-of-the-art while achieves comparable accuracy. Therefore, theproposed method can be integrated to accelerate future SLAMor SfM systems in practice.

Some of the basic principles developed in this letter are alsoapplicable to other multiple view geometry problems, such asthe perspective-n-point problem and the camera resectioningproblem. We will study them in future work.

APPENDIX AGRADIENT AND HESSIAN MATRIX.

The gradient and the hessian matrix of the proposed costfunction, namely (6) and (11), play important roles in theIRMP method. However, their derivations are cumbersome.We thus put them in the appendices. At the beginning, wewould like to introduce two formulations that will be repeat-edly used in the derivations. The first one concerns the na-ture of the matrix Bi namely, Bi = BT

i = BTi Bi . The second

one is about the gradient of ‖q − o‖n , which can be writ-ten as, ∇(‖q − oi‖n ) = n‖q − oi‖n−2(q − oi)T . With thesetwo formulations, the gradient and the hessian matrix of theproposed cost function (4) can be derived in closed form us-ing matrix calculus. Recalling (5), the gradient of ei(p) is

YANG et al.: ITERATIVELY REWEIGHTED MIDPOINT METHOD FOR FAST MULTIPLE VIEW TRIANGULATION 715

computed as

∇ei(p) =1

‖p − oi‖2d‖Bi(p − oi)‖2

dp

+d

(‖p − oi‖−2

)

dp‖Bi(p − oi)‖2

=2(p − oi)

TBi

‖p − oi‖2 − 2(p − oi)

T

‖p − oi‖4 ‖Bi(p − oi)‖2

= 21

‖p − oi‖2 (p − oi)T

(Bi − ‖Bi(p − oi)‖2

‖p − oi‖2 I

)

= 2w2i (p)(p − oi)T (Bi − ei(p)I) .

By summing up the contributions of different ∇ei(p), we reach(6).

The derivation of the hessian matrix is shown as following.

∇2ei(p)2

=d(‖p − oi‖−2

)T

dp(p − oi)T (Bi − ei(p)I)

+1

‖p − oi‖2

d((p − oi)

T (Bi − ei(p)I))

dp

= − 2(p − oi)(p − oi)

T

‖p − oi‖4 (Bi − ei(p)I)

+1

‖p−oi‖2

(−∇ei(p)T(p−oi)

T +Bi−ei(p)I)

= w2i (p) (Bi − ei(p)I)

− 2w2i (p) (Bi − ei(p)I)Ci(p)

− 2w2i (p)Ci(p) (Bi − ei(p)I) ,

with

Ci(p) =(p − oi) (p − oi)

T

‖p − oi‖2 .

Now recalling Ai(p) = w2i (p)Bi , by setting

Di(p) = −4 (Ai(p)Ci(p) + Ci(p)Ai(p))

and

Ei(p) = 2ei(p)w2i (p) (4Ci(p) − I) ,

we have

∇2ei(p) = 2Ai(p) + Di(p) + Ei(p).

After summing ∇2ei(p) up, we reach (11).

ACKNOWLEDGMENT

The authors would like to thank Dr. Z. Li from BeihangUniversity and Dr. Z. Zhang from University of Zurich, for theirhelps to improve the quality of this letter.

REFERENCES

[1] R. I. Hartley and A. Zisserman, Multiple View Geometry in ComputerVision, 2nd ed. Cambridge, U.K.: Cambridge Univ. Press, 2004.

[2] R. I. Hartley and P. Sturm, “Triangulation,” Comput. Vis. Image Under-standing, vol. 68, no. 2, pp. 146–157, Nov. 1997.

[3] P. Lindstrom, “Triangulation made easy,” in Proc. IEEE Conf. Comput.Vis. Pattern Recognit., 2010, pp. 1554–1561.

[4] H. Stewenius, F. Schaffalitzky, and D. Nister, “How hard is 3-view trian-gulation really?” in Proc. 10th IEEE Int. Conf. Comput. Vis., Oct. 2005,vol. 1, pp. 686–693.

[5] J. Nocedal and S. J. Wright, Numerical Optimization, 2nd ed. New York,NY, USA: Springer, 2006.

[6] R. Hartley, F. Kahl, C. Olsson, and Y. Seo, “Verifying global minima for l2minimization problems in multiple view geometry,” Int. J. Comput. Vis.,vol. 101, no. 2, pp. 288–304, Jan. 2013.

[7] R. I. Hartley and F. Schaffalitzky, “L-infinity minimization in geomet-ric reconstruction problems,” in Proc. IEEE Conf. Comput. Vis. PatternRecognit., 2004, pp. 504–509.

[8] F. Kahl and R. Hartley, “Multiple-view geometry under the Linf ty-norm,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 30, no. 9, pp. 1603–1617, Sep. 2008.

[9] A. Eriksson and M. Isaksson, “Pseudoconvex proximal splitting for L-infinity problems in multiview geometry,” in Proc. IEEE Conf. Comput.Vis. Pattern Recognit., Jun. 2014, pp. 4066–4073.

[10] Q. Zhang, T. J. Chin, and D. Suter, “Quasiconvex plane sweep for trian-gulation with outliers,” in Proc. IEEE Int. Conf. Comput. Vis., Oct. 2017,pp. 920–928.

[11] Q. Zhang and T. J. Chin, “Coresets for triangulation,” IEEE Trans. PatternAnal. Mach. Intell., vol. 40, no. 9, pp. 2095–2108, Sep. 2018.

[12] C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO: Fast semi-directmonocular visual odometry,” in Proc. IEEE Int. Conf. Robot. Automat.,May 2014, pp. 15–22.

[13] R. Mur-Artal and J. D. Tardos, “ORB-SLAM2: An open-sourceSLAM system for monocular, stereo and RGB-D cameras,” in IEEETrans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017, doi:10.1109/TRO.2017.2705103.

[14] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale,“Keyframe-based visual–inertial odometry using nonlinear optimization,”Int. J. Robot. Res., vol. 34, no. 3, pp. 314–334, 2015.

[15] K. Sun et al., “Robust stereo visual inertial odometry for fast autonomousflight,” in IEEE Robot. Autom. Letters, vol. 3, no. 2, pp. 965–972, Apr.2018, doi: 10.1109/LRA.2018.2793349.

[16] S. Ramalingam, S. K. Lodha, and P. Sturm, “A generic structure-from-motion framework,” Comput. Vis. Image Understanding, vol. 103, no. 3,pp. 218–228, Sep. 2006.

[17] K. Aftab, R. Hartley, and J. Trumpf, “LQ-closest-point to affine subspacesusing the generalized weiszfeld algorithm,” Int. J. Comput. Vis., vol. 114,no. 1, pp. 1–15, Aug. 2015.

[18] O. Araar, N. Aouf, and I. Vitanov, “Vision based autonomouslanding of multirotor UAV on moving platform,” J. Intell.Robot. Syst., vol. 85, pp. 369–384, 2017. [Online]. Available:https://link.springer.com/article/10.1007%2Fs10846-016-0399-z

[19] C. Cadena et al., “Past, present, and future of simultaneous localizationand mapping: Toward the robust-perception age,” IEEE Trans. Robot.,vol. 32, no. 6, pp. 1309–1332, Dec. 2016.

[20] “3d scanners.” Accessed: Sep. 3, 2018. Online. Available: http://3dprintingsystems.com/products/3d-scanners/

[21] S. Agarwal et al., “Building rome in a day,” Commun. ACM, vol. 54,no. 10, pp. 105–112, Oct. 2011.

[22] O. Enqvist, F. Kahl, and C. Olsson, “Non-sequential structure from mo-tion,” in Proc. IEEE Int. Conf. Comput. Vis. Workshops, Nov. 2011,pp. 264–271.

[23] C. Olsson and O. Enqvist, “Stable structure from motion for unorderedimage collections,” in Image Analysis, A. Heyden and F. Kahl, Eds. Berlin,Germany: Springer, 2011, pp. 524–535.

[24] “Oxford visual geometry group. multi-view and oxford colleges buildingreconstruction.” Accessed: Sep. 3, 2018. [Online]. Available: http://www.robots.ox.ac.uk/vgg/data/data-mview.html

[25] N. Snavely, S. M. Seitz, and R. Szeliski, “Photo tourism: Exploring photocollections in 3d,” ACM Trans. Graph., vol. 25, no. 3, pp. 835–846, 2006.

[26] A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving?The KITTI vision benchmark suite,” in Proc. Conf. Comput. Vis. PatternRecognit., 2012, pp. 3354–3361.

[27] Z. Zhang and D. Scaramuzza, “A tutorial on quantitative trajectory eval-uation for visual(-inertial) odometry,” in Proc. IEEE/RSJ Int. Conf. Intell.Robot. Syst., 2018, pp. 7244–7251.