12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed warning in SmartRangeFactor.h

release/4.3a0
AndreiCostinescu 2018-10-12 19:10:18 -04:00
parent 6ec0ca7982
commit 6c09d8681c
1 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -121,17 +121,18 @@ class SmartRangeFactor: public NoiseModelFactor {
// use best fh to find actual intersection points // use best fh to find actual intersection points
if (bestCircle2 && best_fh) { if (bestCircle2 && best_fh) {
std::list<Point2> intersections = circleCircleIntersection( auto bestCircleCenter = bestCircle2->center;
circle1.center, bestCircle2->center, best_fh); std::list<Point2> intersections = circleCircleIntersection(
circle1.center, bestCircleCenter, best_fh);
// pick winner based on other measurements // pick winner based on other measurements
double error1 = 0, error2 = 0; double error1 = 0, error2 = 0;
Point2 p1 = intersections.front(), p2 = intersections.back(); Point2 p1 = intersections.front(), p2 = intersections.back();
for (const Circle2& it : circles) { for (const Circle2& it : circles) {
error1 += distance2(it.center, p1); error1 += distance2(it.center, p1);
error2 += distance2(it.center, p2); error2 += distance2(it.center, p2);
} }
return (error1 < error2) ? p1 : p2; return (error1 < error2) ? p1 : p2;
} else { } else {
throw std::runtime_error("triangulate failed"); throw std::runtime_error("triangulate failed");
} }