1 #ifndef msm_dist_to_curves_h_
2 #define msm_dist_to_curves_h_
3
4 //:
5 // \file
6 // \brief Functions to calculate distance from points to curves (poly-lines)
7 // \author Tim Cootes
8
9 #include <iostream>
10 #include <algorithm>
11 #include <msm/msm_points.h>
12 #include <msm/msm_curve.h>
13 #ifdef _MSC_VER
14 # include <vcl_msvc_warnings.h>
15 #endif
16
17 //: Return square of distance to closest point on line segment [pt0-pt1]
msm_sqr_dist_to_line_segment(const vgl_point_2d<double> & pt0,const vgl_point_2d<double> & pt1,const vgl_point_2d<double> & pt)18 inline double msm_sqr_dist_to_line_segment(const vgl_point_2d<double>& pt0,
19 const vgl_point_2d<double>& pt1,
20 const vgl_point_2d<double>& pt)
21 {
22 vgl_vector_2d<double> u=pt1-pt0;
23 vgl_vector_2d<double> dp=pt-pt0;
24 double pu = u.x()*dp.x() + u.y()*dp.y();
25 if (pu<=0) return dp.sqr_length(); // pt is closest to pt0
26 double Lu2=u.sqr_length();
27 if (pu>=Lu2) return (pt-pt1).sqr_length(); // pt is closest to pt1
28
29 // pt is closest to some point between pt0 and pt1
30 // Use pythagorus : dp^2 = d^2 + sqr(pu/u.length)
31 return std::max(dp.sqr_length() - (pu*pu)/Lu2,0.0);
32 }
33
34 //: Compute the distance between pt and nearest point on given curve
35 // The curve is treated as a polygon connecting subset of given points.
36 // As long as the points are dense enough, this is a good enough approximation
37 // to fitting a smooth curve through the points.
msm_dist_to_curve(const msm_points & all_points,const msm_curve & curve,const vgl_point_2d<double> & pt)38 inline double msm_dist_to_curve(const msm_points& all_points,
39 const msm_curve& curve,
40 const vgl_point_2d<double>& pt)
41 {
42 if (curve.size()==0) return 0;
43
44 // If only one point, then find distance to it from pt
45 if (curve.size()==1) return (all_points[curve[0]]-pt).length();
46
47 // Compute distance between each line segment and the point
48 double min_d2=msm_sqr_dist_to_line_segment(all_points[curve[0]],all_points[curve[1]],pt);
49 unsigned n=curve.size();
50 for (unsigned i=2;i<n;++i)
51 {
52 double d2 = msm_sqr_dist_to_line_segment(all_points[curve[i-1]],all_points[curve[i]],pt);
53 if (d2<min_d2) min_d2=d2;
54 }
55
56 if (!curve.open())
57 { // Curve is closed, so check segment joining point [n-1] to point [0]
58 double d2 = msm_sqr_dist_to_line_segment(all_points[curve[n-1]],all_points[curve[0]],pt);
59 if (d2<min_d2) min_d2=d2;
60 }
61
62 return std::sqrt(min_d2);
63 }
64
65 //: Compute the distance between pt and nearest point on any of the curves through points
66 // The curves are treated as a polygon connecting subset of given points.
msm_dist_to_curves(const msm_points & points,const msm_curves & curves,const vgl_point_2d<double> & pt)67 inline double msm_dist_to_curves(const msm_points& points,
68 const msm_curves& curves,
69 const vgl_point_2d<double>& pt)
70 {
71 if (curves.empty())
72 return 0.0;
73 double min_d = msm_dist_to_curve(points,curves[0],pt);
74 for (unsigned c=1;c<curves.size();++c)
75 {
76 double d=msm_dist_to_curve(points,curves[c],pt);
77 if (d<min_d) min_d=d;
78 }
79 return min_d;
80 }
81
82 //: Find the mean of closest distance between each point in points and the curves through ref_points
msm_mean_dist_to_curves(const msm_points & ref_points,const msm_curves & curves,const msm_points & points)83 inline double msm_mean_dist_to_curves(const msm_points& ref_points,
84 const msm_curves& curves,
85 const msm_points& points)
86 {
87 if (points.size()==0) return 0.0;
88 double sum=0.0;
89 for (unsigned i=0;i<points.size();++i)
90 sum += msm_dist_to_curves(ref_points,curves,points[i]);
91
92 return sum/points.size();
93 }
94
95 //: Find the mean of closest distance between each point in points and matching ref. curve
96 // Assumes points.size()==ref_points.size().
97 // Goes through each point listed in curves and finds the closest distance to equivalent
98 // curve through ref_points.
msm_mean_dist_to_matching_curves(const msm_points & ref_points,const msm_curves & curves,const msm_points & points)99 inline double msm_mean_dist_to_matching_curves(const msm_points& ref_points,
100 const msm_curves& curves,
101 const msm_points& points)
102 {
103 if (curves.empty())
104 return 0.0;
105 if (points.size()==0) return 0.0;
106 assert(ref_points.size()==points.size());
107 assert(curves.max_index()<ref_points.size());
108
109 double sum=0.0;
110 unsigned n_pts=0;
111
112 for (unsigned c=0;c<curves.size();++c)
113 {
114 // Compare each point on curve c through points, with curve c through ref_points
115 n_pts += curves[c].size();
116 for (unsigned i=0;i<curves[c].size();++i)
117 sum += msm_dist_to_curve(ref_points,curves[c],points[curves[c][i]]);
118 }
119 if (n_pts==0) return 0.0;
120 return sum/n_pts;
121 }
122
123
124 #endif // msm_dist_to_curves_h_
125