#include "StdAfx.h"
|
#include "CHDataFitting.h"
|
|
#include <algorithm>
|
#include "OpenCV/cxcore.h"
|
|
CCHDataFitting::CCHDataFitting(void)
|
{
|
|
}
|
|
CCHDataFitting::~CCHDataFitting(void)
|
{
|
|
}
|
|
inline bool CompareValue(double& a, double& b)
|
{
|
return (a < b);
|
}
|
|
inline double Median(double *pValue, int nSize)
|
{
|
int odd = nSize % 2;
|
int index = nSize / 2;
|
|
// sort
|
double dTemp = 0.0;
|
for(int i=0; i<index+1; i++)
|
{
|
for(int j=i+1; j<nSize; j++)
|
{
|
if(pValue[i]>pValue[j])
|
{
|
dTemp=pValue[i];
|
pValue[i]=pValue[j];
|
pValue[j]=dTemp;
|
}
|
}
|
}
|
|
if (odd==1)
|
{
|
return pValue[index];
|
}
|
|
return ((pValue[index-1]+pValue[index])/2.0);
|
}
|
|
inline double MedianAbsoluteDeviation(double *pValue, int nSize, double dMedian)
|
{
|
for (int i=0; i<nSize; i++)
|
{
|
pValue[i] = fabs(pValue[i]-dMedian);
|
}
|
|
return Median(pValue, nSize);
|
}
|
|
inline double Median(VectorDouble &x)
|
{
|
std::sort(x.begin(), x.end(), CompareValue);
|
|
size_t odd = x.size() % 2;
|
size_t index = x.size() / 2;
|
|
if (odd==1)
|
{
|
return x[index];
|
}
|
|
return ((x[index-1]+x[index])/2.0);
|
}
|
|
inline double MedianAbsoluteDeviation(VectorDouble &x, double medianValue)
|
{
|
VectorDouble mad;
|
for (size_t i=0; i<x.size(); i++)
|
{
|
mad.push_back(fabs(x[i]-medianValue));
|
}
|
|
std::sort(mad.begin(), mad.end(), CompareValue);
|
|
return Median(mad);
|
}
|
|
double CCHDataFitting::OneStepBiweightAlgorithm(double *pResidual, double *pWeight, int nSize, double dWeight, double dEpsilon)
|
{
|
double *pTempValue = new double[nSize];
|
memcpy(pTempValue, pResidual, sizeof(double)*nSize);
|
|
double medianValue = Median(pTempValue, nSize);
|
double MAD = MedianAbsoluteDeviation(pTempValue, nSize, medianValue) * dWeight + dEpsilon;
|
|
double diff, u, uSquare, oneMinusUSqaure;
|
double weightedSumNumer=0.0;
|
double weightedSumDenom=0.0;
|
|
for (int i=0; i<nSize; i++)
|
{
|
diff = pResidual[i] - medianValue;
|
u = diff / MAD;
|
uSquare = u * u;
|
oneMinusUSqaure = 1.0 - uSquare;
|
|
if (fabs(u) <= 1.0)
|
{
|
weightedSumNumer += diff * oneMinusUSqaure * oneMinusUSqaure;
|
weightedSumDenom += oneMinusUSqaure * oneMinusUSqaure;
|
pWeight[i] = oneMinusUSqaure * oneMinusUSqaure;
|
}
|
else
|
{
|
pWeight[i] = 0.0;
|
}
|
}
|
|
double dValue=0.0;
|
if (weightedSumDenom != 0.0)
|
{
|
dValue = medianValue + weightedSumNumer / weightedSumDenom;
|
}
|
|
delete [] pTempValue;
|
|
return dValue;
|
}
|
|
double CCHDataFitting::OneStepBiweightAlgorithm(VectorDouble &vectorResidual, VectorDouble &vectorWeight, double dWeight, double dEpsilon)
|
{
|
VectorDouble tempX = vectorResidual;
|
|
double medianValue = Median(tempX);
|
double MAD = MedianAbsoluteDeviation(tempX, medianValue) * dWeight + dEpsilon;
|
|
double diff, u, uSquare, oneMinusUSqaure;
|
double weightedSumNumer=0.0;
|
double weightedSumDenom=0.0;
|
|
size_t nSize = vectorResidual.size();
|
for (size_t i=0; i<nSize; i++)
|
{
|
diff = vectorResidual[i] - medianValue;
|
u = diff / MAD;
|
uSquare = u * u;
|
oneMinusUSqaure = 1.0 - uSquare;
|
|
if (fabs(u) <= 1.0)
|
{
|
weightedSumNumer += diff * oneMinusUSqaure * oneMinusUSqaure;
|
weightedSumDenom += oneMinusUSqaure * oneMinusUSqaure;
|
|
vectorWeight[i] = oneMinusUSqaure * oneMinusUSqaure;
|
}
|
else
|
{
|
vectorWeight[i] = 0.0;
|
}
|
}
|
|
double dValue=0.0;
|
|
if (weightedSumDenom != 0.0)
|
{
|
dValue = medianValue + weightedSumNumer / weightedSumDenom;
|
}
|
|
return dValue;
|
}
|
|
int CCHDataFitting::SolveMatrix(double *pSrc, double *pSrc2Arg, double *pDst, int nSize, int nMethod)
|
{
|
CvMat src = cvMat( nSize, nSize, CV_64FC1, pSrc );
|
CvMat src2arg = cvMat( nSize, 1, CV_64FC1, pSrc2Arg );
|
CvMat dst = cvMat( nSize, 1, CV_64FC1, pDst );
|
|
return cvSolve(&src, &src2arg, &dst, nMethod);
|
}
|
|
double CCHDataFitting::InvertMatrix(double *pSrc, double*pDst, int nSize, int nMethod)
|
{
|
CvMat src = cvMat( nSize, nSize, CV_64FC1, pSrc );
|
CvMat dst = cvMat( nSize, nSize, CV_64FC1, pDst );
|
|
return cvInvert(&src, &dst, nMethod);
|
}
|
|
int CCHDataFitting::EigenVectorValue(double *pSrcMatrix, double *pEignVector, double *pEignValue, int nSize)
|
{
|
CvMat srcMat = cvMat( nSize, nSize, CV_64FC1, pSrcMatrix);
|
CvMat eigenVector = cvMat( nSize, nSize, CV_64FC1, pEignVector);
|
CvMat eigenValue = cvMat( nSize, 1, CV_64FC1, pEignValue);
|
|
cvEigenVV(&srcMat, &eigenVector, &eigenValue);
|
|
return 1;
|
}
|
|
int CCHDataFitting::MatrixMultiply(double *pSrcA, double *pSrcB, double *pDst, int nRows, int nCols)
|
{
|
CvMat a = cvMat( nRows, nCols, CV_64FC1, pSrcA);
|
CvMat b = cvMat( nCols, nRows, CV_64FC1, pSrcB);
|
CvMat result = cvMat( nRows, nRows, CV_64FC1, pDst);
|
|
cvMatMul(&a, &b, &result);
|
|
return 1;
|
}
|
|
int CCHDataFitting::MatrixTranspose(double *pSource, double *pResult, int nRows, int nCols)
|
{
|
CvMat src = cvMat( nRows, nCols, CV_64FC1, pSource);
|
CvMat result = cvMat( nCols, nRows, CV_64FC1, pResult);
|
|
cvTranspose(&src, &result);
|
|
return 1;
|
}
|
|
|
int CCHDataFitting::PolynomialFitting(VectorDouble& vectorX, VectorDouble& vectorY, VectorDouble& vectorR, int nOrder)
|
{
|
vectorR.clear();
|
|
int nSize = (int)vectorX.size();
|
if (nOrder >= nSize) return 0;
|
|
int nMatrixSize = nOrder + 1;
|
int nMaxOrder = nOrder * 2 + 1;
|
|
double *pX = new double[nMatrixSize * nMatrixSize];
|
double *pA = new double[nMatrixSize];
|
double *pY = new double[nMatrixSize];
|
double *pOrderX = new double[nMaxOrder];
|
|
memset(pX, 0, sizeof(double)*(nMatrixSize*nMatrixSize));
|
memset(pA, 0, sizeof(double)*(nMatrixSize));
|
memset(pY, 0, sizeof(double)*(nMatrixSize));
|
memset(pOrderX, 0, sizeof(double)*(nMaxOrder));
|
|
int nTemp = 1;
|
for (int i=0; i<nSize; i++)
|
{
|
//pOrderX[0] = (pOrderX[0]*i) / nTemp;
|
pOrderX[1] = (pOrderX[1]*i + vectorX[i]) / nTemp;
|
for (int j=2; j<nMaxOrder; j++)
|
{
|
pOrderX[j] = (pOrderX[j]*i + pow(vectorX[i],int(j))) / nTemp;
|
}
|
|
pY[0] = (pY[0]*i + vectorY[i]) / nTemp;
|
pY[1] = (pY[1]*i + vectorX[i]*vectorY[i]) / nTemp;
|
for (int j=2; j<nMatrixSize; j++)
|
{
|
pY[j] = (pY[j]*i + (pow(vectorX[i],int(j))*vectorY[i])) / nTemp;
|
}
|
nTemp++;
|
}
|
pOrderX[0] = 1.0;
|
|
double *pSubX = pX;
|
for (int i=0; i<nMatrixSize; i++)
|
{
|
for (int j=0; j<nMatrixSize; j++)
|
{
|
*pSubX++ = pOrderX[i+j];
|
}
|
}
|
|
if (SolveMatrix(pX, pY, pA, nMatrixSize)!=1)
|
{
|
delete [] pX;
|
delete [] pA;
|
delete [] pY;
|
delete [] pOrderX;
|
|
return 0;
|
}
|
|
for (int i=0; i<nMatrixSize; i++)
|
{
|
vectorR.push_back(pA[i]);
|
}
|
|
delete [] pX;
|
delete [] pA;
|
delete [] pY;
|
delete [] pOrderX;
|
|
return 1;
|
}
|
|
int CCHDataFitting::PolynomialFitting(double *pXValue, double *pYValue, int nSize, double *pResult, int nOrder)
|
{
|
int nMatrixSize = nOrder + 1;
|
int nMaxOrder = nOrder * 2 + 1;
|
|
memset(pResult, 0, sizeof(double)*nMatrixSize);
|
|
if (nOrder >= nSize) return 0;
|
|
double *pX = new double[nMatrixSize * nMatrixSize];
|
double *pA = new double[nMatrixSize];
|
double *pY = new double[nMatrixSize];
|
double *pOrderX = new double[nMaxOrder];
|
|
memset(pX, 0, sizeof(double)*(nMatrixSize*nMatrixSize));
|
memset(pA, 0, sizeof(double)*(nMatrixSize));
|
memset(pY, 0, sizeof(double)*(nMatrixSize));
|
memset(pOrderX, 0, sizeof(double)*(nMaxOrder));
|
|
int nTemp = 1;
|
for (int i=0; i<nSize; i++)
|
{
|
//pOrderX[0] = (pOrderX[0]*i) / nTemp;
|
pOrderX[1] = (pOrderX[1]*i + pXValue[i]) / nTemp;
|
for (int j=2; j<nMaxOrder; j++)
|
{
|
pOrderX[j] = (pOrderX[j]*i + pow(pXValue[i],j)) / nTemp;
|
}
|
|
pY[0] = (pY[0]*i + pYValue[i]) / nTemp;
|
pY[1] = (pY[1]*i + pXValue[i]*pYValue[i]) / nTemp;
|
for (int j=2; j<nMatrixSize; j++)
|
{
|
pY[j] = (pY[j]*i + (pow(pXValue[i],j)*pYValue[i])) / nTemp;
|
}
|
nTemp++;
|
}
|
pOrderX[0] = 1.0;
|
|
double *pSubX = pX;
|
for (int i=0; i<nMatrixSize; i++)
|
{
|
for (int j=0; j<nMatrixSize; j++)
|
{
|
*pSubX++ = pOrderX[i+j];
|
}
|
}
|
|
if (SolveMatrix(pX, pY, pA, nMatrixSize)!=1)
|
{
|
delete [] pX;
|
delete [] pA;
|
delete [] pY;
|
delete [] pOrderX;
|
|
return 0;
|
}
|
|
// copy result
|
memcpy(pResult, pA, sizeof(double)*nMatrixSize);
|
|
delete [] pX;
|
delete [] pA;
|
delete [] pY;
|
delete [] pOrderX;
|
|
return 1;
|
}
|
|
int CCHDataFitting::GaussianFitting(VectorDouble& vectorX, VectorDouble& vectorY, VectorDouble& vectorR)
|
{
|
vectorR.clear();
|
|
size_t nSize = vectorX.size();
|
|
if (nSize < 3) return 0;
|
|
VectorDouble vectorLY;
|
vectorLY.resize(nSize);
|
for (size_t i=0; i<nSize; i++)
|
{
|
vectorLY[i] = log(vectorY[i]);
|
}
|
|
VectorDouble vectorTemp;
|
if (PolynomialFitting(vectorX, vectorLY, vectorTemp, 2)!=1)
|
{
|
return 0;
|
}
|
|
double dVariance = -1.0 * sqrt(fabs(1.0/vectorTemp[2]));
|
double dMean = -1.0 * (vectorTemp[1] / (2.0*vectorTemp[2]));
|
double dHeight = exp(vectorTemp[0] + (dMean*dMean)/(dVariance*dVariance));
|
|
vectorR.push_back(dHeight);
|
vectorR.push_back(dMean);
|
vectorR.push_back(dVariance);
|
|
return 1;
|
}
|
|
int CCHDataFitting::GaussianFitting(double *pXValue, double *pYValue, int nSize, double *pResult)
|
{
|
memset(pResult, 0, sizeof(double)*3);
|
|
if (nSize < 3) return 0;
|
|
double *pYLogValue = new double[nSize];
|
for (int i=0; i<nSize; i++)
|
{
|
pYLogValue[i] = log(pYValue[i]);
|
}
|
|
double *pResultTemp = new double[3];
|
if (PolynomialFitting(pXValue, pYLogValue, nSize, pResultTemp, 2)!=1)
|
{
|
delete [] pYLogValue;
|
delete [] pResultTemp;
|
|
return 0;
|
}
|
|
double dVariance = -1.0 * sqrt(fabs(1.0/pResultTemp[2]));
|
double dMean = -1.0 * (pResultTemp[1] / (2.0*pResultTemp[2]));
|
double dHeight = exp(pResultTemp[0] + (dMean*dMean)/(dVariance*dVariance));
|
|
pResult[0] = dHeight;
|
pResult[1] = dMean;
|
pResult[2] = dVariance;
|
|
delete [] pYLogValue;
|
delete [] pResultTemp;
|
|
return 1;
|
}
|
|
|
int CCHDataFitting::IRLS_PolynomialFitting(VectorDouble& vectorX, VectorDouble& vectorY, VectorDouble& vectorR, int nOrder, int nIteration, double dWeight, double dStopThres)
|
{
|
vectorR.clear();
|
|
int nSize = (int)vectorX.size();
|
if (nOrder >= nSize) return 0;
|
|
int nMatrixSize = nOrder + 1;
|
int nMaxOrder = nOrder * 2 + 1;
|
|
VectorDouble vectorWeight, vectorResidual;
|
vectorWeight.resize(nSize);
|
vectorResidual.resize(nSize);
|
|
for (int i=0; i<nSize; i++)
|
{
|
vectorWeight[i] = 1.0;
|
}
|
|
double *pX = new double[nMatrixSize * nMatrixSize];
|
double *pA = new double[nMatrixSize];
|
double *pY = new double[nMatrixSize];
|
double *pOrderX = new double[nMaxOrder];
|
|
memset(pX, 0, sizeof(double)*(nMatrixSize*nMatrixSize));
|
memset(pA, 0, sizeof(double)*(nMatrixSize));
|
memset(pY, 0, sizeof(double)*(nMatrixSize));
|
memset(pOrderX, 0, sizeof(double)*(nMaxOrder));
|
|
double dCurValue = 0.0;
|
double dPrevValue = 0.0;
|
|
int nReturnIteration = 0;
|
for (nReturnIteration=0; nReturnIteration<nIteration; nReturnIteration++)
|
{
|
int nTemp = 1;
|
for (int i=0; i<nSize; i++)
|
{
|
pOrderX[0] = (pOrderX[0]*i + vectorWeight[i]) / nTemp;
|
pOrderX[1] = (pOrderX[1]*i + vectorWeight[i]*vectorX[i]) / nTemp;
|
for (int j=2; j<nMaxOrder; j++)
|
{
|
pOrderX[j] = (pOrderX[j]*i + vectorWeight[i]*pow(vectorX[i],int(j))) / nTemp;
|
}
|
|
pY[0] = (pY[0]*i + vectorWeight[i]*vectorY[i]) / nTemp;
|
pY[1] = (pY[1]*i + vectorWeight[i]*vectorX[i]*vectorY[i]) / nTemp;
|
for (int j=2; j<nMatrixSize; j++)
|
{
|
pY[j] = (pY[j]*i + vectorWeight[i]*(pow(vectorX[i],int(j))*vectorY[i])) / nTemp;
|
}
|
nTemp++;
|
}
|
|
double *pSubX = pX;
|
for (int i=0; i<nMatrixSize; i++)
|
{
|
for (int j=0; j<nMatrixSize; j++)
|
{
|
*pSubX++ = pOrderX[i+j];
|
}
|
}
|
|
if (SolveMatrix(pX, pY, pA, nMatrixSize))
|
{
|
if (nIteration==1) break;
|
|
for (int i=0; i<nSize; i++)
|
{
|
vectorResidual[i] = pA[0] + (pA[1]*vectorX[i]);
|
for (int j=2; j<nMatrixSize; j++)
|
{
|
vectorResidual[i] += pA[j] * pow(vectorX[i], int(j));
|
}
|
vectorResidual[i] = vectorY[i] - vectorResidual[i];
|
}
|
|
dCurValue = OneStepBiweightAlgorithm(vectorResidual, vectorWeight, dWeight);
|
}
|
else
|
{
|
delete [] pX;
|
delete [] pA;
|
delete [] pY;
|
delete [] pOrderX;
|
|
return 0;
|
}
|
|
if ( fabs(dPrevValue-dCurValue) < dStopThres ) break;
|
|
dPrevValue = dCurValue;
|
}
|
|
for (int i=0; i<nMatrixSize; i++)
|
{
|
vectorR.push_back(pA[i]);
|
}
|
|
delete [] pX;
|
delete [] pA;
|
delete [] pY;
|
delete [] pOrderX;
|
|
return nReturnIteration+1;
|
}
|
|
int CCHDataFitting::IRLS_PolynomialFitting(double *pXValue, double *pYValue, int nSize, double *pResult, int nOrder, int nIteration, double dWeight, double dStopThres)
|
{
|
int nMatrixSize = nOrder + 1;
|
int nMaxOrder = nOrder * 2 + 1;
|
|
memset(pResult, 0, sizeof(double)*nMatrixSize);
|
|
if (nOrder >= nSize) return 0;
|
|
double *pWeight = new double[nSize];
|
double *pResidual = new double[nSize];
|
for (int i=0; i<nSize; i++)
|
{
|
pWeight[i] = 1.0;
|
}
|
|
double *pX = new double[nMatrixSize * nMatrixSize];
|
double *pA = new double[nMatrixSize];
|
double *pY = new double[nMatrixSize];
|
double *pOrderX = new double[nMaxOrder];
|
|
memset(pX, 0, sizeof(double)*(nMatrixSize*nMatrixSize));
|
memset(pA, 0, sizeof(double)*(nMatrixSize));
|
memset(pY, 0, sizeof(double)*(nMatrixSize));
|
memset(pOrderX, 0, sizeof(double)*(nMaxOrder));
|
|
double dCurValue = 0.0;
|
double dPrevValue = 0.0;
|
|
int nReturnIteration = 0;
|
for (nReturnIteration=0; nReturnIteration<nIteration; nReturnIteration++)
|
{
|
int nTemp = 1;
|
for (int i=0; i<nSize; i++)
|
{
|
pOrderX[0] = (pOrderX[0]*i + pWeight[i]) / nTemp;
|
pOrderX[1] = (pOrderX[1]*i + pWeight[i]*pXValue[i]) / nTemp;
|
for (int j=2; j<nMaxOrder; j++)
|
{
|
pOrderX[j] = (pOrderX[j]*i + pWeight[i]*pow(pXValue[i],j)) / nTemp;
|
}
|
|
pY[0] = (pY[0]*i + pWeight[i]*pYValue[i]) / nTemp;
|
pY[1] = (pY[1]*i + pWeight[i]*pXValue[i]*pYValue[i]) / nTemp;
|
for (int j=2; j<nMatrixSize; j++)
|
{
|
pY[j] = (pY[j]*i + pWeight[i]*(pow(pXValue[i],j)*pYValue[i])) / nTemp;
|
}
|
nTemp++;
|
}
|
|
double *pSubX = pX;
|
for (int i=0; i<nMatrixSize; i++)
|
{
|
for (int j=0; j<nMatrixSize; j++)
|
{
|
*pSubX++ = pOrderX[i+j];
|
}
|
}
|
|
if (SolveMatrix(pX, pY, pA, nMatrixSize))
|
{
|
if (nIteration==1) break;
|
|
for (int i=0; i<nSize; i++)
|
{
|
pResidual[i] = pA[0] + (pA[1]*pXValue[i]);
|
for (int j=2; j<nMatrixSize; j++)
|
{
|
pResidual[i] += pA[j]*pow(pXValue[i],j);
|
}
|
pResidual[i] = pYValue[i]-pResidual[i];
|
}
|
|
dCurValue = OneStepBiweightAlgorithm(pResidual, pWeight, nSize, dWeight);
|
}
|
else
|
{
|
delete [] pX;
|
delete [] pA;
|
delete [] pY;
|
delete [] pOrderX;
|
delete [] pWeight;
|
delete [] pResidual;
|
|
return 0;
|
}
|
|
if ( fabs(dPrevValue-dCurValue) < dStopThres ) break;
|
|
dPrevValue = dCurValue;
|
}
|
|
// copy result
|
memcpy(pResult, pA, sizeof(double)*nMatrixSize);
|
|
delete [] pX;
|
delete [] pA;
|
delete [] pY;
|
delete [] pOrderX;
|
delete [] pWeight;
|
delete [] pResidual;
|
|
return nReturnIteration+1;
|
}
|
|
int CCHDataFitting::IRLS_PlaneFitting(VectorDouble& vectorX, VectorDouble& vectorY, VectorDouble& vectorZ, VectorDouble& vectorResult, int nIteration, double dWeight, double dStopThres)
|
{
|
vectorResult.clear();
|
|
size_t nSize = vectorX.size();
|
if (nSize<3) return 0;
|
|
VectorDouble vectorWeight, vectorResidual;
|
vectorWeight.resize(nSize);
|
vectorResidual.resize(nSize);
|
|
for (size_t i=0; i<nSize; i++)
|
{
|
vectorWeight[i] = 1.0;
|
}
|
|
double pX[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
|
double pA[3] = {0, 0, 0};
|
double pY[3] = {0, 0, 0};
|
|
double dCurValue = 0.0;
|
double dPrevValue = 0.0;
|
|
int nReturnIteration = 0;
|
for (nReturnIteration=0; nReturnIteration<nIteration; nReturnIteration++)
|
{
|
int nTemp = 1;
|
for (size_t i=0; i<nSize; i++)
|
{
|
pX[0] = (pX[0]*i + vectorWeight[i]) / nTemp; // 1
|
pX[1] = (pX[1]*i + vectorWeight[i]*vectorY[i]) / nTemp; // y
|
pX[2] = (pX[2]*i + vectorWeight[i]*vectorX[i]) / nTemp; // x
|
pX[4] = (pX[4]*i + vectorWeight[i]*vectorY[i]*vectorY[i]) / nTemp; // yy
|
pX[5] = (pX[5]*i + vectorWeight[i]*vectorX[i]*vectorY[i]) / nTemp; // xy
|
pX[8] = (pX[8]*i + vectorWeight[i]*vectorX[i]*vectorX[i]) / nTemp; // xx
|
|
pY[0] = (pY[0]*i + vectorWeight[i]*vectorZ[i]) / nTemp;
|
pY[1] = (pY[1]*i + vectorWeight[i]*vectorY[i]*vectorZ[i]) / nTemp;
|
pY[2] = (pY[2]*i + vectorWeight[i]*vectorX[i]*vectorZ[i]) / nTemp;
|
|
nTemp++;
|
}
|
|
pX[3] = pX[1];
|
pX[6] = pX[2];
|
pX[7] = pX[5];
|
|
if (SolveMatrix(pX, pY, pA, 3))
|
{
|
if (nIteration==1) break;
|
|
for (size_t i=0; i<nSize; i++)
|
{
|
vectorResidual[i] = vectorZ[i] - (pA[0] + (pA[1]*vectorY[i]) + (pA[2]*vectorX[i]));
|
}
|
|
dCurValue = OneStepBiweightAlgorithm(vectorResidual, vectorWeight, dWeight);
|
}
|
else
|
{
|
return 0;
|
}
|
|
if ( fabs(dPrevValue-dCurValue) < dStopThres ) break;
|
|
dPrevValue = dCurValue;
|
}
|
|
for (size_t i=0; i<3; i++)
|
{
|
vectorResult.push_back(pA[i]);
|
}
|
|
return nReturnIteration+1;
|
}
|
|
int CCHDataFitting::IRLS_PlaneFitting(double *pXValue, double *pYValue, double *pZValue, int nSize, double *pResult, int nIteration, double dWeight, double dStopThres)
|
{
|
memset(pResult, 0, sizeof(double)*3);
|
|
if (nSize<3) return 0;
|
|
double *pWeight = new double[nSize];
|
double *pResidual = new double[nSize];
|
for (int i=0; i<nSize; i++)
|
{
|
pWeight[i] = 1.0;
|
}
|
|
double pX[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
|
double pA[3] = {0, 0, 0};
|
double pY[3] = {0, 0, 0};
|
|
double dCurValue = 0.0;
|
double dPrevValue = 0.0;
|
|
int nReturnIteration = 0;
|
for (nReturnIteration=0; nReturnIteration<nIteration; nReturnIteration++)
|
{
|
int nTemp = 1;
|
for (int i=0; i<nSize; i++)
|
{
|
pX[0] = (pX[0]*i + pWeight[i]) / nTemp; // 1
|
pX[1] = (pX[1]*i + pWeight[i]*pYValue[i]) / nTemp; // y
|
pX[2] = (pX[2]*i + pWeight[i]*pXValue[i]) / nTemp; // x
|
pX[4] = (pX[4]*i + pWeight[i]*pYValue[i]*pYValue[i]) / nTemp; // yy
|
pX[5] = (pX[5]*i + pWeight[i]*pXValue[i]*pYValue[i]) / nTemp; // xy
|
pX[8] = (pX[8]*i + pWeight[i]*pXValue[i]*pXValue[i]) / nTemp; // xx
|
|
pY[0] = (pY[0]*i + pWeight[i]*pZValue[i]) / nTemp; // z
|
pY[1] = (pY[1]*i + pWeight[i]*pZValue[i]*pYValue[i]) / nTemp; // zy
|
pY[2] = (pY[2]*i + pWeight[i]*pZValue[i]*pXValue[i]) / nTemp; // zx
|
|
nTemp++;
|
}
|
|
pX[3] = pX[1];
|
pX[6] = pX[2];
|
pX[7] = pX[5];
|
|
if (SolveMatrix(pX, pY, pA, 3))
|
{
|
if (nIteration==1) break;
|
|
for (int i=0; i<nSize; i++)
|
{
|
pResidual[i] = pZValue[i] - (pA[0] + (pA[1]*pYValue[i]) + (pA[2]*pXValue[i]));
|
}
|
|
dCurValue = OneStepBiweightAlgorithm(pResidual, pWeight, nSize, dWeight);
|
}
|
else
|
{
|
delete [] pWeight;
|
delete [] pResidual;
|
return 0;
|
}
|
|
if ( fabs(dPrevValue-dCurValue) > dStopThres ) break;
|
|
dPrevValue = dCurValue;
|
}
|
|
// copy result
|
memcpy(pResult, pA, sizeof(double)*3);
|
|
delete [] pWeight;
|
delete [] pResidual;
|
|
return nReturnIteration+1;
|
}
|
|
int CCHDataFitting::IRLS_GaussianFitting(VectorDouble& vectorX, VectorDouble& vectorY, VectorDouble& vectorR, int nIteration, double dWeight, double dStopThres)
|
{
|
vectorR.clear();
|
|
size_t nSize = vectorX.size();
|
|
if (nSize < 3) return 0;
|
|
VectorDouble vectorWeight, vectorResidual, vectorLY;
|
vectorWeight.resize(nSize);
|
vectorResidual.resize(nSize);
|
vectorLY.resize(nSize);
|
|
for (size_t i=0; i<nSize; i++)
|
{
|
vectorWeight[i] = 1.0;
|
vectorLY[i] = log(vectorY[i]);
|
}
|
|
double pX[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0};
|
double pY[3] = {0, 0, 0};
|
double pA[3] = {0, 0, 0};
|
|
double dHeight, dMean, dVariance;
|
double dCurValue = 0.0;
|
double dPrevValue = 0.0;
|
|
int nReturnIteration = 0;
|
for (nReturnIteration=0; nReturnIteration<nIteration; nReturnIteration++)
|
{
|
size_t nTemp = 1;
|
for (size_t i=0; i<nSize; i++)
|
{
|
pX[0] = (pX[0]*i + vectorWeight[i]) / nTemp;
|
pX[1] = (pX[1]*i + vectorWeight[i]*vectorX[i]) / nTemp;
|
pX[2] = (pX[2]*i + vectorWeight[i]*vectorX[i]*vectorX[i]) / nTemp;
|
pX[5] = (pX[5]*i + vectorWeight[i]*vectorX[i]*vectorX[i]*vectorX[i]) / nTemp;
|
pX[8] = (pX[8]*i + vectorWeight[i]*vectorX[i]*vectorX[i]*vectorX[i]*vectorX[i]) / nTemp;
|
|
pY[0] = (pY[0]*i + vectorWeight[i]*vectorLY[i]) / nTemp;
|
pY[1] = (pY[1]*i + vectorWeight[i]*vectorX[i]*vectorLY[i]) / nTemp;
|
pY[2] = (pY[2]*i + vectorWeight[i]*vectorX[i]*vectorX[i]*vectorLY[i]) / nTemp;
|
|
nTemp++;
|
}
|
|
pX[3] = pX[1];
|
pX[4] = pX[2];
|
pX[6] = pX[2];
|
pX[7] = pX[5];
|
|
if (SolveMatrix(pX, pY, pA, 3))
|
{
|
dVariance = -1.0 * sqrt(fabs(1.0/pA[2]));
|
dMean = -1.0 * (pA[1] / (2.0*pA[2]));
|
dHeight = exp(pA[0] + (dMean*dMean)/(dVariance*dVariance));
|
|
if (nIteration==1) break;
|
|
for (size_t i=0; i<nSize; i++)
|
{
|
vectorResidual[i] = -1.0 * ( (vectorX[i]-dMean)*(vectorX[i]-dMean) / (dVariance*dVariance) );
|
vectorResidual[i] = vectorY[i] - (dHeight*exp(vectorResidual[i]));
|
}
|
|
dCurValue = OneStepBiweightAlgorithm(vectorResidual, vectorWeight, dWeight);
|
}
|
else
|
{
|
return 0;
|
}
|
|
if ( fabs(dPrevValue-dCurValue) < dStopThres ) break;
|
|
dPrevValue = dCurValue;
|
}
|
|
vectorR.push_back(dHeight);
|
vectorR.push_back(dMean);
|
vectorR.push_back(dVariance);
|
|
return nReturnIteration+1;
|
}
|
|
int CCHDataFitting::IRLS_GaussianFitting(double *pXValue, double *pYValue, int nSize, double *pResult, int nIteration, double dWeight, double dStopThres)
|
{
|
memset(pResult, 0, sizeof(double)*3);
|
|
if (nSize < 3) return 0;
|
|
double *pWeight = new double[nSize];
|
double *pResidual = new double[nSize];
|
double *pYLogValue = new double[nSize];
|
|
for (int i=0; i<nSize; i++)
|
{
|
pWeight[i] = 1.0;
|
pYLogValue[i] = log(pYValue[i]);
|
}
|
|
double pX[9], pY[3], pA[3];
|
double dHeight, dMean, dVariance;
|
double dCurValue = 0.0;
|
double dPrevValue = 0.0;
|
|
memset(pX, 0, sizeof(double)*9);
|
memset(pY, 0, sizeof(double)*3);
|
memset(pA, 0, sizeof(double)*3);
|
|
int nReturnIteration = 0;
|
|
for (nReturnIteration=0; nReturnIteration<nIteration; nReturnIteration++)
|
{
|
int nTemp = 1;
|
for (int i=0; i<nSize; i++)
|
{
|
pX[0] = (pX[0]*i + pWeight[i]) / nTemp;
|
pX[1] = (pX[1]*i + pWeight[i]*pXValue[i]) / nTemp;
|
pX[2] = (pX[2]*i + pWeight[i]*pXValue[i]*pXValue[i]) / nTemp;
|
pX[5] = (pX[5]*i + pWeight[i]*pXValue[i]*pXValue[i]*pXValue[i]) / nTemp;
|
pX[8] = (pX[8]*i + pWeight[i]*pXValue[i]*pXValue[i]*pXValue[i]*pXValue[i]) / nTemp;
|
|
pY[0] = (pY[0]*i + pWeight[i]*pYLogValue[i]) / nTemp;
|
pY[1] = (pY[1]*i + pWeight[i]*pXValue[i]*pYLogValue[i]) / nTemp;
|
pY[2] = (pY[2]*i + pWeight[i]*pXValue[i]*pXValue[i]*pYLogValue[i]) / nTemp;
|
|
nTemp++;
|
}
|
|
pX[3] = pX[1];
|
pX[4] = pX[2];
|
pX[6] = pX[2];
|
pX[7] = pX[5];
|
|
if (SolveMatrix(pX, pY, pA, 3))
|
{
|
dVariance = -1.0 * sqrt(fabs(1.0/pA[2]));
|
dMean = -1.0 * (pA[1] / (2.0*pA[2]));
|
dHeight = exp(pA[0] + (dMean*dMean)/(dVariance*dVariance));
|
|
if (nIteration==1) break;
|
|
for (int i=0; i<nSize; i++)
|
{
|
pResidual[i] = -1.0 * ( (pXValue[i]-dMean)*(pXValue[i]-dMean) / (dVariance*dVariance) );
|
pResidual[i] = pYValue[i] - (dHeight*exp(pResidual[i]));
|
}
|
|
dCurValue = OneStepBiweightAlgorithm(pResidual, pWeight, nSize, dWeight);
|
}
|
else
|
{
|
delete [] pWeight;
|
delete [] pResidual;
|
delete [] pYLogValue;
|
return 0;
|
}
|
|
if ( fabs(dPrevValue-dCurValue) < dStopThres ) break;
|
|
dPrevValue = dCurValue;
|
|
}
|
|
pResult[0] = dHeight;
|
pResult[1] = dMean;
|
pResult[2] = dVariance;
|
|
delete [] pWeight;
|
delete [] pResidual;
|
delete [] pYLogValue;
|
|
return nReturnIteration + 1;
|
}
|
|
int CCHDataFitting::CircleFitting(VectorDouble& vectorX, VectorDouble& vectorY, VectorDouble& vectorR)
|
{
|
vectorR.clear();
|
|
size_t nSize = vectorX.size();
|
if (nSize < 3) return 0;
|
|
double avg_x = 0;
|
double avg_y = 0;
|
for (size_t i=0; i<nSize; i++)
|
{
|
avg_x += vectorX[i];
|
avg_y += vectorY[i];
|
}
|
avg_x /= double(nSize);
|
avg_y /= double(nSize);
|
|
double a[4] = {0, 0, 0, 0};
|
double b[2] = {0, 0};
|
double dx[2] = {0, 0};
|
|
double Suu = 0;
|
double Suv = 0;
|
double Svv = 0;
|
double Suuu = 0;
|
double Svvv = 0;
|
double Suvv = 0;
|
double Suuv = 0;
|
|
double ui, vi;
|
for (size_t i=0; i<nSize; i++)
|
{
|
ui = vectorX[i] - avg_x;
|
vi = vectorY[i] - avg_y;
|
|
Suu += (ui*ui);
|
Suv += (ui*vi);
|
Svv += (vi*vi);
|
|
Suuu += (ui*ui*ui);
|
Svvv += (vi*vi*vi);
|
|
Suvv += (ui*vi*vi);
|
Suuv += (ui*ui*vi);
|
}
|
|
a[0] = Suu; // Suu
|
a[1] = Suv; // Suv
|
a[2] = Suv; // Suv
|
a[3] = Svv; // Svv
|
|
b[0] = (Suuu+Suvv) / 2.0;
|
b[1] = (Svvv+Suuv) / 2.0;
|
|
int nValue = SolveMatrix(a, b, dx, 2);
|
|
if (nValue!=1) return 0;
|
|
double alpha = (dx[0]*dx[0]) + (dx[1]*dx[1]) + ((a[0]+a[3])/double(nSize));
|
|
vectorR.push_back(dx[0] + avg_x);
|
vectorR.push_back(dx[1] + avg_y);
|
vectorR.push_back(sqrt(alpha));
|
|
return 1;
|
}
|
|
int CCHDataFitting::CircleFitting(double *pXValue, double *pYValue, int nSize, double *pResult)
|
{
|
memset(pResult, 0, sizeof(double)*3);
|
|
if (nSize < 3) return 0;
|
|
double avg_x = 0;
|
double avg_y = 0;
|
for (int i=0; i<nSize; i++)
|
{
|
avg_x += pXValue[i];
|
avg_y += pYValue[i];
|
}
|
avg_x /= double(nSize);
|
avg_y /= double(nSize);
|
|
double a[4] = {0, 0, 0, 0};
|
double b[2] = {0, 0};
|
double dx[2] = {0, 0};
|
|
double Suu = 0;
|
double Suv = 0;
|
double Svv = 0;
|
double Suuu = 0;
|
double Svvv = 0;
|
double Suvv = 0;
|
double Suuv = 0;
|
|
double ui, vi;
|
for (size_t i=0; i<nSize; i++)
|
{
|
ui = pXValue[i] - avg_x;
|
vi = pYValue[i] - avg_y;
|
|
Suu += (ui*ui);
|
Suv += (ui*vi);
|
Svv += (vi*vi);
|
|
Suuu += (ui*ui*ui);
|
Svvv += (vi*vi*vi);
|
|
Suvv += (ui*vi*vi);
|
Suuv += (ui*ui*vi);
|
}
|
|
a[0] = Suu; // Suu
|
a[1] = Suv; // Suv
|
a[2] = Suv; // Suv
|
a[3] = Svv; // Svv
|
|
b[0] = (Suuu+Suvv) / 2.0;
|
b[1] = (Svvv+Suuv) / 2.0;
|
|
int nValue = SolveMatrix(a, b, dx, 2);
|
|
if (nValue!=1) return 0;
|
|
double alpha = (dx[0]*dx[0]) + (dx[1]*dx[1]) + ((a[0]+a[3])/double(nSize));
|
|
pResult[0] = dx[0] + avg_x;
|
pResult[0] = dx[1] + avg_y;
|
pResult[0] = sqrt(alpha);
|
|
return 1;
|
}
|
|
int CCHDataFitting::EllipseFitting(double *pXValue, double *pYValue, int nSize, double *pResult)
|
{
|
if( nSize < 5 ) return 0;
|
|
/*
|
* New fitellipse algorithm, contributed by Dr. Daniel Weiss
|
*/
|
cv::AutoBuffer<double> Ad, bd;
|
double gfp[5], rp[5], t;
|
CvMat A, b, x;
|
const double min_eps = 1e-6;
|
|
Ad.allocate(nSize*5);
|
bd.allocate(nSize);
|
|
// first fit for parameters A - E
|
A = cvMat( nSize, 5, CV_64F, Ad );
|
b = cvMat( nSize, 1, CV_64F, bd );
|
x = cvMat( 5, 1, CV_64F, gfp );
|
|
double avg_x = 0.0;
|
double avg_y = 0.0;
|
for (int i=0; i<nSize; i++)
|
{
|
avg_x += pXValue[i];
|
avg_y += pYValue[i];
|
}
|
avg_x /= double(nSize);
|
avg_y /= double(nSize);
|
|
double val_x, val_y;
|
for (int i=0; i<nSize; i++)
|
{
|
val_x = pXValue[i] - avg_x;
|
val_y = pYValue[i] - avg_y;
|
|
bd[i] = 10000.0; // 1.0?
|
Ad[i*5] = -val_x * val_x; // A - C signs inverted as proposed by APP
|
Ad[i*5 + 1] = -val_y * val_y;
|
Ad[i*5 + 2] = -val_x * val_y;
|
Ad[i*5 + 3] = val_x;
|
Ad[i*5 + 4] = val_y;
|
}
|
cvSolve( &A, &b, &x, CV_SVD );
|
|
// now use general-form parameters A - E to find the ellipse center:
|
// differentiate general form wrt x/y to get two equations for cx and cy
|
A = cvMat( 2, 2, CV_64F, Ad );
|
b = cvMat( 2, 1, CV_64F, bd );
|
x = cvMat( 2, 1, CV_64F, rp );
|
|
Ad[0] = 2 * gfp[0];
|
Ad[1] = Ad[2] = gfp[2];
|
Ad[3] = 2 * gfp[1];
|
bd[0] = gfp[3];
|
bd[1] = gfp[4];
|
cvSolve( &A, &b, &x, CV_SVD );
|
|
// re-fit for parameters A - C with those center coordinates
|
A = cvMat( nSize, 3, CV_64F, Ad );
|
b = cvMat( nSize, 1, CV_64F, bd );
|
x = cvMat( 3, 1, CV_64F, gfp );
|
|
for(int i=0; i<nSize; i++)
|
{
|
val_x = pXValue[i] - avg_x;
|
val_y = pYValue[i] - avg_y;
|
|
bd[i] = 1.0;
|
Ad[i * 3] = (val_x - rp[0]) * (val_x - rp[0]);
|
Ad[i * 3 + 1] = (val_y - rp[1]) * (val_y - rp[1]);
|
Ad[i * 3 + 2] = (val_x - rp[0]) * (val_y - rp[1]);
|
}
|
cvSolve(&A, &b, &x, CV_SVD);
|
|
// store angle and radii
|
rp[4] = -0.5 * atan2(gfp[2], gfp[1] - gfp[0]); // convert from APP angle usage
|
t = sin(-2.0 * rp[4]);
|
if( fabs(t) > fabs(gfp[2])*min_eps )
|
t = gfp[2]/t;
|
else
|
t = gfp[1] - gfp[0];
|
rp[2] = fabs(gfp[0] + gfp[1] - t);
|
if( rp[2] > min_eps )
|
rp[2] = sqrt(2.0 / rp[2]);
|
rp[3] = fabs(gfp[0] + gfp[1] + t);
|
if( rp[3] > min_eps )
|
rp[3] = sqrt(2.0 / rp[3]);
|
|
pResult[0] = rp[0] + avg_x; // center x
|
pResult[1] = rp[1] + avg_y; // center y
|
pResult[2] = rp[2]*2.0; // size width
|
pResult[3] = rp[3]*2.0; // size height
|
pResult[4] = 90.0 + rp[4]*180.0/CV_PI; // angle
|
|
if( pResult[2] > pResult[3] )
|
{
|
double tmp = pResult[2];
|
pResult[2] = pResult[3];
|
pResult[3] = tmp;
|
}
|
|
if( pResult[4]< -180 ) pResult[4] += 360;
|
if( pResult[4] > 360 ) pResult[4] -= 360;
|
|
return 1;
|
}
|
|
int CCHDataFitting::EllipseFitting(VectorDouble& vectorX , VectorDouble& vectorY, VectorDouble& vectorResult)
|
{
|
int n = (int) vectorX.size();
|
|
if( n < 5 ) return 0;
|
|
/*
|
* New fitellipse algorithm, contributed by Dr. Daniel Weiss
|
*/
|
cv::AutoBuffer<double> Ad, bd;
|
double gfp[5], rp[5], t;
|
CvMat A, b, x;
|
const double min_eps = 1e-6;
|
|
Ad.allocate(n*5);
|
bd.allocate(n);
|
|
// first fit for parameters A - E
|
A = cvMat( n, 5, CV_64F, Ad );
|
b = cvMat( n, 1, CV_64F, bd );
|
x = cvMat( 5, 1, CV_64F, gfp );
|
|
double avg_x = 0.0;
|
double avg_y = 0.0;
|
for(int i=0; i<n; i++)
|
{
|
avg_x += vectorX[i];
|
avg_y += vectorY[i];
|
}
|
avg_x /= double(n);
|
avg_y /= double(n);
|
|
double val_x, val_y;
|
for(int i=0; i<n; i++)
|
{
|
val_x = vectorX[i] - avg_x;
|
val_y = vectorY[i] - avg_y;
|
|
bd[i] = 10000.0; // 1.0?
|
Ad[i*5] = -val_x * val_x; // A - C signs inverted as proposed by APP
|
Ad[i*5 + 1] = -val_y * val_y;
|
Ad[i*5 + 2] = -val_x * val_y;
|
Ad[i*5 + 3] = val_x;
|
Ad[i*5 + 4] = val_y;
|
}
|
|
cvSolve( &A, &b, &x, CV_SVD );
|
|
// now use general-form parameters A - E to find the ellipse center:
|
// differentiate general form wrt x/y to get two equations for cx and cy
|
A = cvMat( 2, 2, CV_64F, Ad );
|
b = cvMat( 2, 1, CV_64F, bd );
|
x = cvMat( 2, 1, CV_64F, rp );
|
|
Ad[0] = 2 * gfp[0];
|
Ad[1] = Ad[2] = gfp[2];
|
Ad[3] = 2 * gfp[1];
|
bd[0] = gfp[3];
|
bd[1] = gfp[4];
|
cvSolve( &A, &b, &x, CV_SVD );
|
|
// re-fit for parameters A - C with those center coordinates
|
A = cvMat( n, 3, CV_64F, Ad );
|
b = cvMat( n, 1, CV_64F, bd );
|
x = cvMat( 3, 1, CV_64F, gfp );
|
|
for(int i=0; i<n; i++)
|
{
|
val_x = vectorX[i] - avg_x;
|
val_y = vectorY[i] - avg_y;
|
|
bd[i] = 1.0;
|
Ad[i * 3] = (val_x - rp[0]) * (val_x - rp[0]);
|
Ad[i * 3 + 1] = (val_y - rp[1]) * (val_y - rp[1]);
|
Ad[i * 3 + 2] = (val_x - rp[0]) * (val_y - rp[1]);
|
}
|
cvSolve(&A, &b, &x, CV_SVD);
|
|
// store angle and radii
|
rp[4] = -0.5 * atan2(gfp[2], gfp[1] - gfp[0]); // convert from APP angle usage
|
t = sin(-2.0 * rp[4]);
|
if( fabs(t) > fabs(gfp[2])*min_eps )
|
t = gfp[2]/t;
|
else
|
t = gfp[1] - gfp[0];
|
rp[2] = fabs(gfp[0] + gfp[1] - t);
|
if( rp[2] > min_eps )
|
rp[2] = sqrt(2.0 / rp[2]);
|
rp[3] = fabs(gfp[0] + gfp[1] + t);
|
if( rp[3] > min_eps )
|
rp[3] = sqrt(2.0 / rp[3]);
|
|
|
vectorResult.clear();
|
vectorResult.push_back(rp[0] + avg_x); // center x
|
vectorResult.push_back(rp[1] + avg_y); // center y
|
vectorResult.push_back(rp[2]*2.0); // size width
|
vectorResult.push_back(rp[3]*2.0); // size height
|
vectorResult.push_back(90.0 + rp[4]*180.0/CV_PI); // angle
|
|
if( vectorResult[2] > vectorResult[3] )
|
{
|
double tmp = vectorResult[2];
|
vectorResult[2] = vectorResult[3];
|
vectorResult[3] = tmp;
|
}
|
|
if( vectorResult[4]< -180 ) vectorResult[4] += 360;
|
if( vectorResult[4] > 360 ) vectorResult[4] -= 360;
|
|
return 1;
|
}
|
|
int CCHDataFitting::IRLS_ParaboloidFitting(VectorDouble& vectorX, VectorDouble& vectorY, VectorDouble& vectorZ, VectorDouble& vectorResult, int nIteration, double dWeight, double dStopThres)
|
{
|
vectorResult.clear();
|
|
int nSize = (int)vectorX.size();
|
if (nSize<6) return 0;
|
|
VectorDouble vectorWeight, vectorResidual;
|
vectorWeight.resize(nSize);
|
vectorResidual.resize(nSize);
|
|
for (int i=0; i<nSize; i++)
|
{
|
vectorWeight[i] = 1.0;
|
}
|
|
double pX[36] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
|
double pA[6] = {0, 0, 0, 0, 0, 0};
|
double pY[6] = {0, 0, 0, 0, 0, 0};
|
|
double dCurValue = 0.0;
|
double dPrevValue = 0.0;
|
|
int nReturnIteration = 0;
|
for (nReturnIteration=0; nReturnIteration<nIteration; nReturnIteration++)
|
{
|
int nTemp = 1;
|
for (int i=0; i<nSize; i++)
|
{
|
// 1
|
pX[0] = (pX[0]*i + vectorWeight[i]) / nTemp; // 1
|
pX[1] = (pX[1]*i + vectorWeight[i]*vectorY[i]) / nTemp; // y
|
pX[2] = (pX[2]*i + vectorWeight[i]*vectorX[i]) / nTemp; // x
|
pX[3] = (pX[3]*i + vectorWeight[i]*vectorY[i]*vectorY[i]) / nTemp; // yy
|
pX[4] = (pX[4]*i + vectorWeight[i]*vectorX[i]*vectorY[i]) / nTemp; // xy
|
pX[5] = (pX[5]*i + vectorWeight[i]*vectorX[i]*vectorX[i]) / nTemp; // xx
|
// 2
|
pX[9] = (pX[9]*i + vectorWeight[i]*vectorY[i]*vectorY[i]*vectorY[i]) / nTemp; // yyy
|
pX[10] = (pX[10]*i + vectorWeight[i]*vectorX[i]*vectorY[i]*vectorY[i]) / nTemp; // xyy
|
pX[11] = (pX[11]*i + vectorWeight[i]*vectorX[i]*vectorX[i]*vectorY[i]) / nTemp; // xxy
|
// 3
|
pX[17] = (pX[17]*i + vectorWeight[i]*vectorX[i]*vectorX[i]*vectorX[i]) / nTemp; // xxx
|
// 4
|
pX[21] = (pX[21]*i + vectorWeight[i]*vectorY[i]*vectorY[i]*vectorY[i]*vectorY[i]) / nTemp; // yyyy
|
pX[22] = (pX[22]*i + vectorWeight[i]*vectorX[i]*vectorY[i]*vectorY[i]*vectorY[i]) / nTemp; // xyyy
|
pX[23] = (pX[23]*i + vectorWeight[i]*vectorX[i]*vectorX[i]*vectorY[i]*vectorY[i]) / nTemp; // xxyy
|
// 5
|
pX[29] = (pX[29]*i + vectorWeight[i]*vectorX[i]*vectorX[i]*vectorX[i]*vectorY[i]) / nTemp; // xxxy
|
// 6
|
pX[35] = (pX[35]*i + vectorWeight[i]*vectorX[i]*vectorX[i]*vectorX[i]*vectorX[i]) / nTemp; // xxxx
|
|
pY[0] = (pY[0]*i + vectorWeight[i]*vectorZ[i]) / nTemp; // z
|
pY[1] = (pY[1]*i + vectorWeight[i]*vectorZ[i]*vectorY[i]) / nTemp; // zy
|
pY[2] = (pY[2]*i + vectorWeight[i]*vectorZ[i]*vectorX[i]) / nTemp; // zx
|
pY[3] = (pY[3]*i + vectorWeight[i]*vectorZ[i]*vectorY[i]*vectorY[i]) / nTemp; // zyy
|
pY[4] = (pY[4]*i + vectorWeight[i]*vectorZ[i]*vectorX[i]*vectorY[i]) / nTemp; // zxy
|
pY[5] = (pY[5]*i + vectorWeight[i]*vectorZ[i]*vectorX[i]*vectorX[i]) / nTemp; // zxx
|
|
nTemp++;
|
}
|
// 2
|
pX[6] = pX[1]; // y
|
pX[7] = pX[3]; // yy
|
pX[8] = pX[4]; // xy
|
// 3
|
pX[12] = pX[2]; // x
|
pX[13] = pX[4]; // xy
|
pX[14] = pX[5]; // xx
|
pX[15] = pX[10]; // xyy
|
pX[16] = pX[11]; // xxy
|
// 4
|
pX[18] = pX[3]; // yy
|
pX[19] = pX[9]; // yyy
|
pX[20] = pX[10]; // xyy
|
// 5
|
pX[24] = pX[4]; // xy
|
pX[25] = pX[10]; // xyy
|
pX[26] = pX[11]; // xxy
|
pX[27] = pX[22]; // xyyy
|
pX[28] = pX[23]; // xxyy
|
// 6
|
pX[30] = pX[5]; // xx
|
pX[31] = pX[11]; // xxy
|
pX[32] = pX[17]; // xxx
|
pX[33] = pX[23]; // xxyy
|
pX[34] = pX[29]; // xxxy
|
|
if (SolveMatrix(pX, pY, pA, 6))
|
{
|
if (nIteration==1) break;
|
|
for (int i=0; i<nSize; i++)
|
{
|
vectorResidual[i] = vectorZ[i] - (
|
(pA[0]) +
|
(pA[1]*vectorY[i]) +
|
(pA[2]*vectorX[i]) +
|
(pA[3]*vectorY[i]*vectorY[i]) +
|
(pA[4]*vectorX[i]*vectorY[i]) +
|
(pA[5]*vectorX[i]*vectorX[i]) );
|
}
|
|
dCurValue = OneStepBiweightAlgorithm(vectorResidual, vectorWeight, dWeight);
|
}
|
else
|
{
|
return 0;
|
}
|
|
if ( fabs(dPrevValue-dCurValue) < dStopThres ) break;
|
|
dPrevValue = dCurValue;
|
}
|
|
for (size_t i=0; i<6; i++)
|
{
|
vectorResult.push_back(pA[i]);
|
}
|
|
return nReturnIteration;
|
}
|
|
int CCHDataFitting::IRLS_ParaboloidFitting(double *pXValue, double *pYValue, double *pZValue, int nSize, double *pResult, int nIteration, double dWeight, double dStopThres)
|
{
|
memset(pResult, 0, sizeof(double)*6);
|
|
if (nSize<6) return 0;
|
|
double *pWeight = new double[nSize];
|
double *pResidual = new double[nSize];
|
for (int i=0; i<nSize; i++)
|
{
|
pWeight[i] = 1.0;
|
}
|
|
double pX[36] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
|
double pA[6] = {0, 0, 0, 0, 0, 0};
|
double pY[6] = {0, 0, 0, 0, 0, 0};
|
|
double dCurValue = 0.0;
|
double dPrevValue = 0.0;
|
|
int nReturnIteration = 0;
|
for (nReturnIteration=0; nReturnIteration<nIteration; nReturnIteration++)
|
{
|
int nTemp = 1;
|
for (int i=0; i<nSize; i++)
|
{
|
// 1
|
pX[0] = (pX[0]*i + pWeight[i]) / nTemp; // 1
|
pX[1] = (pX[1]*i + pWeight[i]*pYValue[i]) / nTemp; // y
|
pX[2] = (pX[2]*i + pWeight[i]*pXValue[i]) / nTemp; // x
|
pX[3] = (pX[3]*i + pWeight[i]*pYValue[i]*pYValue[i]) / nTemp; // yy
|
pX[4] = (pX[4]*i + pWeight[i]*pXValue[i]*pYValue[i]) / nTemp; // xy
|
pX[5] = (pX[5]*i + pWeight[i]*pXValue[i]*pXValue[i]) / nTemp; // xx
|
// 2
|
pX[9] = (pX[9]*i + pWeight[i]*pYValue[i]*pYValue[i]*pYValue[i]) / nTemp; // yyy
|
pX[10] = (pX[10]*i + pWeight[i]*pXValue[i]*pYValue[i]*pYValue[i]) / nTemp; // xyy
|
pX[11] = (pX[11]*i + pWeight[i]*pXValue[i]*pXValue[i]*pYValue[i]) / nTemp; // xxy
|
// 3
|
pX[17] = (pX[17]*i + pWeight[i]*pXValue[i]*pXValue[i]*pXValue[i]) / nTemp; // xxx
|
// 4
|
pX[21] = (pX[21]*i + pWeight[i]*pYValue[i]*pYValue[i]*pYValue[i]*pYValue[i]) / nTemp; // yyyy
|
pX[22] = (pX[22]*i + pWeight[i]*pXValue[i]*pYValue[i]*pYValue[i]*pYValue[i]) / nTemp; // xyyy
|
pX[23] = (pX[23]*i + pWeight[i]*pXValue[i]*pXValue[i]*pYValue[i]*pYValue[i]) / nTemp; // xxyy
|
// 5
|
pX[29] = (pX[29]*i + pWeight[i]*pXValue[i]*pXValue[i]*pXValue[i]*pYValue[i]) / nTemp; // xxxy
|
// 6
|
pX[35] = (pX[35]*i + pWeight[i]*pXValue[i]*pXValue[i]*pXValue[i]*pXValue[i]) / nTemp; // xxxx
|
|
pY[0] = (pY[0]*i + pWeight[i]*pZValue[i]) / nTemp; // z
|
pY[1] = (pY[1]*i + pWeight[i]*pZValue[i]*pYValue[i]) / nTemp; // zy
|
pY[2] = (pY[2]*i + pWeight[i]*pZValue[i]*pXValue[i]) / nTemp; // zx
|
pY[3] = (pY[3]*i + pWeight[i]*pZValue[i]*pYValue[i]*pYValue[i]) / nTemp; // zyy
|
pY[4] = (pY[4]*i + pWeight[i]*pZValue[i]*pXValue[i]*pYValue[i]) / nTemp; // zxy
|
pY[5] = (pY[5]*i + pWeight[i]*pZValue[i]*pXValue[i]*pXValue[i]) / nTemp; // zxx
|
|
nTemp++;
|
}
|
// 2
|
pX[6] = pX[1]; // y
|
pX[7] = pX[3]; // yy
|
pX[8] = pX[4]; // xy
|
|
// 3
|
pX[12] = pX[2]; // x
|
pX[13] = pX[4]; // xy
|
pX[14] = pX[5]; // xx
|
pX[15] = pX[10]; // xyy
|
pX[16] = pX[11]; // xxy
|
|
// 4
|
pX[18] = pX[3]; // yy
|
pX[19] = pX[9]; // yyy
|
pX[20] = pX[10]; // xyy
|
|
// 5
|
pX[24] = pX[4]; // xy
|
pX[25] = pX[10]; // xyy
|
pX[26] = pX[11]; // xxy
|
pX[27] = pX[22]; // xyyy
|
pX[28] = pX[23]; // xxyy
|
|
// 6
|
pX[30] = pX[5]; // xx
|
pX[31] = pX[11]; // xxy
|
pX[32] = pX[17]; // xxx
|
pX[33] = pX[23]; // xxyy
|
pX[34] = pX[29]; // xxxy
|
|
if (SolveMatrix(pX, pY, pA, 6))
|
{
|
if (nIteration==1) break;
|
|
for (int i=0; i<nSize; i++)
|
{
|
pResidual[i] = pZValue[i] - (
|
(pA[0]) +
|
(pA[1]*pYValue[i]) +
|
(pA[2]*pXValue[i]) +
|
(pA[3]*pYValue[i]*pYValue[i]) +
|
(pA[4]*pXValue[i]*pYValue[i]) +
|
(pA[5]*pXValue[i]*pXValue[i]) );
|
}
|
|
dCurValue = OneStepBiweightAlgorithm(pResidual, pWeight, nSize, dWeight);
|
}
|
else
|
{
|
delete [] pWeight;
|
delete [] pResidual;
|
return 0;
|
}
|
|
if ( fabs(dPrevValue-dCurValue) < dStopThres ) break;
|
|
dPrevValue = dCurValue;
|
}
|
|
// copy result
|
memcpy(pResult, pA, sizeof(double)*6);
|
|
delete [] pWeight;
|
delete [] pResidual;
|
|
return nReturnIteration + 1;
|
}
|
|
void AperB(double **_A, double **_B, double **_res, int _righA, int _colA, int _righB, int _colB)
|
{
|
int p,q,l;
|
for (p=1;p<=_righA;p++)
|
{
|
for (q=1;q<=_colB;q++)
|
{
|
_res[p][q]=0.0;
|
for (l=1;l<=_colA;l++)
|
{
|
_res[p][q]=_res[p][q]+_A[p][l]*_B[l][q];
|
}
|
}
|
}
|
}
|
|
void A_TperB(double **_A, double **_B, double **_res, int _righA, int _colA, int _righB, int _colB)
|
{
|
int p,q,l;
|
for (p=1;p<=_colA;p++)
|
{
|
for (q=1;q<=_colB;q++)
|
{
|
_res[p][q]=0.0;
|
for (l=1;l<=_righA;l++)
|
{
|
_res[p][q]=_res[p][q]+_A[l][p]*_B[l][q];
|
}
|
}
|
}
|
}
|
|
void AperB_T(double **_A, double **_B, double **_res, int _righA, int _colA, int _righB, int _colB)
|
{
|
int p,q,l;
|
for (p=1;p<=_colA;p++)
|
{
|
for (q=1;q<=_colB;q++)
|
{
|
_res[p][q]=0.0;
|
for (l=1;l<=_righA;l++)
|
{
|
_res[p][q]=_res[p][q]+_A[p][l]*_B[q][l];
|
}
|
}
|
}
|
}
|
|
// Perform the Cholesky decomposition
|
// Return the lower triangular L such that L*L'=A
|
void choldc(double **a, int n, double **l)
|
{
|
int i,j,k;
|
double sum;
|
double *p = new double[n+1];
|
memset(p, 0, sizeof(double)*(n+1));
|
|
for (i=1; i<=n; i++)
|
{
|
for (j=i; j<=n; j++)
|
{
|
for (sum=a[i][j],k=i-1;k>=1;k--)
|
{
|
sum -= a[i][k]*a[j][k];
|
if (sum<=0.0)
|
{
|
TRACE("");
|
}
|
|
}
|
|
if (i == j)
|
{
|
if (sum<=0.0)
|
{
|
TRACE("");
|
}
|
else
|
{
|
p[i]=sqrt(sum);
|
}
|
}
|
else
|
{
|
a[j][i]=sum/p[i];
|
}
|
}
|
}
|
|
for (i=1; i<=n; i++)
|
{
|
for (j=i; j<=n; j++)
|
{
|
if (i==j)
|
{
|
l[i][i] = p[i];
|
}
|
else
|
{
|
l[j][i]=a[j][i];
|
l[i][j]=0.0;
|
}
|
}
|
}
|
|
delete [] p;
|
}
|
|
/********************************************************************/
|
/** Calcola la inversa della matrice B mettendo il risultato **/
|
/** in InvB . Il metodo usato per l'inversione e' quello di **/
|
/** Gauss-Jordan. N e' l'ordine della matrice . **/
|
/** ritorna 0 se l'inversione corretta altrimenti ritorna **/
|
/** SINGULAR . **/
|
/********************************************************************/
|
int inverse(double **TB, double **InvB, int N)
|
{
|
int k,i,j,p,q;
|
double mult;
|
double D,temp;
|
double maxpivot;
|
int npivot;
|
|
double **B = new double* [N+1];
|
double **A = new double* [N+1];
|
double **C = new double* [N+1];
|
|
for (int i=0; i<N+1; i++)
|
{
|
B[i] = new double [N+2];
|
memset(B[i], 0, sizeof(double)*(N+2));
|
|
A[i] = new double [2*N+2];
|
memset(A[i], 0, sizeof(double)*(2*N+2));
|
|
C[i] = new double [N+1];
|
memset(C[i], 0, sizeof(double)*(N+1));
|
}
|
|
double eps = 10e-20;
|
|
for(k=1;k<=N;k++)
|
{
|
for(j=1;j<=N;j++)
|
{
|
B[k][j]=TB[k][j];
|
}
|
}
|
|
for (k=1;k<=N;k++)
|
{
|
for (j=1;j<=N+1;j++)
|
{
|
A[k][j]=B[k][j];
|
}
|
for (j=N+2;j<=2*N+1;j++)
|
{
|
A[k][j]=(double)0;
|
}
|
A[k][k-1+N+2]=(double)1;
|
}
|
|
for (k=1;k<=N;k++)
|
{
|
maxpivot=abs((double)A[k][k]);
|
npivot=k;
|
|
for (i=k;i<=N;i++)
|
{
|
if (maxpivot<abs((double)A[i][k]))
|
{
|
maxpivot=abs((double)A[i][k]);
|
npivot=i;
|
}
|
|
if (maxpivot>=eps)
|
{
|
if (npivot!=k)
|
{
|
for (j=k;j<=2*N+1;j++)
|
{
|
temp=A[npivot][j];
|
A[npivot][j]=A[k][j];
|
A[k][j]=temp;
|
}
|
}
|
|
D=A[k][k];
|
for (j=2*N+1;j>=k;j--)
|
{
|
A[k][j]=A[k][j]/D;
|
}
|
|
for (i=1;i<=N;i++)
|
{
|
if (i!=k)
|
{
|
mult=A[i][k];
|
for (j=2*N+1;j>=k;j--)
|
{
|
A[i][j]=A[i][j]-mult*A[k][j] ;
|
}
|
}
|
}
|
}
|
else
|
{
|
return -1;
|
}
|
}
|
}
|
|
/** Copia il risultato nella matrice InvB ***/
|
for (k=1,p=1;k<=N;k++,p++)
|
{
|
for (j=N+2,q=1;j<=2*N+1;j++,q++)
|
{
|
InvB[p][q]=A[k][j];
|
}
|
}
|
|
for (int i=0; i<N+1; i++)
|
{
|
delete [] B[i];
|
delete [] A[i];
|
delete [] C[i];
|
}
|
|
delete [] B;
|
delete [] A;
|
delete [] C;
|
|
return 0;
|
} /* End of INVERSE */
|
|
void ROTATE(double **a, int i, int j, int k, int l, double tau, double s)
|
{
|
double g,h;
|
g = a[i][j];
|
h = a[k][l];
|
a[i][j] = g - s*(h+g*tau);
|
a[k][l]=h+s*(g-h*tau);
|
}
|
|
void jacobi(double **a, int n, double *d , double **v, int nrot)
|
{
|
int j,iq,ip,i;
|
double tresh,theta,tau,t,sm,s,h,g,c;
|
|
double *b = new double[n+1];
|
double *z = new double[n+1];
|
|
memset(b, 0, sizeof(double)*(n+1));
|
memset(z, 0, sizeof(double)*(n+1));
|
|
for (ip=1;ip<=n;ip++)
|
{
|
for (iq=1;iq<=n;iq++)
|
{
|
v[ip][iq] = 0.0;
|
}
|
v[ip][ip] = 1.0;
|
}
|
|
for (ip=1;ip<=n;ip++)
|
{
|
b[ip] = d[ip] = a[ip][ip];
|
z[ip] = 0.0;
|
}
|
|
nrot=0;
|
for (i=1;i<=50;i++)
|
{
|
sm=0.0;
|
for (ip=1;ip<=n-1;ip++)
|
{
|
for (iq=ip+1;iq<=n;iq++)
|
{
|
sm += abs(a[ip][iq]);
|
}
|
}
|
|
if (sm == 0.0)
|
{
|
delete [] b;
|
delete [] z;
|
|
return;
|
}
|
|
if (i < 4)
|
{
|
tresh=0.2*sm/(n*n);
|
}
|
else
|
{
|
tresh=0.0;
|
}
|
|
for (ip=1;ip<=n-1;ip++)
|
{
|
for (iq=ip+1;iq<=n;iq++)
|
{
|
g=100.0*abs(a[ip][iq]);
|
if (i > 4 && abs(d[ip])+g == abs(d[ip]) && abs(d[iq])+g == abs(d[iq]))
|
{
|
a[ip][iq]=0.0;
|
}
|
else if (abs(a[ip][iq]) > tresh)
|
{
|
h=d[iq]-d[ip];
|
if (abs(h)+g == abs(h))
|
{
|
t=(a[ip][iq])/h;
|
}
|
else
|
{
|
theta=0.5*h/(a[ip][iq]);
|
t=1.0/(abs(theta)+sqrt(1.0+theta*theta));
|
if (theta < 0.0)
|
{
|
t = -t;
|
}
|
}
|
|
c=1.0/sqrt(1+t*t);
|
s=t*c;
|
tau=s/(1.0+c);
|
h=t*a[ip][iq];
|
z[ip] -= h;
|
z[iq] += h;
|
d[ip] -= h;
|
d[iq] += h;
|
a[ip][iq]=0.0;
|
|
for (j=1;j<=ip-1;j++)
|
{
|
ROTATE(a,j,ip,j,iq,tau,s);
|
}
|
|
for (j=ip+1;j<=iq-1;j++)
|
{
|
ROTATE(a,ip,j,j,iq,tau,s);
|
}
|
|
for (j=iq+1;j<=n;j++)
|
{
|
ROTATE(a,ip,j,iq,j,tau,s);
|
}
|
|
for (j=1;j<=n;j++) {
|
ROTATE(v,j,ip,j,iq,tau,s);
|
}
|
++nrot;
|
}
|
}
|
}
|
|
for (ip=1;ip<=n;ip++)
|
{
|
b[ip] += z[ip];
|
d[ip]=b[ip];
|
z[ip]=0.0;
|
}
|
}
|
|
delete [] b;
|
delete [] z;
|
|
//printf("Too many iterations in routine JACOBI");
|
}
|
|
int CCHDataFitting::EllipseFitting2(double *pXValue, double *pYValue, int nSize, double *pResult)
|
{
|
if (nSize<6) return 0 ;
|
|
double **D = new double*[nSize+1];
|
for (int i=0; i<=nSize; i++)
|
{
|
D[i] = new double[7];
|
memset(D[i], 0, sizeof(double)*7);
|
}
|
|
double **S = new double*[7];
|
double **L = new double*[7];
|
double **invL = new double*[7];
|
double **Const = new double*[7];
|
double **temp = new double*[7];
|
double **C = new double*[7];
|
double **V = new double*[7];
|
double **sol = new double*[7];
|
|
double *d = new double[7];
|
memset(d, 0, sizeof(double)*7);
|
|
for (int i=0; i<7; i++)
|
{
|
S[i] = new double[7];
|
memset(S[i], 0, sizeof(double)*7);
|
L[i] = new double[7];
|
memset(L[i], 0, sizeof(double)*7);
|
invL[i] = new double[7];
|
memset(invL[i], 0, sizeof(double)*7);
|
Const[i] = new double[7];
|
memset(Const[i], 0, sizeof(double)*7);
|
temp[i] = new double[7];
|
memset(temp[i], 0, sizeof(double)*7);
|
C[i] = new double[7];
|
memset(C[i], 0, sizeof(double)*7);
|
V[i] = new double[7];
|
memset(V[i], 0, sizeof(double)*7);
|
sol[i] = new double[7];
|
memset(sol[i], 0, sizeof(double)*7);
|
}
|
|
double tx,ty;
|
|
int nrot=0;
|
int mode = 0;
|
|
switch (mode)
|
{
|
case 0:
|
Const[1][3]=-2;
|
Const[2][2]=1;
|
Const[3][1]=-2;
|
break;
|
|
case 1:
|
Const[1][1]=2;
|
Const[2][2]=1;
|
Const[3][3]=2;
|
break;
|
}
|
|
|
// Now first fill design matrix
|
for (int i=1; i <= nSize; i++)
|
{
|
tx = pXValue[i-1];
|
ty = pYValue[i-1];
|
D[i][1] = tx*tx;
|
D[i][2] = tx*ty;
|
D[i][3] = ty*ty;
|
D[i][4] = tx;
|
D[i][5] = ty;
|
D[i][6] = 1.0;
|
}
|
|
//pm(Const,"Constraint");
|
// Now compute scatter matrix S
|
A_TperB(D, D, S, nSize, 6, nSize, 6); // (A') * (B)
|
//pm(S,"Scatter");
|
|
choldc(S, 6, L);
|
//pm(L,"Cholesky");
|
|
inverse(L, invL, 6);
|
//pm(invL,"inverse");
|
|
AperB_T(Const, invL, temp, 6, 6, 6, 6);
|
|
AperB(invL, temp, C, 6, 6, 6, 6);
|
//pm(C,"The C matrix");
|
|
jacobi(C, 6, d, V, nrot);
|
//pm(V,"The Eigenvectors"); /* OK */
|
//pv(d,"The eigevalues");
|
|
A_TperB(invL, V, sol, 6, 6, 6, 6);
|
//pm(sol,"The GEV solution unnormalized"); /* SOl */
|
|
// Now normalize them
|
for (int j=1;j<=6;j++) /* Scan columns */
|
{
|
double mod = 0.0;
|
for (int i=1;i<=6;i++)
|
{
|
mod += sol[i][j]*sol[i][j];
|
}
|
for (int i=1;i<=6;i++)
|
{
|
sol[i][j] /= sqrt(mod);
|
}
|
}
|
|
//pm(sol,"The GEV solution"); /* SOl */
|
double zero=10e-20;
|
double minev=10e+20;
|
int solind=0;
|
|
switch (mode)
|
{
|
case 0:
|
for (int i=1; i<=6; i++)
|
{
|
if (d[i]<0.0 && abs(d[i])>zero)
|
{
|
solind = i;
|
}
|
}
|
break;
|
case 1: // smallest eigenvalue
|
for (int i=1; i<=6; i++)
|
{
|
if (d[i]<minev && abs(d[i])>zero)
|
{
|
solind = i;
|
}
|
}
|
break;
|
}
|
|
// Now fetch the right solution
|
for (int j=1;j<=6;j++)
|
{
|
pResult[j-1] = sol[j][solind];
|
}
|
|
for (int i=0; i<=nSize; i++)
|
{
|
delete [] D[i];
|
}
|
|
for (int i=0; i<7; i++)
|
{
|
delete [] S[i];
|
delete [] L[i];
|
delete [] invL[i];
|
delete [] Const[i];
|
delete [] temp[i];
|
delete [] C[i];
|
delete [] V[i];
|
delete [] sol[i];
|
}
|
|
delete [] D;
|
delete [] S;
|
delete [] L;
|
delete [] invL;
|
delete [] Const;
|
delete [] temp;
|
delete [] C;
|
delete [] V;
|
delete [] sol;
|
delete [] d;
|
|
return 1;
|
}
|
|
int CCHDataFitting::EllipseFitting2(VectorDouble& vectorX, VectorDouble& vectorY, VectorDouble& vectorResult)
|
{
|
int nSize = (int) vectorX.size();
|
|
if (nSize<6) return 0 ;
|
|
double **D = new double*[nSize+1];
|
for (int i=0; i<=nSize; i++)
|
{
|
D[i] = new double[7];
|
memset(D[i], 0, sizeof(double)*7);
|
}
|
|
double **S = new double*[7];
|
double **L = new double*[7];
|
double **invL = new double*[7];
|
double **Const = new double*[7];
|
double **temp = new double*[7];
|
double **C = new double*[7];
|
double **V = new double*[7];
|
double **sol = new double*[7];
|
|
double *d = new double[7];
|
memset(d, 0, sizeof(double)*7);
|
|
for (int i=0; i<7; i++)
|
{
|
S[i] = new double[7];
|
memset(S[i], 0, sizeof(double)*7);
|
L[i] = new double[7];
|
memset(L[i], 0, sizeof(double)*7);
|
invL[i] = new double[7];
|
memset(invL[i], 0, sizeof(double)*7);
|
Const[i] = new double[7];
|
memset(Const[i], 0, sizeof(double)*7);
|
temp[i] = new double[7];
|
memset(temp[i], 0, sizeof(double)*7);
|
C[i] = new double[7];
|
memset(C[i], 0, sizeof(double)*7);
|
V[i] = new double[7];
|
memset(V[i], 0, sizeof(double)*7);
|
sol[i] = new double[7];
|
memset(sol[i], 0, sizeof(double)*7);
|
}
|
|
double tx,ty;
|
|
int nrot=0;
|
int mode = 0;
|
|
switch (mode)
|
{
|
case 0:
|
Const[1][3]=-2;
|
Const[2][2]=1;
|
Const[3][1]=-2;
|
break;
|
|
case 1:
|
Const[1][1]=2;
|
Const[2][2]=1;
|
Const[3][3]=2;
|
break;
|
}
|
|
|
// Now first fill design matrix
|
for (int i=1; i <= nSize; i++)
|
{
|
tx = vectorX[i-1];
|
ty = vectorY[i-1];
|
D[i][1] = tx*tx;
|
D[i][2] = tx*ty;
|
D[i][3] = ty*ty;
|
D[i][4] = tx;
|
D[i][5] = ty;
|
D[i][6] = 1.0;
|
}
|
|
//pm(Const,"Constraint");
|
// Now compute scatter matrix S
|
A_TperB(D, D, S, nSize, 6, nSize, 6); // (A') * (B)
|
//pm(S,"Scatter");
|
|
choldc(S, 6, L);
|
//pm(L,"Cholesky");
|
|
inverse(L, invL, 6);
|
//pm(invL,"inverse");
|
|
AperB_T(Const, invL, temp, 6, 6, 6, 6);
|
|
AperB(invL, temp, C, 6, 6, 6, 6);
|
//pm(C,"The C matrix");
|
|
jacobi(C, 6, d, V, nrot);
|
//pm(V,"The Eigenvectors"); /* OK */
|
//pv(d,"The eigevalues");
|
|
A_TperB(invL, V, sol, 6, 6, 6, 6);
|
//pm(sol,"The GEV solution unnormalized"); /* SOl */
|
|
// Now normalize them
|
for (int j=1;j<=6;j++) /* Scan columns */
|
{
|
double mod = 0.0;
|
for (int i=1;i<=6;i++)
|
{
|
mod += sol[i][j]*sol[i][j];
|
}
|
for (int i=1;i<=6;i++)
|
{
|
sol[i][j] /= sqrt(mod);
|
}
|
}
|
|
//pm(sol,"The GEV solution"); /* SOl */
|
double zero=10e-20;
|
double minev=10e+20;
|
int solind=0;
|
|
switch (mode)
|
{
|
case 0:
|
for (int i=1; i<=6; i++)
|
{
|
if (d[i]<0.0 && abs(d[i])>zero)
|
{
|
solind = i;
|
}
|
}
|
break;
|
case 1: // smallest eigenvalue
|
for (int i=1; i<=6; i++)
|
{
|
if (d[i]<minev && abs(d[i])>zero)
|
{
|
solind = i;
|
}
|
}
|
break;
|
}
|
|
// Now fetch the right solution
|
vectorResult.clear();
|
for (int j=1;j<=6;j++)
|
{
|
vectorResult.push_back(sol[j][solind]);
|
}
|
|
for (int i=0; i<=nSize; i++)
|
{
|
delete [] D[i];
|
}
|
|
for (int i=0; i<7; i++)
|
{
|
delete [] S[i];
|
delete [] L[i];
|
delete [] invL[i];
|
delete [] Const[i];
|
delete [] temp[i];
|
delete [] C[i];
|
delete [] V[i];
|
delete [] sol[i];
|
}
|
|
delete [] D;
|
delete [] S;
|
delete [] L;
|
delete [] invL;
|
delete [] Const;
|
delete [] temp;
|
delete [] C;
|
delete [] V;
|
delete [] sol;
|
delete [] d;
|
|
return 1;
|
}
|
|
|
int CCHDataFitting::Conic2Ellipse(double *conic_param, double *ellipse_param)
|
{
|
double a = conic_param[0];
|
double b = conic_param[1];
|
double c = conic_param[2];
|
double d = conic_param[3];
|
double e = conic_param[4];
|
double f = conic_param[5];
|
|
// get ellipse orientation
|
double theta = 0.5 * atan2(b, a-c);
|
|
// get scaled major/minor axes
|
double ct = cos(theta);
|
double st = sin(theta);
|
double ap = a*ct*ct + b*ct*st + c*st*st;
|
double cp = a*st*st + b*ct*st + c*ct*ct;
|
|
// get translations
|
double cx = (2.0*c*d - b*e) / (b*b - 4*a*c);
|
double cy = (2.0*a*e - b*d) / (b*b - 4*a*c);
|
|
// get scale factor
|
double val = a*cx*cx + b*cx*cy + c*cy*cy;
|
double scale_inv = val - f;
|
|
if ((scale_inv/ap) <=0.0 || (scale_inv/cp) <=0.0)
|
{
|
return 0;
|
}
|
|
// const double min_eps = 1e-6;
|
// double t = sin(2.0 * theta);
|
// if( fabs(t) > fabs(b)*min_eps )
|
// t = b / t;
|
// else
|
// t = a - c;
|
//
|
// double min = fabs(c + a - t);
|
// if( min > min_eps )
|
// min = sqrt(2.0 / min);
|
//
|
// double max = fabs(c + a + t);
|
// if( max > min_eps )
|
// max = sqrt(2.0 / max);
|
|
ellipse_param[0] = sqrt(scale_inv / ap);
|
ellipse_param[1] = sqrt(scale_inv / cp);
|
ellipse_param[2] = cx;
|
ellipse_param[3] = cy;
|
ellipse_param[4] = theta;
|
|
return 1;
|
}
|
|
int CCHDataFitting::Conic2Ellipse(VectorDouble& conic_param, VectorDouble& ellipse_param)
|
{
|
if (conic_param.size()!=6) return 0;
|
|
double a = conic_param[0];
|
double b = conic_param[1];
|
double c = conic_param[2];
|
double d = conic_param[3];
|
double e = conic_param[4];
|
double f = conic_param[5];
|
|
// get ellipse orientation
|
double theta = 0.5 * atan2(b, a-c);
|
|
// get scaled major/minor axes
|
double ct = cos(theta);
|
double st = sin(theta);
|
double ap = a*ct*ct + b*ct*st + c*st*st;
|
double cp = a*st*st + b*ct*st + c*ct*ct;
|
|
// get translations
|
double cx = (2.0*c*d - b*e) / (b*b - 4*a*c);
|
double cy = (2.0*a*e - b*d) / (b*b - 4*a*c);
|
|
// get scale factor
|
double val = a*cx*cx + b*cx*cy + c*cy*cy;
|
double scale_inv = val - f;
|
|
if ((scale_inv/ap) <=0.0 || (scale_inv/cp) <=0.0)
|
{
|
return 0;
|
}
|
// const double min_eps = 1e-6;
|
// double t = sin(2.0 * theta);
|
// if( fabs(t) > fabs(b)*min_eps )
|
// t = b / t;
|
// else
|
// t = a - c;
|
//
|
// double min = fabs(c + a - t);
|
// if( min > min_eps )
|
// min = sqrt(2.0 / min);
|
//
|
// double max = fabs(c + a + t);
|
// if( max > min_eps )
|
// max = sqrt(2.0 / max);
|
|
ellipse_param.clear();
|
ellipse_param.push_back(sqrt(scale_inv / ap));
|
ellipse_param.push_back(sqrt(scale_inv / cp));
|
ellipse_param.push_back(cx);
|
ellipse_param.push_back(cy);
|
ellipse_param.push_back(theta);
|
|
return 1;
|
}
|
