double SQFitting::qualityOfFit(pcl::PointCloud<pcl::PointXYZ>& cloud, SQParameters& sqParams) {
pcl::PointXYZ _pnt;
Eigen::MatrixXd m(3, 3), _m(3, 3);
m(0, 0) = cos(sqParams.phi) * cos(sqParams.theta) * cos(sqParams.psi) - sin(sqParams.phi) * sin(sqParams.psi);
m(0, 1) = -cos(sqParams.phi) * cos(sqParams.theta) * sin(sqParams.psi) - sin(sqParams.phi) * cos(sqParams.psi);
m(0, 2) = cos(sqParams.phi) * sin(sqParams.theta);
m(1, 0) = sin(sqParams.phi) * cos(sqParams.theta) * cos(sqParams.psi) + cos(sqParams.phi) * sin(sqParams.psi);
m(1, 1) = -sin(sqParams.phi) * cos(sqParams.theta) * sin(sqParams.psi) + cos(sqParams.phi) * cos(sqParams.psi);
m(1, 2) = sin(sqParams.phi) * sin(sqParams.theta);
m(2, 0) = -sin(sqParams.theta) * cos(sqParams.psi);
m(2, 1) = sin(sqParams.theta) * sin(sqParams.psi);
m(2, 2) = cos(sqParams.theta);
_m = m.inverse();
double f, a, b, c, d, e, error = 0;
for (int i = 0; i < (int) cloud.points.size(); i++) {
Eigen::MatrixXd pntMove(3, 1);
Eigen::MatrixXd pnt(3, 1);
Eigen::MatrixXd _pnt(3, 1);
pntMove(0, 0) = (double) cloud.points[i].x - sqParams.px;
pntMove(1, 0) = (double) cloud.points[i].y - sqParams.py;
pntMove(2, 0) = (double) cloud.points[i].z - sqParams.pz;
_pnt = _m * pntMove;
_pnt(0, 0) = _pnt(0, 0) / (sqParams.kx * _pnt(2, 0) / sqParams.a3 + 1);
_pnt(1, 0) = _pnt(1, 0) / (sqParams.ky * _pnt(2, 0) / sqParams.a3 + 1);
a = pow(fabs(_pnt(0, 0) / sqParams.a1), 2.0 / sqParams.e2);
b = pow(fabs(_pnt(1, 0) / sqParams.a2), 2.0 / sqParams.e2);
c = pow(fabs(_pnt(2, 0) / sqParams.a3), 2.0 / sqParams.e1);
d = pow(a + b, (double) (sqParams.e2 / sqParams.e1));
e = d + c;
f = pow(e, (double) (sqParams.e1));
double tempError = pow((f - 1), 2.0);
error += exp(-tempError * 10000);
}
error = sqrt(error);
return (error);
}
void frgExtractTopologicalEntsFromLinesAlgm::_addto_rtree(Vertex2dsOnSegment2d &stru)
{
const AcGeLineSeg2d &seg = stru.seg;
std::map<double, rlVertex2d *, rl_double_sort1> &vertices = stru.vertex2ds;
std::map<double, rlVertex2d *, rl_double_sort1>::iterator it = vertices.begin();
AcGePoint2d base = seg.startPoint();
AcGeVector2d dir = seg.direction();
for (; it != vertices.end(); ++it)
{
AcGePoint2d pnt = base;
pnt.transformBy(dir * it->first);
Point_2d _pnt(pnt.x, pnt.y);
rlId id = 0;
if (_is_in_rtree(id, _pnt))
{
it->second = _topologies->get<rlVertex2d *>(id);
assert(it->second);
}
else
{
rlVertex2d *_v = _topologies->_new<rlVertex2d>();
_v->set_x(_pnt.get<0>());
_v->set_y(_pnt.get<1>());
Point2d_Id _pair(_pnt, _v->id());
_rtree.insert(_pair);
it->second = _v;
}
}
}
