1 #include <iostream>
2 #include <cmath>
3 #include "bnl_quadratic_interpolator.h"
4 #include "vnl/vnl_vector.h"
5 #include <vnl/algo/vnl_svd.h>
6 #ifdef _MSC_VER
7 # include "vcl_msvc_warnings.h"
8 #endif
9
add_data_point(const double px,const double py,const double v)10 void bnl_quadratic_interpolator::add_data_point(const double px,
11 const double py,
12 const double v)
13 {
14 px_.push_back(px);
15 py_.push_back(py);
16 v_.push_back(v);
17 }
18
clear()19 void bnl_quadratic_interpolator::clear()
20 {
21 px_.clear();
22 py_.clear();
23 v_.clear();
24 }
25
n_points()26 int bnl_quadratic_interpolator::n_points()
27 {
28 return px_.size();
29 }
30
fill_scatter_matrix()31 void bnl_quadratic_interpolator::fill_scatter_matrix()
32 {
33 int n = this->n_points();
34 double x4 = 0, x3y = 0, x2y2 = 0, x3=0, x2y=0, vx2=0, x2 =0;
35 double xy3 = 0, xy2=0, vxy=0, xy=0;
36 double y4 = 0, y3 = 0, vy2 =0, y2 = 0;
37 double vx = 0, x =0;
38 double vy = 0, y =0;
39 double v2 =0, v =0;
40 for (int i=0; i<n; i++)
41 {
42 double pxi = px_[i]; x += pxi;
43 double pyi = py_[i]; y += pyi;
44 double vi = v_[i]; v += vi;
45 double xi2 = pxi*pxi; x2 += xi2;
46 double xi3 = xi2*pxi; x3 += xi3;
47 double xi4 = xi3*pxi; x4 += xi4;
48 double yi2 = pyi*pyi; y2 += yi2;
49 double yi3 = yi2*pyi; y3 += yi3;
50 double yi4 = yi3*pyi; y4 += yi4;
51 double xiyi = pxi*pyi; xy += xiyi;
52 double xi2yi = pxi*xiyi; x2y += xi2yi;
53 double xi3yi = xi3*pyi; x3y += xi3yi;
54 double xi2yi2 = xi2*yi2; x2y2+= xi2yi2;
55 double xiyi2 = pxi*yi2; xy2 += xiyi2;
56 double xiyi3 = pxi*yi3; xy3 += xiyi3;
57 double vixi2 = vi*xi2; vx2 += vixi2;
58 double vixiyi = vi*xiyi; vxy += vixiyi;
59 double viyi2 = vi*yi2; vy2 += viyi2;
60 double vixi = vi*pxi; vx += vixi;
61 double viyi = vi*pyi; vy += viyi;
62 double vi2 = vi*vi; v2 += vi2;
63 }
64 //solution vector is in the order
65 s_ = vnl_matrix<double>(7,7);
66 s_.put(0,0,x4); s_.put(0,1,x3y); s_.put(0,2,x2y2);s_.put(0,3,x3); s_.put(0,4,x2y);s_.put(0,5,-vx2);s_.put(0,6,x2);
67 s_.put(1,0,x3y); s_.put(1,1,x2y2);s_.put(1,2,xy3); s_.put(1,3,x2y);s_.put(1,4,xy2);s_.put(1,5,-vxy);s_.put(1,6,xy);
68 s_.put(2,0,x2y2);s_.put(2,1,xy3); s_.put(2,2,y4); s_.put(2,3,xy2);s_.put(2,4,y3); s_.put(2,5,-vy2);s_.put(2,6,y2);
69 s_.put(3,0,x3); s_.put(3,1,x2y); s_.put(3,2,xy2); s_.put(3,3,x2); s_.put(3,4,xy); s_.put(3,5,-vx); s_.put(3,6,x);
70 s_.put(4,0,x2y); s_.put(4,1,xy2); s_.put(4,2,y3); s_.put(4,3,xy); s_.put(4,4,y2); s_.put(4,5,-vy); s_.put(4,6,y);
71 s_.put(5,0,-vx2);s_.put(5,1,-vxy);s_.put(5,2,-vy2);s_.put(5,3,-vx);s_.put(5,4,-vy);s_.put(5,5,v2); s_.put(5,6,-v);
72 s_.put(6,0,x2); s_.put(6,1,xy); s_.put(6,2,y2); s_.put(6,3,x); s_.put(6,4,y); s_.put(6,5,-v); s_.put(6,6,n);
73 }
74
solve()75 bool bnl_quadratic_interpolator::solve()
76 {
77 if (this->n_points() < 6)
78 return false;
79 this->fill_scatter_matrix();
80 vnl_svd<double> svd(s_);
81 vnl_vector<double> nv = svd.nullvector();
82 std::cout << "W: " << svd.W() << '\n'
83 << "NV: " << nv << '\n';
84 if (std::fabs(nv[5])<1e-06)
85 return false;
86 // NV = [Ixx Ixy Iyy Ix Iy 1 I0]
87 double Ixx = nv[0]/nv[5], Ixy = nv[1]/nv[5], Iyy = nv[2]/nv[5];
88 double Ix = nv[3]/nv[5], Iy = nv[4]/nv[5], I0 = nv[6]/nv[5];
89 std::cout << "coef (" << Ixx << ' ' << Ixy << ' ' << Iyy << ' ' << Ix
90 << ' ' << Iy << ' ' << I0 << ")\n";
91 //solve for extremum
92 double det = 4*Ixx*Iyy-Ixy*Ixy;
93 //Is the system singular?
94 if (std::fabs(det)<1e-06)
95 return false;
96 //Is the Det Negative?
97 if (det<0)
98 return false;
99 //Is the Trace Negative?
100 double tr = Ixx + Iyy;
101 if (tr>=0)
102 return false;
103
104 // - -
105 // px_ext 1 | 2Iyy -Ixy | Ix
106 // = - --- | |
107 // py_ext det |-Ixy 2Ixx | Iy
108 // - -
109 // px_ext = -1/det ( 2Iyy*Ix - Ixy*Iy)
110 // py_ext = -1/det (-Ixy*Ix + 2Ixx*Iy)
111 //
112 px_ext_ = -( 2*Iyy*Ix - Ixy*Iy)/det;
113 py_ext_ = -(-Ixy*Ix + 2*Ixx*Iy)/det;
114 return true;
115 }
116
extremum(double & px,double & py)117 void bnl_quadratic_interpolator::extremum(double& px, double& py)
118 {
119 px = px_ext_;
120 py = py_ext_;
121 }
122
print()123 void bnl_quadratic_interpolator::print()
124 {
125 std::cout << "P / V\n";
126 for (int i = 0; i<this->n_points(); i++)
127 std::cout << px_[i] << '\t' << py_[i] << '\t' << v_[i] << '\n';
128 std::cout << std::flush;
129 }
130