C++ Point2D::norm方法代码示例

	static Point2D intersec(const Point2D& b1, const Point2D& e1, const Point2D& b2, const Point2D& e2)
	{
		Point2D result = nil();

		Point2D v = e1 - b1;
		double n = v.norm();
		Point2D u = v / n;

		Point2D seg1 = b2 - b1;
		Point2D seg2 = e2 - b1;

		double cos1 = seg1 * u;
		double cos2 = seg2 * u;

		double sin1 = seg1 ^ u;
		double sin2 = seg2 ^ u;

		if(sin1 * sin2 <= 0)
		{
			double sin = sin1 - sin2;
			if(sin)
			{
				double ratio = (cos2 - cos1) * sin1 / sin; // Congruent Triangles
				double scale = cos1 + ratio;
				if(0 < scale && scale < n)
				{
					result = b1 + scale * u;
				}
			}
		}

		return result;
	}

	void intersection()
	{
		Paint paint("figure-intersection.tex");

		Point2D b1(0.0, 0.0), e1(4.0, 4.0), b2(0.0, 4.0), e2(6.0, 0.0);

		paint.line(b1, e1);
		paint.line(b2, e2);

		paint.text("$b_1$", b1, Paint::LEFT);
		paint.text("$e_1$", e1, Paint::RIGHT);
		paint.text("$b_2$", b2, Paint::LEFT);
		paint.text("$e_2$", e2, Paint::RIGHT);

		Point2D v = e1 - b1;
		double n = v.norm();
		Point2D u = v / n;

		Point2D arm1 = b2 - b1;
		Point2D arm2 = e2 - b1;

		double cos1 = arm1 * u;
		double cos2 = arm2 * u;

		double sin1 = arm1 ^ u;
		double sin2 = arm2 ^ u;

		if(sin1 * sin2 <= 0)
		{
			double sin = sin1 - sin2;
			if(sin != 0)
			{
				double ratio = (cos2 - cos1) * sin1 / sin; // Congruent Triangles
				double scale = cos1 + ratio;
				if(0 < scale && scale < n)
				{
					Point2D result = b1 + scale * u;

					Point2D a1 = b1 + cos1 * u;
					Point2D a2 = b1 + cos2 * u;
					paint.dot(a1);
					paint.dot(a2);
					paint.text("$a_1$", a1, Paint::RIGHT | Paint::BELOW);
					paint.text("$a_2$", a2, Paint::LEFT | Paint::ABOVE);
					paint.line(b2, a1, Paint::DASH);
					paint.line(e2, a2, Paint::DASH);

					paint.dot(result);
				}
			}
		}
	}

float Classifier::classify2D_checkcondnum(const Point2D& P, const Point2D& R, float& condnumber) const
{
	condnumber = 0;
	if (path.size() < 2)
	{
		assert(false);
		return 0;
	}

	// consider each path segment as a mini-classifier
	// the segment PR[Pt-->Refpt] and each path's segment cross
	// iff each one classifies the end point of the other in different classes
	Point2D PR = R-P;
	Point2D u = PR; u.normalize();

	unsigned numcross = 0;

	//we'll also look for the distance between P and the nearest segment
	float closestSquareDist = -1.0f;

	size_t segCount = path.size() - 1;
	for (size_t i=0; i<segCount; ++i)
	{
		//current path segment (or half-line!)
		Point2D AP = P - path[i];
		Point2D AB = path[i+1] - path[i];
		Point2D v = AB; v.normalize();

		condnumber = std::max<float>(condnumber, fabs(v.dot(u)));

		// Compute whether PR[Pt-->Refpt] and that segment cross
		float denom = (u.x*v.y-v.x*u.y);
		if (denom != 0)
		{
			// 1. check whether the given pt and the refpt are on different sides of the classifier line
			//we search for alpha and beta so that
			// P + alpha * PR = A + beta * AB
			float alpha = (AP.y * v.x - AP.x * v.y)/denom;
			bool pathIntersects = (alpha >= 0 && alpha*alpha <= PR.norm2());
			if (pathIntersects)
			{
				float beta = (AP.y * u.x - AP.x * u.y)/denom;

				// first and last lines are projected to infinity
				bool refSegIntersects = ((i == 0 || beta >= 0) && (i+1 == segCount || beta*beta < AB.norm2())); //not "beta*beta <= AB.norm2()" because the equality case will be managed by the next segment!

				// crossing iif each segment/line separates the other
				if (refSegIntersects)
					numcross++;
			}
		}

		// closest distance from the point to that segment
		// 1. projection of the point of the line
		float squareDistToSeg = 0;
		float distAH = v.dot(AP);
		if ((i == 0 || distAH >= 0.0) && (i+1 == segCount || distAH <= AB.norm()))
		{
			// 2. Is the projection within the segment limit? yes => closest
			Point2D PH = (path[i] + v * distAH) - P;
			squareDistToSeg = PH.norm2();
		}
		else
		{
			// 3. otherwise closest is the minimum of the distance to the segment ends
			Point2D BP = P - path[i+1];
			squareDistToSeg = std::min( AP.norm2(), BP.norm2() );
		}

		if (closestSquareDist < 0 || squareDistToSeg < closestSquareDist)
		{
			closestSquareDist = squareDistToSeg;
		}
	}

	assert(closestSquareDist >= 0);
	float deltaNorm = sqrt(closestSquareDist);

	return ((numcross & 1) == 0 ? deltaNorm : -deltaNorm);
}

