识别圆的强化RANSAC算法

什么是RANSAC算法？

​    随机抽样一致算法（RANdom SAmple  Consensus,RANSAC），是一种采用迭代的方式从一组包含噪音的数据中找到想要的数学模型。这个算法的使用前提是给定一组数据，存在一种方法能计算出符合这些数据的模型的参数。RANSAC算法把数据分为了两类，正确数据（内点）和异常数据（噪声），并通过一个参数sigma来将数据中的点分类。**该算法的核心思想在于其随机性和假设性**，随机性指的是抽象选取数据是随机的，根据大数定律，当样本足够大时，随机抽样可以得出近似正确的结果，而假设性指的是随机抽样的数据都假设为正确数据，根据给的阈值（sigma）来用数据集中的其他数据对随机抽样建立的模型进行评价，在反复抽样的过程中保留最优解，即是我们寻找的模型。

​    RANSAC算法一般用于直线拟合、平面拟合等方面。但对于识别圆形，RANSAC算法有一定的缺陷。为方便读者理解，我们从一个案例出发来阐述RANSAC算法拟合圆的缺点。

案例

​    在一个主要由直线搭建的场地中，存在一个圆（半径已知），我们要用一个带有激光雷达和stm32的小车在场地中寻找到这个圆。

​    简单分析一下，这个问题从算法层面可以拆分为两个基本算法，即RANSAC算法和人工势场法，后者的意义在于小车的运动，这里不过多论述，这里只讨论RANSAC算法。

​    由于小车带有激光雷达，我们将问题抽象出来，转化为，如何在一个有大量直线和一个圆的坐标系中找出圆心坐标。

方案一

  ​直接用RANSAC算法识别圆形，判定方式为按顺序等间距取点（考虑到激光雷达成图的特点），两点确定两圆，找出圆心后，由于半径已知，所以可以确定圆的位置，取一个合适的阈值sigma，从而找到内点。通过比较内点个数找到圆的模型。
  该方案存在两个问题，即由于激光雷达的特点，导致近处点密集且数量大，远处点稀疏且数量小。所以如果sigma是一个定值的话，可能会导致近处的直线会被判定为圆，远处的圆未被发现。所以考虑将sigma设置为一个随距离变化的函数。第二个问题是，如果点的数量较少，一个直角和一个圆在判定时是等价的。所以需要一个很大的数据集。但由于stm32单片机效率有限，所以我们考虑优化算法。这里提出了方案二。
​    

方案二

  由于RANSAC识别圆的效果不佳，我们考虑从直线出发，但由于对于一个弧度较大的弧形而言，RANSAC算法可能会将其识别为直线，所以我们改进了RANSAC算法，改进如下：设置一个数组储存所有点，表示该点在直线上的概率，当某点被判定为内点时，该点在数组中对应的元素+1。在遍历所有点后，我们可以绘制一个图，将该数组的大小转变为颜色的深浅，那么由于直线上有多个点，在遍历的时候会被多次调用，且直线上的点均为内点，故颜色较深，而由于遍历到圆上的点的时候，确定的直线都在圆的切线上，故圆的颜色较浅。那么可以排除掉直线。这里运用人工势场法可以让小车避开直线，即小车逐渐往存在圆的位置前进，此时在用方案一中识别圆的算法即可识别圆形。

代码展示

强化RANSAC算法版本

