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
if (bestCircle2 && best_fh) {
std::list<Point2> intersections = circleCircleIntersection(
circle1.center, bestCircle2->center, best_fh);
auto bestCircleCenter = bestCircle2->center;
std::list<Point2> intersections = circleCircleIntersection(
circle1.center, bestCircleCenter, best_fh);
// pick winner based on other measurements
double error1 = 0, error2 = 0;
Point2 p1 = intersections.front(), p2 = intersections.back();
for (const Circle2& it : circles) {
error1 += distance2(it.center, p1);
error2 += distance2(it.center, p2);
}
return (error1 < error2) ? p1 : p2;
// pick winner based on other measurements
double error1 = 0, error2 = 0;
Point2 p1 = intersections.front(), p2 = intersections.back();
for (const Circle2& it : circles) {
error1 += distance2(it.center, p1);
error2 += distance2(it.center, p2);
}
return (error1 < error2) ? p1 : p2;
} else {
throw std::runtime_error("triangulate failed");
}