double pseudoAngle(const Point2D & p1, const Point2D & p2){
	return (1.0 - (p1 * p2)/(p1.norm() * p2.norm()));
}

double angle(const Point2D & p1, const Point2D & p2){
	return acos((p1 * p2) / (p1.norm() * p2.norm()));
}

void ROSInterface::newLaserscanAvailable(const sensor_msgs::LaserScan::ConstPtr& laserscan)
{
    // Convert laserscan into Cartesian coordinates
    std::vector<Point2D> pointsInCartesianCoords;
    for(size_t pointIndex = 0; pointIndex < laserscan->ranges.size(); pointIndex++) {
        double phi = laserscan->angle_min + laserscan->angle_increment * pointIndex;
        double rho = laserscan->ranges[pointIndex];

        Point2D point;
        if(rho > laserscan->range_max || rho < laserscan->range_min) {
            // Out-of-range measurement, set x and y to NaN
            // NOTE: We cannot omit the measurement completely since this would screw up point indices
            point(0) = point(1) = std::numeric_limits<double>::quiet_NaN();
        }
        else {
            point(0) =  cos(phi) * rho;
            point(1) = -sin(phi) * rho;
        }

        pointsInCartesianCoords.push_back(point);
    }    

    // Perform segmentation
    std::vector<srl_laser_segmentation::LaserscanSegment> resultingSegments;
    m_segmentationAlgorithm->performSegmentation(pointsInCartesianCoords, resultingSegments);

    // Read filter parameters
    int minPointsPerSegment = 3, maxPointsPerSegment = 50;
    m_privateNodeHandle.getParamCached("min_points_per_segment", minPointsPerSegment);
    m_privateNodeHandle.getParamCached("max_points_per_segment", maxPointsPerSegment);
    ROS_INFO_ONCE("Filtering out all resulting segments with less than %d or more than %d points!", minPointsPerSegment, maxPointsPerSegment);

    double minAvgDistanceFromSensor = 0;
    m_privateNodeHandle.getParamCached("min_avg_distance_from_sensor", minAvgDistanceFromSensor);   
    ROS_INFO_ONCE("Minimum allowed average segment distance from sensor is %f meters!", minAvgDistanceFromSensor);

    double maxAvgDistanceFromSensor = 10;
    m_privateNodeHandle.getParamCached("max_avg_distance_from_sensor", maxAvgDistanceFromSensor);   
    ROS_INFO_ONCE("Maximum allowed average segment distance from sensor is %f meters!", maxAvgDistanceFromSensor);

    // Filter segments
    srl_laser_segmentation::LaserscanSegmentation laserscanSegmentation;
    laserscanSegmentation.segments.reserve(resultingSegments.size());

    for(size_t i = 0; i < resultingSegments.size(); i++) {
        srl_laser_segmentation::LaserscanSegment& currentSegment = resultingSegments[i];

        Point2D mean = Point2D::Zero();
        size_t numValidPoints = 0;
        for(size_t j = 0; j < currentSegment.measurement_indices.size(); j++) {
            Point2D& point = pointsInCartesianCoords[currentSegment.measurement_indices[j]]; 
            if(std::isnan(point(0))) continue;
            mean += point;
            numValidPoints++;
        }

        // Filter by point count
        if(numValidPoints < minPointsPerSegment || numValidPoints > maxPointsPerSegment) continue;
        mean /= (double) numValidPoints;

        // Filter by maximum distance from sensor
        double sensorDistance = mean.norm();
        if(sensorDistance < minAvgDistanceFromSensor || sensorDistance > maxAvgDistanceFromSensor) continue;

        // Segment looks okay
        laserscanSegmentation.segments.push_back(currentSegment);
    }

    // Set message header and publish
    laserscanSegmentation.header = laserscan->header;
    m_laserscanSegmentationPublisher.publish(laserscanSegmentation);
}

bool JumpDistanceSegmentation::isJumpBetween(const Point2D* p1, const Point2D* p2)
{
    const Point2D diff = *p1 - *p2;
    return diff.norm() > m_jumpDistance;
}