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/*! 34Solve for update dx such that diagmult(1+lambda,transpose(J)%J)%dx= -Jtf 35using only upper half of JtJ, destroying lower half below diagonal in the process 36dimension is n and d should point to n allocated doubles of scratch memory 37*/ 38inline 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/*! 51Solve for update dx such that diagmult(1+lambda,transpose(J)%J)%dx= -Jtf 52using only upper half of JtJ, destroying lower half below diagonal in the process 53*/ 54inline 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