---

ApproxPoly.cpp

Go to the documentation of this file.

#include "Debug.h"
#include "ApproxPoly.h"
#include "HelperFunctions.h"

#define GROW_FACT 10
#define PI 3.14159265

ApproxPoly::ApproxPoly(double dMinError, double dMinSlope, double dMaxSegments)
{
        m_dMinError = dMinError;
        m_dMinSlope = dMinSlope;
        m_dMaxSegments = dMaxSegments;
        m_bDbgMode = false;
}

double ApproxPoly::LeastSquares(const POINT* vertices, int fromIdx, int toIdx, double& m, double& b) const
{
        return LeastSquares(vertices + fromIdx, toIdx - fromIdx + 1, m, b);
}

double ApproxPoly::LeastSquares(const POINT* vertices, int n, double& m, double& b) const
{
        int i;
        double x, y, sumx = 0, sumy = 0, sumx2 = 0, sumxy = 0, sum2x = 0;

        ASSERT(n > 1);
        
        for (i = 0; i < n; i++)
        {
                x = vertices[i].x;
                y = vertices[i].y;
                
                sumx += x;
                sumy += y;
                sumx2 += x * x;
                sumxy += x * y;
        }
        
        sum2x = sumx * sumx;
        
        m = (n * sumxy - sumx * sumy) / (n * sumx2 - sum2x);
        b = (sumy - m * sumx) / n; //b = (sumx2 * sumy - sumxy * sumx) / double(n * sumx2 - sum2x);

        ASSERT_VALID_NUM(m);
        ASSERT_VALID_NUM(b);
        
        double e, sume2 = 0;
        
        for(i = 0; i < n; i++)
        {
                e = m * vertices[i].x + b - vertices[i].y;
                sume2 += e * e;
        }
        
        return sume2;
}

bool ApproxPoly::IsRelativeSmall(double x0, double xn, double m, double b)
{
        double mx, dx, r;
        
        mx = (x0 + xn) / 2;
        dx = fabs(x0 - xn);
                
        r = fabs(m * dx) / (m * mx + b); // dy / mean y
        
        ASSERT(r >= 0.0);
                
        return r < m_dMinSlope;
}

void ApproxPoly::Fit(const POINTS pts)
{
        int seg_num, n = pts.GetSize();
        MEMDATA d;
        
        ASSERT(n > 1);
        
        m_points = pts;
        m_segments.ReSize(n, n);
        m_minerrors.ReSize(n);
        m_knots.Clear();
        
        //SetYMinMax(pts, n);
        
        for (seg_num = 1; seg_num < n; seg_num++)
        {
                d = Min(seg_num, 0, n - 1);
                
                if (d.GetMinError() <= m_dMinError || seg_num >= m_dMaxSegments)
                        break;
        }
        
        if (seg_num == n)
                seg_num--;
                
        AddKnots(seg_num, 0, n - 1);
        
        if (m_bDbgMode)
                PlotKnots(seg_num);
}

MEMDATA ApproxPoly::Min(int seg_num, int s, int e)
{       
        if (m_minerrors[seg_num].GetSize() == 0)
                m_minerrors[seg_num].ReSize(m_points.GetSize(), m_points.GetSize());
                
        if (m_minerrors[seg_num][s][e].IsEmpty())
        {
                double minerror = 99999;
                int minpt;
                
                if (seg_num == 1)
                {
                        double slope, b;
                        minerror = LeastSquares(m_points, s, e, slope, b);
                        m_segments[s][e].Set(slope, b, m_points[s], m_points[e]);
                        minpt = s;
                }
                else
                {       
                        double m;
                        int n_left = seg_num / 2;
                        int n_right = seg_num / 2 + seg_num % 2;
                
                        for (int i = s + 1; i <= e - 1 ; i++)
                        {
                                m = Min(n_left, s, i) + Min(n_right, i, e);
                        
                                if (m < minerror)
                                {
                                        minerror = m;
                                        minpt = i;
                                }
                        }
                }
                
                m_minerrors[seg_num][s][e].Set(minerror, minpt);
        }
        
        return m_minerrors[seg_num][s][e];
}

