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);
    }
}
Ejemplo n.º 2
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);
			}
		}
	}
}