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

Fixed remaining lint errors

release/4.3a0
Frank Dellaert 2018-10-09 08:46:30 -04:00
parent e1466b2609
commit a34a9b8ff1
1 changed files with 14 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -28,8 +28,7 @@ namespace gtsam {
* @addtogroup SLAM * @addtogroup SLAM
*/ */
class SmartRangeFactor: public NoiseModelFactor { class SmartRangeFactor: public NoiseModelFactor {
protected: protected:
struct Circle2 { struct Circle2 {
Circle2(const Point2& p, double r) : Circle2(const Point2& p, double r) :
center(p), radius(r) { center(p), radius(r) {
@ -40,11 +39,10 @@ protected:
typedef SmartRangeFactor This; typedef SmartRangeFactor This;
std::vector<double> measurements_; ///< Range measurements std::vector<double> measurements_; ///< Range measurements
double variance_; ///< variance on noise double variance_; ///< variance on noise
public:
public:
/** Default constructor: don't use directly */ /** Default constructor: don't use directly */
SmartRangeFactor() { SmartRangeFactor() {
} }
@ -53,7 +51,7 @@ public:
* Constructor * Constructor
* @param s standard deviation of range measurement noise * @param s standard deviation of range measurement noise
*/ */
SmartRangeFactor(double s) : explicit SmartRangeFactor(double s) :
NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) { NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) {
} }
@ -91,7 +89,7 @@ public:
* Raise runtime_error if not well defined. * Raise runtime_error if not well defined.
*/ */
Point2 triangulate(const Values& x) const { Point2 triangulate(const Values& x) const {
//gttic_(triangulate); // gttic_(triangulate);
// create n circles corresponding to measured range around each pose // create n circles corresponding to measured range around each pose
std::list<Circle2> circles; std::list<Circle2> circles;
size_t n = size(); size_t n = size();
@ -105,11 +103,11 @@ public:
boost::optional<Circle2> bestCircle2; boost::optional<Circle2> bestCircle2;
// loop over all circles // loop over all circles
for(const Circle2& it: circles) { for (const Circle2& it : circles) {
// distance between circle centers. // distance between circle centers.
double d = distance2(circle1.center, it.center); double d = distance2(circle1.center, it.center);
if (d < 1e-9) if (d < 1e-9)
continue; // skip circles that are in the same location continue; // skip circles that are in the same location
// Find f and h, the intersection points in normalized circles // Find f and h, the intersection points in normalized circles
boost::optional<Point2> fh = circleCircleIntersection( boost::optional<Point2> fh = circleCircleIntersection(
circle1.radius / d, it.radius / d); circle1.radius / d, it.radius / d);
@ -129,7 +127,7 @@ public:
// 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);
} }
@ -147,19 +145,20 @@ public:
boost::optional<std::vector<Matrix>&> H = boost::none) const { boost::optional<std::vector<Matrix>&> H = boost::none) const {
size_t n = size(); size_t n = size();
if (n < 3) { if (n < 3) {
if (H) if (H) {
// set Jacobians to zero for n<3 // set Jacobians to zero for n<3
for (size_t j = 0; j < n; j++) for (size_t j = 0; j < n; j++)
(*H)[j] = Matrix::Zero(3, 1); (*H)[j] = Matrix::Zero(3, 1);
}
return Z_1x1; return Z_1x1;
} else { } else {
Vector error = Z_1x1; Vector error = Z_1x1;
// triangulate to get the optimized point // triangulate to get the optimized point
// TODO: Should we have a (better?) variant that does this in relative coordinates ? // TODO(dellaert): Should we have a (better?) variant that does this in relative coordinates ?
Point2 optimizedPoint = triangulate(x); Point2 optimizedPoint = triangulate(x);
// TODO: triangulation should be followed by an optimization given poses // TODO(dellaert): triangulation should be followed by an optimization given poses
// now evaluate the errors between predicted and measured range // now evaluate the errors between predicted and measured range
for (size_t j = 0; j < n; j++) { for (size_t j = 0; j < n; j++) {
const Pose2& pose = x.at<Pose2>(keys_[j]); const Pose2& pose = x.at<Pose2>(keys_[j]);
@ -178,8 +177,6 @@ public:
return boost::static_pointer_cast<gtsam::NonlinearFactor>( return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); gtsam::NonlinearFactor::shared_ptr(new This(*this)));
} }
}; };
} // \namespace gtsam
} // \namespace gtsam