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

13
gtsam_unstable/slam/SmartRangeFactor.h
View File

@ -29,7 +29,6 @@ namespace gtsam {
*/ */
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) {
@ -44,7 +43,6 @@ protected:
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) {
} }
@ -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