Photogrammetry & Robotics Lab                             A Tool for Graph-Based SLAM 
           An Informal Introduction to                                Kalman  Particle               Graph-
           Least Squares                                                filter          filter        based 
           Cyrill Stachniss                                                          least squares  
                                                                                  approach to SLAM 
                                                       1                                                         2 
           Least Squares in General                                  Least Squares History 
           §  Approach for computing a solution for                  §  Method developed by Carl 
             an overdetermined system                                  Friedrich Gauss in 1795 
           §  “More equations than unknowns”                           (he was 18 years old) 
           §  Minimizes the sum of the squared                       §  First showcase: predicting 
             errors in the equations                                   the future location of the 
           §  Standard approach to a large set of                      asteroid Ceres in 1801            Courtesy:  
                                                                                                       Astronomische 
             problems                                                                                 Nachrichten, 1828 
           §  Often used to estimate model 
             parameters given observations 
                                                       3                                                         4 
            Our Problem                                                       Graphical Explanation 
            §  Given a system described by a set of n 
              observation functions  
            §  Let 
               §      be the state vector 
               §      be a measurement of the state x 
               §                    be a function which maps     to a 
                 predicted measurement 
            §  Given n noisy measurements         about 
              the state                                                             state        predicted          real 
            §  Goal: Estimate the state    which bests                           (unknown)     measurements    measurements 
              explains the measurements 
                                                              5                                                                 6 
            Example                                                           Error Function 
                                                                              §  Error     is typically the difference between 
                                                                                actual and predicted measurement  
                                                                                  
            §     position of 3D features                                     §  We assume that the error has zero mean 
                                                                                and is normally distributed  
            §      coordinates of the 3D features projected                   §  Gaussian error with information matrix 
              on camera images                                                §  The squared error of a measurement 
            §  Estimate the most likely 3D position of the                      depends only on the state and is a scalar 
              features based on the image projections                             
              (given the camera poses) 
                                                              7                                                                 8 
            Goal: Find the Minimum                                            Goal: Find the Minimum 
            §  Find the state x* which minimizes the                          §  Find the state x* which minimizes the 
              error given all measurements                                      error given all measurements 
                                                                               
                                           global error (scalar) 
                                          squared error terms (scalar)        §  A general solution is to derive the 
                                                                                global error function and find its nulls 
                                                                              §  In general complex and no closed form 
                                          error terms (vector)                  solution 
                                                                                   Numerical approaches 
                                                              9                                                               10 
            Assumption                                                        Solve Via Iterative Local 
            §  A “good” initial guess is available                            Linearizations 
            §  The error functions are “smooth” in                            §  Linearize the error terms around the 
              the neighborhood of the (hopefully                                current solution/initial guess 
              global) minima                                                  §  Compute the first derivative of the 
                                                                                squared error function 
            §  Then, we can solve the problem by                              §  Set it to zero and solve linear system 
              iterative local linearizations                                  §  Obtain the new state (that is hopefully 
                                                                                closer to the minimum) 
                                                                              §  Iterate 
                                                             11                                                               12 
           Linearizing the Error Function                            Squared Error 
           §  Approximate the error functions                        §  With the previous linearization, we 
             around an initial guess x via Taylor                     can fix    and carry out the 
             expansion                                                minimization in the increments  
                                                                     §  We replace the Taylor expansion in 
                                                                      the squared error terms: 
           §  Reminder: Jacobian 
                                                      13                                                        14 
           Squared Error                                             Squared Error 
           §  With the previous linearization, we                    §  With the previous linearization, we 
            can fix    and carry out the                              can fix    and carry out the 
            minimization in the increments                            minimization in the increments  
           §  We replace the Taylor expansion in                     §  We replace the Taylor expansion in 
            the squared error terms:                                  the squared error terms: 
                                                      15                                                        16