Fixed remaining lint errors
							parent
							
								
									560ee010c2
								
							
						
					
					
						commit
						7da4824568
					
				| 
						 | 
				
			
			@ -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