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

View File

@ -29,7 +29,6 @@ namespace gtsam {
*/
class SmartRangeFactor: public NoiseModelFactor {
protected:
struct Circle2 {
Circle2(const Point2& p, double r) :
center(p), radius(r) {
@ -44,7 +43,6 @@ protected:
double variance_; ///< variance on noise
public:
/** Default constructor: don't use directly */
SmartRangeFactor() {
}
@ -53,7 +51,7 @@ public:
* Constructor
* @param s standard deviation of range measurement noise
*/
SmartRangeFactor(double s) :
explicit SmartRangeFactor(double s) :
NoiseModelFactor(noiseModel::Isotropic::Sigma(1, s)), variance_(s * s) {
}
@ -147,19 +145,20 @@ public:
boost::optional<std::vector<Matrix>&> H = boost::none) const {
size_t n = size();
if (n < 3) {
if (H)
if (H) {
// set Jacobians to zero for n<3
for (size_t j = 0; j < n; j++)
(*H)[j] = Matrix::Zero(3, 1);
}
return Z_1x1;
} else {
Vector error = Z_1x1;
// 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);
// 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
for (size_t j = 0; j < n; j++) {
const Pose2& pose = x.at<Pose2>(keys_[j]);
@ -178,8 +177,6 @@ public:
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
};
} // \namespace gtsam