void Feature05::evaluate(const Segment& segment, Eigen::VectorXd& result) const
{
result = Eigen::VectorXd::Zero(getNDimensions());
const size_t numPoints = segment.points.size();
if (numPoints > 1) {
// width
result(0) = (segment.points.back() - segment.points.front()).norm();
if (m_extended) {
// min and max distance between points
result(1) = DBL_MAX;
result(2) = 0.0;
for (size_t pIndex = 0; pIndex < numPoints - 1; ++pIndex) {
double dist = (segment.points[pIndex] - segment.points[pIndex + 1]).norm();
if (dist < result(1)) result(1) = dist;
if (dist > result(2)) result(2) = dist;
}
// ratio between min and max distance between points
if (result(2) != 0.0) {
result(3) = result(1) / result(2);
}
}
}
else {
result.setConstant(getNDimensions(), -1.0);
}
}
void Feature06::evaluate(const Segment& segment, Eigen::VectorXd& result) const
{
result = Eigen::VectorXd::Zero(getNDimensions());
const size_t numPoints = segment.points.size();
if (numPoints > 2) {
double px[numPoints];
double py[numPoints];
// copy
for (size_t pIndex = 0; pIndex < numPoints; ++pIndex) {
px[pIndex] = segment.points[pIndex](0);
py[pIndex] = segment.points[pIndex](1);
}
double xc, yc, rc;
fitCircle(numPoints, px, py, xc, yc, rc);
result(1) = rc;
if (m_extended) {
result(3) = DBL_MAX;
result(4) = 0.0;
}
// residual sum
Eigen::Vector2d centerOfCircle(xc, yc);
double residual;
for (size_t pIndex = 0; pIndex < numPoints; ++pIndex) {
double dist = (centerOfCircle - segment.points[pIndex]).norm();
if (m_extended) {
if (dist < result(3)) {
result(3) = dist;
}
if (dist > result(4)) {
result(4) = dist;
}
}
residual = rc - dist;
result(0) += residual * residual;
}
if (m_extended) {
result(2) = result(0) / (numPoints - 1);
if (result(4) != 0.0) {
result(5) = result(3) / result(4);
}
}
}
}