fix error using value for HybridNonlinearFactor
							parent
							
								
									cfe2ad56bf
								
							
						
					
					
						commit
						48e087e083
					
				|  | @ -119,7 +119,7 @@ class GTSAM_EXPORT HybridGaussianFactor : public HybridFactor { | |||
|   /// @name Standard API
 | ||||
|   /// @{
 | ||||
| 
 | ||||
|   /// Get the factor and scalar at a given discrete assignment.
 | ||||
|   /// Get factor at a given discrete assignment.
 | ||||
|   sharedFactor operator()(const DiscreteValues &assignment) const; | ||||
| 
 | ||||
|   /**
 | ||||
|  |  | |||
|  | @ -138,7 +138,7 @@ class HybridNonlinearFactor : public HybridFactor { | |||
|     auto errorFunc = | ||||
|         [continuousValues](const std::pair<sharedFactor, double>& f) { | ||||
|           auto [factor, val] = f; | ||||
|           return factor->error(continuousValues) + val; | ||||
|           return factor->error(continuousValues) + (0.5 * val * val); | ||||
|         }; | ||||
|     DecisionTree<Key, double> result(factors_, errorFunc); | ||||
|     return result; | ||||
|  | @ -157,7 +157,7 @@ class HybridNonlinearFactor : public HybridFactor { | |||
|     auto [factor, val] = factors_(discreteValues); | ||||
|     // Compute the error for the selected factor
 | ||||
|     const double factorError = factor->error(continuousValues); | ||||
|     return factorError + val; | ||||
|     return factorError + (0.5 * val * val); | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue