Fixed remaining lint errors
parent
e1466b2609
commit
a34a9b8ff1
|
@ -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
|
||||
|
||||
|
|
Loading…
Reference in New Issue