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;
}
