59 pts_.
push_back(PointWidth(pt, halfwidth));
75 int pt_count = pts_.
size();
77 if (skip_first >= pt_count) skip_first = pt_count - 1;
80 for (
int i = skip_first; i < end_i; ++i) {
81 starts[start_count++] = &pts_[i].pt;
84 if (skip_last >= pt_count) skip_last = pt_count - 1;
87 for (
int i = pt_count - 1 - skip_last; i >= end_i; --i) {
88 ends[end_count++] = &pts_[i].pt;
102 double best_uq = -1.0;
104 for (
int i = 0; i < start_count; ++i) {
105 ICOORD* start = starts[i];
106 for (
int j = 0; j < end_count; ++j) {
108 if (*start != *end) {
109 ComputeDistances(*start, *end);
111 double dist = EvaluateLineFit();
112 if (dist < best_uq || best_uq < 0.0) {
121 return best_uq > 0.0 ? sqrt(best_uq) : best_uq;
131 double min_dist,
double max_dist,
132 bool debug,
ICOORD* line_pt) {
133 ComputeConstrainedDistances(direction, min_dist, max_dist);
135 if (pts_.
empty() || distances_.empty()) {
140 int median_index = distances_.choose_nth_item(distances_.size() / 2);
141 *line_pt = distances_[median_index].data;
143 tprintf(
"Constrained fit to dir %g, %g = %d, %d :%d distances:\n",
144 direction.
x(), direction.
y(),
145 line_pt->
x(), line_pt->
y(), distances_.size());
146 for (
int i = 0; i < distances_.size(); ++i) {
147 tprintf(
"%d: %d, %d -> %g\n", i, distances_[i].data.x(),
148 distances_[i].data.y(), distances_[i].key);
150 tprintf(
"Result = %d\n", median_index);
153 double dist_origin = direction * *line_pt;
154 for (
int i = 0; i < distances_.size(); ++i) {
155 distances_[i].key -= dist_origin;
157 return sqrt(EvaluateLineFit());
171 double error =
Fit(&start, &end);
172 if (end.
x() != start.
x()) {
173 *m = static_cast<float>(end.
y() - start.
y()) / (end.
x() - start.
x());
174 *c = start.
y() - *m * start.
x();
191 double cos = 1.0 / sqrt(1.0 + m * m);
192 FCOORD direction(cos, m * cos);
194 double error =
ConstrainedFit(direction, -FLT_MAX, FLT_MAX,
false, &line_pt);
195 *c = line_pt.
y() - line_pt.
x() * m;
200 double DetLineFit::EvaluateLineFit() {
202 double dist = ComputeUpperQuartileError();
209 dist = NumberOfMisfittedPoints(threshold);
216 double DetLineFit::ComputeUpperQuartileError() {
217 int num_errors = distances_.size();
218 if (num_errors == 0)
return 0.0;
220 for (
int i = 0; i < num_errors; ++i) {
221 if (distances_[i].key < 0) distances_[i].key = -distances_[i].key;
224 int index = distances_.choose_nth_item(3 * num_errors / 4);
225 double dist = distances_[index].key;
228 return square_length_ > 0.0 ? dist * dist / square_length_ : 0.0;
232 int DetLineFit::NumberOfMisfittedPoints(
double threshold)
const {
234 int num_dists = distances_.size();
236 for (
int i = 0; i < num_dists; ++i) {
237 if (distances_[i].key > threshold)
247 void DetLineFit::ComputeDistances(
const ICOORD& start,
const ICOORD& end) {
248 distances_.truncate(0);
250 line_vector -= start;
251 square_length_ = line_vector.
sqlength();
254 int prev_abs_dist = 0;
256 for (
int i = 0; i < pts_.
size(); ++i) {
257 ICOORD pt_vector = pts_[i].pt;
259 int dot = line_vector % pt_vector;
261 int dist = line_vector * pt_vector;
262 int abs_dist = dist < 0 ? -dist : dist;
263 if (abs_dist > prev_abs_dist && i > 0) {
265 int separation = abs(dot - prev_dot);
266 if (separation < line_length * pts_[i].halfwidth ||
267 separation < line_length * pts_[i - 1].halfwidth)
270 distances_.push_back(DistPointPair(dist, pts_[i].pt));
271 prev_abs_dist = abs_dist;
279 void DetLineFit::ComputeConstrainedDistances(
const FCOORD& direction,
280 double min_dist,
double max_dist) {
281 distances_.truncate(0);
282 square_length_ = direction.
sqlength();
284 for (
int i = 0; i < pts_.
size(); ++i) {
285 FCOORD pt_vector = pts_[i].pt;
287 double dist = direction * pt_vector;
288 if (min_dist <= dist && dist <= max_dist)
289 distances_.push_back(DistPointPair(dist, pts_[i].pt));