#
source:
foam/trunk/vision/src/SVD.cpp
@
96

Revision 89, 11.8 KB checked in by dave, 11 years ago (diff) |
---|

Line | |
---|---|

1 | // Copyright (C) 2009 foam |

2 | // |

3 | // This program is free software; you can redistribute it and/or modify |

4 | // it under the terms of the GNU General Public License as published by |

5 | // the Free Software Foundation; either version 2 of the License, or |

6 | // (at your option) any later version. |

7 | // |

8 | // This program is distributed in the hope that it will be useful, |

9 | // but WITHOUT ANY WARRANTY; without even the implied warranty of |

10 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |

11 | // GNU General Public License for more details. |

12 | // |

13 | // You should have received a copy of the GNU General Public License |

14 | // along with this program; if not, write to the Free Software |

15 | // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. |

16 | |

17 | // from http://www.public.iastate.edu/~dicook/JSS/paper/paper.html |

18 | |

19 | /************************************************************ |

20 | * * |

21 | * Permission is hereby granted to any individual or * |

22 | * institution for use, copying, or redistribution of * |

23 | * this code and associated documentation, provided * |

24 | * that such code and documentation are not sold for * |

25 | * profit and the following copyright notice is retained * |

26 | * in the code and documentation: * |

27 | * Copyright (c) held by Dianne Cook * |

28 | * All Rights Reserved. * |

29 | * * |

30 | * Questions and comments are welcome, and I request * |

31 | * that you share any modifications with me. * |

32 | * * |

33 | * Dianne Cook * |

34 | * dicook@iastate.edu * |

35 | * * |

36 | ************************************************************/ |

37 | |

38 | /* |

39 | * svdcomp - SVD decomposition routine. |

40 | * Takes an mxn matrix a and decomposes it into udv, where u,v are |

41 | * left and right orthogonal transformation matrices, and d is a |

42 | * diagonal matrix of singular values. |

43 | * |

44 | * This routine is adapted from svdecomp.c in XLISP-STAT 2.1 which is |

45 | * code from Numerical Recipes adapted by Luke Tierney and David Betz. |

46 | * |

47 | * Input to dsvd is as follows: |

48 | * a = mxn matrix to be decomposed, gets overwritten with u |

49 | * m = row dimension of a |

50 | * n = column dimension of a |

51 | * w = returns the vector of singular values of a |

52 | * v = returns the right orthogonal transformation matrix |

53 | */ |

54 | |

55 | |

56 | #include <stdio.h> |

57 | #include <stdlib.h> |

58 | #include <math.h> |

59 | #include <algorithm> |

60 | #include <list> |

61 | #include "SVD.h" |

62 | |

63 | using namespace std; |

64 | |

65 | #define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) |

66 | #define MAX(x,y) ((x)>(y)?(x):(y)) |

67 | |

68 | /////////////////////////////////////////////////////// |

69 | |

70 | Vector<float> SVD(Matrix<float> &m) |

71 | { |

72 | Vector<float> w(m.GetRows()); |

73 | Matrix<float> v(m.GetRows(),m.GetCols()); |

74 | dsvd(m, m.GetRows(), m.GetCols(), w.GetRawData(), v); |

75 | m.SortCols(w); |

76 | return w; |

77 | } |

78 | |

79 | /////////////////////////////////////////////////////// |

80 | |

81 | static double PYTHAG(double a, double b) |

82 | { |

83 | double at = fabs(a), bt = fabs(b), ct, result; |

84 | |

85 | if (at > bt) { ct = bt / at; result = at * sqrt(1.0 + ct * ct); } |

86 | else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); } |

87 | else result = 0.0; |

88 | return(result); |

89 | } |

90 | |

91 | int dsvd(Matrix<float> &a, int m, int n, float *w, Matrix<float> &v) |

