00001
00002
00003
00004
00005 #ifndef IMAGESTACK_LINEARALGEBRA_H
00006 #define IMAGESTACK_LINEARALGEBRA_H
00007 #include <stdio.h>
00008 #include <math.h>
00009
00010 namespace ImageStack {
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115 template<int N, int M>
00116 class LeastSquaresSolver {
00117 public:
00118
00119 double AtA[N *N];
00120 double Atb[N *M];
00121
00122 LeastSquaresSolver() {
00123 reset();
00124 }
00125
00126 void reset() {
00127 for (int i = 0; i < N*N; i++) {
00128 AtA[i] = 0;
00129 }
00130 for (int i = 0; i < N*M; i++) {
00131 Atb[i] = 0;
00132 }
00133 }
00134
00135 void addCorrespondence(float *in, float *out, float weight) {
00136
00137 for (int i = 0; i < N; i++) {
00138 for (int j = 0; j < N; j++) {
00139 AtA[i *N+j] += in[i]*in[j]*weight;
00140 }
00141 }
00142
00143
00144 for (int i = 0; i < N; i++) {
00145 for (int j = 0; j < M; j++) {
00146 Atb[i *M+j] += in[i]*out[j]*weight;
00147 }
00148 }
00149 }
00150
00151 void addCorrespondence(float *in, float *out) {
00152
00153 for (int i = 0; i < N; i++) {
00154 for (int j = 0; j < N; j++) {
00155 AtA[i *N+j] += in[i]*in[j];
00156 }
00157 }
00158
00159
00160 for (int i = 0; i < N; i++) {
00161 for (int j = 0; j < M; j++) {
00162 Atb[i *M+j] += in[i]*out[j];
00163 }
00164 }
00165 }
00166
00167 void addCorrespondence(double *in, double *out, double weight) {
00168
00169 for (int i = 0; i < N; i++) {
00170 for (int j = 0; j < N; j++) {
00171 AtA[i *N+j] += in[i]*in[j]*weight;
00172 }
00173 }
00174
00175
00176 for (int i = 0; i < N; i++) {
00177 for (int j = 0; j < M; j++) {
00178 Atb[i *M+j] += in[i]*out[j]*weight;
00179 }
00180 }
00181 }
00182
00183 void addCorrespondence(double *in, double *out) {
00184
00185 for (int i = 0; i < N; i++) {
00186 for (int j = 0; j < N; j++) {
00187 AtA[i *N+j] += in[i]*in[j];
00188 }
00189 }
00190
00191
00192 for (int i = 0; i < N; i++) {
00193 for (int j = 0; j < M; j++) {
00194 Atb[i *M+j] += in[i]*out[j];
00195 }
00196 }
00197 }
00198
00199
00200 bool solve(double *solution) {
00201 bool isspd = true;
00202
00203 double L[N*N];
00204 for (int i = 0; i < N*N; i++) {
00205 L[i] = 0.0;
00206 }
00207
00208 for (int j = 0; j < N; j++) {
00209 double d = 0.0;
00210 for (int k = 0; k < j; k++) {
00211 double s = 0.0;
00212 for (int i = 0; i < k; i++) {
00213 s += L[k*N+i]*L[j*N+i];
00214 }
00215 L[j *N+k] = s = (AtA[j*N+k] - s)/L[k*N+k];
00216 d = d + s*s;
00217 isspd = isspd && (AtA[k *N+j] == AtA[j*N+k]);
00218 }
00219 d = AtA[j*N+j] - d;
00220 isspd = isspd && (d > 0.0);
00221 L[j *N+j] = sqrt(d > 0.0 ? d : 0.0);
00222 for (int k = j+1; k < N; k++) {
00223 L[j *N+k] = 0.0;
00224 }
00225 }
00226
00227
00228 if (!isspd) { return false; }
00229
00230 for (int i = 0; i < M*N; i++) { solution[i] = Atb[i]; }
00231
00232
00233
00234 for (int j = 0; j < M; j++) {
00235 for (int k = 0; k < N; k++) {
00236 for (int i = 0; i < k; i++) {
00237 solution[j+k *M] -= solution[j+i*M]*L[k*N+i];
00238 }
00239 solution[j+k *M] /= L[k*N+k];
00240 }
00241
00242
00243 for (int k = N-1; k >= 0; k--) {
00244 for (int i = k+1; i < N; i++) {
00245 solution[j+k *M] -= solution[j+i*M]*L[i*N+k];
00246 }
00247 solution[j+k *M] /= L[k*N+k];
00248 }
00249 }
00250
00251
00252 return true;
00253 }
00254 };
00255
00256 }
00257 #endif