1 /* 2 * Copyright (C) 2011 The Android Open Source Project 3 * 4 * Licensed under the Apache License, Version 2.0 (the "License"); 5 * you may not use this file except in compliance with the License. 6 * You may obtain a copy of the License at 7 * 8 * http://www.apache.org/licenses/LICENSE-2.0 9 * 10 * Unless required by applicable law or agreed to in writing, software 11 * distributed under the License is distributed on an "AS IS" BASIS, 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 * See the License for the specific language governing permissions and 14 * limitations under the License. 15 */ 16 17 /* $Id: db_bundle.h,v 1.2 2011/06/17 14:03:30 mbansal Exp $ */ 18 19 #ifndef DB_BUNDLE_H 20 #define DB_BUNDLE_H 21 22 23 /***************************************************************** 24 * Lean and mean begins here * 25 *****************************************************************/ 26 /*! 27 * \defgroup LMBundle (LM) Bundle adjustment utilities (a.k.a. Levenberg-Marquardt algorithm) 28 */ 29 /*\{*/ 30 31 #include "db_utilities.h" 32 33 /*! 34 Solve for update dx such that diagmult(1+lambda,transpose(J)%J)%dx= -Jtf 35 using only upper half of JtJ, destroying lower half below diagonal in the process 36 dimension is n and d should point to n allocated doubles of scratch memory 37 */ 38 inline void db_Compute_dx(double *dx,double **JtJ,double *min_Jtf,double lambda,double *d,int n) 39 { 40 int i; 41 double opl; 42 43 opl=1.0+lambda; 44 for(i=0;i<n;i++) d[i]=JtJ[i][i]*opl; 45 46 db_CholeskyDecompSeparateDiagonal(JtJ,d,n); 47 db_CholeskyBacksub(dx,JtJ,d,n,min_Jtf); 48 } 49 50 /*! 51 Solve for update dx such that diagmult(1+lambda,transpose(J)%J)%dx= -Jtf 52 using only upper half of JtJ, destroying lower half below diagonal in the process 53 */ 54 inline void db_Compute_dx_3x3(double dx[3],double JtJ[9],const double min_Jtf[3],double lambda) 55 { 56 double d[3],opl; 57 58 opl=1.0+lambda; 59 d[0]=JtJ[0]*opl; 60 d[1]=JtJ[4]*opl; 61 d[2]=JtJ[8]*opl; 62 db_CholeskyDecomp3x3SeparateDiagonal(JtJ,d); 63 db_CholeskyBacksub3x3(dx,JtJ,d,min_Jtf); 64 } 65 66 /*\}*/ 67 68 #endif /* DB_BUNDLE_H */ 69