import numpy as np
import matplotlib.pyplot as plt#数据接收和预处理
a = [0,1648,0,1650,0,1648,0,1650,0,1648,0,1650,2,1653,2,1653,4,1656,4,1656,4,1658,4,1656,4,1658,8,1480,8,1480,8,1480,10,1486,10,1486,18,1515,18,1515,36,873,36,871,36,870,38,848,38,818,38,820,40,799,40,796,40,796,42,777,42,757,42,775,42,756,42,775,44,742,44,739,44,756,46,731,46,720,48,703,48,688,50,675,52,663,52,650,54,640,56,632,58,627,114,596,114,598,116,604,118,613,118,622,120,631,120,624,120,632,122,641,122,651,122,642,122,652,124,661,124,664,126,674,126,676,128,688,128,701,128,689,128,704,130,715,130,717,130,733,132,730,132,747,132,751,134,764,134,785,134,768,134,787,136,805,136,809,138,824,138,848,138,830,140,873,140,852,140,879,142,905,144,937,144,938,146,920,146,919,148,905,148,889,148,903,148,887,150,877,150,876,152,867,152,865,160,243,160,239,162,243,162,239,164,238,164,239,164,237,166,240,166,243,166,237,166,239,168,241,174,770,174,769,174,770,174,770,176,769,176,769,178,769,178,770,178,769,180,771,180,770,180,771,182,772,182,774,182,772,184,777,184,775,184,777,186,782,186,782,194,385,194,385,194,382,196,373,196,375,198,371,198,373,200,373,200,378,204,918,204,920,206,933,206,935,208,947,208,963,208,950,208,966,210,980,210,981,212,998,212,1020,212,1001,212,1023,214,1042,214,1044,216,1065,216,1092,218,1114,226,239,226,235,228,235,230,234,230,234,232,222,232,220,234,217,234,216,238,1652,242,400,242,399,244,399,244,398,246,395,246,391,246,396,246,392,248,388,248,386,250,386,250,383,250,384,252,382,252,383,252,380,254,380,254,379,256,378,256,376,256,379,256,378,260,44,260,373,260,373,260,373,260,373,260,372,262,373,262,373,262,372,264,372,264,373,264,372,264,372,264,372,266,372,266,372,266,372,266,372,268,372,268,372,268,372,268,371,270,372,270,372,270,372,270,372,270,372,272,373,272,373,272,373,272,373,274,374,274,373,274,374,274,373,274,374,276,375,276,375,276,375,278,377,278,378,278,376,278,378,278,377,278,380,280,380,280,380,280,382,282,382,282,384,282,382,282,384,282,383,282,385,284,385,284,385,284,388,284,388,286,388,286,392,286,392,286,391,286,395,288,396,288,395,288,401,288,398,290,401,290,402,302,1496,304,1501,304,1474,304,1476,304,1495,304,1472,306,1466,306,1461,306,1458,308,1458,308,1452,308,1454,308,1453,308,1454,308,1453,312,925,312,924,312,924,314,905,314,889,314,904,314,885,314,904,314,888,316,872,316,867,316,872,318,854,318,839,318,853,318,842,318,853,318,839,320,826,320,823,322,808,322,806,324,791,324,777,324,792,324,774,324,785,324,775,326,763,326,765,328,763,328,762,328,765,340,1732,340,1731,342,1714,342,1696,342,1713,342,1696,342,1711,342,1694,344,1684,344,1684,344,1683,346,1674,346,1668,346,1674,346,1668,346,1674,346,1668,348,1666,348,1665,348,1665,350,1663,350,1662,350,1663,352,1651,352,1652,352,1651,354,1647,354,1647,354,1646,356,1644,356,1643,356,1643,356,1643,356,1644,356,1643,358,1645,358,1645,358,1645,0,1648,0,1650,0,1648]
b = [0,700,0,700,2,701,2,701,4,704,4,708,4,704,4,707,6,712,6,719,6,712,6,719,8,731,8,731,10,94,10,757,10,745,10,760,10,744,10,760,12,780,12,779,16,1574,18,1578,18,1586,82,1833,84,1816,84,1812,84,1823,84,1814,84,1828,84,1813,86,1809,86,1810,86,1811,88,1806,88,1804,88,1806,88,1804,88,1808,88,1804,90,1803,90,1803,90,1803,94,577,94,576,96,580,96,582,96,579,96,580,98,583,98,582,100,584,100,588,100,583,100,586,102,590,102,589,104,592,104,595,104,591,104,593,106,599,106,596,106,601,108,602,108,606,108,605,110,612,110,610,110,614,112,616,112,620,112,621,114,627,114,628,114,633,116,634,116,639,118,647,118,655,118,644,118,653,120,663,120,662,122,672,122,668,126,1616,126,1622,128,1573,130,1524,130,1487,130,1526,130,1486,132,1454,132,1454,134,1423,134,1397,134,1424,134,1398,136,1374,136,1374,138,1351,138,1331,138,1351,138,1330,140,1310,140,1310,142,1290,142,1271,142,1290,142,1271,144,1252,144,1234,144,1252,146,1214,146,1233,146,1213,148,1198,148,1187,148,1198,148,1186,150,1178,150,1176,152,1168,152,1156,152,1166,152,1156,154,1144,154,1144,156,1131,156,1131,158,1109,158,1120,158,1108,164,362,164,362,166,357,166,355,166,357,166,355,168,356,168,358,168,356,168,358,174,1036,176,1028,176,1023,176,1027,176,1023,178,1020,178,1020,180,1018,180,1016,180,1017,180,1015,182,1015,182,1015,184,1015,184,1013,184,1014,184,1013,186,1012,186,1012,186,1012,188,1013,188,1012,188,1013,190,1015,190,1016,190,1014,192,1017,192,1016,192,1018,194,1019,194,1022,194,1020,196,1026,196,1023,196,1027,198,1030,198,1035,198,1032,200,1042,200,1037,200,1043,202,1050,202,1051,208,313,210,309,210,312,210,309,212,308,212,308,212,308,212,309,214,311,224,986,226,974,226,978,226,965,228,952,228,929,228,942,230,907,230,919,230,899,232,888,232,869,232,880,234,850,234,859,234,840,236,831,236,814,236,823,238,799,238,806,240,782,240,767,240,790,240,775,242,754,242,761,242,748,244,743,244,737,246,731,246,721,246,726,248,713,248,716,248,707,250,703,250,694,250,699,252,687,252,691,252,684,254,681,254,675,254,678,256,669,256,672,256,667,260,657,260,654,260,658,260,655,262,651,262,652,264,649,264,648,264,650,264,648,266,646,266,646,268,645,268,645,268,645,268,645,270,645,270,645,272,646,272,647,272,645,272,646,274,648,274,648,274,650,276,650,276,653,276,652,278,656,278,655,278,658,280,659,280,663,280,661,282,666,282,665,282,669,284,670,284,674,284,673,286,679,286,684,286,677,286,682,288,690,288,687,290,696,290,704,290,693,290,701,292,711,292,709,294,719,294,728,294,716,294,724,296,736,296,732,296,742,298,744,298,754,298,750,300,763,300,760,300,770,302,773,302,783,302,780,304,793,304,791,304,802,306,805,306,815,306,813,308,827,308,840,308,825,308,837,310,852,310,851,312,866,312,881,312,864,312,878,314,897,314,895,314,911,316,916,316,937,316,930,318,958,318,951,318,975,320,979,320,1003,320,999,322,1032,322,1063,322,1024,338,1341,344,1582,350,759,350,761,350,743,352,742,354,718,354,719,356,711,356,706,356,711,356,706,358,703,358,701,358,703,358,701]g = [0,1328,0,1327,2,1329,2,1328,2,1330,4,1331,4,1333,4,1332,6,1335,6,1335,8,1338,8,1341,8,1337,8,1340,10,1348,10,1346,12,1356,12,1364,12,1354,12,1363,14,1372,14,1371,16,1382,16,1393,16,1381,16,1393,18,1405,18,1404,20,1415,20,1427,20,1414,20,1426,22,1440,22,1438,24,1453,24,1467,24,1451,24,1464,26,1485,26,1481,28,1506,28,1524,28,1500,30,1543,32,1569,34,1593,34,1616,36,1643,38,1676,66,1836,68,1836,68,1833,70,1828,72,1826,72,1828,78,1493,106,28,110,456,110,462,110,477,110,479,112,459,114,477,114,486,116,499,118,470,120,464,120,464,120,469,122,466,122,470,122,465,124,446,126,437,126,441,128,430,128,428,128,434,128,431,130,429,132,431,132,433,132,433,134,431,134,428,134,431,136,428,136,429,136,429,138,430,138,428,138,427,140,425,140,424,140,425,142,427,142,428,142,427,144,425,146,415,146,419,146,418,146,416,148,423,148,419,150,419,150,417,150,415,150,414,152,419,152,421,154,419,154,425,154,421,154,423,156,429,156,427,240,1991,240,1996,240,1971,242,1968,242,1947,242,1951,250,1537,250,1540,252,1533,252,1534,254,1525,254,1517,254,1525,254,1518,256,1508,256,1509,260,1486,260,1487,262,1481,262,1482,262,1478,264,1477,264,1475,264,1475,266,1474,266,1473,266,1472,268,1474,268,1472,268,1472,270,1472,270,1472,272,1474,272,1476,272,1474,272,1476,274,1479,274,1478,276,1482,276,1487,276,1482,276,1486,278,1494,278,1492,280,1501,280,1509,280,1499,280,1507,282,1517,282,1515,284,1524,284,1523,284,1531,286,1533,286,1545,286,1541,288,1558,288,1553,288,1570,290,1573,290,1589,290,1585,292,1604,292,1599,292,1615,294,1620,294,1638,296,1657,296,1653,296,1679,298,1683,298,1713,298,1704,300,1739,300,1733,300,1758,302,1764,302,1790,302,1784,304,1813,306,1843,306,1838,308,1917,308,1909,310,1954,310,1971,310,1947,310,1971,312,1950,312,1956,314,1900,314,1856,314,1909,314,1862,316,1820,316,1825,318,1783,318,1787,318,1754,320,1746,320,1712,320,1720,322,1674,322,1680,322,1644,324,1638,324,1612,324,1616,326,1588,326,1593,328,1561,328,1537,328,1566,328,1541,330,1518,330,1522,332,1499,332,1479,332,1503,332,1483,334,1461,336,1448,336,1450,336,1437,338,1435,338,1425,338,1426,340,1413,340,1414,340,1403,342,1402,342,1391,342,1392,344,1379,344,1382,346,1369,346,1361,346,1371,346,1362,348,1354,348,1353,350,1346,350,1345,352,1336,352,1337,354,1334,354,1332,354,1334,354,1332,356,1330,356,1330,358,1329,358,1328,358,1328,358,1327,0,1328,0,1327,2,1329,2,1328,2,1330,4,1331,4,1333,4,1332,6,1335,6,1335,8,1338,8,1341,8,1337,8,1340,10,1348,10,1346,12,1356,12,1364,12,1354,12,1363,14,1372,14,1371,16,1382,16,1393,16,1381,16,1393,18,1405,18,1404,20,1415,20,1427,20,1414,20,1426,22,1440,22,1438,24,1453,24,1467,24,1451,24,1464,26,1485,26,1481,28,1506,28,1524,28,1500,30,1543,32,1569,34,1593,34,1616,36,1643,38,1676,66,1836,68,1836,68,1833,70,1828,72,1826,72,1828,78,1493,106,28,110,456,110,462,110,477,110,479,112,459,114,477,114,486,116,499,118,470,120,464,120,464,120,469,122,466,122,470,122,465,124,446,126,437,126,441,128,430,128,428,128,434,128,431,130,429,132,431,132,433,132,433,134,431,134,428,134,431,136,428,136,429,136,429,138,430,138,428,138,427,140,425,140,424,140,425,142,427,142,428,142,427,144,425,146,415,146,419,146,418,146,416,148,423,148,419,150,419,150,417,150,415,150,414,152,419,152,421,154,419,154,425,154,421,154,423,156,429,156,427,240,1991,240,1996,240,1971,242,1968,242,1947,242,1951,250,1537,250,1540,252,1533,252,1534,254,1525,254,1517,254,1525,254,1518,256,1508,256,1509,260,1486,260,1487,262,1481,262,1482,262,1478,264,1477,264,1475,264,1475,266,1474,266,1473,266,1472,268,1474,268,1472,268,1472,270,1472,270,1472,272,1474,272,1476,272,1474,272,1476,274,1479,274,1478,276,1482,276,1487,276,1482,276,1486,278,1494,278,1492,280,1501,280,1509,280,1499,280,1507,282,1517,282,1515,284,1524,284,1523,284,1531,286,1533,286,1545,286,1541,288,1558,288,1553,288,1570,290,1573,290,1589,290,1585,292,1604,292,1599,292,1615,294,1620,294,1638,296,1657,296,1653,296,1679,298,1683,298,1713,298,1704,300,1739,300,1733,300,1758,302,1764,302,1790,302,1784,304,1813,306,1843,306,1838,308,1917,308,1909,310,1954,310,1971,310,1947,310,1971,312,1950,312,1956,314,1900,314,1856,314,1909,314,1862,316,1820,316,1825,318,1783,318,1787,318,1754,320,1746,320,1712,320,1720,322,1674,322,1680,322,1644,324,1638,324,1612,324,1616,326,1588,326,1593,328,1561,328,1537,328,1566,328,1541,330,1518,330,1522,332,1499,332,1479,332,1503,332,1483,334,1461,336,1448,336,1450,336,1437,338,1435,338,1425,338,1426,340,1413,340,1414,340,1403,342,1402,342,1391,342,1392,344,1379,344,1382,346,1369,346,1361,346,1371,346,1362,348,1354,348,1353,350,1346,350,1345,352,1336,352,1337,354,1334,354,1332,354,1334,354,1332,356,1330,356,1330,358,1329,358,1328,358,1328,358,1327]
h = [2,746,2,748,4,729,4,732,6,716,6,708,6,719,6,709,8,702,8,702,10,697,10,694,10,698,10,695,12,693,12,693,14,693,14,695,14,692,14,694,16,697,16,697,18,701,18,706,18,701,18,706,20,715,20,713,22,727,22,742,22,725,22,741,24,760,24,758,28,1505,30,1519,30,1527,32,1547,32,1551,102,734,102,730,102,731,104,732,104,734,104,729,104,734,106,737,106,741,108,744,108,743,108,744,110,748,110,751,110,749,112,754,112,754,112,758,114,758,114,764,114,762,116,770,116,775,116,768,116,773,118,782,118,779,120,790,120,785,120,790,122,797,134,445,134,443,136,441,136,440,138,441,138,444,138,440,138,442,140,448,140,446,142,456,142,454,144,1431,144,1437,146,1403,146,1406,146,1381,148,1376,148,1365,156,255,158,256,158,256,158,257,158,255,158,256,158,257,160,257,160,257,162,258,162,258,162,258,162,258,164,257,166,258,166,260,166,262,166,259,166,262,168,265,168,264,170,267,170,268,170,266,170,268,172,270,172,273,172,270,172,273,174,276,174,275,176,279,176,277,176,281,178,282,178,285,178,285,180,290,180,288,180,293,182,298,184,301,190,1080,190,1078,190,1079,192,1082,192,1082,192,1086,194,1087,194,1092,194,1091,202,272,202,304,202,305,202,304,204,302,204,302,204,301,204,301,204,300,206,302,206,304,206,302,206,301,208,305,208,310,208,304,208,309,214,996,214,993,216,1016,216,1016,220,960,222,903,222,913,222,915,224,877,224,888,224,862,224,888,224,862,226,852,226,831,226,839,226,839,228,812,228,819,228,800,228,820,228,801,230,794,230,777,230,783,230,783,232,761,232,768,232,753,232,768,232,753,234,747,234,733,234,739,234,739,236,725,236,726,236,712,238,707,238,694,238,712,238,699,238,699,240,683,240,688,240,687,242,672,242,661,242,676,242,665,242,676,242,665,244,652,244,656,244,656,246,643,246,646,246,638,246,646,246,638,248,634,248,625,248,629,248,629,250,618,250,621,250,614,250,621,250,614,252,611,252,604,252,607,252,607,254,597,254,600,254,593,254,600,254,593,256,590,256,585,256,588,256,587,260,574,260,569,260,576,260,571,260,576,260,571,262,563,262,566,262,566,264,559,264,560,264,556,264,560,264,556,266,555,266,552,266,553,266,553,268,548,268,549,268,546,268,549,268,546,270,545,270,542,270,544,270,543,272,540,272,541,272,538,272,540,274,538,274,536,274,537,276,534,276,534,276,532,278,531,278,529,278,530,280,527,280,527,280,525,282,525,282,523,282,523,284,521,284,522,284,520,286,519,286,518,286,519,288,517,288,518,290,517,290,515,290,516,290,515,292,515,292,515,294,516,294,516,294,516,294,516,296,517,296,517,298,519,298,521,298,518,298,520,300,523,300,522,302,525,302,528,302,524,302,527,304,532,304,530,304,534,306,536,306,540,306,539,308,546,308,544,310,552,310,558,310,550,310,557,312,565,312,564,312,570,314,572,314,580,314,578,316,588,316,586,316,594,318,596,318,605,318,603,320,615,320,612,320,622,322,625,322,637,322,634,324,650,324,647,324,662,326,666,326,683,326,679,328,702,328,697,328,719,334,832,336,858,336,852,352,1152]
i = [0,1613,2,1617,4,839,6,816,8,799,8,785,12,777,16,846,20,898,20,923,26,1726,26,1733,28,1741,28,1770,30,1802,32,1817,32,1832,34,1850,36,1877,40,426,42,416,46,393,46,385,48,376,52,359,52,354,54,349,56,341,56,334,58,329,60,325,62,312,62,319,62,315,64,311,66,306,68,300,68,302,68,299,70,298,70,296,72,294,72,295,74,291,74,293,74,291,78,288,80,284,80,286,82,283,82,282,86,281,86,280,122,1679,124,1622,124,1631,124,1589,126,1583,128,1484,128,1496,128,1459,130,1454,130,1424,130,1432,132,1393,132,1402,132,1369,134,1364,134,1336,134,1342,136,1307,136,1313,136,1285,138,1277,138,1246,138,1254,140,1221,140,1225,140,1205,142,1196,142,1175,142,1182,144,1158,146,1136,146,1119,146,1140,146,1123,148,1105,148,1106,150,1088,150,1073,150,1091,150,1076,152,1061,152,1064,154,1047,154,1035,154,1049,154,1036,156,1026,156,1026,158,1015,158,1006,158,1017,158,1009,160,997,160,998,162,986,162,974,162,988,162,975,164,963,164,964,166,957,166,952,166,958,166,953,168,948,168,948,170,944,170,940,170,944,170,940,172,936,172,936,174,933,174,930,174,933,174,929,176,927,176,928,178,924,178,922,178,924,178,922,180,922,180,923,182,921,182,921,182,923,182,921,184,922,184,921,186,923,186,923,186,922,186,923,188,926,188,926,188,930,190,930,190,933,190,932,192,937,192,936,192,941,194,941,194,947,194,946,196,951,204,723,204,723,206,730,206,737,206,729,206,737,208,746,208,745,210,757,210,767,210,755,210,766,212,777,212,774,214,788,214,798,214,784,214,801,216,789,218,767,218,768,222,694,222,698,222,678,224,652,226,633,226,635,228,615,228,599,228,600,230,585,230,587,232,572,232,558,232,572,232,558,234,545,234,547,236,533,236,521,236,535,236,522,238,511,238,513,240,503,240,506,240,498,242,496,242,487,242,488,244,480,244,482,246,474,246,465,246,475,246,469,248,457,248,460,250,451,250,447,250,452,250,447,252,443,252,443,254,439,254,436,254,439,254,436,256,433,256,433,260,425,260,425,262,423,262,422,262,423,262,422,264,421,264,421,266,419,266,419,266,420,266,419,268,419,268,418,270,419,270,418,270,419,270,419,272,418,272,419,274,418,274,419,274,419,274,419,276,421,276,421,278,423,278,425,278,423,278,425,280,426,280,426,282,428,282,432,282,428,282,432,284,436,284,436,286,438,286,441,286,438,286,441,288,445,288,449,288,445,288,448,290,453,290,452,292,459,292,465,292,457,294,470,294,463,294,468,296,477,296,483,296,474,296,482,298,489,298,488,300,494,300,500,300,494,300,500,302,508,304,517,304,526,304,516,304,526,306,537,306,537,308,547,308,556,308,547,308,556,310,566,310,564,310,575,312,578,312,589,314,599,316,631,318,644,318,657,322,697,322,714,324,733,326,755,328,801,344,1362,346,1355,346,1341,354,1606,354,1605,356,1605,358,1610,358,1610]
j = [88,1329,90,1315,90,1317,90,1279,90,1233,92,1322,92,1270,92,1271,92,1222,92,1220,94,1379,94,1380,94,1328,94,1334,94,1278,94,1223,96,1387,96,1342,96,1287,96,1297,96,1226,96,1230,98,1395,98,1403,98,1353,98,1363,98,1307,98,1237,100,1419,100,1313,100,1245,100,1251,102,1439,102,1377,102,1393,102,1329,102,1257,104,1452,104,1404,104,1419,104,1346,104,1358,104,1264,106,1651,106,1673,106,1586,106,1462,106,1477,106,1434,106,1369,106,1281,108,1696,108,1599,108,1446,108,1461,108,1388,108,1300,108,1309,110,1718,110,1619,110,1646,110,1478,110,1413,110,1320,110,1332,112,1833,112,1939,112,1954,112,1802,112,1670,112,1582,112,1494,112,1515,112,1429,112,1442,112,1347,114,1860,114,1968,114,1995,114,1832,114,1864,114,1693,114,1719,114,1535,114,1468,114,1363,114,1377,116,1889,116,1923,116,1902,116,1758,116,1637,116,1660,116,1608,116,1495,118,1950,118,1939,118,1975,118,1802,118,1834,118,1683,118,1630,118,1656,118,1517,118,1548,118,1423,120,1986,120,1867,120,1708,120,1681,120,1585,120,1444,122,1831,122,1707,122,1616,122,1640,122,1461,124,1979,124,1867,124,1800,124,1667,126,1905,126,1835,126,1875,126,1534,126,1566,128,1917,128,1793,128,1604,128,1636,130,1955,130,1830,130,1867,130,1663,132,1911,132,1693,134,1793,136,1832,136,1873,138,1918,140,1963,152,235,152,234,156,247,160,244,162,242,166,244,168,250,176,222,188,705,190,705,190,709,192,711,194,714,194,717,196,723,198,729,198,735,200,742,202,749,202,758,204,768,204,776,206,786,208,250,208,797,210,245,212,240,214,235,214,231,214,229,214,279,216,159,216,161,216,275,218,229,220,229,220,229,222,228,222,227,222,259,224,254,224,252,226,225,226,252,226,251,228,223,228,223,228,250,230,223,232,223,232,248,232,246,234,225,234,244,236,229,236,244,238,246,248,1369,254,186,254,169,256,186,256,165,256,162,260,181,260,157,260,155,260,148,262,179,262,179,264,152,264,151,264,139,264,138,264,137,266,150,266,136,268,148,268,148,268,133,268,131,270,148,270,130,272,148,272,130,272,129,274,128,276,128,276,125,278,127,278,127,278,123,278,118,278,115,280,127,280,127,280,113,280,112,282,127,356,248]
f=[0,1648,0,1650,0,1648,0,1650,0,1648,0,1650,2,1653,2,1653,4,1656,4,1656,4,1658,4,1656,4,1658,8,1480,8,1480,8,1480,10,1486,10,1486,18,1515,18,1515,36,873,36,871,36,870,38,848,38,818,38,820,40,799,40,796,40,796,42,777,42,757,42,775,42,756,42,775,44,742,44,739,44,756,46,731,46,720,48,703,48,688,50,675,52,663,52,650,54,640,56,632,58,627,114,596,114,598,116,604,118,613,118,622,120,631,120,624,120,632,122,641,122,651,122,642,122,652,124,661,124,664,126,674,126,676,128,688,128,701,128,689,128,704,130,715,130,717,130,733,132,730,132,747,132,751,134,764,134,785,134,768,134,787,136,805,136,809,138,824,138,848,138,830,140,873,140,852,140,879,142,905,144,937,144,938,146,920,146,919,148,905,148,889,148,903,148,887,150,877,150,876,152,867,152,865,160,243,160,239,162,243,162,239,164,238,164,239,164,237,166,240,166,243,166,237,166,239,168,241,174,770,174,769,174,770,174,770,176,769,176,769,178,769,178,770,178,769,180,771,180,770,180,771,182,772,182,774,182,772,184,777,184,775,184,777,186,782,186,782,194,385,194,385,194,382,196,373,196,375,198,371,198,373,200,373,200,378,204,918,204,920,206,933,206,935,208,947,208,963,208,950,208,966,210,980,210,981,212,998,212,1020,212,1001,212,1023,214,1042,214,1044,216,1065,216,1092,218,1114,226,239,226,235,228,235,230,234,230,234,232,222,232,220,234,217,234,216,238,1652,242,400,242,399,244,399,244,398,246,395,246,391,246,396,246,392,248,388,248,386,250,386,250,383,250,384,252,382,252,383,252,380,254,380,254,379,256,378,256,376,256,379,256,378,260,44,260,373,260,373,260,373,260,373,260,372,262,373,262,373,262,372,264,372,264,373,264,372,264,372,264,372,266,372,266,372,266,372,266,372,268,372,268,372,268,372,268,371,270,372,270,372,270,372,270,372,270,372,272,373,272,373,272,373,272,373,274,374,274,373,274,374,274,373,274,374,276,375,276,375,276,375,278,377,278,378,278,376,278,378,278,377,278,380,280,380,280,380,280,382,282,382,282,384,282,382,282,384,282,383,282,385,284,385,284,385,284,388,284,388,286,388,286,392,286,392,286,391,286,395,288,396,288,395,288,401,288,398,290,401,290,402,302,1496,304,1501,304,1474,304,1476,304,1495,304,1472,306,1466,306,1461,306,1458,308,1458,308,1452,308,1454,308,1453,308,1454,308,1453,312,925,312,924,312,924,314,905,314,889,314,904,314,885,314,904,314,888,316,872,316,867,316,872,318,854,318,839,318,853,318,842,318,853,318,839,320,826,320,823,322,808,322,806,324,791,324,777,324,792,324,774,324,785,324,775,326,763,326,765,328,763,328,762,328,765,340,1732,340,1731,342,1714,342,1696,342,1713,342,1696,342,1711,342,1694,344,1684,344,1684,344,1683,346,1674,346,1668,346,1674,346,1668,346,1674,346,1668,348,1666,348,1665,348,1665,350,1663,350,1662,350,1663,352,1651,352,1652,352,1651,354,1647,354,1647,354,1646,356,1644,356,1643,356,1643,356,1643,356,1644,356,1643,358,1645,358,1645,358,1645,0,1648,0,1650,0,1648]a=b
a = a[:720]
random_t = a[::2]
random_r = a[1::2]
SIZE = len(random_r)for i in range(SIZE):random_t[i] = -random_t[i] * np.pi / 180random_x = []
random_y = []
random_x = random_r * np.cos(random_t)
random_y = random_r * np.sin(random_t)
RANDOM_T = np.array(random_t)
RANDOM_R = np.array(random_r)
RANDOM_X = np.array(random_x)
RANDOM_Y = np.array(random_y)# 使用强化RANSAC算法估算模型
u = 226
sigma = 0.000015
sigma_line = 0.0005
num = 1best_x = 0
best_y = 0
a=0
b=0
pretotal = 0
x=[]
y=[]
xx=[]
yy=[]
is_line = []
for i in range(SIZE):is_line.append(0)for time in range(0,SIZE,num):sample_index = [time%SIZE,(time+num)%SIZE]x_1 = RANDOM_X[sample_index[0]]x_2 = RANDOM_X[sample_index[1]]y_1 = RANDOM_Y[sample_index[0]]y_2 = RANDOM_Y[sample_index[1]]# 求解出a,ba = (y_2 - y_1) / (x_2 - x_1)b = y_1 - a * x_1# 算出内点数目for index in range(SIZE):if abs(a * RANDOM_X[index] + b - RANDOM_Y[index])/np.sqrt(a**2 + b**2) < sigma_line * RANDOM_R[index]:is_line[index] += 1for time in range(0,SIZE,num):sample_index = [time%SIZE,(time+num)%SIZE]x_1 = RANDOM_X[sample_index[0]]x_2 = RANDOM_X[sample_index[1]]y_1 = RANDOM_Y[sample_index[0]]y_2 = RANDOM_Y[sample_index[1]]# 求解出x,ya = (y_2 - y_1) / (x_2 - x_1)x_0 = (x_1 + x_2) / 2y_0 = (y_1 + y_2) / 2temp1 = np.sqrt(u**2 - 0.25*(y_2 - y_1)**2 - 0.25*(x_2 - x_1)**2)tempcos = 1/np.sqrt(1/a**2+1)tempsin = (-1/a)*tempcostempdx = temp1 * tempcostempdy = temp1 * tempsintempx1 = x_0 - tempdxtempx2 = x_0 + tempdxtempy1 = y_0 - tempdytempy2 = y_0 + tempdy# 算出内点数目total_inlier = 0for index in range(SIZE):if abs(np.sqrt((RANDOM_X[index]-tempx1)**2 + (RANDOM_Y[index]-tempy1)**2)-u) < sigma*RANDOM_R[index]*RANDOM_R[index]:x.append(RANDOM_X[index])y.append(RANDOM_Y[index])total_inlier += 1 / is_line[index]# 判断当前的模型是否比之前估算的模型好if total_inlier > pretotal:pretotal = total_inlierbest_x = tempx1best_y = tempy1xx = xyy = yelse:x=[]y=[]#total_inlier = 0for index in range(SIZE):if abs(np.sqrt((RANDOM_X[index]-tempx2)**2 + (RANDOM_Y[index]-tempy2)**2)-u) < sigma*RANDOM_R[index]*RANDOM_R[index]:x.append(RANDOM_X[index])y.append(RANDOM_Y[index])total_inlier += 1 / is_line[index]# 判断当前的模型是否比之前估算的模型好if total_inlier > pretotal:pretotal = total_inlierbest_x = tempx2best_y = tempy2xx = xyy = yelse:x=[]y=[]f = [0,0]for i in range(SIZE):f[0] += 100000*RANDOM_X[i] / (RANDOM_R[i])**3f[1] += 100000*RANDOM_Y[i] / (RANDOM_R[i])**3#作图显示
print(pretotal)
ax1 = plt.subplot(111, projection='polar')
ax1.scatter(RANDOM_T, RANDOM_R, s=6,c=is_line,cmap="coolwarm")
for i in range(len(xx)):ax1.scatter(np.arctan2(yy[i],xx[i]), np.sqrt(yy[i]**2+xx[i]**2), s=10, c="black")
#
# ax1.scatter(np.arctan2(f[1],f[0]), np.sqrt(f[1]**2+f[0]**2), s=20, c="y")
plt.show()

