Incremental Real-time Bundle Adjustment for Multi-camera Systems with Points at Infinity Johannes Schneider, Thomas Läbe, Wolfgang Förstner 1 Department of Photogrammetry Institute of Geodesy and Geoinformation University of Bonn Rostock, UAV-g 2013
DFG-Project: Mapping on Demand Goal of the project Mapping on Demand Development of a autonomously navigating UAV for fast three-dimensional semantic mapping of inaccessible objects Sensor Platform 2
DFG-Project: Mapping on Demand Goal of the project Mapping on Demand Development of a autonomously navigating UAV for fast three-dimensional semantic mapping of inaccessible objects Sensor Platform Four fisheye cameras as two stereo pairs On-board computer 2
Images sequence I A set of four synchronized taken frames I I wide field of view (multi-view and large field angles) corresponding points with KLT-Tracker 185 Johannes Schneider 4. September 2013 3 Incremental Real-time Bundle Adjustment
Images sequence I A set of four synchronized taken frames I I wide field of view (multi-view and large field angles) corresponding points with KLT-Tracker 185 3 Ô Perspective camera model not applicable Johannes Schneider 4. September 2013 Incremental Real-time Bundle Adjustment
Images sequence I A set of four synchronized taken frames I I wide field of view (multi-view and large field angles) corresponding points with KLT-Tracker 185 3 Ô Perspective camera model not applicable Ô Points at infinity may cause numerical difficulties Johannes Schneider 4. September 2013 Incremental Real-time Bundle Adjustment
Camera-system calibration 4 I determined in advance with calibrating bundle adjustment (Schneider and Fo rstner, PFG 2013(4)) Johannes Schneider 4. September 2013 Incremental Real-time Bundle Adjustment
Camera-system calibration 4 I determined in advance with calibrating bundle adjustment (Schneider and Fo rstner, PFG 2013(4)) Ô Relative poses should be considered within bundle adjustment Johannes Schneider 4. September 2013 Incremental Real-time Bundle Adjustment
Challenges Extended projective collinearity equation: x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i 5 1 Matlab software: http://www.ipb.uni-bonn.de/bacs/
Challenges Extended projective collinearity equation: x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i Omnidirectional cameras use ray directions, not image coordinates 5 1 Matlab software: http://www.ipb.uni-bonn.de/bacs/
Challenges Extended projective collinearity equation: x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i Omnidirectional cameras use ray directions, not image coordinates Points at infinity use homogeneous coordinates and estimate reduced coordinates 5 1 Matlab software: http://www.ipb.uni-bonn.de/bacs/
Challenges Extended projective collinearity equation: x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i Omnidirectional cameras use ray directions, not image coordinates Points at infinity use homogeneous coordinates and estimate reduced coordinates Multi-view camera systems use extended version of collinearity equation 5 1 Matlab software: http://www.ipb.uni-bonn.de/bacs/
Challenges Extended projective collinearity equation: x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i Omnidirectional cameras 1 use ray directions, not image coordinates Points at infinity 1 use homogeneous coordinates and estimate reduced coordinates Multi-view camera systems 1 use extended version of collinearity equation 5 1 Matlab software: http://www.ipb.uni-bonn.de/bacs/
Challenges Extended projective collinearity equation: x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i Omnidirectional cameras 1 use ray directions, not image coordinates Points at infinity 1 use homogeneous coordinates and estimate reduced coordinates Multi-view camera systems 1 use extended version of collinearity equation Enable real-time processing fast and reliable data associations and approximate values reduce processing on geometrically useful frames solve bundle adjustment incrementally 5 1 Matlab software: http://www.ipb.uni-bonn.de/bacs/
Outline 1. Motivation 2. Approach On-line data acquisition and association Orientation of a set of frames and keyframe selection Incremental bundle adjustment 3. Results 6 4. Conclusions and Outlook
Outline 1. Motivation 2. Approach On-line data acquisition and association Orientation of a set of frames and keyframe selection Incremental bundle adjustment 3. Results 6 4. Conclusions and Outlook
Data acquisition and association Four image streams (time of exposure synchronized) Frame rate: 14 Hz Feature point detection and tracking KLT-Tracker from OpenCV library Associate feature points across cameras Check correlation coefficients of features on epipolar line 7 demo
Outline 1. Motivation 2. Approach On-line data acquisition and association Orientation of a set of frames and keyframe selection Incremental bundle adjustment 3. Results 8 4. Conclusions and Outlook
Initialization of a map X Map: set of scene points X = {X i, i = 1,..., I} Initialization with forward intersection of the stereo matches Initiating frame set defines the coordinate system 9
Initialization of a map X Map: set of scene points X = {X i, i = 1,..., I} Initialization with forward intersection of the stereo matches Initiating frame set defines the coordinate system M 1 9 X
Orientation of a set of frames Determine motion matrix M t of a new frame set spatial resection x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i M 1 10 X robust iterative ML-type estimator down-weights and eliminates outliers uses M t 1 as initial approximate value converges after 2-3 fast iterations allows for high frame rates
Orientation of a set of frames Determine motion matrix M t of a new frame set spatial resection x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i M 1 M 2 10 X robust iterative ML-type estimator down-weights and eliminates outliers uses M t 1 as initial approximate value converges after 2-3 fast iterations allows for high frame rates
Orientation of a set of frames Determine motion matrix M t of a new frame set spatial resection x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i M 1 M 2 M 3 10 X robust iterative ML-type estimator down-weights and eliminates outliers uses M t 1 as initial approximate value converges after 2-3 fast iterations allows for high frame rates
Orientation of a set of frames Determine motion matrix M t of a new frame set spatial resection x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i M 1 M 2 M 3 M 4 10 X robust iterative ML-type estimator down-weights and eliminates outliers uses M t 1 as initial approximate value converges after 2-3 fast iterations allows for high frame rates
Orientation of a set of frames Determine motion matrix M t of a new frame set spatial resection x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i M 1 M 2 M 3 M 4 M 5 10 X robust iterative ML-type estimator down-weights and eliminates outliers uses M t 1 as initial approximate value converges after 2-3 fast iterations allows for high frame rates
Orientation of a set of frames Determine motion matrix M t of a new frame set spatial resection x s itc = N ( [I 3 0 3 ] M 1 c M 1 ) t X i M 1 M 2 M 3 M 4 M 5 M 6 10 X robust iterative ML-type estimator down-weights and eliminates outliers uses M t 1 as initial approximate value converges after 2-3 fast iterations allows for high frame rates
Keyframe selection We have orientated the frame sets T = {M t, t = 1,..., T } 11
Keyframe selection We have orientated the frame sets T = {M t, t = 1,..., T } Reduce processing on some useful frames Keyframes are a subset K = {M k, k = 1,..., K} T Initiate keyframe if K = distance d(mk,m T ) is more than e.g. 1 m or 30 11
Keyframe selection We have orientated the frame sets T = {M t, t = 1,..., T } Reduce processing on some useful frames Keyframes are a subset K = {M k, k = 1,..., K} T Initiate keyframe if K = distance d(mk,m T ) is more than e.g. 1 m or 30 11 M 1
Keyframe selection We have orientated the frame sets T = {M t, t = 1,..., T } Reduce processing on some useful frames Keyframes are a subset K = {M k, k = 1,..., K} T Initiate keyframe if K = distance d(mk,m T ) is more than e.g. 1 m or 30 11 M 1 M 2
Keyframe selection We have orientated the frame sets T = {M t, t = 1,..., T } Reduce processing on some useful frames Keyframes are a subset K = {M k, k = 1,..., K} T Initiate keyframe if K = distance d(mk,m T ) is more than e.g. 1 m or 30 11 M 1 M 2 M 3
Keyframe selection We have orientated the frame sets T = {M t, t = 1,..., T } Reduce processing on some useful frames Keyframes are a subset K = {M k, k = 1,..., K} T Initiate keyframe if K = distance d(mk,m T ) is more than e.g. 1 m or 30 11 M 4 M 1 M 2 M 3
Expanding the map X Map: set of scene points X = {X i, i = 1,..., I} Forward intersection x s itc = N ( [I 3 0 3 ] M 1 c M 1 k X ) i 12
Expanding the map X Map: set of scene points X = {X i, i = 1,..., I} Forward intersection x s itc = N ( [I 3 0 3 ] M 1 c M 1 k X ) i 12
Expanding the map X Map: set of scene points X = {X i, i = 1,..., I} Forward intersection x s itc = N ( [I 3 0 3 ] M 1 c M 1 k X ) i 12
Expanding the map X Map: set of scene points X = {X i, i = 1,..., I} Forward intersection x s itc = N ( [I 3 0 3 ] M 1 c M 1 k X ) i 12
Expanding the map X Map: set of scene points X = {X i, i = 1,..., I} Forward intersection x s itc = N ( [I 3 0 3 ] M 1 c M 1 k X ) i Scene points at infinity can be included e.g. points at the horizon or with small intersection angles 12
Outline 1. Motivation 2. Approach On-line data acquisition and association Orientation of a set of frames and keyframe selection Incremental bundle adjustment 3. Results 13 4. Conclusions and Outlook
Incremental real-time bundle adjustment Batch bundle adjustment is too time consuming normal equation matrix grows with each new keyframe iterative re-linearization requires re-building 14
Incremental real-time bundle adjustment Batch bundle adjustment is too time consuming normal equation matrix grows with each new keyframe iterative re-linearization requires re-building But: New measurements have only local effects on the normal equation matrix 14
Incremental real-time bundle adjustment Batch bundle adjustment is too time consuming normal equation matrix grows with each new keyframe iterative re-linearization requires re-building But: New measurements have only local effects on the normal equation matrix 14 Idea: Incremental bundle adjustment only some variables have to be re-linearized do not solve for unaffected variables everytime
isam2 algorithm isam2 (Kaess et al., 2012) algorithm for sparse non-linear incremental optimization software on the internet 2 15 2 software from https://collab.cc.gatech.edu/borg/gtsam/
isam2 algorithm isam2 (Kaess et al., 2012) algorithm for sparse non-linear incremental optimization software on the internet 2 Encodes normal equations into a Bayes-tree keeps sparsity pattern Bayes-tree can be updated efficiently with new observations new variables 15 2 software from https://collab.cc.gatech.edu/borg/gtsam/
isam2 algorithm isam2 (Kaess et al., 2012) algorithm for sparse non-linear incremental optimization software on the internet 2 Encodes normal equations into a Bayes-tree keeps sparsity pattern Bayes-tree can be updated efficiently with new observations new variables Solves only for variables that are actually affected sub-trees can stay untouched Executes re-linearization only where necessary performs in that case also re-ordering 15 2 software from https://collab.cc.gatech.edu/borg/gtsam/
Outline 1. Motivation 2. Approach On-line data acquisition and association Orientation of a set of frames and keyframe selection Incremental bundle adjustment 3. Results 16 4. Conclusions and Outlook
Test flight Flight of the UAV with four fisheye cameras image sequence with 1,800 frame sets (14 Hz) system initiates a new keyframe after each 1m Demo 17
Time requirements 0.8 0.6 0.4 0.2 0 0 10 20 30 40 50 60 70 80 90 100 110 Timings in sec. 1500 1000 incremental update completed within a second 18 500 0 0 10 20 30 40 50 60 70 80 90 100 110 Nb. of cliques affected by relinearization 300 200 100 0 0 10 20 30 40 50 60 70 80 90 100 110 Nb. of new Factors
Time requirements 0.8 0.6 0.4 0.2 0 0 10 20 30 40 50 60 70 80 90 100 110 Timings in sec. 1500 1000 500 0 0 10 20 30 40 50 60 70 80 90 100 110 Nb. of cliques affected by relinearization 300 incremental update completed within a second depends on number of re-linearized variables 18 200 100 0 0 10 20 30 40 50 60 70 80 90 100 110 Nb. of new Factors
Time requirements 0.8 0.6 0.4 0.2 0 0 10 20 30 40 50 60 70 80 90 100 110 Timings in sec. 1500 1000 500 0 0 10 20 30 40 50 60 70 80 90 100 110 Nb. of cliques affected by relinearization 300 200 100 0 0 10 20 30 40 50 60 70 80 90 100 110 Nb. of new Factors incremental update completed within a second depends on number of re-linearized variables number of new observations has no effect on duration 18
Comparison to batch bundle adjustment Deviations between the estimated pose parameters 0.04 0.02 meter grad 0 0.02 0.04 0 10 20 30 40 50 60 70 80 90 100 110 translations x axis 0.1 y axis 0.05 z axis 0 0.05 0.1 0 10 20 30 40 50 60 70 80 90 100 110 rotation angles max. 2cm max. 0.1 19
Comparison to batch bundle adjustment Deviations between the estimated pose parameters 0.04 0.02 meter grad 0 0.02 0.04 0 10 20 30 40 50 60 70 80 90 100 110 translations x axis 0.1 y axis 0.05 z axis 0 0.05 0.1 0 10 20 30 40 50 60 70 80 90 100 110 rotation angles max. 2cm max. 0.1 19 Estimated uncertainty in rotations: up to 0.3 in translations: up to 8cm
Outline 1. Motivation 2. Approach On-line data acquisition and association Orientation of a set of frames and keyframe selection Incremental bundle adjustment 3. Results 20 4. Conclusions and Outlook
Conclusions and Outlook Conclusions system for a keyframe-based bundle adjustment allows for points at infinity multi-camera systems omnidirectional cameras real-time capabilities with isam2 algorithm near to the optimal solution 21
Conclusions and Outlook Conclusions system for a keyframe-based bundle adjustment allows for points at infinity multi-camera systems omnidirectional cameras real-time capabilities with isam2 algorithm near to the optimal solution 21 Outlook GPS integration less re-linearization robustification through incremental down-weighting
Conclusions and Outlook Conclusions system for a keyframe-based bundle adjustment allows for points at infinity multi-camera systems omnidirectional cameras real-time capabilities with isam2 algorithm near to the optimal solution 21 Outlook GPS integration less re-linearization robustification through incremental down-weighting Thank you for your attention!
References Förstner, W., 2012: Minimal Representations for Testing and Estimation in Projective Spaces. Photogrammetrie, Fernerkundung und Geoinformation (PFG), Vol. 3. Kaess, M., Johannsson, H., Roberts, R., Ila, V., Leonard, J., Dellaert, F., 2012: isam2: Incremental Smoothing and Mapping Using the Bayes Tree. International Journal of Robotics Research (IJRR). Schneider, J., Schindler, F., Läbe, T., Förstner, W., 2012: Bundle Adjustment for Multi-camera Systems with Points at Infinity. In ISPRS Annals of Photogrammetry, Remote Sensing and Spatial Information Sciences, Vol. I-3. 22 Schneider, J., Förstner, W., 2013: Bundle Adjustment and System Calibration with Points at Infinity for Omnidirectional Camera Systems. Photogrammetrie, Fernerkundung und Geoinformation (PFG), Vol. 4.