Main Page Namespace List Class Hierarchy Compound List File List Namespace Members Compound Members File Members Related Pages

# ApproxPoly.cpp

Go to the documentation of this file.

    00001
    00035 #include "Debug.h"
    00036 #include "ApproxPoly.h"
    00037 #include "HelperFunctions.h"
    00038
    00039 #define GROW_FACT 10
    00040 #define PI 3.14159265
    00041
    00042 ApproxPoly::ApproxPoly(double dMinError, double dMinSlope, double dMaxSegments)
    00043 {
    00044         m_dMinError = dMinError;
    00045         m_dMinSlope = dMinSlope;
    00046         m_dMaxSegments = dMaxSegments;
    00047         m_bDbgMode = false;
    00048 }
    00049
    00050 double ApproxPoly::LeastSquares(const POINT* vertices, int fromIdx, int toIdx, double& m, double& b) const
    00051 {
    00052         return LeastSquares(vertices + fromIdx, toIdx - fromIdx + 1, m, b);
    00053 }
    00054
    00063 double ApproxPoly::LeastSquares(const POINT* vertices, int n, double& m, double& b) const
    00064 {
    00065         int i;
    00066         double x, y, sumx = 0, sumy = 0, sumx2 = 0, sumxy = 0, sum2x = 0;
    00067
    00068         ASSERT(n > 1);
    00069
    00070         for (i = 0; i < n; i++)
    00071         {
    00072                 x = vertices[i].x;
    00073                 y = vertices[i].y;
    00074
    00075                 sumx += x;
    00076                 sumy += y;
    00077                 sumx2 += x * x;
    00078                 sumxy += x * y;
    00079         }
    00080
    00081         sum2x = sumx * sumx;
    00082
    00083         m = (n * sumxy - sumx * sumy) / (n * sumx2 - sum2x);
    00084         b = (sumy - m * sumx) / n; //b = (sumx2 * sumy - sumxy * sumx) / double(n * sumx2 - sum2x);
    00085
    00086         ASSERT_VALID_NUM(m);
    00087         ASSERT_VALID_NUM(b);
    00088
    00089         double e, sume2 = 0;
    00090
    00091         for(i = 0; i < n; i++)
    00092         {
    00093                 e = m * vertices[i].x + b - vertices[i].y;
    00094                 sume2 += e * e;
    00095         }
    00096
    00097         return sume2;
    00098 }
    00099
    00100 bool ApproxPoly::IsRelativeSmall(double x0, double xn, double m, double b)
    00101 {
    00102         double mx, dx, r;
    00103
    00104         mx = (x0 + xn) / 2;
    00105         dx = fabs(x0 - xn);
    00106
    00107         r = fabs(m * dx) / (m * mx + b); // dy / mean y
    00108
    00109         ASSERT(r >= 0.0);
    00110
    00111         return r < m_dMinSlope;
    00112 }
    00113
    00114 void ApproxPoly::Fit(const POINTS pts)
    00115 {
    00116         int seg_num, n = pts.GetSize();
    00117         MEMDATA d;
    00118
    00119         ASSERT(n > 1);
    00120
    00121         m_points = pts;
    00122         m_segments.ReSize(n, n);
    00123         m_minerrors.ReSize(n);
    00124         m_knots.Clear();
    00125
    00126         //SetYMinMax(pts, n);
    00127
    00128         for (seg_num = 1; seg_num < n; seg_num++)
    00129         {
    00130                 d = Min(seg_num, 0, n - 1);
    00131
    00132                 if (d.GetMinError() <= m_dMinError || seg_num >= m_dMaxSegments)
    00133                         break;
    00134         }
    00135
    00136         if (seg_num == n)
    00137                 seg_num--;
    00138
    00139         AddKnots(seg_num, 0, n - 1);
    00140
    00141         if (m_bDbgMode)
    00142                 PlotKnots(seg_num);
    00143 }
    00144
    00145 MEMDATA ApproxPoly::Min(int seg_num, int s, int e)
    00146 {
    00147         if (m_minerrors[seg_num].GetSize() == 0)
    00148                 m_minerrors[seg_num].ReSize(m_points.GetSize(), m_points.GetSize());
    00149
    00150         if (m_minerrors[seg_num][s][e].IsEmpty())
    00151         {
    00152                 double minerror = 99999;
    00153                 int minpt;
    00154
    00155                 if (seg_num == 1)
    00156                 {
    00157                         double slope, b;
    00158                         minerror = LeastSquares(m_points, s, e, slope, b);
    00159                         m_segments[s][e].Set(slope, b, m_points[s], m_points[e]);
    00160                         minpt = s;
    00161                 }
    00162                 else
    00163                 {
    00164                         double m;
    00165                         int n_left = seg_num / 2;
    00166                         int n_right = seg_num / 2 + seg_num % 2;
    00167
    00168                         for (int i = s + 1; i <= e - 1 ; i++)
    00169                         {
    00170                                 m = Min(n_left, s, i) + Min(n_right, i, e);
    00171
    00172                                 if (m < minerror)
    00173                                 {
    00174                                         minerror = m;
    00175                                         minpt = i;
    00176                                 }
    00177                         }
    00178                 }
    00179
    00180                 m_minerrors[seg_num][s][e].Set(minerror, minpt);
    00181         }
    00182
    00183         return m_minerrors[seg_num][s][e];
    00184 }
    00185
    00186 int ApproxPoly::AddKnots(int seg_num, int s, int e)
    00187 {
    00188         const MEMDATA& d = m_minerrors[seg_num][s][e];
    00189         int j = d.GetIdx();
    00190
    00191         if (seg_num > 1)
    00192         {
    00193                 int i = AddKnots(seg_num / 2, s, j);
    00194                 int k = AddKnots(seg_num / 2 + seg_num % 2, j, e);
    00195                 return k;
    00196         }
    00197         else if (seg_num == 1)
    00198         {
    00199                 KNOT knot(e, m_segments[s][e], d.GetMinError());
    00200                 SEGMENT& s = knot.seg;
    00201
    00202                 knot.dir = IsRelativeSmall(s.p0.x, s.p1.x, s.m, s.b) ? 0:SIGN(s.m);
    00203                 m_knots.AddTail(knot);
    00204         }
    00205
    00206         return j;
    00207 }
    00208
    00209 void ApproxPoly::PlotKnots(int seg_num)
    00210 {
    00211         static int dataidx = 0;
    00212
    00213         cout << "\n% DATA " << ++dataidx << "...\na = ";
    00214         m_points.Print(cout, "\n");
    00215         cout << ";\n% Fitting data " << dataidx << "...\n\n";
    00216
    00217         MEMDATA d = m_minerrors[seg_num][0][m_points.GetSize() - 1];
    00218
    00219         cout << "\n% For " << seg_num << " lines: \n";
    00220         cout << "plot(a(:,1),a(:,2),'x');\nhold on;\n";
    00221
    00222         for (int j = 0; j < m_knots.GetSize(); j++)
    00223         {
    00224                 const KNOT& k = m_knots[j];
    00225                 const char* c = k.dir == 0 ? "g":(j % 2) ? "b":"r";
    00226
    00227                 cout << "x = " << k.seg.p0.x << ':' << k.seg.p1.x << ";\n";
    00228                 cout << "y = " << k.seg.m << " * x + " << k.seg.b << ";\n";
    00229                 cout << "plot(x,y,'" << c << "');\n";
    00230
    00231                 //if (j < m_knots.GetSize() - 1)
    00232                 //      cout << "disp('Angle: " << CompAcuteAngle(j, j + 1) << "');\n";
    00233         }
    00234
    00235         cout << "title('Num of segments: " << seg_num
    00236                 << ", error: " << d.GetMinError() << ",  min error: " << m_dMinError
    00237                 << ", min slope: " << m_dMinSlope << ", max segs: " << m_dMaxSegments
    00238                 << "');\n";
    00239
    00240         cout << "xlabel('Radius');\n";
    00241         cout << "ylabel('Shock point index');\n";
    00242
    00243         cout << "hold off;\npause;\n\n";
    00244         cout << "% End fitting...\n";
    00245 }
    00246
    00247 void ApproxPoly::SetYMinMax()
    00248 {
    00249         double y;
    00250
    00251         m_ymin = -1;
    00252         m_ymax = -1;
    00253
    00254         for (int i = 0; i < m_points.GetSize(); i++)
    00255         {
    00256                 y = m_points[i].y;
    00257
    00258                 if (m_ymin == -1 || y < m_ymin)
    00259                         m_ymin = y;
    00260
    00261                 if (m_ymax == -1 || y > m_ymax)
    00262                         m_ymax = y;
    00263         }
    00264 }
    00265
    00269 double ApproxPoly::CompAcuteAngle(int seg1, int seg2) const
    00270 {
    00271         ASSERT(seg1 < seg2);
    00272
    00273         const SEGMENT& s1 = m_knots[seg1].seg;
    00274         const SEGMENT& s2 = m_knots[seg2].seg;
    00275
    00276         double s = (s2.p1.x - s1.p0.x) / (m_ymax - m_ymin);
    00277
    00278         cout << "disp('scaling: " << m_ymin << "," << m_ymax << "," << s << "');\n";
    00279
    00280         //find vectors
    00281         POINT v1(s1.p0.x - s1.p1.x, (s1.p0.y - s1.p1.y) * s);
    00282         POINT v2(s2.p1.x - s2.p0.x, (s2.p1.y - s2.p0.y) * s);
    00283
    00284         cout << "disp('v1: " << v1 << " v2: " << v2 << "');\n";
    00285
    00286         // compute angle
    00287         return acos(v1.Dot(v2) / (v1.L2() * v2.L2())) * (180 / PI);
    00288
    00289         /*const SEGMENT& s1 = m_knots[seg1].seg;
    00290         const SEGMENT& s2 = m_knots[seg2].seg;
    00291
    00292         double d = (s2.m > s1.m) ? s2.m - s1.m:s1.m - s2.m;
    00293
    00294         return atan(d / (1 + s1.m * s2.m)) * (180 / PI);*/
    00295 }
    00296
    00300 double ApproxPoly::CompObtuseAngle(int seg1, int seg2) const
    00301 {
    00302         return 180 - CompAcuteAngle(seg1, seg2);
    00303 }
    00304
    00305 /*
    00306 1.517,  0.187
    00307 1.200,  2.923
    00308 1.353,  7.465
    00309 3.444,  9.187
    00310 6.017,  9.933
    00311 1.464,  14.00
    00312 7.320,  16.26
    00313 9.478,  17.15
    00314 7.472,  22.14
    00315 13.48,  23.18
    00316 */
    00317 void ApproxPoly::Test()
    00318 {
    00319         static POINT testData[] =
    00320         {
    00321                 POINT(1.517,  0.187),
    00322                 POINT(1.200,  2.923),
    00323                 POINT(1.353,  7.465),
    00324                 POINT(3.444,  9.187),
    00325                 POINT(6.017,  9.933),
    00326                 POINT(1.464,  14.00),
    00327                 POINT(7.320,  16.26),
    00328                 POINT(9.478,  17.15),
    00329                 POINT(7.472,  22.14),
    00330                 POINT(13.48,  23.18)
    00331         };
    00332
    00333         int n = sizeof(testData) / sizeof(*testData);
    00334
    00335         for (int i = 0; i < n; i++)
    00336
    00337                 cout << endl << testData[i];
    00338
    00339         cout << endl;
    00340
    00341         double e, m, b;
    00342         e = LeastSquares(testData, n, m, b);
    00343
    00344         cerr << "\nError: " << e << ", slope: " << m << "b: " << b << endl;
    00345
    00346         cerr << "\nShould be: e: 0.68210106, line: y = 4.273150085299648 + 1.5109204502228364x\n";
    00347 }
    00348
    00350 // ModelFit
    00351
    00352 double ModelFit::Fit(const POINTS& vertices, const SEGMENTS& segs)
    00353 {
    00354         ASSERT(segs.GetSize() >= 1);
    00355         ASSERT(vertices.GetSize() > 1);
    00356
    00357         int i;
    00358         m_minLineLen.ReSize(segs.GetSize());
    00359         m_minDataLen.ReSize(vertices.GetSize());
    00360
    00361         // See of the data can possible be fit with the model given the constraints (m_minLenCoeff)
    00362         m_minDataLen[0] = 0;
    00363
    00364         for (i = 1; i < m_minDataLen.GetSize(); i++)
    00365                 m_minDataLen[i] = m_minDataLen[i - 1] + vertices[i].L2(vertices[i - 1]);
    00366
    00367         m_minLineLen[0] = segs[0].GetLen() / m_minLenCoeff;
    00368
    00369         for (i = 1; i < m_minLineLen.GetSize(); i++)
    00370                 m_minLineLen[i] = m_minLineLen[i - 1] + (segs[i].GetLen() / m_minLenCoeff);
    00371
    00372         m_totalDataLen = m_minDataLen.GetTail();
    00373         m_totalMinLineLen = m_minLineLen.GetTail();
    00374         m_totalMaxLineLen = m_totalMinLineLen * m_minLenCoeff * m_minLenCoeff;
    00375
    00376         if (m_totalDataLen < m_totalMinLineLen || m_totalDataLen > m_totalMaxLineLen)
    00377         {
    00378                 m_points = vertices; // for display purposes. see plot.
    00379                 return INFINITY;
    00380         }
    00381
    00382         // There is enought room, so we have work to do
    00383         int dim = segs.GetSize() + 1; //(int)ceil(segs.GetSize() / 2.0);
    00384         m_points = vertices;
    00385         m_segments = segs;
    00386         m_minerrors.ReSize(dim, dim);
    00387
    00388         MEMDATA d = Min(0, segs.GetSize(), 0, m_points.GetSize() - 1);
    00389
    00390         // Modify segments
    00391         UpdateSegments(0, segs.GetSize(), 0, m_points.GetSize() - 1);
    00392
    00393         return d.GetMinError();
    00394 }
    00395
    00396 void ModelFit::UpdateSegments(int ls, int le, int ps, int pe)
    00397 {
    00398         if (le - ls == 1)
    00399         {
    00400                 m_segments[ls].p0 = m_points[ps];
    00401                 m_segments[ls].p1 = m_points[pe];
    00402         }
    00403         else
    00404         {
    00405                 const MEMDATA& d = m_minerrors[ls][le][ps][pe];
    00406                 ASSERT(!d.IsEmpty());
    00407
    00408                 int li = (int)ceil(le / 2.0);//(int)floor(le / 2.0);
    00409
    00410                 UpdateSegments(ls, li, ps, d.GetIdx());
    00411                 UpdateSegments(li, le, d.GetIdx(), pe);
    00412         }
    00413 }
    00414
    00418 MINMAX ModelFit::GetMinMaxLen(int ls, int li, int le, int ps, int pe) const
    00419 {
    00420         double minSegLen, d;
    00421         int i, j;
    00422
    00423         if (!(li > ls && li < le))
    00424         {
    00425                 DBG_MSG("\nThis should not happen\n")
    00426                 DBG_SHOW(ls)
    00427                 DBG_SHOW(li)
    00428                 DBG_SHOW(le)
    00429         }
    00430         ASSERT(li > ls && li < le);
    00431
    00432         d = (ls == 0) ? 0:m_minLineLen[ls - 1];
    00433         minSegLen = m_minLineLen[li - 1] - d;
    00434
    00435         for (i = ps + 1; i < pe && (m_minDataLen[i] - m_minDataLen[ps]) < minSegLen; i++);
    00436
    00437         d = (li == 0) ? 0:m_minLineLen[li - 1];
    00438         minSegLen = m_minLineLen[le - 1] - d;
    00439
    00440         for (j = pe - 1; j > ps && j > i && (m_minDataLen[pe] - m_minDataLen[j]) < minSegLen; j--);
    00441
    00442         //ASSERT(i <= j);
    00443         if (i > j)
    00444         {
    00445                 DBG_SHOW(ps)
    00446                 DBG_SHOW(pe)
    00447                 DBG_SHOW(m_totalDataLen)
    00448                 DBG_SHOW(m_totalMinLineLen)
    00449                 DBG_SHOW(m_totalMaxLineLen)
    00450                 DBG_SHOW(i)
    00451                 DBG_SHOW(j)
    00452         }
    00453
    00454         return MINMAX(i, j);
    00455 }
    00456
    00457 MEMDATA ModelFit::Min(int ls, int le, int ps, int pe)
    00458 {
    00459         if (m_minerrors[ls][le].GetSize() == 0)
    00460         {
    00461                 int dim = m_points.GetSize(); //(int)ceil(m_points.GetSize() / 2.0);
    00462                 m_minerrors[ls][le].ReSize(dim, dim);
    00463         }
    00464
    00465         MEMDATA& d = m_minerrors[ls][le][ps][pe];
    00466
    00467         if (d.IsEmpty())
    00468         {
    00469                 if(le - ls == 1)
    00470                         d.Set(CompLSError(ls, ps, pe), pe);
    00471                 else
    00472                 {
    00473                         double m, minerror = INFINITY;
    00474                         int i = pe; //In case it never enters the for loop
    00475                         int li = (int)ceil(le / 2.0); //(int)floor(le / 2.0);
    00476
    00477                         MINMAX mm = GetMinMaxLen(ls, li, le, ps, pe);
    00478
    00479                         for (int pi = mm.min; pi <= mm.max; pi++)
    00480                         {
    00481                                 m = Min(ls, li, ps, pi) + Min(li, le, pi, pe);
    00482
    00483                                 if (m < minerror)
    00484                                 {
    00485                                         minerror = m;
    00486                                         i = pi;
    00487                                 }
    00488                         }
    00489
    00490                         d.Set(minerror, i);
    00491                 }
    00492         }
    00493
    00494         return d;
    00495 }
    00496
    00497 double ModelFit::CompLSError(int ls, int ps, int pe) const
    00498 {
    00499         const SEGMENT& s = m_segments[ls];
    00500         double e = 0;
    00501
    00502         for (int i = ps; i <= pe; i++)
    00503                 e += fabs(m_points[i].y - (s.m * m_points[i].x + s.b));
    00504
    00505         return e;
    00506 }
    00507
    00508 void ModelFit::GetSegmentError(int s, double* error, double* scaleFactor) const
    00509 {
    00510         /*const MEMDATA& d = m_minerrors[s][s + 1][m_segments[s].p0][m_segments[s].p1];
    00511         ASSERT(!d.IsEmpty());
    00512
    00513         *error = d.GetMinError();
    00514
    00515         double oldLen = (s == 0) ? m_minLineLen[s]:m_minLineLen[s] - m_minLineLen[s - 1];
    00516         *scaleFactor = m_segments[s].GetLen() / (oldLen * m_minLenCoeff);*/
    00517 }
    00518
    00519 void ModelFit::Plot(ostream& os, const POINTS& points2, const SEGMENTS& segments2) const
    00520 {
    00521         if (m_points.GetSize() > 0)
    00522         {
    00523                 os << "\n% DATA 1...\na = ";
    00524                 m_points.Print(os, "\n");
    00525                 os << ";\nplot(a(:,1),a(:,2),'xb');\nhold on;\n";
    00526         }
    00527
    00528         if (points2.GetSize() > 0)
    00529         {
    00530                 os << "\n% DATA 2...\nb = ";
    00531                 points2.Print(os, "\n");
    00532                 os << ";\nplot(b(:,1),b(:,2),'or');\n";
    00533
    00534                 if (m_points.GetSize() == 0)
    00535                         os << "hold on;\n";
    00536         }
    00537
    00538         int j;
    00539
    00540         // Original model
    00541         for (j = 0; j < segments2.GetSize(); j++)
    00542         {
    00543                 const SEGMENT& seg = segments2[j];
    00544                 const char* c = "k";
    00545
    00546                 os << "x = " << seg.p0.x << ':' << seg.p1.x << ";\n";
    00547                 os << "y = " << seg.m << " * x + " << seg.b << ";\n";
    00548                 os << "plot(x,y,'" << c << "');\n";
    00549         }
    00550
    00551         // Model with new params
    00552         for (j = 0; j < m_segments.GetSize(); j++)
    00553         {
    00554                 const SEGMENT& seg = m_segments[j];
    00555                 const char* c = (j % 2) ? "b":"r";
    00556
    00557                 os << "x = " << seg.p0.x << ':' << seg.p1.x << ";\n";
    00558                 os << "y = " << seg.m << " * x + " << seg.b << ";\n";
    00559                 os << "plot(x,y,'" << c << "', 'linewidth', 2);\n";
    00560         }
    00561
    00562         double minerror = INFINITY;
    00563
    00564         if (m_segments.GetSize() > 0 && m_points.GetSize() > 0)
    00565                 minerror = m_minerrors[0][m_segments.GetSize()][0][m_points.GetSize() - 1].GetMinError();
    00566
    00567         os << "title('Num of segments: " << m_segments.GetSize() << ", min lines len: " << m_totalMinLineLen;
    00568         os << ", data len: " << m_totalDataLen  << ", error: " << minerror << ",  min len coeff: " << m_minLenCoeff << "');\n";
    00569         os << "hold off;\npause;\n\n";
    00570         os << "% End fitting...\n";
    00571 }
    00572
    00573