普通RANSAC算法版本
 

import random
import math
import numpy as np
import matplotlib.pyplot as plt# 生成数据集
distance = 3
c = 2 - np.sqrt(2)
u = np.sqrt(2)
SIZE = 90
theta = np.linspace(0 / 180 * np.pi, 90 / 180 * np.pi, SIZE)
r = distance / np.cos(theta - (45 / 180 * np.pi))
#r = -(((c + u) * np.cos(theta)) ** 2 + u ** 2 - (c + u) ** 2) ** (1/2) + (c + u) * np.cos(theta)
random_t = []
random_r = []
random_x = []
random_y = []# 加上噪音
delta = 0.03
for i in range(SIZE):random_t.append(theta[i] + random.uniform(-delta, delta))random_r.append(r[i] + random.uniform(delta, delta))
for i in range(SIZE):random_t.append(theta[i] + random.uniform(-delta, delta))random_r.append(r[i] + random.uniform(delta, delta))random_x = random_r * np.cos(random_t)
random_y = random_r * np.sin(random_t)RANDOM_T = np.array(random_t)
RANDOM_R = np.array(random_r)
RANDOM_X = np.array(random_x)
RANDOM_Y = np.array(random_y)# 使用RANSAC算法估算模型
# 迭代最大次数
iters = 10000
time = 0
# 数据和模型之间可接受的差值
sigma = 0.01
# 最好模型的参数估计和内点数目
best_a = 0
best_b = 0
pretotal = 0
# 希望的得到正确模型的概率
P = 0.99
while(time<=iters):time+=1# 随机在数据中红选出两个点去求解模型sample_index = random.sample(range(SIZE * 2),2)x_1 = RANDOM_X[sample_index[0]]x_2 = RANDOM_X[sample_index[1]]y_1 = RANDOM_Y[sample_index[0]]y_2 = RANDOM_Y[sample_index[1]]# y = ax + b 求解出a，ba = (y_2 - y_1) / (x_2 - x_1)b = y_1 - a * x_1# 算出内点数目total_inlier = 0for index in range(SIZE * 2):y_estimate = a * RANDOM_X[index] + bif abs(y_estimate - RANDOM_Y[index]) < sigma:total_inlier = total_inlier + 1# 判断当前的模型是否比之前估算的模型好if total_inlier > pretotal:iters = math.log(1 - P) / math.log(1 - pow(total_inlier / (SIZE * 2), 2))pretotal = total_inlierbest_a = abest_b = b# 画图
ax1 = plt.subplot(111, projection='polar')
ax1.scatter(random_t, random_r, s=5, c="r")best_f = np.arctan(-1 / best_a)
best_d = np.pi * abs(b) / (best_a**2 + best_b**2)**(1/2)R = best_d / np.cos(RANDOM_T - best_f)
ax1.plot(RANDOM_T, R)
text1 = "acc = " + str(round((pretotal / (2 * SIZE)), 2))
text2 = "times = " + str(time)
text3 = round((pretotal / (2 * SIZE)/time), 2)
#ax.text(1.05, 0.1, text1, transform=ax.transAxes)
#ax.text(1.05, 0, text2, transform=ax.transAxes)
ax1.text(1.05, 0, text2, transform=ax1.transAxes)Y =  best_a * RANDOM_X + best_b
ax2 = plt.figure().add_subplot(1,1, 1)
ax2.plot(RANDOM_X, Y)
ax2.scatter(RANDOM_X, RANDOM_Y)
plt.show()

c语言版本后期会上传到下载中，请持续关注。