From 39910717d6557cf78642314c65ce499db9d7aed9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 24 Jun 2013 16:25:02 +0000 Subject: [PATCH] Picked the winner --- gtsam_unstable/slam/SmartRangeFactor.h | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/gtsam_unstable/slam/SmartRangeFactor.h b/gtsam_unstable/slam/SmartRangeFactor.h index 37c29ac83..be3735961 100644 --- a/gtsam_unstable/slam/SmartRangeFactor.h +++ b/gtsam_unstable/slam/SmartRangeFactor.h @@ -105,11 +105,17 @@ public: } // use best fh to find actual intersection points - std::list solutions = Point2::CircleCircleIntersection( + std::list intersections = Point2::CircleCircleIntersection( circle1.center, best_circle->center, best_fh); - // pick winner based on other measurement - return solutions.front(); + // pick winner based on other measurements + double error1 = 0, error2 = 0; + Point2 p1 = intersections.front(), p2 = intersections.back(); + BOOST_FOREACH(const Circle2& it, circles) { + error1 += it.center.dist(p1); + error2 += it.center.dist(p2); + } + return (error1 < error2) ? p1 : p2; } /**