double SignedPlaneDistance(const vgl_plane_3d<double> &Plane, const vgl_point_3d<double> &Point) { double d = vgl_distance(Plane, Point); vgl_point_3d<double> ClosestPoint = vgl_closest_point(Plane, Point); vgl_vector_3d<double> V = Point - ClosestPoint; if(AngleBetween(Plane.normal(), V) < M_PI/2.0) return d; else return -d; }
void ClosestPointOnLine() { //define a line vgl_point_3d<double> LineP0(0.0,0.0,0.0); vgl_point_3d<double> LineP1(1.0,1.0,1.0); vgl_line_3d_2_points<double> Line(LineP0, LineP1); //define a test point vgl_point_3d<double> Point(1.0,1.0,1.0); vgl_point_3d<double> ClosestPoint = vgl_closest_point(Line, Point); std::cout << "Closest Point: " << ClosestPoint << std::endl; }
bool VglPlus::mergeLinesegments(const vcl_vector<vgl_line_segment_2d<double> > & segments, const vcl_vector<vcl_vector<vgl_point_2d<double> > > & segment_pts, const double distance_threshold, vcl_vector<vgl_line_segment_2d<double> > & merged_segments, vcl_vector<vcl_vector<vgl_point_2d<double> > > & merged_pts) { assert(segments.size() == segment_pts.size()); const double inlier_ratio_threshold = 0.7; // vcl_vector<LinePoints> line_pts; for (int i = 0; i<segments.size(); i++) { vgl_line_2d<double> line(segments[i].point1(), segments[i].point2()); LinePoints lp; lp.line_ = line; lp.pts_ = segment_pts[i]; lp.seg_ = segments[i]; line_pts.push_back(lp); } vcl_sort(line_pts.begin(), line_pts.end(), vcl_greater<LinePoints>::greater()); // greedy mergy for (int i = 0; i<line_pts.size(); i++) { for (int j = i+1; j<line_pts.size(); j++) { if (line_pts[i].valid_ && line_pts[j].valid_) { vgl_line_2d<double> cur_line = line_pts[i].line_; vcl_vector<vgl_point_2d<double> > cur_pts = line_pts[j].pts_; // points to be merged vcl_vector<vgl_point_2d<double> > inlier_pts; for (int k = 0; k<cur_pts.size(); k++) { double dis = vgl_distance(cur_pts[k], cur_line); if (dis < distance_threshold) { inlier_pts.push_back(cur_pts[k]); } } if (1.0 * inlier_pts.size()/cur_pts.size() > inlier_ratio_threshold) { vcl_vector<vgl_point_2d<double> > all_pts; all_pts.insert(all_pts.end(), inlier_pts.begin(), inlier_pts.end()); all_pts.insert(all_pts.end(), line_pts[i].pts_.begin(), line_pts[i].pts_.end()); vgl_line_2d<double> estimated_line = vgl_fit_line_2d(line_pts[i].pts_); // project the two line segment to the line and merge vgl_point_2d<double> p1 = vgl_closest_point(estimated_line, line_pts[i].seg_.point1()); vgl_point_2d<double> p2 = vgl_closest_point(estimated_line, line_pts[i].seg_.point2()); vgl_point_2d<double> p3 = vgl_closest_point(estimated_line, line_pts[j].seg_.point1()); vgl_point_2d<double> p4 = vgl_closest_point(estimated_line, line_pts[j].seg_.point2()); vgl_line_segment_2d<double> merged_seg; bool isMerged = VglPlus::mergeTwolineSegmentOnALine(vgl_line_segment_2d<double>(p1, p2), vgl_line_segment_2d<double>(p3, p4), merged_seg); if (isMerged) { line_pts[i].pts_.insert(line_pts[i].pts_.end(), inlier_pts.begin(), inlier_pts.end()); line_pts[i].line_ = estimated_line; line_pts[i].seg_ = merged_seg; line_pts[j].valid_ = false; } } } } } vcl_sort(line_pts.begin(), line_pts.end(), vcl_greater<LinePoints>::greater()); // output for (int i = 0; i<line_pts.size(); i++) { if (line_pts[i].valid_) { merged_segments.push_back(line_pts[i].seg_); merged_pts.push_back(line_pts[i].pts_); } } return true; }