diff options
Diffstat (limited to 'jni/feature_stab/db_vlvm/db_bundle.h')
-rw-r--r-- | jni/feature_stab/db_vlvm/db_bundle.h | 68 |
1 files changed, 68 insertions, 0 deletions
diff --git a/jni/feature_stab/db_vlvm/db_bundle.h b/jni/feature_stab/db_vlvm/db_bundle.h new file mode 100644 index 0000000..e4fb8db --- /dev/null +++ b/jni/feature_stab/db_vlvm/db_bundle.h @@ -0,0 +1,68 @@ +/* + * Copyright (C) 2011 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* $Id: db_bundle.h,v 1.2 2011/06/17 14:03:30 mbansal Exp $ */ + +#ifndef DB_BUNDLE_H +#define DB_BUNDLE_H + + +/***************************************************************** +* Lean and mean begins here * +*****************************************************************/ +/*! + * \defgroup LMBundle (LM) Bundle adjustment utilities (a.k.a. Levenberg-Marquardt algorithm) + */ +/*\{*/ + +#include "db_utilities.h" + +/*! +Solve for update dx such that diagmult(1+lambda,transpose(J)%J)%dx= -Jtf +using only upper half of JtJ, destroying lower half below diagonal in the process +dimension is n and d should point to n allocated doubles of scratch memory +*/ +inline void db_Compute_dx(double *dx,double **JtJ,double *min_Jtf,double lambda,double *d,int n) +{ + int i; + double opl; + + opl=1.0+lambda; + for(i=0;i<n;i++) d[i]=JtJ[i][i]*opl; + + db_CholeskyDecompSeparateDiagonal(JtJ,d,n); + db_CholeskyBacksub(dx,JtJ,d,n,min_Jtf); +} + +/*! +Solve for update dx such that diagmult(1+lambda,transpose(J)%J)%dx= -Jtf +using only upper half of JtJ, destroying lower half below diagonal in the process +*/ +inline void db_Compute_dx_3x3(double dx[3],double JtJ[9],const double min_Jtf[3],double lambda) +{ + double d[3],opl; + + opl=1.0+lambda; + d[0]=JtJ[0]*opl; + d[1]=JtJ[4]*opl; + d[2]=JtJ[8]*opl; + db_CholeskyDecomp3x3SeparateDiagonal(JtJ,d); + db_CholeskyBacksub3x3(dx,JtJ,d,min_Jtf); +} + +/*\}*/ + +#endif /* DB_BUNDLE_H */ |