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

Fixed uninitialized problem

release/4.3a0
Frank Dellaert 2018-10-09 08:45:42 -04:00
parent 1cf5a2a80b
commit 560ee010c2
1 changed files with 14 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -14,7 +14,12 @@
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <list>
#include <map> #include <map>
#include <stdexcept>
#include <string>
#include <vector>
namespace gtsam { namespace gtsam {
@ -83,6 +88,7 @@ public:
/** /**
* Triangulate a point from at least three pose-range pairs * Triangulate a point from at least three pose-range pairs
* Checks for best pair that includes first point * Checks for best pair that includes first point
* Raise runtime_error if not well defined.
*/ */
Point2 triangulate(const Values& x) const { Point2 triangulate(const Values& x) const {
//gttic_(triangulate); //gttic_(triangulate);
@ -96,7 +102,7 @@ public:
Circle2 circle1 = circles.front(); Circle2 circle1 = circles.front();
boost::optional<Point2> best_fh; boost::optional<Point2> best_fh;
boost::optional<Circle2> best_circle; boost::optional<Circle2> bestCircle2;
// loop over all circles // loop over all circles
for(const Circle2& it: circles) { for(const Circle2& it: circles) {
@ -111,13 +117,14 @@ public:
// if h is large, the intersections are well defined. // if h is large, the intersections are well defined.
if (fh && (!best_fh || fh->y() > best_fh->y())) { if (fh && (!best_fh || fh->y() > best_fh->y())) {
best_fh = fh; best_fh = fh;
best_circle = it; bestCircle2 = it;
} }
} }
// use best fh to find actual intersection points // use best fh to find actual intersection points
if (bestCircle2 && best_fh) {
std::list<Point2> intersections = circleCircleIntersection( std::list<Point2> intersections = circleCircleIntersection(
circle1.center, best_circle->center, best_fh); circle1.center, bestCircle2->center, best_fh);
// pick winner based on other measurements // pick winner based on other measurements
double error1 = 0, error2 = 0; double error1 = 0, error2 = 0;
@ -127,7 +134,10 @@ public:
error2 += distance2(it.center, p2); error2 += distance2(it.center, p2);
} }
return (error1 < error2) ? p1 : p2; return (error1 < error2) ? p1 : p2;
//gttoc_(triangulate); } else {
throw std::runtime_error("triangulate failed");
}
// gttoc_(triangulate);
} }
/** /**