Skip to content

张士柳 / eyemLib

转到一个项目
切换导航栏
切换导航栏固定状态
Commit 0361ce5a 由 张士柳
选项

无

1 个父辈 9c0967be
内嵌 并排
正在显示 3 个修改的文件 包含 486 行增加 和 363 行删除
eyemLib-Sharp/EyemLib.cs
......@@ -611,6 +611,17 @@ namespace eyemLib_Sharp
public double dMatchDeg; // 匹配度
public IntPtr lpszName; // 名称
}
public struct EyemRigidMatrix
{
public double a00; // a00
public double a01; // a01
public double b00; // b00
public double a10; // a10
public double a11; // a11
public double b10; // b10
}
#endregion
#region 通用
......@@ -878,6 +889,16 @@ namespace eyemLib_Sharp
[DllImport("eyemLib.dll", CharSet = CharSet.None, CallingConvention = CallingConvention.Cdecl)]
private static extern int eyemFitLine(int iPtnNum, IntPtr taPoint, int numToIgnore, ref EyemOcsDABC tpLine);
/// <summary>
/// RANSAC拟合圆
/// </summary>
/// <param name="iPtnNum">点数量</param>
/// <param name="taPoint">点</param>
/// <param name="dClippingFactor">消除异常值因子</param>
/// <param name="tpLine">线</param>
/// <returns></returns>
[DllImport("eyemLib.dll", CharSet = CharSet.None, CallingConvention = CallingConvention.Cdecl)]
private static extern int eyemFitLineRANSAC(int iPtnNum, IntPtr taPoint, double dClippingFactor, ref EyemOcsDABC tpLine);
/// <summary>
/// 鲁棒圆拟合
/// </summary>
/// <param name="iPtnNum">点数量</param>
......@@ -900,6 +921,39 @@ namespace eyemLib_Sharp
/// <returns></returns>
[DllImport("eyemLib.dll", CharSet = CharSet.None, CallingConvention = CallingConvention.Cdecl)]
private static extern int eyemFitCircle(int iPtnNum, IntPtr taPoint, int numToIgnore, ref double dRMS, ref EyemOcsDXYR tpCircle);
/// <summary>
/// 鲁棒平面拟合
/// </summary>
/// <param name="iPtnNum">点数量</param>
/// <param name="taPoint">点</param>
/// <param name="iCalcMode">计算模式</param>
/// <param name="dRobustCoef">鲁棒系数,如果采用预设定的值则设置为0</param>
/// <param name="tpPlane">平面</param>
/// <returns></returns>
[DllImport("eyemLib.dll", CharSet = CharSet.None, CallingConvention = CallingConvention.Cdecl)]
private static extern int eyemRobustFitPlane(int iPtnNum, IntPtr taPoint, int iCalcMode, double dRobustCoef, ref EyemOcsDABCD tpPlane);
/// <summary>
/// 刚性变换
/// </summary>
/// <param name="iPtnNum">点数量</param>
/// <param name="taPointA">A坐标系中的点</param>
/// <param name="taPointB">B坐标系中的点</param>
/// <param name="tpResult">结果</param>
/// <returns></returns>
[DllImport("eyemLib.dll", CharSet = CharSet.None, CallingConvention = CallingConvention.Cdecl)]
private static extern int eyemFitRTMatrix(int iPtnNum, IntPtr taPointA, IntPtr taPointB, ref EyemRigidMatrix tpResult);
/// <summary>
/// 鲁棒椭圆拟合
/// </summary>
/// <param name="iPtnNum">点数量</param>
/// <param name="taPoint">点</param>
/// <param name="iCalcMode">计算方式</param>
/// <param name="dRobustCoef">鲁棒系数,如果采用预设定的值则设置为0</param>
/// <param name="tpEllipse">椭圆</param>
/// <returns></returns>
[DllImport("eyemLib.dll", CharSet = CharSet.None, CallingConvention = CallingConvention.Cdecl)]
private static extern int eyemRobustFitEllipse(int iPtnNum, IntPtr taPoint, int iCalcMode, double dRobustCoef, ref EyemOcsDXYLSQ tpEllipse);
#endregion
#region 二维几何计算
......@@ -1287,376 +1341,92 @@ namespace eyemLib_Sharp
#region Test RobustFitLine
//EyemOcsDXY taPoint = new EyemOcsDXY();
//List<EyemOcsDXY> taPoints = new List<EyemOcsDXY>();
EyemOcsDXY taPoint = new EyemOcsDXY();
List<EyemOcsDXY> taPoints = new List<EyemOcsDXY>();
//taPoint.dX = 61;
//taPoint.dY = 41;
//taPoints.Add(taPoint);
//taPoint.dX = 62;
//taPoint.dY = 41;
//taPoints.Add(taPoint);
//taPoint.dX = 63;
//taPoint.dY = 41;
//taPoints.Add(taPoint);
//taPoint.dX = 56;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 57;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 58;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 59;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 60;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 64;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 65;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 66;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 67;
//taPoint.dY = 42;
//taPoints.Add(taPoint);
//taPoint.dX = 53;
//taPoint.dY = 43;
//taPoints.Add(taPoint);
//taPoint.dX = 54;
//taPoint.dY = 43;
//taPoints.Add(taPoint);
//taPoint.dX = 55;
//taPoint.dY = 43;
//taPoints.Add(taPoint);
//taPoint.dX = 68;
//taPoint.dY = 43;
//taPoints.Add(taPoint);
//taPoint.dX = 69;
//taPoint.dY = 43;
//taPoints.Add(taPoint);
//taPoint.dX = 70;
//taPoint.dY = 43;
//taPoints.Add(taPoint);
//taPoint.dX = 51;
//taPoint.dY = 44;
//taPoints.Add(taPoint);
//taPoint.dX = 52;
//taPoint.dY = 44;
//taPoints.Add(taPoint);
//taPoint.dX = 71;
//taPoint.dY = 44;
//taPoints.Add(taPoint);
//taPoint.dX = 72;
//taPoint.dY = 44;
//taPoints.Add(taPoint);
//taPoint.dX = 50;
//taPoint.dY = 45;
//taPoints.Add(taPoint);
//taPoint.dX = 73;
//taPoint.dY = 45;
//taPoints.Add(taPoint);
//taPoint.dX = 49;
//taPoint.dY = 46;
//taPoints.Add(taPoint);
//taPoint.dX = 74;
//taPoint.dY = 46;
//taPoints.Add(taPoint);
//taPoint.dX = 48;
//taPoint.dY = 47;
//taPoints.Add(taPoint);
//taPoint.dX = 75;
//taPoint.dY = 47;
//taPoints.Add(taPoint);
//taPoint.dX = 47;
//taPoint.dY = 48;
//taPoints.Add(taPoint);
//taPoint.dX = 76;
//taPoint.dY = 48;
//taPoints.Add(taPoint);
//taPoint.dX = 46;
//taPoint.dY = 49;
//taPoints.Add(taPoint);
//taPoint.dX = 77;
//taPoint.dY = 49;
//taPoints.Add(taPoint);
//taPoint.dX = 45;
//taPoint.dY = 50;
//taPoints.Add(taPoint);
//taPoint.dX = 78;
//taPoint.dY = 50;
//taPoints.Add(taPoint);
//taPoint.dX = 44;
//taPoint.dY = 51;
//taPoints.Add(taPoint);
//taPoint.dX = 79;
//taPoint.dY = 51;
//taPoints.Add(taPoint);
//taPoint.dX = 44;
//taPoint.dY = 52;
//taPoints.Add(taPoint);
//taPoint.dX = 79;
//taPoint.dY = 52;
//taPoints.Add(taPoint);
//taPoint.dX = 43;
//taPoint.dY = 53;
//taPoints.Add(taPoint);
//taPoint.dX = 80;
//taPoint.dY = 53;
//taPoints.Add(taPoint);
//taPoint.dX = 43;
//taPoint.dY = 54;
//taPoints.Add(taPoint);
//taPoint.dX = 80;
//taPoint.dY = 54;
//taPoints.Add(taPoint);
//taPoint.dX = 43;
//taPoint.dY = 55;
//taPoints.Add(taPoint);
//taPoint.dX = 80;
//taPoint.dY = 55;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 56;
//taPoints.Add(taPoint);
//taPoint.dX = 81;
//taPoint.dY = 56;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 57;
//taPoints.Add(taPoint);
//taPoint.dX = 81;
//taPoint.dY = 57;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 58;
//taPoints.Add(taPoint);
//taPoint.dX = 81;
//taPoint.dY = 58;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 59;
//taPoints.Add(taPoint);
//taPoint.dX = 41;
//taPoint.dY = 60;
//taPoints.Add(taPoint);
//taPoint.dX = 82;
//taPoint.dY = 60;
//taPoints.Add(taPoint);
//taPoint.dX = 41;
//taPoint.dY = 61;
//taPoints.Add(taPoint);
//taPoint.dX = 82;
//taPoint.dY = 61;
//taPoints.Add(taPoint);
//taPoint.dX = 41;
//taPoint.dY = 62;
//taPoints.Add(taPoint);
//taPoint.dX = 82;
//taPoint.dY = 62;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 63;
//taPoints.Add(taPoint);
//taPoint.dX = 82;
//taPoint.dY = 63;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 64;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 65;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 66;
//taPoints.Add(taPoint);
//taPoint.dX = 81;
//taPoint.dY = 66;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 67;
//taPoints.Add(taPoint);
//taPoint.dX = 81;
//taPoint.dY = 67;
//taPoints.Add(taPoint);
//taPoint.dX = 43;
//taPoint.dY = 68;
//taPoints.Add(taPoint);
//taPoint.dX = 81;
//taPoint.dY = 68;
//taPoints.Add(taPoint);
//taPoint.dX = 43;
//taPoint.dY = 69;
//taPoints.Add(taPoint);
//taPoint.dX = 80;
//taPoint.dY = 69;
//taPoints.Add(taPoint);
//taPoint.dX = 43;
//taPoint.dY = 70;
//taPoints.Add(taPoint);
//taPoint.dX = 80;
//taPoint.dY = 70;
//taPoints.Add(taPoint);
//taPoint.dX = 44;
//taPoint.dY = 71;
//taPoints.Add(taPoint);
//taPoint.dX = 79;
//taPoint.dY = 71;
//taPoints.Add(taPoint);
//taPoint.dX = 44;
//taPoint.dY = 72;
//taPoints.Add(taPoint);
//taPoint.dX = 79;
//taPoint.dY = 72;
//taPoints.Add(taPoint);
//taPoint.dX = 45;
//taPoint.dY = 73;
//taPoints.Add(taPoint);
//taPoint.dX = 78;
//taPoint.dY = 73;
//taPoints.Add(taPoint);
//taPoint.dX = 46;
//taPoint.dY = 74;
//taPoints.Add(taPoint);
//taPoint.dX = 77;
//taPoint.dY = 74;
//taPoints.Add(taPoint);
//taPoint.dX = 47;
//taPoint.dY = 75;
//taPoints.Add(taPoint);
//taPoint.dX = 76;
//taPoint.dY = 75;
//taPoints.Add(taPoint);
//taPoint.dX = 48;
//taPoint.dY = 76;
//taPoints.Add(taPoint);
//taPoint.dX = 75;
//taPoint.dY = 76;
//taPoints.Add(taPoint);
//taPoint.dX = 49;
//taPoint.dY = 77;
//taPoints.Add(taPoint);
//taPoint.dX = 74;
//taPoint.dY = 77;
//taPoints.Add(taPoint);
//taPoint.dX = 50;
//taPoint.dY = 78;
//taPoints.Add(taPoint);
//taPoint.dX = 73;
//taPoint.dY = 78;
//taPoints.Add(taPoint);
//taPoint.dX = 51;
//taPoint.dY = 79;
//taPoints.Add(taPoint);
//taPoint.dX = 52;
//taPoint.dY = 79;
//taPoints.Add(taPoint);
//taPoint.dX = 71;
//taPoint.dY = 79;
//taPoints.Add(taPoint);
//taPoint.dX = 72;
//taPoint.dY = 79;
//taPoints.Add(taPoint);
//taPoint.dX = 53;
//taPoint.dY = 80;
//taPoints.Add(taPoint);
//taPoint.dX = 54;
//taPoint.dY = 80;
//taPoints.Add(taPoint);
//taPoint.dX = 69;
//taPoint.dY = 80;
//taPoints.Add(taPoint);
//taPoint.dX = 70;
//taPoint.dY = 80;
//taPoints.Add(taPoint);
//taPoint.dX = 55;
//taPoint.dY = 81;
//taPoints.Add(taPoint);
//taPoint.dX = 56;
//taPoint.dY = 81;
//taPoints.Add(taPoint);
//taPoint.dX = 57;
//taPoint.dY = 81;
//taPoints.Add(taPoint);
//taPoint.dX = 64;
//taPoint.dY = 81;
//taPoints.Add(taPoint);
//taPoint.dX = 65;
//taPoint.dY = 81;
//taPoints.Add(taPoint);
//taPoint.dX = 66;
//taPoint.dY = 81;
//taPoints.Add(taPoint);
//taPoint.dX = 67;
//taPoint.dY = 81;
//taPoints.Add(taPoint);
//taPoint.dX = 68;
//taPoint.dY = 81;
//taPoints.Add(taPoint);
//taPoint.dX = 60;
//taPoint.dY = 82;
//taPoints.Add(taPoint);
//taPoint.dX = 61;
//taPoint.dY = 82;
//taPoints.Add(taPoint);
//taPoint.dX = 62;
//taPoint.dY = 82;
//taPoints.Add(taPoint);
//taPoint.dX = 63;
//taPoint.dY = 82;
//taPoints.Add(taPoint);
//x^2/4+y^2/9=1
//taPoint.dX = 41;
//taPoint.dY = 53;
//taPoints.Add(taPoint);
for (double i = -2.0; i <= 2.0; i += 0.1)
{
taPoint.dX = i;
taPoint.dY = 3 * Math.Sqrt(1 - Math.Pow(i, 2) / 4);
taPoints.Add(taPoint);
taPoint.dX = i;
taPoint.dY = -3 * Math.Sqrt(1 - Math.Pow(i, 2) / 4);
taPoints.Add(taPoint);
}
//taPoint.dX = 40;
//taPoint.dY = 56;
taPoint.dX = 0;
taPoint.dY = 2;
taPoints.Add(taPoint);
taPoint.dX = 2;
taPoint.dY = 1;
taPoints.Add(taPoint);
//taPoint.dX = 1;
//taPoint.dY = -1;
//taPoints.Add(taPoint);
//taPoint.dX = 38;
//taPoint.dY = 57;
//taPoint.dX = -1;
//taPoint.dY = -2;
//taPoints.Add(taPoint);
//taPoint.dX = 39;
//taPoint.dY = 51;
//taPoint.dX = -3;
//taPoint.dY = 1;
//taPoints.Add(taPoint);
//taPoint.dX = 42;
//taPoint.dY = 49;
//taPoint.dX = -1;
//taPoint.dY = -1;
//taPoints.Add(taPoint);
////taPoint.dX = 62;
////taPoint.dY = 61;
////taPoints.Add(taPoint);
//taPoint.dX = 61;
//taPoint.dY = 19;
//EyemOcsDXYZ taPoint = new EyemOcsDXYZ();
//List<EyemOcsDXYZ> taPoints = new List<EyemOcsDXYZ>();
//for (int x = 0; x < 6; x++)
//{
// for (int y = 0; y < 6; y++)
// {
// taPoint.dX = x;
// taPoint.dY = y;
// taPoint.dZ = x + 2 * y + 5;
// taPoints.Add(taPoint);
// }
//}
//taPoint.dX = 10;
//taPoint.dY = 10;
//taPoint.dZ = 10;
//taPoints.Add(taPoint);
//taPoint.dX = 56;
//taPoint.dY = 39;
//taPoint.dX = 5;
//taPoint.dY = 5;
//taPoint.dZ = 12;
//taPoints.Add(taPoint);
//taPoint.dX = 49;
//taPoint.dY = 20;
//taPoint.dX = 7;
//taPoint.dY = 7;
//taPoint.dZ = -2;
//taPoints.Add(taPoint);
//taPoint.dX = 49;
//taPoint.dY = 21;
//taPoint.dX = 2;
//taPoint.dY = 2;
//taPoint.dZ = -4;
//taPoints.Add(taPoint);
//EyemOcsDABC tpLine = new EyemOcsDABC();
//IntPtr tpPoint = eyemStructArray2IntPtr(taPoints.ToArray());
//EyemOcsDABCD tpPlane = new EyemOcsDABCD();
IntPtr tpPoint = eyemStructArray2IntPtr(taPoints.ToArray());
//eyemRobustFitPlane(taPoints.Count, tpPoint, 6, 0, ref tpPlane);
EyemOcsDXYLSQ tpEllipse = new EyemOcsDXYLSQ();
eyemRobustFitEllipse(taPoints.Count, tpPoint, 6, 0, ref tpEllipse);
return;
//eyemFitLine(taPoints.Count, tpPoint, 10, ref tpLine);
//eyemRobustFitLine(taPoints.Count, tpPoint, 2, 0, ref tpLine);
//eyemFitLineRANSAC(taPoints.Count, tpPoint, 5, ref tpLine);
//EyemOcsDXYR tpCircle = new EyemOcsDXYR(); double dRMS = 0.0;
//eyemRobustFitCircle(taPoints.Count, tpPoint, 6, 0, ref tpCircle);
//eyemFitCircle(taPoints.Count, tpPoint, 15, ref dRMS, ref tpCircle);
......
eyemLib/eyemFit.cpp
......@@ -335,6 +335,72 @@ int eyemFitLine(int iPtnNum, EyemOcsDXY *taPoint, int numToIgnore, EyemOcsDABC &
return FUNC_OK;
}
int eyemFitLineRANSAC(int iPtnNum, EyemOcsDXY *taPoint, double dClippingFactor, EyemOcsDABC &tpLine)
{
int iterations = 0;
const int maxIterations = 1000;
int nBestInliersCount = -INT_MAX;
int skippedCount = 0;
const int max_skip = maxIterations * 10;
std::vector<int> shuffledIndices(iPtnNum);
std::vector<EyemOcsDXY2D> taPoints(iPtnNum);
for (int i = 0; i < iPtnNum; i++) {
shuffledIndices[i] = i;
taPoints[i] = EyemOcsDXY2D(0, taPoint[i].dX, taPoint[i].dY, false);
}
EyemOcsDABC mTpLine;
while (iterations < maxIterations && skippedCount < max_skip)
{
std::vector<int> samples(2);
std::vector<cv::Point2d> samplesPts(2);
for (int i = 0; i < 1000; i++) {
for (int j = 0; j < 2; j++) {
srand((i + 100)*(j + 20) + (iterations + 39) * 100);
int iRand = rand();
samples[j] = shuffledIndices[iRand % (shuffledIndices.size() - 1)];
}
//判断两点是否满足条件(满足距离不能过小)
double dist = cv::norm(cv::Point2d(taPoints[samples[0]].dX, taPoints[samples[0]].dY)
- cv::Point2d(taPoints[samples[1]].dX, taPoints[samples[1]].dY));
if (dist > 2) {
taPoints[samples[0]].bValid = taPoints[samples[1]].bValid = true;
samplesPts[0] = cv::Point2d(taPoints[samples[0]].dX, taPoints[samples[0]].dY);
samplesPts[1] = cv::Point2d(taPoints[samples[1]].dX, taPoints[samples[1]].dY);
break;
}
}
//计算直线参数
mTpLine.dA = -(samplesPts[0].y - samplesPts[1].y);
mTpLine.dB = (samplesPts[0].x - samplesPts[1].x);
mTpLine.dC = samplesPts[1].x*samplesPts[0].y - samplesPts[0].x*samplesPts[1].y;
//判断大部分是否已经计算过（这样可以减少计算时间,但可能会不准）
int nSkippedCount = 0, nInliersCcount(0);
for (auto&p : taPoints) {
if (p.bValid) {
nSkippedCount++;
}
double dpDist = std::abs(mTpLine.dA*p.dX + mTpLine.dB*p.dY + mTpLine.dC) / sqrt(mTpLine.dA*mTpLine.dA + mTpLine.dB*mTpLine.dB);
if (dpDist < dClippingFactor) {
nInliersCcount++;
}
}
if (nInliersCcount > nBestInliersCount) {
nBestInliersCount = nInliersCcount;
tpLine = mTpLine;
}
iterations++;
if (iterations > maxIterations || nSkippedCount >= iPtnNum - 1)
break;
}
return FUNC_OK;
}
static void fitCircle2D_wods(int iPtnNum, const EyemOcsDXY2D *taPoints, EyemOcsDXYR &tpCircle)
{
std::vector<cv::Point2d> points;
......@@ -523,23 +589,306 @@ int eyemRobustFitCircle(int iPtnNum, EyemOcsDXY *taPoints, int iCalcMode, double
return FUNC_OK;
}
int eyemFitPlane(int iPtnNum, EyemOcsDXYZ *taPoint, int numToIgnore, double &dRMS, EyemOcsDABCD &tpPlane)
int eyemFitRTMatrix(int iPtnNum, EyemOcsDXY *taPointA, EyemOcsDXY *taPointB, EyemRigidMatrix &dpResult)
{
double sa[36], sb[6], sx[6];
cv::Mat a = cv::Mat(6, 6, CV_64F, sa), b = cv::Mat(6, 1, CV_64F, sb);
cv::Mat x = cv::Mat(6, 1, CV_64F, sx);
memset(sa, 0, sizeof(sa));
memset(sb, 0, sizeof(sb));
memset(sx, 0, sizeof(sx));
for (int i = 0; i < iPtnNum; i++)
{
sa[0] += taPointA[i].dX*taPointA[i].dX;
sa[1] += taPointA[i].dY*taPointA[i].dX;
sa[2] += taPointA[i].dX;
sa[6] += taPointA[i].dX*taPointA[i].dY;
sa[7] += taPointA[i].dY*taPointA[i].dY;
sa[8] += taPointA[i].dY;
sa[12] += taPointA[i].dX;
sa[13] += taPointA[i].dY;
sa[14] += 1.0;
sb[0] += taPointA[i].dX*taPointB[i].dX;
sb[1] += taPointA[i].dY*taPointB[i].dX;
sb[2] += taPointB[i].dX;
sb[3] += taPointA[i].dX*taPointB[i].dY;
sb[4] += taPointA[i].dY*taPointB[i].dY;
sb[5] += taPointB[i].dY;
}
sa[21] = sa[0];
sa[22] = sa[1];
sa[23] = sa[2];
sa[27] = sa[6];
sa[28] = sa[7];
sa[29] = sa[8];
sa[33] = sa[12];
sa[34] = sa[13];
sa[35] = sa[14];
cv::solve(a, b, x, cv::DECOMP_SVD);
dpResult.a00 = x.at<double>(0, 0); dpResult.a01 = x.at<double>(0, 1); dpResult.b00 = x.at<double>(0, 2);
dpResult.a10 = x.at<double>(1, 0); dpResult.a11 = x.at<double>(1, 1); dpResult.b10 = x.at<double>(1, 2);
return FUNC_OK;
}
int eyemFitLineRANSAC(int iPtnNum, EyemOcsDXY *taPoint, EyemOcsDABC &tpLine)
static void fitPlane_wods(int iPtnNum, const EyemOcsDXYZ *taPoints, float *weights, float *linebuf)
{
//计算系数(z=ax+by+z)
double sa[9], sb[3], sx[3];
cv::Mat a = cv::Mat(3, 3, CV_64F, sa), b = cv::Mat(3, 1, CV_64F, sb);
//系数矩阵A*X=B;
cv::Mat x = cv::Mat(3, 1, CV_64F, sx);
memset(sa, 0, sizeof(sa));
memset(sb, 0, sizeof(sb));
memset(sx, 0, sizeof(sx));
for (int i = 0; i < iPtnNum; i++)
{
sa[0] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX;
sa[1] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY;
sa[2] += (double)weights[i] * taPoints[i].dX;
sa[4] += (double)weights[i] * taPoints[i].dY*taPoints[i].dY;
sa[5] += (double)weights[i] * taPoints[i].dY;
sa[8] += (double)weights[i];
sb[0] += (double)weights[i] * taPoints[i].dX*taPoints[i].dZ;
sb[1] += (double)weights[i] * taPoints[i].dY*taPoints[i].dZ;
sb[2] += (double)weights[i] * taPoints[i].dZ;
}
sa[3] = sa[1];
sa[6] = sa[2];
sa[7] = sa[5];
cv::solve(a, b, x, cv::DECOMP_SVD);
linebuf[0] = (float)sx[0];
linebuf[1] = (float)sx[1];
linebuf[2] = (float)sx[2];
}
int eyemRobustFitPlane(int iPtnNum, EyemOcsDXYZ *taPoint, int iCalcMode, double dRobustCoef, EyemOcsDABCD &tpPlane)
{
double min_err = DBL_MAX, err = 0;
void(*calc_weights_param) (float *, int, float *, float) = 0;
float linebuf[3] = { .0f };
memset(linebuf, 0, 3 * sizeof(float));
std::vector<float> weights(iPtnNum, 1.0);
switch (iCalcMode)
{
case EYEM_DIST_L1:
calc_weights_param = weightL1;
break;
case EYEM_DIST_L12:
calc_weights_param = weightL12;
break;
case EYEM_DIST_FAIR:
calc_weights_param = weightFair;
break;
case EYEM_DIST_WELSCH:
calc_weights_param = weightWelsch;
break;
case EYEM_DIST_HUBER:
calc_weights_param = weightHuber;
break;
case EYEM_DIST_TUKEY:
calc_weights_param = weightTukey;
break;
case EYEM_DIST_CAUCHY:
calc_weights_param = weightCauchy;
break;
case EYEM_DIST_LOGISTIC:
calc_weights_param = weightLogistic;
break;
case EYEM_DIST_ANDREWS:
calc_weights_param = weightAndrews;
break;
case EYEM_DIST_ATLWORTH:
calc_weights_param = weightTalworth;
break;
case EYEM_DIST_USER:
calc_weights_param = weightUser;
break;
default:
break;
}
//迭代计算
for (int n = 0; n < 100; n++)
{
float sum_dist = .0, sum_w = .0;
fitPlane_wods(iPtnNum, taPoint, &weights[0], linebuf);
float a = linebuf[0], b = linebuf[1], c = linebuf[2];
cv::AutoBuffer<float> dist(iPtnNum);
for (int k = 0; k < iPtnNum; k++) {
dist[k] = fabs((a*(float)taPoint[k].dX + b*(float)taPoint[k].dY - (float)taPoint[k].dZ + c) / (sqrt(a*a + b*b + 1)));
sum_dist += dist[k];
}
err = sum_dist;
if (sum_dist < FLT_EPSILON)
break;
/*calculate weight*/
calc_weights_param(dist, iPtnNum, &weights[0], (float)dRobustCoef);
if (std::abs(err - min_err) < FLT_EPSILON)
break;
min_err = err;
}
tpPlane.dA = linebuf[0], tpPlane.dB = linebuf[1], tpPlane.dC = -1, tpPlane.dD = linebuf[2];
return FUNC_OK;
}
int eyemFitCircleRANSAC(int iPtnNum, EyemOcsDXY *taPoint, EyemOcsDXYR &tpCircle)
static void fitEllipse_wods(int iPtnNum, const EyemOcsDXY *taPoints, float *weights, float *linebuf)
{
//计算系数
double sa[25], sb[5], sx[5];
cv::Mat a = cv::Mat(5, 5, CV_64F, sa), b = cv::Mat(5, 1, CV_64F, sb);
//系数矩阵A*X=B;
cv::Mat x = cv::Mat(5, 1, CV_64F, sx);
memset(sa, 0, sizeof(sa));
memset(sb, 0, sizeof(sb));
memset(sx, 0, sizeof(sx));
for (int i = 0; i < iPtnNum; i++)
{
sa[0] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dX*taPoints[i].dX;
sa[1] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dX*taPoints[i].dY;
sa[2] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dY*taPoints[i].dY;
sa[3] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dX;
sa[4] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dY;
sa[5] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dX*taPoints[i].dY;
sa[6] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dY*taPoints[i].dY;
sa[7] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY*taPoints[i].dY*taPoints[i].dY;
sa[8] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dY;
sa[9] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY*taPoints[i].dY;
sa[10] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dY*taPoints[i].dY;
sa[11] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY*taPoints[i].dY*taPoints[i].dY;
sa[12] += (double)weights[i] * taPoints[i].dY*taPoints[i].dY*taPoints[i].dY*taPoints[i].dY;
sa[13] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY*taPoints[i].dY;
sa[14] += (double)weights[i] * taPoints[i].dY*taPoints[i].dY*taPoints[i].dY;
sa[15] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dX;
sa[16] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dY;
sa[17] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY*taPoints[i].dY;
sa[18] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX;
sa[19] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY;
sa[20] += (double)weights[i] * taPoints[i].dX*taPoints[i].dX*taPoints[i].dY;
sa[21] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY*taPoints[i].dY;
sa[22] += (double)weights[i] * taPoints[i].dY*taPoints[i].dY*taPoints[i].dY;
sa[23] += (double)weights[i] * taPoints[i].dX*taPoints[i].dY;
sa[24] += (double)weights[i] * taPoints[i].dY*taPoints[i].dY;
sb[0] += (double)weights[i] * (-taPoints[i].dX*taPoints[i].dX);
sb[1] += (double)weights[i] * (-taPoints[i].dX*taPoints[i].dY);
sb[2] += (double)weights[i] * (-taPoints[i].dY*taPoints[i].dY);
sb[3] += (double)weights[i] * (-taPoints[i].dY);
sb[3] += (double)weights[i] * (-taPoints[i].dX);
}
cv::solve(a, b, x, cv::DECOMP_SVD);
//计算椭圆参数Ax^2+Bxy+Cy^2+Dx+Ey+F=0 F=1
float s, l, t, x0, y0, A, B, C, D, E, F;
//对应的
A = (float)sx[0], B = (float)sx[1], C = (float)sx[2], D = (float)sx[3], E = (float)sx[4], F = 1.0f;
//中心x
x0 = (B*E - 2.0f * C*D) / (4.0f * A*C - B*B);
//中心y
y0 = (B*D - 2.0f * A*E) / (4.0f * A*C - B*B);
//倾角
if (abs(B / A) <= 0.0001f&&1.0f < C / A)
t = 0.0f;
else if (abs(B / A) <= 0.0001f&&1.0f > C / A)
t = (float)PI_BY_2;
else if (1.0f < C / A)
t = 0.5f*atan((B / A) / (1.0f - C / A));
else t = (float)PI_BY_2 + 0.5f*atan((B / A) / (1.0f - C / A));
//长轴半径
l = sqrt(2.0f*(A*x0*x0 + C*y0*y0*B*x0*y0 - 1.0f) / (A + C - sqrt((A - C)*(A - C) + B*B)));
//短轴半径
s = sqrt(2.0f*(A*x0*x0 + C*y0*y0*B*x0*y0 - 1.0f) / (A + C + sqrt((A - C)*(A - C) + B*B)));
//结果
linebuf[0] = x0; linebuf[1] = y0; linebuf[2] = cv::max(l, s); linebuf[3] = cv::min(l, s); linebuf[4] = t;
}
int eyemRobustFitEllipse(int iPtnNum, EyemOcsDXY * taPoint, int iCalcMode, double dRobustCoef, EyemOcsDXYLSQ & tpEllipse)
{
double min_err = DBL_MAX, err = 0;
void(*calc_weights_param) (float *, int, float *, float) = 0;
float linebuf[5] = { .0f };
memset(linebuf, 0, 5 * sizeof(float));
std::vector<float> weights(iPtnNum, 1.0);
switch (iCalcMode)
{
case EYEM_DIST_L1:
calc_weights_param = weightL1;
break;
case EYEM_DIST_L12:
calc_weights_param = weightL12;
break;
case EYEM_DIST_FAIR:
calc_weights_param = weightFair;
break;
case EYEM_DIST_WELSCH:
calc_weights_param = weightWelsch;
break;
case EYEM_DIST_HUBER:
calc_weights_param = weightHuber;
break;
case EYEM_DIST_TUKEY:
calc_weights_param = weightTukey;
break;
case EYEM_DIST_CAUCHY:
calc_weights_param = weightCauchy;
break;
case EYEM_DIST_LOGISTIC:
calc_weights_param = weightLogistic;
break;
case EYEM_DIST_ANDREWS:
calc_weights_param = weightAndrews;
break;
case EYEM_DIST_ATLWORTH:
calc_weights_param = weightTalworth;
break;
case EYEM_DIST_USER:
calc_weights_param = weightUser;
break;
default:
break;
}
//迭代计算
for (int n = 0; n < 100; n++)
{
float sum_dist = .0, sum_w = .0;
fitEllipse_wods(iPtnNum, taPoint, &weights[0], linebuf);
//椭圆参数
float x0 = linebuf[0], y0 = linebuf[1], l = linebuf[2], s = linebuf[3], t = linebuf[4], c = sqrt(abs(l*l - s*s));
//焦点
cv::Point2f f1(x0 - c*cos(t), y0 - c*sin(t)), f2(x0 + c*cos(t), y0 + c*sin(t));
cv::AutoBuffer<float> dist(iPtnNum);
for (int k = 0; k < iPtnNum; k++) {
dist[k] = fabs(((float)cv::norm(cv::Point2f((float)taPoint[k].dX, (float)taPoint[k].dY) - f1) + (float)cv::norm(cv::Point2f((float)taPoint[k].dX, (float)taPoint[k].dY) - f2)) - 2.0f*l);
sum_dist += dist[k];
}
err = sum_dist;
if (sum_dist < FLT_EPSILON)
break;
/*calculate weight*/
calc_weights_param(dist, iPtnNum, &weights[0], (float)dRobustCoef);
if (std::abs(err - min_err) < FLT_EPSILON)
break;
min_err = err;
}
tpEllipse.dL = linebuf[2], tpEllipse.dS = linebuf[3], tpEllipse.dQ = linebuf[4], tpEllipse.dXo = linebuf[0], tpEllipse.dYo = linebuf[1];
return FUNC_OK;
}
eyemLib/eyemLib.h
......@@ -102,10 +102,6 @@
typedef intptr_t IntPtr;
#endif
//#ifndef LPSTR
//typedef char* LPSTR;
//#endif
// 图像边界处理
#ifndef __EYEM_BORDER
......@@ -322,7 +318,7 @@ typedef struct {
/********************************************************************************************/
/* 每个特定于源的标头 */
/* 每个特定于源的标头 */
/********************************************************************************************/
//////////////////////////////////////////////////////////////////////////////////////////////
......@@ -423,6 +419,15 @@ enum
EYEM_DIST_ATLWORTH = 10 /**< w = 1 * (abs(r)<1) c=2.975*/
};
typedef struct {
double a00; // a00
double a01; // a01
double b00; // b00
double a10; // a10
double a11; // a11
double b10; // b10
} EyemRigidMatrix;
#ifdef __cplusplus
extern "C" {
#endif
......@@ -430,15 +435,12 @@ extern "C" {
// 函数接口
EXPORTS int eyemFitLine(int iPtnNum, EyemOcsDXY *taPoint, int numToIgnore, EyemOcsDABC &tpLine);
EXPORTS int eyemRobustFitLine(int iPtnNum, EyemOcsDXY *taPoint, int iCalcMode, double dRobustCoef, EyemOcsDABC &tpLine);
EXPORTS int eyemFitLineRANSAC(int iPtnNum, EyemOcsDXY *taPoint, double dClippingFactor, EyemOcsDABC &tpLine);
EXPORTS int eyemFitCircle(int iPtnNum, EyemOcsDXY *taPoint, int numToIgnore, double &dRMS, EyemOcsDXYR &tpCircle);
EXPORTS int eyemRobustFitCircle(int iPtnNum, EyemOcsDXY *taPoint, int iCalcMode, double dRobustCoef, EyemOcsDXYR &tpCircle);
EXPORTS int eyemFitPlane(int iPtnNum, EyemOcsDXYZ *taPoint, int iCalcMode, double &dRMS, EyemOcsDABCD &tpPlane);
EXPORTS int eyemFitEllipse(int, EyemOcsDXY[], int, double, EyemOcsDXYLSQ *);
EXPORTS int eyemFitEllipseC(int, EyemOcsDXY[], int, double, double[]);
EXPORTS int eyemFitConics(int, EyemOcsDXY[], int, double, double[]);
EXPORTS int eyemFitParabola(int, EyemOcsDXY[], int, double, EyemOcsDABC *);
EXPORTS int eyemFitEllipsoid(int, EyemOcsDXYZ[], int, double, EyemOcsDCRUVW *);
EXPORTS int eyemFitCone(int, EyemOcsDXYZ[], int, double, double[]);
EXPORTS int eyemFitRTMatrix(int iPtnNum, EyemOcsDXY *taPointA, EyemOcsDXY *taPointB, EyemRigidMatrix &dpResult);
EXPORTS int eyemRobustFitPlane(int iPtnNum, EyemOcsDXYZ *taPoint, int iCalcMode, double dRobustCoef, EyemOcsDABCD &tpPlane);
EXPORTS int eyemRobustFitEllipse(int iPtnNum, EyemOcsDXY *taPoint, int iCalcMode, double dRobustCoef, EyemOcsDXYLSQ &tpEllipse);
#ifdef __cplusplus
}
......@@ -833,7 +835,6 @@ typedef struct {
char* lpszName; // 名称
} EyemModelID;
#ifdef __cplusplus
extern "C" {
#endif
......@@ -859,6 +860,9 @@ extern "C" {
EXPORTS int eyemMarkerTracing(EyemImage tpImage, double dThreshold, EyemOcsFXYR *tpCircle, bool bHighAccuracy = false);
EXPORTS int eyemMulFuncTool(EyemImage tpImage, EyemRect tpRoi, const char *funcName, double dThreshold, int iNumToIgnore, EyemOcsFXYR *tpCircle, EyemImage *tpDstImg);
EXPORTS int eyemLibImpl(EyemImage tpImage, EyemImage *tpDstImg);
EXPORTS int eyemDrawLine(EyemImage tpImage, EyemOcsDABC tpLine);
EXPORTS int eyemDrawCircle(EyemImage tpImage, EyemOcsDXYR tpCircle);
EXPORTS int eyemDrawRectangle(EyemImage tpImag, EyemRect tpRect);
#ifdef __cplusplus
}
......
支持 Markdown 格式
你添加了 0 人 到此讨论。请谨慎行事。
Finish editing this message first!