1 #include <iostream>
2 #include <cassert>
3 #include "vnl_discrete_diff.h"
4 #include <vnl/vnl_least_squares_function.h>
5
6 /*
7 fsm
8 */
9
vnl_discrete_diff_fwd(vnl_least_squares_function * lsf,double h_,vnl_vector<double> const & x,vnl_matrix<double> & J)10 bool vnl_discrete_diff_fwd(vnl_least_squares_function *lsf,
11 double h_,
12 vnl_vector<double> const &x,
13 vnl_matrix<double> &J)
14 {
15 vnl_vector<double> y(lsf->get_number_of_residuals());
16 lsf->f(x,y);
17 if (lsf->failure)
18 return false;
19 vnl_vector<double> h(lsf->get_number_of_unknowns());
20 h.fill(h_);
21 return vnl_discrete_diff_fwd(lsf,h,x,y,J);
22 }
23
vnl_discrete_diff_fwd(vnl_least_squares_function * lsf,vnl_vector<double> const & h,vnl_vector<double> const & x,vnl_matrix<double> & J)24 bool vnl_discrete_diff_fwd(vnl_least_squares_function *lsf,
25 vnl_vector<double> const &h,
26 vnl_vector<double> const &x,
27 vnl_matrix<double> &J)
28 {
29 vnl_vector<double> y(lsf->get_number_of_residuals());
30 lsf->f(x,y);
31 if (lsf->failure)
32 return false;
33 return vnl_discrete_diff_fwd(lsf,h,x,y,J);
34 }
35
vnl_discrete_diff_fwd(vnl_least_squares_function * lsf,vnl_vector<double> const & h,vnl_vector<double> const & x,vnl_vector<double> const & y,vnl_matrix<double> & J)36 bool vnl_discrete_diff_fwd(vnl_least_squares_function *lsf,
37 vnl_vector<double> const &h,
38 vnl_vector<double> const &x,
39 vnl_vector<double> const &y,
40 vnl_matrix<double> &J)
41 {
42 unsigned m=J.rows();
43 unsigned n=J.columns();
44 assert(m==lsf->get_number_of_residuals());
45 assert(m==y.size());
46 assert(n==lsf->get_number_of_unknowns());
47 assert(n==h.size());
48 assert(n==x.size());
49
50 vnl_vector<double> tx(n);
51 vnl_vector<double> ty(m);
52
53 for (unsigned j=0;j<n;j++) {
54 tx=x; tx(j) += h(j);
55 lsf->f(tx,ty);
56 if (lsf->failure)
57 return false;
58 for (unsigned i=0;i<m;i++)
59 J(i,j) = (ty(i)-y(i))/h(j);
60 }
61 return true;
62 }
63
vnl_discrete_diff_sym(vnl_least_squares_function * lsf,double h_,vnl_vector<double> const & x,vnl_matrix<double> & J)64 bool vnl_discrete_diff_sym(vnl_least_squares_function *lsf,
65 double h_,
66 vnl_vector<double> const &x,
67 vnl_matrix<double> &J)
68 {
69 vnl_vector<double> h(lsf->get_number_of_unknowns());
70 h.fill(h_);
71 return vnl_discrete_diff_sym(lsf,h,x,J);
72 }
73
vnl_discrete_diff_sym(vnl_least_squares_function * lsf,vnl_vector<double> const & h,vnl_vector<double> const & x,vnl_matrix<double> & J)74 bool vnl_discrete_diff_sym(vnl_least_squares_function *lsf,
75 vnl_vector<double> const &h,
76 vnl_vector<double> const &x,
77 vnl_matrix<double> &J)
78 {
79 unsigned m=J.rows();
80 unsigned n=J.columns();
81 assert(m==lsf->get_number_of_residuals());
82 assert(n==lsf->get_number_of_unknowns());
83 assert(n==h.size());
84 assert(n==x.size());
85
86 vnl_vector<double> xp(n),xm(n);
87 vnl_vector<double> yp(m),ym(m);
88
89 for (unsigned j=0;j<n;j++) {
90 xp=x; xp(j) += h(j);
91 lsf->f(xp,yp);
92 if (lsf->failure)
93 return false;
94
95 xm=x; xm(j) -= h(j);
96 lsf->f(xm,ym);
97 if (lsf->failure)
98 return false;
99
100 for (unsigned i=0;i<m;i++)
101 J(i,j) = (yp(i)-ym(i))/(2*h(j));
102 }
103 return true;
104 }
105
106 //----------------------------------------------------------------------
107
vnl_discrete_diff_test_lsf(vnl_least_squares_function * lsf,vnl_vector<double> const & x)108 void vnl_discrete_diff_test_lsf(vnl_least_squares_function *lsf, vnl_vector<double> const &x)
109 {
110 unsigned int m = lsf->get_number_of_residuals();
111 unsigned int n = lsf->get_number_of_unknowns ();
112 assert(x.size() == n);
113
114 vnl_matrix<double> J1(m, n);
115 lsf->gradf(x, J1);
116
117 vnl_matrix<double> J2(m, n);
118 vnl_discrete_diff_sym(lsf, 0.0001, x, J2);
119
120 double e = (J1 - J2).fro_norm();
121 //assert(e <= 1e-3);
122 double t = cos_angle(J1, J2);
123 //assert(t >= 0.99);
124
125 std::cerr << __FILE__ ": e = " << e << std::endl
126 << __FILE__ ": t = " << t << std::endl;
127 }
128