1 //*******************************************************************
2 //
3 // License: See top level LICENSE.txt file.
4 //
5 // Author: Garrett Potts (gpotts@imagelinks.com
6 //
7 // Description: Source code produced by Dave Knopp
8 //
9 //*******************************************************************
10 // $Id: ossimLeastSquaresBilin.cpp 9963 2006-11-28 21:11:01Z gpotts $
11 #include <iostream> // for debugging
12 using namespace std;
13 #include <ossim/base/ossimLeastSquaresBilin.h>
14 #include <ossim/base/ossimNotifyContext.h>
15
ossimLeastSquaresBilin()16 ossimLeastSquaresBilin::ossimLeastSquaresBilin()
17 : bl_a(0.0)
18 , bl_b(0.0)
19 , bl_c(0.0)
20 , bl_d(0.0)
21 , AtA(NULL)
22 , Atb(NULL)
23 {
24 // allocate normal system accumulation matrices
25 AtA = new NEWMAT::Matrix(4,4);
26 Atb = new NEWMAT::Matrix(4,1);
27
28 // ensure initilization to zero
29 *AtA = 0.0;
30 *Atb = 0.0;
31
32 return;
33 }
ossimLeastSquaresBilin(const ossimLeastSquaresBilin & rhs)34 ossimLeastSquaresBilin::ossimLeastSquaresBilin(const ossimLeastSquaresBilin &rhs)
35 {
36 // allocate normal system accumulation matrices
37 AtA = new NEWMAT::Matrix(4,4);
38 Atb = new NEWMAT::Matrix(4,1);
39
40 bl_a = rhs.bl_a;
41 bl_b = rhs.bl_b;
42 bl_c = rhs.bl_c;
43 bl_d = rhs.bl_d;
44
45 *AtA = *rhs.AtA;
46 *Atb = *rhs.Atb;
47 }
48
~ossimLeastSquaresBilin()49 ossimLeastSquaresBilin::~ossimLeastSquaresBilin()
50 {
51 if(AtA)
52 {
53 delete AtA;
54 AtA = NULL;
55 }
56 if(Atb)
57 {
58 delete Atb;
59 Atb = NULL;
60 }
61 }
operator =(const ossimLeastSquaresBilin & rhs)62 ossimLeastSquaresBilin & ossimLeastSquaresBilin::operator = (const ossimLeastSquaresBilin &rhs)
63 {
64 bl_a = rhs.bl_a;
65 bl_b = rhs.bl_b;
66 bl_c = rhs.bl_c;
67 bl_d = rhs.bl_d;
68
69 *AtA = *rhs.AtA;
70 *Atb = *rhs.Atb;
71
72 return *this;
73 }
74
clear()75 void ossimLeastSquaresBilin::clear()
76 {
77 *AtA = 0.0;
78 *Atb = 0.0;
79 bl_a = 0.0;
80 bl_b = 0.0;
81 bl_c = 0.0;
82 bl_d = 0.0;
83 }
84
addSample(double xx,double yy,double zmea)85 void ossimLeastSquaresBilin::addSample(double xx, double yy, double zmea)
86 {
87 // form normal system layer
88 NEWMAT::Matrix AtA_layer(4,1);
89 AtA_layer(1,1) = 1.0;
90 AtA_layer(2,1) = xx;
91 AtA_layer(3,1) = yy;
92 AtA_layer(4,1) = xx*yy;
93
94 // accumulate layer into normal system
95 *AtA += AtA_layer * AtA_layer.t();
96 *Atb += AtA_layer * zmea;
97
98 }
99
solveLS()100 bool ossimLeastSquaresBilin::solveLS()
101 {
102 NEWMAT::Matrix Soln(4,1);
103 Soln = AtA->i() * (*Atb);
104 bl_a = Soln(1,1);
105 bl_b = Soln(2,1);
106 bl_c = Soln(3,1);
107 bl_d = Soln(4,1);
108
109
110 return true;
111 }
112
getLSParms(double & pa,double & pb_x,double & pc_y,double & pd_xy) const113 bool ossimLeastSquaresBilin::getLSParms(double& pa,
114 double& pb_x,
115 double& pc_y,
116 double& pd_xy)const
117 {
118 pa = bl_a;
119 pb_x = bl_b;
120 pc_y = bl_c;
121 pd_xy = bl_d;
122
123 return true;
124 }
125
setLSParams(double pa,double pb_x,double pc_y,double pd_xy)126 void ossimLeastSquaresBilin::setLSParams(double pa,
127 double pb_x,
128 double pc_y,
129 double pd_xy)
130 {
131 bl_a = pa;
132 bl_b = pb_x;
133 bl_c = pc_y;
134 bl_d = pd_xy;
135 }
136
137
138