Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Developer: Mikiya Shibuya, Kai Okawa Supporter: Open Source AI Association 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) What is BundleFit? Bundle Fitting Library for 3D Reconstruction • Functions • Bundel adjustment and pose graph optimization using Ceres Solver • Ceres Solver: C++ Non-linear optimization OSS library developed by Google • Provide analytic derivatives for reprojection errors in common image space • Faster than automatic differentiation • Support 6 and 7-DOF pose graph optimizations • If there is a scale change (e.g. monocular Visual SLAM), 7-DOF optimizations are required. • Compatible with multi-camera systems • Compatible with multiple cameras fixed to the rig • Features • C++17 • Python wrapper • Tutorial and sample codes 2 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Table of contents • Preliminary knowledge • Coordinate transformations and camera projection models • Rotation matrix and rotation vector • Schur complement • Bundle adjustment • Loop closing and pose graph optimization • Source code explanation 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Preliminary: Coordinate transformations and camera projection models 4 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) , ワールド R , Coordinate transformations and projection models Project 3D points onto an image plane • World coordinate system→ camera coordinate system→ image coordinate system • Coordinate system • World. : Common for all cameras • Camera: Fixed to any camera • Image. : Fixed to any image plane • Intrinsic parameters • Image to camera coordinate system conversion • Extrinsic parameters • Camera to world coordinate system conversion 5 Relationship between camera and image coordinate systems Relationship between world and camera coordinate systems World coordinate system Camera coordinate system R𝑤𝑐 , 𝐭𝑤𝑐 R𝑐𝑤 , 𝐭𝑐𝑤 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) , ワールド R , Coordinate transformations and projection models Camera records a scene by projecting light reflected off objects in space onto the image sensor • Represented by the intrinsic camera parameter K • The simplest perspective model 𝑓𝑢 , 𝑓𝑣 ：Focal length 𝑐𝑢 , 𝑐𝑣 ：Optical Center 𝐗𝑐 = 𝑋𝑐 , 𝑌𝑐 , 𝑍𝑐 T：3D points in camera coordinate system 𝐳𝑐 = 𝑢, 𝑣 T ：2D points in image coordinate system K′ ≔ 𝑓𝑢 0 𝑐𝑢 0 𝑓𝑣 𝑐𝑣 K ≔ 𝑓𝑢 0 𝑐𝑢 0 𝑓𝑣 𝑐𝑣 0 0 1 𝐳 = 1 𝑍𝑐 K′𝐗𝑐 Relationship between camera and image coordinate systems Relationship between world and camera coordinate systems World coordinate system Camera coordinate system R𝑤𝑐 , 𝐭𝑤𝑐 R𝑐𝑤 , 𝐭𝑐𝑤 6 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Coordinate transformations and projection models If the camera’s coordinate system is different from the world’s, convert from world to camera coordinates. • Substitute 𝐗𝑐 = R𝑐𝑤 𝐭𝑐𝑤 ] ഥ 𝐗𝑤 to the projection model on the previous page 𝐳 = 1 𝑍𝑐 K′ R𝑐𝑤 𝐭𝑐𝑤 ] ഥ 𝐗𝑤 𝑢 𝑣 = 1 𝑍𝑐 𝑓𝑢 0 𝑐𝑢 0 𝑓𝑣 𝑐𝑣 𝑟𝑐𝑤 11 𝑟𝑐𝑤 12 𝑟𝑐𝑤 13 𝑡𝑐𝑤 1 𝑟𝑐𝑤 21 𝑟𝑐𝑤 22 𝑟𝑐𝑤 23 𝑡𝑐𝑤 2 𝑟𝑐𝑤 31 𝑟𝑐𝑤 32 𝑟𝑐𝑤 33 𝑡𝑐𝑤 3 𝑋𝑤 𝑌𝑤 𝑍𝑤 1 𝐗𝑐 = 𝑋𝑐 , 𝑌𝑐 , 𝑍𝑐 T ：3D points in camera coordinate system 𝐗𝑤 = 𝑋𝑤 , 𝑌𝑤 , 𝑍𝑤 T ：3D points in world coordinate system R𝑐𝑤 , 𝐭𝑐𝑤 ：Rotation matrix, Translation vector ഥ 𝐗𝑤 = 𝑋𝑤 , 𝑌𝑤 , 𝑍𝑤 , 1 T ：Homogeneous coordinates of 𝐗𝑤 , ワールド R , Relationship between camera and image coordinate systems Relationship between world and camera coordinate systems World coordinate system Camera coordinate system R𝑤𝑐 , 𝐭𝑤𝑐 R𝑐𝑤 , 𝐭𝑐𝑤 7 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Preliminary: Rotation matrix and rotation vector 8 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Rotation matrix and rotation vector The rigid body transformation T (6-DOF) and similarity transformation S (7-DOF) are expressed as follows using rotation matrix R ∈ SO 3 , translation vector 𝐭 ∈ ℝ3, scale parameter 𝑠, respectively SO 3 ： Special Orthogonal group of order 3 ⇒ 3D rotation SE(3) ： Special Euclidean group of order 3 ⇒ 3D rotation + translation Sim(3)： Similarity transformation group of order 3 ⇒ 3D rotation + translation + scale Lie algebra corresponding to the above Lie group 𝔰𝔬 3 ， 𝔰𝔢 3 ， 𝔰𝔦𝔪 3 The matrix representation R (3×3) is not suitable as an optimization variable • Expressed by 9 parameters despite the 3 degrees of freedom • Needs to also satisfy the conditions of the rotation matrix (orthogonality and determinant 1) 9 H. Strasdat et al., “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Rotation matrix and rotation vector R is represented by the rotation vector 𝝎 = 𝜔1 , 𝜔2 , 𝜔3 T ∈ 𝔰𝔬 3 • Exponential map from 𝔰𝔬 3 to SO 3 exp ∶ 𝔰𝔬 3 → SO 3 𝝎 → R • 𝜃 = 𝝎 2 : Rotation angle of 𝝎 • ∙ 2 is L2 norm (Euclidean distance) • This mapping relation is expressed by the following equation through Rodrigues’ rotation formula I𝑛 is the 𝑛 × 𝑛 identity matrix I𝑛 , and ∙ × is the operator for the skew-symmetric matrix • The logarithmic map from SO 3 to 𝔰𝔬 3 is denoted as log ∶ SO 3 → 𝔰𝔬 3 R → 𝝎 H. Strasdat et al., “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Rotation matrix and rotation vector • Mapping from the camera pose 𝐩 = 𝝎T, 𝒗T T ∈ 𝔰𝔢 3 including the translation 𝐯 to SE(3) • Mapping from the camera pose 𝐬 = 𝝎T, 𝜎, 𝒗T T ∈ 𝔰𝔦𝔪 3 including scale 𝜎 to Sim(3) H. Strasdat et al., “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Preliminary: Schur complement 12 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Define the block matrix M 𝑝+𝑞 × 𝑝+𝑞 using the matrices A𝑝×𝑝 , B𝑝×𝑞 , C𝑞×𝑝 , and D𝑞×𝑞 • When D is nonsingular, the Schur complement of the partitioned matrix with respect to the block D is M/D𝑝×𝑝 • When A is nonsingular, the Schur complement of the partitioned matrix with respect to the block A is M/A𝑞×𝑞 Schur complement M/D ≡ A − BD−1C M−1 = M/D −1 − M/D −1BD−1 −D−1C M/D −1 D−1 + D−1C M/D −1BD−1 M ≡ A B C D M/A ≡ D − CA−1B M−1 = A−1 + A−1B M/𝐴 −1CA−1 −A−1B M/𝐴 −1 − M/𝐴 −1CA−1 M/𝐴 −1 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) M ≡ A B C D = ഥ A B Ο D I𝑝 Ο D−1C I𝑞 = I𝑝 BD−1 Ο I𝑞 ഥ 𝐀 Ο Ο 𝐃 I𝑝 Ο D−1C I𝑞 Let ഥ A be the Schur complement of the partitioned matrix M with respect to the block D, denoted as ഥ A ≡ M/D𝑝×𝑝 ≡ A − BD−1C Decompose M into a matrix product containing a block diagonal matrix using ഥ A XYZ −1 = Z−1Y−1X−1, so if ഥ A and D are nonsingular (invertible) Inverse matrix using Schur complement (for M/D𝑝×𝑝 ) A B C D −1 = I𝑝 Ο −D−1C I𝑞 ഥ 𝐀−𝟏 Ο Ο 𝐃−𝟏 I𝑝 −BD−1 Ο I𝑞 = ഥ A−1 Ο −D−1Cഥ A−1 D−1 I𝑝 −BD−1 Ο I𝑞 = ഥ A−1 −ഥ A−1BD−1 −D−1Cഥ A−1 D−1 + D−1Cഥ A−1BD−1 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Decompose the linear equation of the block matrix M into a matrix product using the Schur complement ഥ A: Expand into two equations: Calculate 𝐱1 from the first equation, and then substitute the result into the second equation to calculate 𝐱2 Solving linear equations using Schur complement (for M/D𝑝×𝑝 ) ഥ A𝐱1 = ҧ 𝐛1 , D𝐱2 = 𝐛2 − C𝐱1 A B C D 𝐱1 𝐱2 = 𝐛1 𝐛2 I𝑝 −BD−1 Ο I𝑞 A B C D 𝐱1 𝐱2 = I𝑝 −BD−1 Ο I𝑞 𝐛1 𝐛2 ഥ A Ο C D 𝐱1 𝐱2 = ҧ 𝐛1 𝐛2 , where ҧ 𝐛1 ≡ 𝐛1 − BD−1𝐛2 𝐱1 = ഥ A−1 ҧ 𝐛1 𝐱2 = D−1 𝐛2 − C𝐱1 If the size of ഥ A is small, the computational cost of ഥ A−1 is low If D is a block diagonal matrix, the computational cost of D−1 is low 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Let ഥ A be the Schur complement of the partitioned matrix M with respect to the block A, denoted as ഥ D ≡ M/A𝑞×𝑞 ≡ D − CA−1B Decompose M into a matrix product containing a block diagonal matrix using ഥ D XYZ −1 = Z−1Y−1X−1, so if A and ഥ D are nonsingular (invertible) Inverse matrix using Schur complement (for M/A𝑞×𝑞 ) M ≡ A B C D = A Ο C ഥ D I𝑝 A−1B Ο I𝑞 = I𝑝 Ο CA−1 I𝑞 𝐀 Ο Ο ഥ 𝐃 I𝑝 A−1B Ο I𝑞 A B C D −1 = I𝑝 −A−1B Ο I𝑞 𝐀−𝟏 Ο Ο ഥ 𝐃−𝟏 I𝑝 Ο −CA−1 I𝑞 = A−1 −A−1Bഥ D−1 Ο ഥ D−1 I𝑝 Ο −CA−1 I𝑞 = A−1 + A−1Bഥ D−1CA−1 −A−1Bഥ D−1 −ഥ D−1CA−1 ഥ D−1 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Decompose the linear equation of the block matrix M into a matrix product using the Schur complement ഥ D: Expand into two equations: Calculate 𝐱2 from the first equation, and then substitute the result into the second equation to calculate 𝐱1 Solving linear equations using Schur complement (forM/A𝑞×𝑞 ) A B C D 𝐱1 𝐱2 = 𝐛1 𝐛2 I𝑝 Ο −CA−1 I𝑞 A B C D 𝐱1 𝐱2 = I𝑝 Ο −CA−1 I𝑞 𝐛1 𝐛2 A B Ο ഥ D 𝐱1 𝐱2 = 𝐛1 ҧ 𝐛2 , where ҧ 𝐛2 ≡ 𝐛2 − CA−1𝐛1 𝐱2 = ഥ D−1 ҧ 𝐛2 𝐱1 = A−1 𝐛1 − B𝐱2 ഥ D𝐱2 = ҧ 𝐛2 , A𝐱1 = 𝐛1 − B𝐱2 If the size of ഥ D is small, the computational cost of ഥ D−1 is low If Ais a block diagonal matrix, the computational cost of A−1 is low 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Bundle adjustment 18 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) ( ) ( ) ( ) ( , ) ( ) , ( ) ( ) ( ) ( ) ( , ) ( , ) ( , ) ( ) ( , , ) ( , Bundle adjustment (BA) Optimize camera poses and 3D points to minimize reprojection errors • There are errors in the observations of the image sensor • 𝐳 = 1 𝑍𝑐 K′𝐗𝑐 between the observed 2D point 𝐳 and 3D point 𝐗 is not exactly satisfied • Assume this observation error as a Gaussian distribution and solve it as MAP estimation or maximum likelihood estimation • Given 𝑚 cameras and 𝑛 3D points, the camera poses 𝐩 and 3D points 𝐗 are estimated so that the sum of the distances (geometric error) between the reprojection point 𝒛 𝐩, 𝐗 of the 3D point observed by each camera and the corresponding 2D point 𝒛 is minimized geometric error 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Bundle adjustment (BA) • SfM using images from the Internet • The exact intrinsic parameters K of the camera are also unknown, so they are optimized simultaneously with BA • Visual SLAM • To emphasize calculation speed and stability, it is common practice to perform camera calibration beforehand and consider it as known • Optimization variables • Motion-only BA ：Camera pose 𝐩 • Structure-only BA：3D point 𝐗 • Full BA ：Camera pose 𝐩, 3D point 𝐗 • Optimization range • Local BA ：From the latest frame to a fixed number of past frames • Global BA：All images • When minimizing photometric error, it is called photometric bundle adjustment 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Numerical minimization of reprojection error Minimize the error function, assuming observation errors are normally distributed with mean 0 and covariance matrix ∑ • Since 𝐸 𝐱 is a nonlinear function, estimate the value of ො 𝐱 that provides the minimum value through numerical calculation by iterative methods • Use the camera pose and 3D point positions estimated by epipolar geometry or triangulation as the initial value 𝐱0 • Compute ො 𝐱 by updating it as ො 𝐱 → ො 𝐱 + 𝛿𝐱 : pose of camera 𝑖 𝑖 = 1, … , 𝑚 : position of 3D point 𝑗 𝑗 = 1, … , 𝑛 ∈ 𝔰𝔢 3 B. Triggs et al., “Bundle Adjustment ― A Modern Synthesis”, International Workshop on Vision Algorithms, 1999 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Newton’s method • Expand 𝐸 𝐱 up to the second order term around the current estimate • When the right side is differentiated with respect to 𝛿𝐱, the 𝛿𝐱 that gives local minimum satisfies the following equation: • Gauss-Newton method • The second term of is sufficiently small compared to the first term, thus it is approximated by .: • Levenberg-Marquardt Method • Introduce a damping factor to the left side: Numerical minimization of reprojection error Jacobian: Gradient (vector): Hessian : (where 𝑘 is the element number of 𝐞) B. Triggs et al., “Bundle Adjustment ― A Modern Synthesis”, International Workshop on Vision Algorithms, 1999 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Jacobian for 𝑚 = 𝑛 = 3 (grey: zero matrix) Calculation of the Jacobian in BA : Differentiate with respect to all camera poses and 3D point positions 𝐱 • Problem setting • Three camera viewpoints and three 3D points 𝑚 = 𝑛 = 3 • Perspective projection model • Known intrinsic parameters • Observation depends only on the pose of camera 𝑖 and the position of 3D point 𝑗 (Many other elements that are being differentiated are zero matrices ) ( ) ( ) ( ) ( , ) ( ) , ( ) ( ) ( ) ( ) ( , ) ( , ) ( , ) ( ) ( , , ) ( , , ) Example of problem setting B. Triggs et al., “Bundle Adjustment ― A Modern Synthesis”, International Workshop on Vision Algorithms, 1999 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Calculation of the Jacobian in BA • ， ：Relationship of 3D points in the camera and world coordinate systems • Using the chain law, expand and as follows: • Expand the projection as follows: • Differentiate ，and by the camera pose and the 3D point position • can be calculated as follows: （∵ ） B. Triggs et al., “Bundle Adjustment ― A Modern Synthesis”, International Workshop on Vision Algorithms, 1999 G. Gallego et al., “A compact formula for the derivative of a 3-d rotation in exponential coordinates”, JMIV, 2015 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Speeding up BA using Schur complement • In BA, it is generally true that: • The block diagonal matrix, which is formed by the partial derivative elements of only the 3D point parameters, occupies most of the Hessian • Using the inverse matrix calculation with the Schur complement, the inverse of H ො 𝐱 in Newton’s method equation H ො 𝐱 𝛿𝐱 = −𝐠 ො 𝐱 is sped up m ≪ 𝑛 m: Number of viewpoints, 𝑛: Number of 3D points 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Schematic case m = 𝑛 ( ) ( ) ( ) ( ) ( ) ( ) Jacobian and Hessian in BA 𝐗(1) 𝐗(2) 𝐗(3) 𝐩(1) 𝐩(3) 𝐩(2) 𝐗(1) 𝐗(2) 𝐗(3) 𝐩(1) 𝐩(3) 𝐩(2) 𝐗(1) 𝐗(2) 𝐗(3) 𝐩(1) 𝐩(3) 𝐩(2) Jacobian Hessian Camera : 3 3D points: 3 Partial derivative by camera pose H𝐩𝐩 Partial derivative by 3D point position H𝐱𝐱 H = H𝐩𝐩 H𝐩𝐱 H𝐱𝐩 H𝐱𝐱 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Jacobian and Hessian in BA ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 𝐗(1) 𝐗(8) 𝐗(2) 𝐗(3) 𝐗(4) 𝐗(5) 𝐗(6) 𝐗(7) 𝐗(10) 𝐗(9) 𝐗(11) 𝐗(12) 𝐗(13) 𝐗(14) 𝐗(15) 𝐩(1) 𝐩(4) 𝐩(5) 𝐩(3) 𝐩(2) 𝐗(1) 𝐗(8) 𝐗(2) 𝐗(3) 𝐗(4) 𝐗(5) 𝐗(6) 𝐗(7) 𝐗(10) 𝐗(9) 𝐗(11) 𝐗(12) 𝐗(13) 𝐗(14) 𝐗(15) 𝐩(1) 𝐩(4) 𝐩(5) 𝐩(3) 𝐩(2) 𝐗(1) 𝐗(8) 𝐗(2) 𝐗(3) 𝐗(4) 𝐗(5) 𝐗(6) 𝐗(7) 𝐗(10) 𝐗(9) 𝐗(11)𝐗(12)𝐗(13)𝐗(14)𝐗(15) 𝐩(1) 𝐩(4) 𝐩(5) 𝐩(3) 𝐩(2) H = H𝐩𝐩 H𝐩𝐱 H𝐱𝐩 H𝐱𝐱 Practical case m ≪ 𝑛 Partial derivative by camera pose H𝐩𝐩 Partial derivative by 3D point position H𝐱𝐱 Camera : 5 3D points: 15 Jacobian Hessian 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) In the example, D = H𝐱𝐱 is a block diagonal matrix larger than A = H𝐩𝐩 , so we use the Schur complement ഥ A ≡ ഥ H𝐩𝐩 ≡ H/H𝐗𝑿 ≡ H𝐩𝐩 − H𝐩𝐱 H𝐱𝐱 −1H𝐱𝐩 Expanding this into two equations: Calculate ∆𝐱1 from the first set and substitute the result into the second equation to calculate ∆𝐱2 Speeding up BA using Schur complement H𝐩𝐩 H𝐩𝐱 H𝐱𝐩 H𝐱𝐱 ∆𝐱1 ∆𝐱2 = 𝐛1 𝐛2 ഥ H𝐩𝐩 Ο H𝐱𝐩 H𝐱𝐱 ∆𝐱1 ∆𝐱2 = ҧ 𝐛1 𝐛2 , where ҧ 𝐛1 ≡ 𝐛1 − H𝐩𝐱 H𝐱𝐱 −1𝐛2 ∆𝐱1 = ഥ H𝐩𝐩 −1 ҧ 𝐛1 ∆𝐱2 = H𝐱𝐱 −1 𝐛2 − H𝐱𝐩 ∆𝐱1 The size of ഥ H𝐩𝐩 is relatively small because it is proportional to the number of camera viewpoints H𝐱𝐱 is a block diagonal matrix with respect to 3D points ഥ H𝐩𝐩 ∆𝐱1 = ҧ 𝐛1 H𝐱𝐱 ∆𝐱2 = 𝐛2 − H𝐱𝐩 ∆𝐱1 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Loop closing and Pose Graph Optimization (PGO) 29 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Loop closing 30 Correct the entire camera trajectory • Recognizing returning from an unknown area to a measured area • Utilize the accumulation of relative pose becoming zero • Two-stage processing • Two-stage processing • Loop detection • Loop candidate detection by image retrieval • Loop verification by Sim(3) estimation • Loop correction • Loop construction • Pose graph optimization considering scale drift • Global BA 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) (c) Correct accumulated errors by PGO and Global BA (b) Move the latest keyframe and its 3D points to construct the loop Loop construction z PGO (+ BA) Latest keyframe Keyframe detected as a loop Loop verification by Sim(3) estimation 31 Geometric verification using Sim(3) estimation & RANSAC • Compare the shapes of reconstructed 3D points to estimate similarity • In the case of a monocular camera, scale drift occurs • Transform and compare 3D points using Sim(3) transformation • Estimate Sim(3) transformation S from the multiple correspondences of 3D points • Several methods exist, including Horn’s and Umeyama’s (a) Loop verification by Sim(3) estimation 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Loop verification by Sim(3) estimation 32 Geometric verification using Sim(3) estimation & RANSAC • Specific processing steps 1. Estimate a provisional S using Sim(3) estimation and RANSAC 2. Search for corresponding points using the provisional S as a guide 3. Obtain an optimized ෠ S through motion-only BA extended to Sim(3) 4. Re-estimate the correct corresponding points using this ෠ S 5. If the number of correct corresponding points exceeds a certain threshold, determine it as a loop keyframe 𝐾𝑙 (c) Correct accumulated errors by PGO and Global BA (b) Move the latest keyframe and its 3D points to construct the loop Loop construction z PGO (+ BA) Latest keyframe Keyframe detected as a loop (a) Loop verification by Sim(3) estimation 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) (c) Correct accumulated errors by PGO and Global BA (b) Move the latest keyframe and its 3D points to construct the loop Loop construction z PGO (+ BA) Latest keyframe Keyframe detected as a loop (a) Loop verification by Sim(3) estimation Loop construction 33 Move the local map towards the loop keyframe 𝐾𝑙 1. Apply the Sim(3) estimated transformation ෠ S to the following: • The latest keyframe 𝐾𝑖 • The group of keyframes surrounding 𝐾𝑖 • 3D points 𝐗𝑖 and 𝐗𝑐 observed from 𝐾𝑖 and 2. Project the 3D points 𝐗𝑙 , which are observed from the keyframes surrounding 𝐾𝑙 , onto the moved 𝐾′𝑖 and , and associate them with 2D points • If 3D points are duplicated, they are merged 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Scale drift-aware pose graph optimization Pose Graph Optimization (PGO) • Correct camera trajectory errors using constraints when returning to the same location • In the case of a monocular camera: • Scale-drift is present (Figure a) • Scale errors cannot be corrected with 6-DOF PGO (Figure b) • Errors can be corrected with scale drift-aware 7-DOF PGO (Figure c) 34 Scale drift-aware pose graph optimization (PGO) H. Strasdat, “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010（image referenced） 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Scale drift-aware pose graph optimization 35 • Convert SE 3 (rigid) to Sim 3 (similarity) parameters • Absolute pose: T𝑖 ∈ SE 3 → S𝑖 ∈ Sim 3 • Relative pose: ΔT𝑖𝑗 ∈ SE(3) → ΔS𝑖𝑗 ∈ Sim 3 • Define and minimize residuals and error functions like BA • Optimize absolute pose S with relative pose ΔS as a fixed constraint • Correct positions of 3D points observed from each KF using estimated S W：Weight matrix for residuals 𝐬 ∈ 𝔰𝔦𝔪 3 ： Lie algebra of S H. Strasdat, “Scale Drift-Aware Large Scale Monocular SLAM”, Robotics: Science and Systems VI, 2010 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Global BA 6-DOF BA for all cameras and 3D points • In PGO, only the relative camera poses are used as constraints to optimize camera poses • After PGO, include 3D points for global optimization • After Global BA: • During loop closing, local mapping processed in parallel generates new KFs and 3D points • Propagate corrections to KFs and 3D points not involved in loop closing 36 (c) Correct accumulated errors by PGO and Global BA (b) Move the latest keyframe and its 3D points to construct the loop Loop construction z PGO (+ BA) Latest keyframe Keyframe detected as a loop (a) Loop verification by Sim(3) estimation 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Source code explanation 37 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Class for Bundle Adjustment bundle_adjuster.h 38 num_threads_: Number of thread use_analytic_diff: Using analytic differentiation Sim3_cache: Reuse of past calculations 𝑥s 𝑦s 𝑧s shot’s c.s. 𝑥1 𝑦1 𝑧1 𝑥2 𝑦2 𝑧2 cam1’s c.s. cam2’s c.s. 𝑥o 𝑦o 𝑧o world’s c.s. Multi-camera system (2 units) Add a perspective camera model id: camera model ID, fx, fy: focal length, cx, cy: camera center pixel_baseline: base line length of stereo camera (in pixels) pose_offset: pose relative to the shot coordinate system flags: Fixing or optimizing intrinsic parameter and pose_offset Add Equirectangular camera model id: camera model ID, rows, cols: image resolution pose_offset: relative pose in the shot coordinate flags: Fixing or optimizing pose_offset pose_offset Add and get camera (shot) pose id: camera (shot) ID Vec6_t params: 𝔰𝔢 3 pose parameter Vec7_t params: 𝔰𝔦𝔪 3 pose parameter 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Class for Bundle Adjustment bundle_adjuster.cc 39 Types of loss functions Figure citation: http://ceres-solver.org/nnls_modeling.html Trivial loss: 𝜌 𝑠 = 𝑠 Huber loss: 𝜌 𝑠 = ቊ 𝑠 𝑠 ≤ 1 2 𝑠 − 1 𝑠 > 1 Soft L1 loss: 𝜌 𝑠 = 2 1 + 𝑠 − 1 Cauchy loss: 𝜌 𝑠 = log 1 + 𝑠 Simple squared error ( 𝑠 is a squared norm ： 𝑠 = 𝑓𝑖 2 ) When the error is small, the behavior is quadratic; when large, it is linear (robust to outliers) Capable of handling severe outliers 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Class for Bundle Adjustment bundle_adjuster.cc 40 Add intrinsic parameters of camera model to the problem Fix intrinsic parameters of camera model (not optimized) Fix ext_params* of camera model (not optimized) Add ext_params* to the problem *Pose relative to shot coordinate system (pose_offset in bundle_adjuster.h) Add extrinsic parameter of camera to the problem Fix extrinsic parameters of camera (not optimized) 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Class for Bundle Adjustment bundle_adjuster.cc 41 Fix parameters of 3D points to the problem (not optimized) Compute reprojection error and its derivative Set loss function type and its scale parameter Add parameters of 3D points to the problem Add const function to the problem 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Class for Bundle Adjustment bundle_adjuster.cc 42 Enable cache (reuse of past calculation results) Utilize sparse Schur complement during optimization Perform optimization Disable cache 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Reprojection error reprojection_error.h 43 Compute reprojection error and analytical derivative Compute reprojection error and automatic derivative 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in SE(3) demo_corridor_ba.cc Set intrinsic parameters 𝑓: focal length, 𝑊, 𝐻: image resolution Generate a camera trajectory moving straight ahead of the 3D point cloud, with added randomness 46 Generate a point cloud representing a corridor with a floor and walls on both sides, measuring 4m wide × 4m high × 20m deep (resolution of 4 10−1 = 0.444…m) 𝑥 𝑦 𝑧 Set the number of viewpoints Set 3D points and noise parameters of camera rotation and translation 4m 4m 20m Generate rotation parameters as r ∈ 𝔰𝔬 3 and transform to R ∈ SO(3) 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in SE(3) demo_corridor_ba.cc Set R, 𝐭 of the 1st viewpoint (shot) • Lie::SE3::log(): Transform SE 3 to 𝔰𝔢 3 • FLAG_FIX_PARAMS: Fix extrinsic parameters 47 Declare a BA class (with 1 thread, using analytic derivative) Set intrinsic parameters for a perspective projection camera (id=0) FLAG_FIX_INTRINSIC_PARAMS: Fix the intrinsic parameters Set R, 𝐭 of the 0th viewpoint (shot) • Lie::SE3::log(): Transform SE 3 to 𝔰𝔢 3 • FLAG_FIX_PARAMS：Fixe extrinsic parameters Fix the origin and scale Generate noise of camera pose d𝐫, d𝐭 ∈ 𝔰𝔢 3 Add noise to camera poses for viewpoints (shots) starting from the 2nd Convert R, 𝐭 ∈ SE(3) to 𝐫, 𝐭 ∈ 𝔰𝔢 3 Add noise d𝐫, d𝐭 to 𝐫, 𝐭 Set extrinsic parameters for optimization 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in SE(3) demo_corridor_ba.cc 48 Add noise to the i-th 3D point and register it false: Do not fix the parameters of 3D point (set as optimization variables) Compute the observed coordinates 𝑢, 𝑣 of the i-th 3D point at the j-th viewpoint (shot) If the 3D point is in front of the camera and the observed coordinates 𝑢, 𝑣 is within the image range, set the following as parameters for the reprojection errors: • Camera model（id=0） • Coordinates 𝑢, 𝑣 at which the j-th viewpoint observed the i-th 3D point • Set Trivial Loss 𝜌 𝑠 = 𝑠 (no alteration to Loss) If the environmental variable ”SHOW_IMAGE” is set, configure visualization of the results Generate images for visualization Display the images Construct a BA problem setting from the information registered in the solver ≈ 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in SE(3) demo_corridor_ba.cc 49 Perform BA (Maximum number of iterations: 20) Display the time taken for the optimization of the BA 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in Sim(3) demo_sim3_point_alignment.cc 50 Set the Sim(3) parameters S21 for cam2 to cam1 as: S21 = 𝑠21 R21 𝐭21 0 1 Set the camera position trj in the world coordinate sysmte by adding noise to 0,0, −7 Generate a point cloud on a sphere with radius 4, divided into 8 point in the longitude direction and 5 in the the latitude direction 𝑥 𝑦 𝑧 ∅ 𝜃 𝑟 Compute the translation vector 𝐭𝑤1 from world to cam1 Set the pose of the reference camera 1 Convert Sim(3) parameters S21 from cam2 to cam1 into Sim(3) parameters S𝑤2 for world to cam2 transformation ≈ Set the pose of camera 2 transformed by Sim(3) S𝑤2 = 𝑠𝑤2 R𝑤2 𝐭𝑤2 0 1 = 𝑠12 R12 𝐭12 0 1 R𝑤1 𝐭𝑤1 0 1 = 𝑠21 R21 𝐭21 0 1 −1 R1 𝐭1 0 1 = 1 𝑠21 R21 T − 1 𝑠21 R21 T𝐭21 0 1 R𝑤1 𝐭𝒘1 0 1 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in Sim(3) demo_sim3_point_alignment.cc 51 Set the 0th viewpoint (shot) as a multi-camera system with three cameras Randomly set the offset from the local (shot) to each camera coordinate system Set intrinsic parameters 𝑓: focal length、𝑊, 𝐻: image resolution Declare a BA class (with 2 thread, using analytic derivative) Set and fix intrinsic and extrinsic parameters for a perspective projection cameras (id=0, 1, 2) FLAG_FIX_INTRINSIC_PARAMS : Set intrinsic parameters FLAG_FIX_EXTRINSIC_PARAMS: Fix extrinsic parameters offset 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in Sim(3) demo_sim3_point_alignment.cc 52 Fix the SE(3) parameters of the 0th viewpoint (shot), and set the Sim(3) parameters of the 1st viewpoint (shot) as optimization variables Register the i-th 3D points of point1 and 2 as observations for the 0th and 1st viewpoints (shot) true: Set the parameters of the 3D points as optimization varibles Compute the observed coordinate 𝑢, 𝑣 of the i-th 3D point at the j-th viewpoint (shot) If the 3D point is in front of the camera and its observed coordinates 𝑢, 𝑣 are within the image range, set the following as parameters for the reprojection error: • Camera model (id=j) • Coordinates 𝑢, 𝑣 at which the 1st viewpoint observes the i-th 3D point • Set Trivial Loss 𝜌 𝑠 = 𝑠 (no alteration to Loss) 

Tutorial of Bundle Fitting Library for 3D Reconstruction (BundleFit) Sample code for BA in Sim(3) demo_sim3_point_alignment.cc 53 Construct a BA problem setting from the information registered in the solver Perform BA (Maximum number of iterations: 20) Assign the optimized Sim(3) parameters to the variables Display the results of the optimization