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