這次又更新了軟件，下載請看置頂的博客。
好了，說一下本次代碼的主要更新內容：
1.加入了很多很多的QT5.9的測試代碼，將一些官方的例子演示加入進來，放到了幫助菜單下。
2.利用opengl在界面前繪製了一個漸變的正方形，學會了如何在QT中進行紋理載入和使用。
3.加入了擬合球的代碼，用所有點雲擬合一個球。
4.重新調整了燈光，讓燈光從相機（也就是咱們自己）打出，這樣物體就總是明亮的。不會出現因爲旋轉而導致的場景暗淡。
老規矩我們首先上一些效果：
好，我們上乾貨。首先是擬合球代碼：
//#define FLT_EPSILON 1.192092896e-07F /* smallest such that 1.0+FLT_EPSILON != 1.0 */
#ifndef ZERO_TOLERANCE
#define ZERO_TOLERANCE static_cast<double>(FLT_EPSILON)
#endifbool refineSphereLS(vector<QPt>cloud, Vector3d center,
double radius, double minReltaiveCenterShift)
{if (cloud.size() == 0 || cloud.size() < 5)
{return false;
}Vector3d c; c = center;
unsigned count = cloud.size();
Vector3d G; G.setZero();
//計算這些好點的質心//compute barycenter
{for (unsigned i = 0; i < count; i++)
{Vector3d P;
P << cloud[i].x, cloud[i].y, cloud[i].z;
G += P;
}G /= count;
}static const unsigned MAX_ITERATIONS = 200;
for (unsigned it = 0; it < MAX_ITERATIONS; ++it)
{// Compute average L, dL/da, dL/db, dL/dc.
double meanNorm = 0.0;
Vector3d derivatives; derivatives.setZero();
unsigned realCount = 0;
for (unsigned i = 0; i < count; ++i)
{Vector3d Pi;
Pi << cloud[i].x, cloud[i].y, cloud[i].z;
Vector3d Di = Pi - c;
double norm = Di.norm();
if (norm < ZERO_TOLERANCE)
continue;
meanNorm += norm;
derivatives = Di / norm;
++realCount; }meanNorm /= count;
derivatives /= count;
//backup previous center(備份中心)
Vector3d c0 = c;
//deduce new center
c = G - derivatives * meanNorm;
double r = meanNorm;
double shift = (c - c0).norm();
double relativeShift = shift / r;
if (relativeShift < minReltaiveCenterShift)
break;
}}
int dmat_solve2(int n, int rhs_num, double a[])
{for (int j = 0; j < n; j++)
{// Choose a pivot row.
int ipivot = j;
double apivot = a[j + j*n];
for (int i = j; i < n; i++)
{if (fabs(apivot) < fabs(a[i + j*n]))
{apivot = a[i + j*n];
ipivot = i;
} }if (apivot == 0.0)
{return j;
}// Interchange.
for (int i = 0; i < n + rhs_num; i++)
{std::swap(a[ipivot + i*n], a[j + i*n]);
}// A(J,J) becomes 1.
a[j + j*n] = 1.0;
for (int k = j; k < n + rhs_num; k++)
{a[j + k*n] = a[j + k*n] / apivot;
}// A(I,J) becomes 0.
for (int i = 0; i < n; i++)
{if (i != j)
{double factor = a[i + j*n];
a[i + j*n] = 0.0;
for (int k = j; k < n + rhs_num; k++)
{a[i + k*n] = a[i + k*n] - factor * a[j + k*n];
} } } }return 0;
}
//Fit - sphere - through - points.pdf
bool computeSphereFrom4(
const QPt PointA, const QPt PointB,
const QPt PointC, const QPt PointD,
Vector3d ¢er,//球心
double&radius//球半徑
){//inspired from 'tetrahedron_circumsphere_3d' by Adrian Bowyer and John Woodwark
//Set up the linear system.
double a[12];
Vector3d VPointA;
Vector3d VPointB;
Vector3d VPointC;
Vector3d VPointD;
VPointA << PointA.x, PointA.y, PointA.z;
VPointB << PointB.x, PointB.y, PointB.z;
VPointC << PointC.x, PointC.y, PointC.z;
VPointD << PointD.x, PointD.y, PointD.z;
{Vector3d AB = VPointB - VPointA;
a[0] = AB.x();
a[3] = AB.y();
a[6] = AB.z();
a[9] = AB.squaredNorm();
} {Vector3d AC = VPointC - VPointA;
a[1] = AC.x();
a[4] = AC.y();
a[7] = AC.z();
a[10] = AC.squaredNorm();
} {Vector3d AD = VPointD - VPointA;
a[2] = AD.x();
a[5] = AD.y();
a[8] = AD.z();
a[11] = AD.squaredNorm();
}// Solve the linear system (with Gauss-Jordan elimination)
if (dmat_solve2(3, 1, a) != 0)
{//system is singular?判斷是不是奇異的
return false;
}// Compute the radius and center.
Vector3d u;
u << a[0 + 3 * 3] / 2.0, a[1 + 3 * 3] / 2.0, a[2 + 3 * 3] / 2.0;//指向圓心的一個向量：
radius = u.norm();
center = VPointA + u;
return true;
}
bool CCdetectSphereRobust(QPt*&cloudPtr ,int PtNum,
double outliersRatio,//離羣值
Vector3d ¢er,//球心
double&radius,//球半徑
double&rms,//
double confidence)
{if (PtNum < 4)
{//invalid input
return false;
}assert(confidence < 1.0);
confidence = std::min(confidence, 1.0 - FLT_EPSILON);
const unsigned p = 4;
//unsigned n = cloud.size();
unsigned n = PtNum;
//Reload cloud
vector<QPt>cloud;
for(int i=0;i<PtNum;i++)
cloud.push_back(cloudPtr[i]);//we'll need an array (sorted) to compute the medians
vector<double >values;
values.resize(n);//計算採樣次數m：
unsigned m = 1;
if (n > p)
m = static_cast<unsigned>(log(1.0 - confidence) / log(1.0 - pow(1.0 - outliersRatio, static_cast<double>(p))));
//now we are going to randomly extract a subset of 4 points and test the resulting sphere each time
std::random_device rd; // non-deterministic generator非確定性隨機數(C++11新特性)
std::mt19937 gen(rd()); // to seed mersenne twister. 週期長度通常取Mersenne質數
std::uniform_int_distribution<unsigned> dist(0, n - 1);//將隨機數映射到1~n-1
unsigned sampleCount = 0;
unsigned attempts = 0;
double minError = -1.0;
while (sampleCount < m && attempts < 2 * m)
{//get 4 random (different) indexes
unsigned indexes[4] = { 0, 0, 0, 0 };
for (unsigned j = 0; j<4; ++j)
{bool isOK = false;
while (!isOK)
{indexes[j] = dist(gen);
isOK = true;
for (unsigned k = 0; k<j && isOK; ++k)
if (indexes[j] == indexes[k])
isOK = false;
} }const QPt PointA = cloud[indexes[0]];
const QPt PointB = cloud[indexes[1]];
const QPt PointC = cloud[indexes[2]];
const QPt PointD = cloud[indexes[3]];
++attempts;Vector3d thisCenter;//球心
double thisRadius;//球半徑
if (!computeSphereFrom4(PointA, PointB, PointC, PointD, thisCenter, thisRadius))
continue;
//compute residuals
for (unsigned i = 0; i < n; ++i)
{double error;
Vector3d EachPointV;
EachPointV << cloud[i].x, cloud[i].y, cloud[i].z;
error = (EachPointV - thisCenter).norm() - thisRadius;
values[i] = error*error;
}std::sort(values.begin(), values.end());
//the error is the median of the squared residuals
double error = values[n / 2];
//we keep track of the solution with the least error
if (error < minError || minError < 0.0)
{minError = error;
center = thisCenter;
radius = thisRadius;
} ++sampleCount; }//too many failures?!
if (sampleCount < m)
{return false;
}//last step: robust estimation
vector<QPt>candidates;//挑出候選點
if (n > p)
{double sigma = 1.4826 * (1.0 + 5.0 / (n - p)) * sqrt(minError);
//compute the least-squares best-fitting sphere with the points
//having residuals below 2.5 sigma
double maxResidual = 2.5 * sigma;
//compute residuals and select the points
//candidates.resize(n);
for (unsigned i = 0; i<n; ++i)
{Vector3d EachPointV;
EachPointV << cloud[i].x, cloud[i].y, cloud[i].z;
double error = abs((EachPointV - center).norm() - radius);
if (error < maxResidual)
candidates.push_back(cloud[i]); }//eventually estimate the robust sphere parameters with least squares (iterative)
if (refineSphereLS(candidates, center, radius, 1.0e-3))
{//replace input cloud by this subset!
vector<QPt>().swap(cloud);//
for (int i = 0; i < candidates.size(); i++)
cloud.push_back(candidates[i]);n = cloud.size();
} }//update residuals
{double residuals = 0;
for (unsigned i = 0; i<n; ++i)
{Vector3d P;
P << cloud[i].x, cloud[i].y, cloud[i].z;
double e = (P - center).norm() - radius;
residuals += e*e;
}rms = sqrt(residuals / n);
}return true;
}
bool Q_ScarletCloudIO::Do_Fit_Sphere(int StationIndex,int CloudIndex)
{qDebug()<<"Do Sphere Fitting for cloud:"<<StationIndex<<" "<<CloudIndex;
QPt *CloudPtr =m_QClouds[CloudIndex].PtCloud;
int PtNum = m_QClouds[CloudIndex].PtNum;
Vector3d center; center.setZero();//圓心
double radius;//半徑
double rms;//殘差
double outliersRatio = 0.5;
double confidence = 0.99;
bool success = CCdetectSphereRobust(CloudPtr,PtNum,//點雲數據
outliersRatio,//離羣數值
center,//out圓心
radius,//out半徑
rms,//out殘差
confidence);//置信值？
NINBallObject* BallObject = new NINBallObject();
//base information
//PolygonObject->set_Vertices(m_2DPolygonFitter-> get_hullpoints3D());
//PolygonObject->set_Indexes();//we have no Indexes here...按順序繪製即可
BallObject->set_StationIndex(StationIndex); BallObject->set_CloudIndex(CloudIndex);BallObject->set_ObjectIndex(m_BallVec.size());
BallObject->set_Name("BallObject-"+QString::number(StationIndex,10)+"-"+QString::number(CloudIndex,10));
//parameter information
BallObject->set_BallCenter(center); BallObject->set_Radius(radius); BallObject->set_RMSE(rms);BallObject->set_IfWire(false);
m_BallVec.push_back(BallObject);
cout << "R=" << radius << ";殘差=" << rms << endl;
cout<<"center:"<<center[0]<<" "<<center[1]<<" "<<center[2]<<endl;
return true;
}
大家稍加修改就可以爲自己所用了。其餘代碼在下一篇博客補充。
有任何問題請加本人QQ 498771026