diff --git a/tests/testExtendedKalmanFilter.cpp b/tests/testExtendedKalmanFilter.cpp index 9f22a5840..7b27293df 100644 --- a/tests/testExtendedKalmanFilter.cpp +++ b/tests/testExtendedKalmanFilter.cpp @@ -192,16 +192,15 @@ public: * Ax-b \approx h(x1+dx1,x2+dx2)-z = h(x1,x2) + A2*dx1 + A2*dx2 - z * Hence b = z - h(x1,x2) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& c) const { const X1& x1 = c.at(key1()); const X2& x2 = c.at(key2()); Matrix A1, A2; Vector b = -evaluateError(x1, x2, A1, A2); - const Key var1 = ordering[key1()], var2 = ordering[key2()]; SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != NULL) { - return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, + return JacobianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), A2, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor @@ -209,7 +208,7 @@ public: A1 = Qinvsqrt*A1; A2 = Qinvsqrt*A2; b = Qinvsqrt*b; - return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, var2, + return GaussianFactor::shared_ptr(new JacobianFactor(key1(), A1, key2(), A2, b, noiseModel::Unit::Create(b.size()))); } @@ -329,21 +328,20 @@ public: * Ax-b \approx h(x1+dx1)-z = h(x1) + A1*dx1 - z * Hence b = z - h(x1) = - error_vector(x) */ - boost::shared_ptr linearize(const Values& c, const Ordering& ordering) const { + boost::shared_ptr linearize(const Values& c) const { const X& x1 = c.at(key()); Matrix A1; Vector b = -evaluateError(x1, A1); - const Key var1 = ordering[key()]; SharedDiagonal constrained = boost::dynamic_pointer_cast(this->noiseModel_); if (constrained.get() != NULL) { - return JacobianFactor::shared_ptr(new JacobianFactor(var1, A1, b, constrained)); + return JacobianFactor::shared_ptr(new JacobianFactor(key(), A1, b, constrained)); } // "Whiten" the system before converting to a Gaussian Factor Matrix Rinvsqrt = RInvSqrt(x1); A1 = Rinvsqrt*A1; b = Rinvsqrt*b; - return GaussianFactor::shared_ptr(new JacobianFactor(var1, A1, b, noiseModel::Unit::Create(b.size()))); + return GaussianFactor::shared_ptr(new JacobianFactor(key(), A1, b, noiseModel::Unit::Create(b.size()))); } /** vector of errors */