int ApproxPoly::AddKnots(int seg_num, int s, int e)
{
        const MEMDATA& d = m_minerrors[seg_num][s][e];
        int j = d.GetIdx();
        
        if (seg_num > 1)
        {
                int i = AddKnots(seg_num / 2, s, j);
                int k = AddKnots(seg_num / 2 + seg_num % 2, j, e);
                return k;
        }
        else if (seg_num == 1)
        {
                KNOT knot(e, m_segments[s][e], d.GetMinError());
                SEGMENT& s = knot.seg;
                
                knot.dir = IsRelativeSmall(s.p0.x, s.p1.x, s.m, s.b) ? 0:SIGN(s.m);
                m_knots.AddTail(knot);
        }
        
        return j;
}

void ApproxPoly::PlotKnots(int seg_num)
{
        static int dataidx = 0; 
                
        cout << "\n% DATA " << ++dataidx << "...\na = ";
        m_points.Print(cout, "\n");
        cout << ";\n% Fitting data " << dataidx << "...\n\n";
        
        MEMDATA d = m_minerrors[seg_num][0][m_points.GetSize() - 1];
        
        cout << "\n% For " << seg_num << " lines: \n";
        cout << "plot(a(:,1),a(:,2),'x');\nhold on;\n";
                
        for (int j = 0; j < m_knots.GetSize(); j++)
        {
                const KNOT& k = m_knots[j];
                const char* c = k.dir == 0 ? "g":(j % 2) ? "b":"r";
                        
                cout << "x = " << k.seg.p0.x << ':' << k.seg.p1.x << ";\n";
                cout << "y = " << k.seg.m << " * x + " << k.seg.b << ";\n";
                cout << "plot(x,y,'" << c << "');\n";
                
                //if (j < m_knots.GetSize() - 1)
                //      cout << "disp('Angle: " << CompAcuteAngle(j, j + 1) << "');\n";                 
        }
        
        cout << "title('Num of segments: " << seg_num
                << ", error: " << d.GetMinError() << ",  min error: " << m_dMinError
                << ", min slope: " << m_dMinSlope << ", max segs: " << m_dMaxSegments
                << "');\n";

        cout << "xlabel('Radius');\n";
        cout << "ylabel('Shock point index');\n";
                                
        cout << "hold off;\npause;\n\n";
        cout << "% End fitting...\n";
}

void ApproxPoly::SetYMinMax()
{
        double y;
        
        m_ymin = -1;
        m_ymax = -1;
        
        for (int i = 0; i < m_points.GetSize(); i++)
        {
                y = m_points[i].y;
                
                if (m_ymin == -1 || y < m_ymin)
                        m_ymin = y;
                        
                if (m_ymax == -1 || y > m_ymax)
                        m_ymax = y;
        }
}

double ApproxPoly::CompAcuteAngle(int seg1, int seg2) const
{
        ASSERT(seg1 < seg2);
        
        const SEGMENT& s1 = m_knots[seg1].seg;
        const SEGMENT& s2 = m_knots[seg2].seg;
        
        double s = (s2.p1.x - s1.p0.x) / (m_ymax - m_ymin);
        
        cout << "disp('scaling: " << m_ymin << "," << m_ymax << "," << s << "');\n";    
        
        //find vectors
        POINT v1(s1.p0.x - s1.p1.x, (s1.p0.y - s1.p1.y) * s);
        POINT v2(s2.p1.x - s2.p0.x, (s2.p1.y - s2.p0.y) * s);
        
        cout << "disp('v1: " << v1 << " v2: " << v2 << "');\n"; 
        
        // compute angle
        return acos(v1.Dot(v2) / (v1.L2() * v2.L2())) * (180 / PI);
        
        /*const SEGMENT& s1 = m_knots[seg1].seg;
        const SEGMENT& s2 = m_knots[seg2].seg;
                
        double d = (s2.m > s1.m) ? s2.m - s1.m:s1.m - s2.m;
        
        return atan(d / (1 + s1.m * s2.m)) * (180 / PI);*/
}

double ApproxPoly::CompObtuseAngle(int seg1, int seg2) const
{
        return 180 - CompAcuteAngle(seg1, seg2);
}

