On Temporal and Spatial Calibration for High Accuracy Visual-Inertial Motion Estimation

200 Pages·2011·7.87 MB·English

by  Jonathan Scott Kelly

ON TEMPORAL AND SPATIAL CALIBRATION FOR
HIGH ACCURACY VISUAL-INERTIAL MOTION ESTIMATION
by
Jonathan Scott Kelly
A Dissertation Presented to the
FACULTY OF THE USC GRADUATE SCHOOL
UNIVERSITY OF SOUTHERN CALIFORNIA
In Partial Fulfillment of the
Requirements for the Degree
DOCTOR OF PHILOSOPHY
(COMPUTER SCIENCE)
December 2011
Copyright 2011 Jonathan Scott Kelly
Epigraph
1. Astronomers as a class are men of the most scrupulous sagacity; it is to them therefore that I
choosetopropoundthosedoubtsthatIhavesometimesentertainedabouttheuniversallyaccepted
rule for handling several slightly discrepant observations of the same event. By this rule the
observations are added together and the sum divided by the number of observations; the quotient
is then accepted as the true value of the required quantity until better and more information is
obtained. In this way, if the several observations can be considered as having, as it were, the same
weight, the centre of gravity is accepted as the true position of the objects under investigation.
This rule agrees with that used in the theory of probability when all errors of observations are of
the same weight or moment.
2. But is it right to hold that several observations are of the same weight or moment or equally
prone to any and every error? Are errors of some degrees as easy to make as others of as many
minutes? Is there everywhere the same probability? Such an assertion would be quite absurd,
which is undoubtedly the reason why astronomers prefer to reject completely observations which
theyjudgetobewideofthetruthwhileretainingtherestand,indeed,assigningtothemthesame
reliability. Thispracticemakesitmorethanclearthattheyarefarfromassigningthesamevalidity
toeachoftheobservationstheyhavemade, fortheyrejectsomeintheirentirety, whileinthecase
of others they not only retain them all but, moreover, treat them alike. I see no way of drawing a
dividinglinebetweenthosethataretobeutterlyrejectedandthosethataretobewhollyretained;
it may even happen that the rejected observation is the one that would have supplied the best
correction to the others. Nevertheless, I do not condemn in every case the principle of rejecting
one or other of the observations, indeed I approve it, whenever in the course of observation an
accident occurs which in itself raises an immediate scruple in the mind of the observer, before he
has considered the event and compared it with the other observations. If there is no such reason
to dissatisfaction, I think each and every observation should be admitted whatever its quality, as
long as the observer is conscious that he has taken every care.
In paper entitled “The Most Probable Choice Between Several Discrepant
Observations and the Formation Therefrom of the Most Likely Induction”
— Daniel Bernoulli (1777), translated from the Latin by C. G. Allen (1961)
ii
Acknowledgements
The great German mathematician Carl Friedrich Gauss once commented, “You know that I write
slowly. This is chiefly because I am never satisfied until I have said as much as possible in a few
words, andwritingbrieflytakesfarmoretimethanwritingatlength.” Iwonder(onlyhalfinjest)
if this was in response to the question, “When will your dissertation be finished?”
Gaussalsosaid,“Itisnotknowledge,buttheactoflearning,notpossessionbuttheactofgetting
there, which grants the greatest enjoyment.” I’d say Gauss was right on both counts, although,
in reference to the former remark, I’m not able to make claims about the comprehensiveness or
brevity of the present volume. Writing takes some time, regardless, and the process of putting
this dissertation together has afforded a chance to reflect and consider all of the people who have
helped me along the way.
I’m very much indebted to my Ph.D. supervisor, Dr. Gaurav Sukhatme, for his invaluable
guidance and mentorship. Gaurav has made my professional development a priority, promoted
my work, and advocated on my behalf. I hadn’t initially realized (years ago) what an enormous
differencethismakes—Gaurav’spersonalinterestinmysuccesshasledtomanyterrificopportu-
nities,bothpastandpresent,forwhichIamgrateful. Gauravalsogavemethefreedomtoexplore
research topics of personal interest, while trusting that I’d find a direction and not flail around
indefinitely (a brave thing to do).
Thanks also go to my Ph.D. qualifying exam and defense committee members: Dr. Laurent
Itti, Dr. Larry Matthies, Dr. Paul Newton, Dr. Stefan Schaal, and Dr. Fei Sha. In particular, I
would like to extend special thanks to Dr. Matthies for his advice, suggestions, and for always
asking good questions.
iii
I was fortunate, while working towards my doctoral degree, to be surrounded by a great com-
munity of peers. Rajay “Russell” Kumar and Alex Lee kept me smiling through many late nights
in the USC Laboratory for Molecular Robotics. During my early days in the Robotic Embed-
ded Systems Laboratory, Srikanth Saripalli provided essential advice and a helping hand getting
startedonanewresearchpathandwithunfamiliarhardware. Sri,andfellowteammemberStefan
Hrabar, made working on the AVATAR helicopter at USC a wonderful experience. I would also
like to express my appreciation to our safety pilot, Alan Butler, for his enthusiasm and good hu-
mor. Whenchoosingadissertationtopic,IwasstronglyinfluencedbymanydiscussionswithGabe
Sibley; Gabe’s excitement about navigation, perception, and estimation was infectious. To every-
one in RESL and in the USC Interaction Laboratory, thanks for the laughs and for the trips to
the all-important Swiss Bar.
Looking back a bit further, I would like to thank Dr. Russ Greiner and Dr. Hong Zhang at the
University of Alberta, and Dr. Leo Hartman at the Canadian Space Agency, for all their help and
encouragement. I’mindebtedtoRussforgivingmemyfirstchancetobecomeinvolvedinresearch
work. Hong was largely responsible for steering my interests towards robotics, and, in turn, for
my decision to pursue my doctorate. I must also thank Hong for not giving up on me, and on my
M.Sc. degree, when I moved to California (I still have the receipt for the sushi lunch, Hong). Leo
has been a fantastic source of ideas, inspiration, and relevant papers over the years since I was an
intern student at the CSA. My appreciation goes to all three for writing many reference letters on
my behalf. Additionally, I would like to acknowledge the generous funding assistance I received
from the National Science and Engineering Research Council (NSERC) of Canada, and from the
Annenberg Graduate Fellowship program at USC.
Finally, and most importantly, I would like to thank my mother, Barbara Kelly, my grand-
mother,HelenAitken,andmygrandfather,JohnAitken,fortheirunwaveringsupport. It’sbeena
longhaul,andmycontinuedstudieswouldnothavebeenpossiblewithouttheirloveandwillingness
to help in whatever way they could. I owe them my deepest gratitude.
iv
Table of Contents
Epigraph ii
Acknowledgements iii
List of Tables viii
List of Figures x
Abstract xiv
Chapter 1: Introduction 1
1.1 Background and Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Approach and Scope . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.3 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
1.4 Organization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
Chapter 2: Mathematical Methods and Models 10
2.1 Preliminaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
2.1.1 Notation and Conventions . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.2 Coordinate Frames and Spatial Transforms . . . . . . . . . . . . . . . . . . 11
2.1.2.1 Parameterizing Rotations . . . . . . . . . . . . . . . . . . . . . . . 13
2.1.2.2 Rotating Reference Frames . . . . . . . . . . . . . . . . . . . . . . 14
2.1.3 Estimation Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
2.1.3.1 State-Space Models and Stochastic Processes . . . . . . . . . . . . 16
2.1.3.2 Filtering for Optimal Estimation . . . . . . . . . . . . . . . . . . . 18
2.2 Visual Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.2.1 Camera Geometry . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
2.2.1.1 The Ideal Perspective Camera . . . . . . . . . . . . . . . . . . . . 22
2.2.1.2 Lens Distortion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22
2.2.1.3 Intrinsic Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . 26
2.2.1.4 Extrinsic Parameters . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.2.2 Image Features . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.2.2.1 Known Calibration Objects . . . . . . . . . . . . . . . . . . . . . . 28
2.2.2.2 Unknown Environments . . . . . . . . . . . . . . . . . . . . . . . . 29
2.2.3 Camera Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
2.3 Inertial Sensors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
2.3.1 Strapdown Inertial Sensing . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
2.3.2 Accelerometers . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
2.3.3 Gyroscopes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
2.4 Sensor Integration and Data Fusion. . . . . . . . . . . . . . . . . . . . . . . . . . . 36
2.4.1 System Parameterization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
v
2.4.2 Process Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38
2.4.3 Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39
2.4.3.1 Cartesian Landmark Parameterization . . . . . . . . . . . . . . . . 40
2.4.3.2 Inverse Depth Landmark Parameterization . . . . . . . . . . . . . 41
2.4.4 Effects of Calibration Errors on Motion Estimates . . . . . . . . . . . . . . 42
2.5 Summary and Historical Notes . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44
Chapter 3: Temporal Calibration 47
3.1 Temporal Calibration as a Registration Problem . . . . . . . . . . . . . . . . . . . 48
3.2 A Spatiotemporal Model of the Camera-IMU System . . . . . . . . . . . . . . . . . 50
3.2.1 Modeling the Time Delay . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50
3.2.2 IMU Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52
3.2.3 Camera Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . 54
3.3 The TD-ICP Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
3.3.1 A Distance Metric . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
3.3.2 Iterative Nonlinear Registration. . . . . . . . . . . . . . . . . . . . . . . . . 57
3.3.3 Outlier Rejection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59
3.3.4 Calibration Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
3.4 Simulation Studies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
3.4.1 Simulation Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
3.4.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
3.5 Experiments with a Handheld Sensor Platform . . . . . . . . . . . . . . . . . . . . 64
3.5.1 Hardware and Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
3.5.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
3.6 Experiments with the PR2 Robot . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
3.6.1 Hardware and Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
3.6.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
3.7 Summary and Related Work. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68
Chapter 4: On the Observability of Spatial Self-Calibration 73
4.1 The Observability Criterion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
4.2 Projective Structure from Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
4.3 Differential Geometry and Nonlinear Observability . . . . . . . . . . . . . . . . . . 77
4.4 Analysis of the Observability of Spatial Self-Calibration . . . . . . . . . . . . . . . 81
4.4.1 Calibration Using a Known Reference Object . . . . . . . . . . . . . . . . . 87
4.4.2 Calibration Under Constrained Motion. . . . . . . . . . . . . . . . . . . . . 88
4.5 Summary and Related Work. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88
4.6 Supporting Lemmas . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91
Chapter 5: Filtering for Spatial Self-Calibration 94
5.1 The Unscented and Iterated Sigma Point Kalman Filters. . . . . . . . . . . . . . . 95
5.2 The Unscented Quaternion Estimator . . . . . . . . . . . . . . . . . . . . . . . . . 98
5.3 Filter Operation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
5.3.1 Initialization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 101
5.3.1.1 Sensor State Initialization . . . . . . . . . . . . . . . . . . . . . . . 102
5.3.1.2 Landmark Position Initialization . . . . . . . . . . . . . . . . . . . 103
5.3.2 Outlier Rejection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
5.3.3 Efficient State Vector Propagation and Updates. . . . . . . . . . . . . . . . 104
5.4 Landmark Management . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
5.4.1 Adding New Landmarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 105
5.4.2 Removing Landmarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 106
vi
5.4.3 Switching to New Anchor Landmarks . . . . . . . . . . . . . . . . . . . . . 106
5.5 Simulation Studies . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
5.5.1 Simulation Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
5.5.2 Target-Based Calibration Results . . . . . . . . . . . . . . . . . . . . . . . . 108
5.5.3 Target-Free Self-Calibration Results . . . . . . . . . . . . . . . . . . . . . . 113
5.5.4 Gravity and Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
5.6 Experiments with a Handheld Sensor Platform . . . . . . . . . . . . . . . . . . . . 117
5.6.1 Hardware and Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . 118
5.6.2 Target-Based Calibration Results . . . . . . . . . . . . . . . . . . . . . . . . 119
5.6.3 Target-Free Self-Calibration Results . . . . . . . . . . . . . . . . . . . . . . 120
5.6.4 Is Full Self-Calibration Possible? . . . . . . . . . . . . . . . . . . . . . . . . 122
5.7 Summary and Related Work. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124
Chapter 6: Conclusion 140
6.1 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 140
6.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
6.3 Final Remarks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143
References 145
Appendix A: Representing and Parameterizing Rotations 159
A.1 Direction Cosine Matrices . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 160
A.1.1 Time Rate of Change . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 161
A.2 Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 162
A.2.1 Yaw-Pitch-Roll Euler Angles . . . . . . . . . . . . . . . . . . . . . . . . . . 162
A.2.2 Time Rate of Change . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163
A.3 Quaternions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 164
A.3.1 The Unit Quaternion. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
A.3.2 Time Rate of Change . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
A.4 Modified Rodrigues Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 168
A.4.1 Composition of Rotations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 171
A.4.2 Time Rate of Change . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 172
A.4.3 Jacobians and the System Matrix . . . . . . . . . . . . . . . . . . . . . . . . 173
Appendix B: Elements of Differential Geometry 175
B.1 Manifolds . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 175
B.2 The Tangent Bundle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177
B.3 Vector Fields . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 177
Appendix C: Calibration of the MicroStrain 3DM-GX3-25 IMU 178
C.1 Static and Dynamic Calibration . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 179
C.2 Noise Characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180
C.2.1 Allan Variance . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 180
C.2.2 Testing Procedure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183
C.2.3 Analysis Method . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183
C.2.4 Results for 3DM-GX3-25 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 184
vii
List of Tables
2.1 Diacritical marks used to identify specific quantities and variables. . . . . . . . . . 20
3.1 Simulation results for temporal calibration; a total of 100 Monte Carlo trials were
run for each of the four camera-IMU trajectories. . . . . . . . . . . . . . . . . . . . 59
3.2 Experimental results for temporal calibration of the 3DM-GX3-25 IMU and Flea
camera system. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
3.3 Experimental results for temporal calibration of multiple sensors on-board the PR2
robot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67
5.1 Simulation results for target-based calibration of the camera-IMU relative transla-
tion: trials TB-1 and TB-2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
5.2 Simulation results for target-based calibration of the camera-IMU relative orienta-
tion: trials TB-1 and TB-2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 114
5.3 Simulation results for target-free self-calibration of the camera-IMU relative trans-
lation: trial TF-1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
5.4 Simulation results for target-free self-calibration of the camera-IMU relative orien-
tation: trial TF-1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 115
5.5 Experimentalresultsfortarget-basedandtarget-freecalibrationofthecamera-IMU
relative translation: trials TB-1, TF-1 and TF-2. . . . . . . . . . . . . . . . . . . . 118
5.6 Experimentalresultsfortarget-basedandtarget-freecalibrationofthecamera-IMU
relative orientation: trials TB-1, TF-1 and TF-2. . . . . . . . . . . . . . . . . . . . 118
5.7 Experimental results for full self-calibration of the camera-IMU relative translation. 124
5.8 Experimental results for full self-calibration of the camera-IMU relative orientation. 124
5.9 Experimentalresultsforfullself-calibrationoftheFleacameraintrinsicparameters:
x and y axis focal lengths, f and f , and principal point coordinates, c and c . . 125
x y x y
viii
5.10 Experimental results for full self-calibration of the Navitar lens distortion coeffi-
cients: radialdistortioncoefficients,κ andκ ,andtangentialdistortioncoefficients,
1 2
τ and τ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
1 2
C.1 AverageAllandeviationvaluesacrossfiveindependentfour-hourtrials,forthe3DM-
GX3-25 IMU x, y and z axis gyroscope measurements. . . . . . . . . . . . . . . . . 183
C.2 AverageAllandeviationvaluesacrossfiveindependentfour-hourtrials,forthe3DM-
GX3-25 IMU x, y and z axis accelerometer measurements. . . . . . . . . . . . . . . 184
ix
List of Figures
1.1 Examplesofrobotsystemswhichemployvisualandinertialsensing: WillowGarage
PR2, Ramius amphibious hexapod, NASA Robonaut Centaur, USC AVATAR
helicopter. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
1.2 Integration of egomotion estimation, local structure estimation, and self-calibration
in a tightly-coupled loop. This estimation core provides a basis for more advanced
capabilities. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2.1 Relationshipbetweenthethreeprimaryvisual-inertialreferenceframes: thecamera
frame, {C}, IMU frame, {I}, and world frame, {W}. . . . . . . . . . . . . . . . . . 12
2.2 Idealperspectivecameraprojection. Theimageplaneisshowninfrontofthecamera
optical center for convenience. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23
2.3 Examples of the displacement of normalized image point coordinates caused by
(a) radial and (b) tangential distortion. . . . . . . . . . . . . . . . . . . . . . . . . 25
2.4 Example of (a) an undistorted image of a famous architectural landmark (the
Brandenburg Gate), and (b) the same image with significant radial distortion. . . . 25
2.5 Point Grey Research Flea FireWire camera, shown without a lens attached. . . . . 26
2.6 Viewofthe(a)frontand(b)rearoftheFleaFireWirecamera,coupledtoaNavitar
4 mm machine vision lens. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
2.7 Calibration image showing (a) a cross-junction on the planar checkerboard target,
and(b)thepositionofthejunctionrefinedtosubpixelaccuracyusingasaddlepoint
detector. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
2.8 Simplified block diagram of a strapdown inertial navigation system. . . . . . . . . 32
2.9 MicroStrain 3DM-GX3-25 IMU, shown at actual size of 44 mm x 25 mm x 11 mm. 33
2.10 Handheld sensor beam with 3DM-GX3-25 IMU and Flea camera attached. The
beam is 12 cm in length and has a mass of less than 350 g. . . . . . . . . . . . . . 37
x