Skip to content

Commit c7bc3ea

Browse files
tuxbotixtuxbotix
committed
Few other approaches and more config flags
1 parent f9164e2 commit c7bc3ea

File tree

1 file changed

+175
-97
lines changed

1 file changed

+175
-97
lines changed

‎crates/vision/src/calibration_center_circle_detection.rs

Lines changed: 175 additions & 97 deletions
Original file line numberDiff line numberDiff line change
@@ -208,7 +208,7 @@ impl CalibrationMeasurementDetection {
208208

209209
filtered_calibration_circles_ground
210210
.iter()
211-
.flat_map(|(_, lines, _)| lines.clone())
211+
.flat_map(|(_, line, _)| line.clone())
212212
.collect()
213213
});
214214
context
@@ -282,14 +282,96 @@ fn refine_center_circle(
282282
.cloned()
283283
.collect();
284284

285-
let min_distance_from_center = ((min_dim - roi_padding) * 0.20);
286-
let middle_and_source_lines = get_center_circle_line(
287-
circle_center,
288-
context,
289-
roi,
290-
&roi_points,
291-
min_distance_from_center,
292-
);
285+
let min_distance_from_center = (min_dim - roi_padding) * 0.20;
286+
let middle_and_source_lines = context
287+
.line_data
288+
.and_then(|line_data| {
289+
let line_thickness = context.field_dimensions.line_width / 2.0;
290+
let circle_center_ground = center_circle.circle.center;
291+
292+
line_data
293+
.lines
294+
.iter()
295+
.flat_map(|l| {
296+
let line: Line2<Ground> = (*l).into();
297+
let center_distance = line.distance_to(circle_center_ground);
298+
if center_distance > line_thickness * 4.0 {
299+
// println!(
300+
// "\tSkipping: too far away {} {}",
301+
// center_distance,
302+
// line_thickness * 4.0
303+
// );
304+
return None;
305+
}
306+
307+
let projected_base_line =
308+
context.camera_matrix.ground_to_pixel(l.0).and_then(|p| {
309+
context
310+
.camera_matrix
311+
.ground_to_pixel(l.1)
312+
.map(|p2| Line2::<Pixel> {
313+
point: p.into(),
314+
direction: (p2 - p).normalize(),
315+
})
316+
});
317+
318+
if projected_base_line.is_err() {
319+
print!("Skipping: no projected line");
320+
return None;
321+
}
322+
323+
let projected_center_on_line = l.closest_point(circle_center_ground);
324+
let direction = line.direction.normalize();
325+
let orthogonal_direction = point![
326+
direction.y() * line_thickness,
327+
-direction.x() * line_thickness
328+
];
329+
330+
let line_length = l.length() / 2.2;
331+
let lengthened_direction = direction * line_length;
332+
333+
let point_above_line = (orthogonal_direction.coords()
334+
+ projected_center_on_line.coords())
335+
.as_point();
336+
let point_below_line = (-orthogonal_direction.coords()
337+
+ projected_center_on_line.coords())
338+
.as_point();
339+
340+
let edge_lines = [point_above_line, point_below_line]
341+
.iter()
342+
.flat_map(|shifted_point| {
343+
context
344+
.camera_matrix
345+
.ground_to_pixel(*shifted_point)
346+
.and_then(|projected_first_point| {
347+
context
348+
.camera_matrix
349+
.ground_to_pixel(*shifted_point + lengthened_direction)
350+
.map(|projected_second_point| {
351+
Line2::<Pixel>::from_points(
352+
projected_first_point,
353+
projected_second_point,
354+
)
355+
})
356+
})
357+
})
358+
.collect_vec();
359+
// println!("Found edge lines: {:?}", projected_base_line);
360+
Some((projected_base_line.unwrap(), edge_lines, center_distance))
361+
})
362+
.max_by(|(_, _, a), (_, _, b)| a.total_cmp(b))
363+
.map(|v| (v.0, v.1))
364+
})
365+
.or_else(|| {
366+
println!("Using fallback line detection!");
367+
get_center_circle_line(
368+
circle_center,
369+
context,
370+
roi,
371+
&roi_points,
372+
min_distance_from_center,
373+
)
374+
});
293375

294376
let min_distance_from_line = context.center_line_point_exclusion_distance.abs();
295377
// let min_distance_from_line = 6.0f32
@@ -302,10 +384,10 @@ fn refine_center_circle(
302384
// let cleaned_center = circle_center;
303385
let (filtered_circle_points, rejected_roi_points): (Vec<_>, Vec<_>) =
304386
circle_points_pixel.iter().partition(|&&point| {
305-
// source_lines
306-
// .iter()
307-
// .all(|source_line| source_line.distance_to(point) > min_distance_from_line)
308-
line.distance_to(point) > min_distance_from_line
387+
source_lines
388+
.iter()
389+
.all(|source_line| source_line.distance_to(point) > min_distance_from_line)
390+
// line.distance_to(point) > min_distance_from_line
309391
// && (point - cleaned_center).norm_squared() > min_distance_from_center_squared
310392
});
311393
// .copied()
@@ -397,11 +479,11 @@ fn get_center_circle_line(
397479
let last = lines.last().unwrap();
398480
assert!(first.1.len() <= last.1.len());
399481
}
400-
// println!("lines: {:?}", lines.len());
401482
}
402483

403-
let clustering_max_line_to_line_distance = 5.0;
404-
let clustering_direction_cosine_similarity = (5.0f32).to_radians().cos();
484+
let clustering_max_line_to_line_distance =
485+
5.0f32.max(*context.refine_ransac_maximum_inclusion_distance * 4.0);
486+
let clustering_direction_cosine_similarity = (10.0f32).to_radians().cos();
405487
let middle_and_source_lines = match lines.len() {
406488
0 => None,
407489
1 => lines.first().map(|(line, _)| (*line, vec![*line])),
@@ -414,6 +496,7 @@ fn get_center_circle_line(
414496
line.squared_distance_to(circle_center) < min_distance_from_center_squared
415497
})
416498
.collect();
499+
println!("remaining_lines: {:?}", remaining_lines.len());
417500

418501
if remaining_lines.is_empty() {
419502
return None;
@@ -424,13 +507,14 @@ fn get_center_circle_line(
424507
clusters.push(current_cluster);
425508
continue;
426509
}
427-
510+
let chosen_direction = chosen_line.direction.normalize();
428511
remaining_lines[..remaining_lines.len() - 1]
429512
.iter()
430513
.for_each(|(line2, used_points)| {
431-
if chosen_line.direction.dot(&line2.direction).abs()
514+
let line2_center_pt = line2.closest_point(circle_center);
515+
if chosen_direction.dot(&line2.direction.normalize()).abs()
432516
>= clustering_direction_cosine_similarity
433-
&& chosen_line.distance_to(line2.point)
517+
&& chosen_line.distance_to(line2_center_pt)
434518
<= clustering_max_line_to_line_distance
435519
{
436520
current_cluster.push((line2, used_points));
@@ -446,95 +530,89 @@ fn get_center_circle_line(
446530
// .max_by_key(|lines_and_counts| lines_and_counts.iter().fold(0, |a, (_, c)| a + c))
447531
// // .map(|(line, _)| *line)
448532
// ;
449-
450533
// best_cluster.map(|cluster|{
451534
// let mut x=Vec::with_capacity(capacity)
452535
// // let (x,y)=cluster.iter().map(|)
453-
454536
// });
455537

456538
let clustered_lines: Vec<_> = clusters
457539
.iter()
458-
.flat_map(|cluster| {
459-
let total_points = cluster.iter().map(|(_, points)| points.len()).sum();
460-
461-
let mut x = Vec::with_capacity(total_points);
462-
let mut y = Vec::with_capacity(total_points);
463-
464-
for (_line, points) in cluster {
465-
for point in points.iter() {
466-
x.push(point.x());
467-
y.push(point.y());
468-
}
469-
}
470-
x.resize(x.len() * 2, 1.0);
471-
x[0..total_points].iter().minmax().into_option().and_then(
472-
|(&start_x, &end_x)| {
473-
// y = mx + c
474-
// lstsq finds C by solving A * C = B
475-
// A -> Nx2, with x values and 1 as columns.
476-
// B -> y values. Thus C -> [m, c]
477-
478-
lstsq(
479-
&DMatrix::from_column_slice(y.len(), 2, &x),
480-
&DVector::from_column_slice(&y),
481-
1e-7,
540+
.map(|cluster| {
541+
let (merged_point, merged_direction, merged_point_count) = cluster.iter().fold(
542+
(point![0.0, 0.0], vector![0.0, 0.0], 0),
543+
|accum, (line, used_points)| {
544+
(
545+
accum.0 + line.closest_point(circle_center).coords(),
546+
accum.1 + line.direction,
547+
accum.2 + used_points.len(),
482548
)
483-
.ok()
484-
.map(|result| {
485-
println!("result: {}", result.solution);
486-
// result.solution
487-
let start = point![
488-
start_x,
489-
(start_x * result.solution[0] + result.solution[1])
490-
];
491-
let end = point![
492-
end_x,
493-
(end_x * result.solution[0] + result.solution[1])
494-
];
495-
(Line2::from_points(start, end), total_points)
496-
})
497549
},
550+
);
551+
let lines: Vec<_> = cluster.iter().map(|(lines, _)| *lines).collect();
552+
(
553+
Line2::<Pixel> {
554+
point: merged_point / cluster.len() as f32,
555+
direction: merged_direction / cluster.len() as f32,
556+
},
557+
lines,
558+
merged_point_count,
498559
)
560+
561+
// let total_points = cluster.iter().map(|(_, points)| points.len()).sum();
562+
// let mut x = Vec::with_capacity(total_points);
563+
// let mut y = Vec::with_capacity(total_points);
564+
// let mut lines = Vec::with_capacity(cluster.len());
565+
566+
// for (_line, points) in cluster {
567+
// for point in points.iter() {
568+
// x.push(point.x());
569+
// y.push(point.y());
570+
// }
571+
// lines.push(*_line);
572+
// }
573+
// x.resize(x.len() * 2, 1.0);
574+
// x[0..total_points].iter().minmax().into_option().and_then(
575+
// |(&start_x, &end_x)| {
576+
// // y = mx + c
577+
// // lstsq finds C by solving A * C = B
578+
// // A -> Nx2, with x values and 1 as columns.
579+
// // B -> y values. Thus C -> [m, c]
580+
581+
// lstsq(
582+
// &DMatrix::from_column_slice(y.len(), 2, &x),
583+
// &DVector::from_column_slice(&y),
584+
// 1e-7,
585+
// )
586+
// .ok()
587+
// .map(|result| {
588+
// // println!("result: {}", result.solution);
589+
// // result.solution
590+
// let start = point![
591+
// start_x,
592+
// (start_x * result.solution[0] + result.solution[1])
593+
// ];
594+
// let end = point![
595+
// end_x,
596+
// (end_x * result.solution[0] + result.solution[1])
597+
// ];
598+
// (Line2::from_points(start, end), lines, total_points)
599+
// })
600+
// },
601+
// )
499602
})
500603
.collect();
501604

502-
// let clustered_lines: Vec<_> = clusters
503-
// .iter()
504-
// .map(|cluster| {
505-
// let (merged_point, merged_direction, merged_point_count) = cluster.iter().fold(
506-
// (point![0.0, 0.0], vector![0.0, 0.0], 0),
507-
// |accum, (line, point_count)| {
508-
// (
509-
// accum.0 + line.closest_point(circle_center).coords(),
510-
// accum.1 + line.direction,
511-
// accum.2 + point_count,
512-
// )
513-
// },
514-
// );
515-
516-
// (
517-
// Line2::<Pixel> {
518-
// point: merged_point / cluster.len() as f32,
519-
// direction: merged_direction / cluster.len() as f32,
520-
// },
521-
// merged_point_count,
522-
// )
523-
// })
524-
// .collect();
525-
let best_line = clustered_lines
605+
clustered_lines
526606
.iter()
527-
.max_by_key(|(_, count)| *count)
528-
.map(|(line, _)| *line)
529-
.unwrap();
530-
531-
Some((
532-
best_line,
533-
clustered_lines
534-
.into_iter()
535-
.map(|(line, _)| line)
536-
.collect_vec(),
537-
))
607+
.max_by_key(|(_, _, count)| *count)
608+
.map(|(line, source_lines, _)| {
609+
println!(
610+
"cluster lines: {:?}, total: {}",
611+
source_lines.len(),
612+
lines.len()
613+
);
614+
(*line, source_lines.into_iter().map(|l| **l).collect())
615+
})
538616
}
539617
};
540618
middle_and_source_lines
@@ -663,7 +741,7 @@ fn detect_and_filter_circles(
663741
edge_points: &[Point2<Pixel>],
664742
context: &CycleContext,
665743
y_exclusion_threshold: u32,
666-
) -> Vec<(CenterCirclePoints<Pixel>, Vec<LineSegment<Pixel>>, f32)> {
744+
) -> Vec<(CenterCirclePoints<Pixel>, Option<LineSegment<Pixel>>, f32)> {
667745
let camera_matrix = context.camera_matrix;
668746
let transformer =
669747
|pixel_coordinates: &Point2<Pixel>| camera_matrix.pixel_to_ground(*pixel_coordinates).ok();
@@ -709,13 +787,13 @@ fn detect_and_filter_circles(
709787
// edge_points,
710788
context,
711789
)
712-
.map(|v| (v.0, v.2, result.score)),
790+
.map(|v| (v.0, Some(v.1), result.score)),
713791
(true, false) => Some((
714792
CenterCirclePoints {
715793
center: circle_center_px,
716794
points: result.used_points_original,
717795
},
718-
vec![],
796+
None,
719797
result.score,
720798
)),
721799
(false, _) => None,

0 commit comments

Comments
 (0)