/*
1.517,  0.187
1.200,  2.923
1.353,  7.465
3.444,  9.187
6.017,  9.933
1.464,  14.00
7.320,  16.26
9.478,  17.15
7.472,  22.14
13.48,  23.18
*/
void ApproxPoly::Test()
{
        static POINT testData[] =
        {
                POINT(1.517,  0.187),
                POINT(1.200,  2.923),
                POINT(1.353,  7.465),
                POINT(3.444,  9.187),
                POINT(6.017,  9.933),
                POINT(1.464,  14.00),
                POINT(7.320,  16.26),
                POINT(9.478,  17.15),
                POINT(7.472,  22.14),
                POINT(13.48,  23.18)
        };

        int n = sizeof(testData) / sizeof(*testData);
        
        for (int i = 0; i < n; i++)

                cout << endl << testData[i];
        
        cout << endl;

        double e, m, b;
        e = LeastSquares(testData, n, m, b);

        cerr << "\nError: " << e << ", slope: " << m << "b: " << b << endl;

        cerr << "\nShould be: e: 0.68210106, line: y = 4.273150085299648 + 1.5109204502228364x\n";
}

// ModelFit

double ModelFit::Fit(const POINTS& vertices, const SEGMENTS& segs)
{
        ASSERT(segs.GetSize() >= 1);
        ASSERT(vertices.GetSize() > 1);
        
        int i;          
        m_minLineLen.ReSize(segs.GetSize());
        m_minDataLen.ReSize(vertices.GetSize());
        
        // See of the data can possible be fit with the model given the constraints (m_minLenCoeff)
        m_minDataLen[0] = 0;
        
        for (i = 1; i < m_minDataLen.GetSize(); i++)
                m_minDataLen[i] = m_minDataLen[i - 1] + vertices[i].L2(vertices[i - 1]);
                
        m_minLineLen[0] = segs[0].GetLen() / m_minLenCoeff;
        
        for (i = 1; i < m_minLineLen.GetSize(); i++)
                m_minLineLen[i] = m_minLineLen[i - 1] + (segs[i].GetLen() / m_minLenCoeff);

        m_totalDataLen = m_minDataLen.GetTail();
        m_totalMinLineLen = m_minLineLen.GetTail();
        m_totalMaxLineLen = m_totalMinLineLen * m_minLenCoeff * m_minLenCoeff;
        
        if (m_totalDataLen < m_totalMinLineLen || m_totalDataLen > m_totalMaxLineLen)
        {
                m_points = vertices; // for display purposes. see plot.
                return INFINITY;
        }
                
        // There is enought room, so we have work to do
        int dim = segs.GetSize() + 1; //(int)ceil(segs.GetSize() / 2.0);
        m_points = vertices;
        m_segments = segs;
        m_minerrors.ReSize(dim, dim);   

        MEMDATA d = Min(0, segs.GetSize(), 0, m_points.GetSize() - 1);
        
        // Modify segments
        UpdateSegments(0, segs.GetSize(), 0, m_points.GetSize() - 1);
        
        return d.GetMinError();
}

void ModelFit::UpdateSegments(int ls, int le, int ps, int pe)
{
        if (le - ls == 1)
        {
                m_segments[ls].p0 = m_points[ps];
                m_segments[ls].p1 = m_points[pe];
        }
        else
        {
                const MEMDATA& d = m_minerrors[ls][le][ps][pe];
                ASSERT(!d.IsEmpty());
                
                int li = (int)ceil(le / 2.0);//(int)floor(le / 2.0);
                
                UpdateSegments(ls, li, ps, d.GetIdx());
                UpdateSegments(li, le, d.GetIdx(), pe);
        }
}

MINMAX ModelFit::GetMinMaxLen(int ls, int li, int le, int ps, int pe) const
{
        double minSegLen, d;
        int i, j;
        
        if (!(li > ls && li < le))
        {
                DBG_MSG("\nThis should not happen\n")
                DBG_SHOW(ls)
                DBG_SHOW(li)
                DBG_SHOW(le)
        }
        ASSERT(li > ls && li < le);
        
        d = (ls == 0) ? 0:m_minLineLen[ls - 1];
        minSegLen = m_minLineLen[li - 1] - d;
        
        for (i = ps + 1; i < pe && (m_minDataLen[i] - m_minDataLen[ps]) < minSegLen; i++);                      

        d = (li == 0) ? 0:m_minLineLen[li - 1]; 
        minSegLen = m_minLineLen[le - 1] - d;
        
        for (j = pe - 1; j > ps && j > i && (m_minDataLen[pe] - m_minDataLen[j]) < minSegLen; j--);                     
                
        //ASSERT(i <= j);
        if (i > j)
        {
                DBG_SHOW(ps)
                DBG_SHOW(pe)
                DBG_SHOW(m_totalDataLen)
                DBG_SHOW(m_totalMinLineLen)
                DBG_SHOW(m_totalMaxLineLen)
                DBG_SHOW(i)
                DBG_SHOW(j)
        }
        
        return MINMAX(i, j);
}

