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