92 | { |

93 | int flag, i, its, j, jj, k, l, nm; |

94 | double c, f, h, s, x, y, z; |

95 | double anorm = 0.0, g = 0.0, scale = 0.0; |

96 | double *rv1; |

97 | |

98 | if (m < n) |

99 | { |

100 | fprintf(stderr, "#rows must be > #cols \n"); |

101 | return(0); |

102 | } |

103 | |

104 | rv1 = (double *)malloc((unsigned int) n*sizeof(double)); |

105 | |

106 | /* Householder reduction to bidiagonal form */ |

107 | for (i = 0; i < n; i++) |

108 | { |

109 | /* left-hand reduction */ |

110 | l = i + 1; |

111 | rv1[i] = scale * g; |

112 | g = s = scale = 0.0; |

113 | if (i < m) |

114 | { |

115 | for (k = i; k < m; k++) |

116 | scale += fabs((double)a[k][i]); |

117 | if (scale) |

118 | { |

119 | for (k = i; k < m; k++) |

120 | { |

121 | a[k][i] = (float)((double)a[k][i]/scale); |

122 | s += ((double)a[k][i] * (double)a[k][i]); |

123 | } |

124 | f = (double)a[i][i]; |

125 | g = -SIGN(sqrt(s), f); |

126 | h = f * g - s; |

127 | a[i][i] = (float)(f - g); |

128 | if (i != n - 1) |

129 | { |

130 | for (j = l; j < n; j++) |

131 | { |

132 | for (s = 0.0, k = i; k < m; k++) |

133 | s += ((double)a[k][i] * (double)a[k][j]); |

134 | f = s / h; |

135 | for (k = i; k < m; k++) |

136 | a[k][j] += (float)(f * (double)a[k][i]); |

137 | } |

138 | } |

139 | for (k = i; k < m; k++) |

140 | a[k][i] = (float)((double)a[k][i]*scale); |

141 | } |

142 | } |

143 | w[i] = (float)(scale * g); |

144 | |

145 | /* right-hand reduction */ |

146 | g = s = scale = 0.0; |

147 | if (i < m && i != n - 1) |

148 | { |

149 | for (k = l; k < n; k++) |

150 | scale += fabs((double)a[i][k]); |

151 | if (scale) |

152 | { |

153 | for (k = l; k < n; k++) |

154 | { |

155 | a[i][k] = (float)((double)a[i][k]/scale); |

156 | s += ((double)a[i][k] * (double)a[i][k]); |

157 | } |

158 | f = (double)a[i][l]; |

159 | g = -SIGN(sqrt(s), f); |

160 | h = f * g - s; |

161 | a[i][l] = (float)(f - g); |

162 | for (k = l; k < n; k++) |

163 | rv1[k] = (double)a[i][k] / h; |

164 | if (i != m - 1) |

165 | { |

166 | for (j = l; j < m; j++) |

167 | { |

168 | for (s = 0.0, k = l; k < n; k++) |

169 | s += ((double)a[j][k] * (double)a[i][k]); |

170 | for (k = l; k < n; k++) |

171 | a[j][k] += (float)(s * rv1[k]); |

172 | } |

173 | } |

174 | for (k = l; k < n; k++) |

175 | a[i][k] = (float)((double)a[i][k]*scale); |

176 | } |

177 | } |

178 | anorm = MAX(anorm, (fabs((double)w[i]) + fabs(rv1[i]))); |

179 | } |

180 | |

181 | /* accumulate the right-hand transformation */ |

182 | for (i = n - 1; i >= 0; i--) |

183 | { |

184 | if (i < n - 1) |

185 | { |

186 | if (g) |

187 | { |

188 | for (j = l; j < n; j++) |

189 | v[j][i] = (float)(((double)a[i][j] / (double)a[i][l]) / g); |

190 | /* double division to avoid underflow */ |

191 | for (j = l; j < n; j++) |

192 | { |

193 | for (s = 0.0, k = l; k < n; k++) |

194 | s += ((double)a[i][k] * (double)v[k][j]); |

195 | for (k = l; k < n; k++) |

196 | v[k][j] += (float)(s * (double)v[k][i]); |

197 | } |

198 | } |

199 | for (j = l; j < n; j++) |

200 | v[i][j] = v[j][i] = 0.0; |

201 | } |

202 | v[i][i] = 1.0; |

203 | g = rv1[i]; |

204 | l = i; |

205 | } |

206 | |

207 | /* accumulate the left-hand transformation */ |

208 | for (i = n - 1; i >= 0; i--) |

209 | { |

210 | l = i + 1; |

211 | g = (double)w[i]; |

212 | if (i < n - 1) |

213 | for (j = l; j < n; j++) |

214 | a[i][j] = 0.0; |

215 | if (g) |

216 | { |

217 | g = 1.0 / g; |

218 | if (i != n - 1) |

219 | { |

220 | for (j = l; j < n; j++) |

221 | { |

222 | for (s = 0.0, k = l; k < m; k++) |

223 | s += ((double)a[k][i] * (double)a[k][j]); |

224 | f = (s / (double)a[i][i]) * g; |

225 | for (k = i; k < m; k++) |

226 | a[k][j] += (float)(f * (double)a[k][i]); |

227 | } |

228 | } |

229 | for (j = i; j < m; j++) |

230 | a[j][i] = (float)((double)a[j][i]*g); |

231 | } |

232 | else |

233 | { |

234 | for (j = i; j < m; j++) |

235 | a[j][i] = 0.0; |

236 | } |

237 | ++a[i][i]; |

238 | } |

239 | |

240 | /* diagonalize the bidiagonal form */ |

241 | for (k = n - 1; k >= 0; k--) |

242 | { /* loop over singular values */ |

243 | for (its = 0; its < 30; its++) |

244 | { /* loop over allowed iterations */ |

245 | flag = 1; |

246 | for (l = k; l >= 0; l--) |

247 | { /* test for splitting */ |

248 | nm = l - 1; |

249 | if (fabs(rv1[l]) + anorm == anorm) |

250 | { |

251 | flag = 0; |

252 | break; |

253 | } |

254 | if (fabs((double)w[nm]) + anorm == anorm) |

255 | break; |

256 | } |

257 | if (flag) |

258 | { |

259 | c = 0.0; |

260 | s = 1.0; |

261 | for (i = l; i <= k; i++) |

262 | { |

263 | f = s * rv1[i]; |

264 | if (fabs(f) + anorm != anorm) |

265 | { |

266 | g = (double)w[i]; |

267 | h = PYTHAG(f, g); |

268 | w[i] = (float)h; |

269 | h = 1.0 / h; |

270 | c = g * h; |

271 | s = (- f * h); |

272 | for (j = 0; j < m; j++) |

273 | { |

274 | y = (double)a[j][nm]; |

275 | z = (double)a[j][i]; |

276 | a[j][nm] = (float)(y * c + z * s); |

277 | a[j][i] = (float)(z * c - y * s); |

278 | } |

279 | } |

280 | } |

281 | } |

282 | z = (double)w[k]; |

283 | if (l == k) |

284 | { /* convergence */ |

285 | if (z < 0.0) |

286 | { /* make singular value nonnegative */ |

287 | w[k] = (float)(-z); |

288 | for (j = 0; j < n; j++) |

289 | v[j][k] = (-v[j][k]); |

290 | } |

291 | break; |

292 | } |

293 | if (its >= 30) { |

294 | free((void*) rv1); |

295 | fprintf(stderr, "No convergence after 30,000! iterations \n"); |

296 | return(0); |

297 | } |

298 | |

299 | /* shift from bottom 2 x 2 minor */ |

300 | x = (double)w[l]; |

301 | nm = k - 1; |

302 | y = (double)w[nm]; |

303 | g = rv1[nm]; |

304 | h = rv1[k]; |

305 | f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); |

306 | g = PYTHAG(f, 1.0); |

307 | f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x; |

308 | |

309 | /* next QR transformation */ |

310 | c = s = 1.0; |

311 | for (j = l; j <= nm; j++) |

312 | { |

313 | i = j + 1; |

314 | g = rv1[i]; |

315 | y = (double)w[i]; |

316 | h = s * g; |

317 | g = c * g; |

318 | z = PYTHAG(f, h); |

319 | rv1[j] = z; |

320 | c = f / z; |

321 | s = h / z; |

322 | f = x * c + g * s; |

323 | g = g * c - x * s; |

324 | h = y * s; |

325 | y = y * c; |

326 | for (jj = 0; jj < n; jj++) |

327 | { |

328 | x = (double)v[jj][j]; |

329 | z = (double)v[jj][i]; |

330 | v[jj][j] = (float)(x * c + z * s); |

331 | v[jj][i] = (float)(z * c - x * s); |

332 | } |

333 | z = PYTHAG(f, h); |

334 | w[j] = (float)z; |

335 | if (z) |

336 | { |

337 | z = 1.0 / z; |

338 | c = f * z; |

339 | s = h * z; |

340 | } |

341 | f = (c * g) + (s * y); |

342 | x = (c * y) - (s * g); |

343 | for (jj = 0; jj < m; jj++) |

344 | { |

345 | y = (double)a[jj][j]; |

346 | z = (double)a[jj][i]; |

347 | a[jj][j] = (float)(y * c + z * s); |

348 | a[jj][i] = (float)(z * c - y * s); |

349 | } |

350 | } |

351 | rv1[l] = 0.0; |

352 | rv1[k] = f; |

353 | w[k] = (float)x; |

354 | } |

355 | } |

356 | free((void*) rv1); |

357 | return(1); |

358 | } |

359 |

**Note:**See TracBrowser for help on using the repository browser.