MEMDATA ModelFit::Min(int ls, int le, int ps, int pe)
{
        if (m_minerrors[ls][le].GetSize() == 0)
        {
                int dim = m_points.GetSize(); //(int)ceil(m_points.GetSize() / 2.0);
                m_minerrors[ls][le].ReSize(dim, dim);
        }
        
        MEMDATA& d = m_minerrors[ls][le][ps][pe];
                
        if (d.IsEmpty())
        {
                if(le - ls == 1)
                        d.Set(CompLSError(ls, ps, pe), pe);
                else
                {
                        double m, minerror = INFINITY;
                        int i = pe; //In case it never enters the for loop
                        int li = (int)ceil(le / 2.0); //(int)floor(le / 2.0);
                        
                        MINMAX mm = GetMinMaxLen(ls, li, le, ps, pe);
                        
                        for (int pi = mm.min; pi <= mm.max; pi++)
                        {
                                m = Min(ls, li, ps, pi) + Min(li, le, pi, pe);
                                
                                if (m < minerror)
                                {
                                        minerror = m;
                                        i = pi;
                                }
                        }
                        
                        d.Set(minerror, i);
                }
        }
        
        return d;
}

double ModelFit::CompLSError(int ls, int ps, int pe) const
{
        const SEGMENT& s = m_segments[ls];
        double e = 0;
        
        for (int i = ps; i <= pe; i++)
                e += fabs(m_points[i].y - (s.m * m_points[i].x + s.b));
                
        return e;
}

void ModelFit::GetSegmentError(int s, double* error, double* scaleFactor) const
{
        /*const MEMDATA& d = m_minerrors[s][s + 1][m_segments[s].p0][m_segments[s].p1];
        ASSERT(!d.IsEmpty());
                
        *error = d.GetMinError();
        
        double oldLen = (s == 0) ? m_minLineLen[s]:m_minLineLen[s] - m_minLineLen[s - 1];
        *scaleFactor = m_segments[s].GetLen() / (oldLen * m_minLenCoeff);*/
}

void ModelFit::Plot(ostream& os, const POINTS& points2, const SEGMENTS& segments2) const
{       
        if (m_points.GetSize() > 0)
        {
                os << "\n% DATA 1...\na = ";
                m_points.Print(os, "\n");
                os << ";\nplot(a(:,1),a(:,2),'xb');\nhold on;\n";
        }

        if (points2.GetSize() > 0)
        {
                os << "\n% DATA 2...\nb = ";
                points2.Print(os, "\n");
                os << ";\nplot(b(:,1),b(:,2),'or');\n";
                
                if (m_points.GetSize() == 0)
                        os << "hold on;\n";
        }
        
        int j;
        
        // Original model
        for (j = 0; j < segments2.GetSize(); j++)
        {
                const SEGMENT& seg = segments2[j];
                const char* c = "k";
                        
                os << "x = " << seg.p0.x << ':' << seg.p1.x << ";\n";
                os << "y = " << seg.m << " * x + " << seg.b << ";\n";
                os << "plot(x,y,'" << c << "');\n";
        }
                
        // Model with new params
        for (j = 0; j < m_segments.GetSize(); j++)
        {
                const SEGMENT& seg = m_segments[j];
                const char* c = (j % 2) ? "b":"r";
                        
                os << "x = " << seg.p0.x << ':' << seg.p1.x << ";\n";
                os << "y = " << seg.m << " * x + " << seg.b << ";\n";
                os << "plot(x,y,'" << c << "', 'linewidth', 2);\n";
        }

        double minerror = INFINITY;
        
        if (m_segments.GetSize() > 0 && m_points.GetSize() > 0)
                minerror = m_minerrors[0][m_segments.GetSize()][0][m_points.GetSize() - 1].GetMinError();
        
        os << "title('Num of segments: " << m_segments.GetSize() << ", min lines len: " << m_totalMinLineLen;
        os << ", data len: " << m_totalDataLen  << ", error: " << minerror << ",  min len coeff: " << m_minLenCoeff << "');\n";                 
        os << "hold off;\npause;\n\n";
        os << "% End fitting...\n";
}

---