test compiling!! but 34 tests failed out of 198
parent
b7204b100d
commit
d0e1c1a770
|
@ -36,7 +36,7 @@ class ExpressionFactor: public NoiseModelFactor {
|
|||
Expression<T> expression_; ///< the expression that is AD enabled
|
||||
FastVector<int> dims_; ///< dimensions of the Jacobian matrices
|
||||
|
||||
static const int Dim = traits::dimension<T>::value;
|
||||
static const int Dim = traits_x<T>::dimension;
|
||||
|
||||
public:
|
||||
|
||||
|
@ -63,13 +63,12 @@ public:
|
|||
*/
|
||||
virtual Vector unwhitenedError(const Values& x,
|
||||
boost::optional<std::vector<Matrix>&> H = boost::none) const {
|
||||
DefaultChart<T> chart;
|
||||
if (H) {
|
||||
const T value = expression_.value(x, keys_, dims_, *H);
|
||||
return chart.local(measurement_, value);
|
||||
return traits_x<T>::Local(measurement_, value);
|
||||
} else {
|
||||
const T value = expression_.value(x);
|
||||
return chart.local(measurement_, value);
|
||||
return traits_x<T>::Local(measurement_, value);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -97,8 +96,7 @@ public:
|
|||
T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here !
|
||||
|
||||
// Evaluate error and set RHS vector b
|
||||
DefaultChart<T> chart;
|
||||
Ab(size()).col(0) = -chart.local(measurement_, value);
|
||||
Ab(size()).col(0) = -traits_x<T>::Local(measurement_, value);
|
||||
|
||||
// Whiten the corresponding system, Ab already contains RHS
|
||||
Vector dummy(Dim);
|
||||
|
|
|
@ -107,7 +107,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_,
|
|||
const gtsam::Expression<T>& expression, const gtsam::Values& values,
|
||||
double nd_step, double tolerance) {
|
||||
// Create factor
|
||||
size_t size = traits::dimension<T>::value;
|
||||
size_t size = traits_x<T>::dimension;
|
||||
ExpressionFactor<T> f(noiseModel::Unit::Create(size),
|
||||
expression.value(values), expression);
|
||||
testFactorJacobians(result_, name_, f, values, nd_step, tolerance);
|
||||
|
|
|
@ -107,7 +107,7 @@ protected:
|
|||
/// shorthand for this class
|
||||
typedef SmartStereoProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
|
||||
|
||||
typedef traits::dimension<gtsam::StereoPoint2> ZDim_t; ///< Dimension trait of measurement type
|
||||
enum {ZDim = traits_x<gtsam::StereoPoint2>::dimension}; ///< Dimension trait of measurement type
|
||||
|
||||
public:
|
||||
|
||||
|
@ -482,7 +482,7 @@ public:
|
|||
if (triangulateForLinearize(cameras))
|
||||
return Base::createJacobianSVDFactor(cameras, point_, lambda);
|
||||
else
|
||||
return boost::make_shared< JacobianFactorSVD<D, ZDim_t::value> >(this->keys_);
|
||||
return boost::make_shared< JacobianFactorSVD<D, ZDim> >(this->keys_);
|
||||
}
|
||||
|
||||
/// Returns true if nonDegenerate
|
||||
|
|
Loading…
Reference in New Issue