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