使用QT搭建点云显示框架系列八---关于拟合球
这次又更新了软件,下载请看置顶的博客。
好了,说一下本次代码的主要更新内容:
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)
#endif
bool 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