12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed left-over "ordered" test factor in testExtendedKalmanFilter.cpp

release/4.3a0
Richard Roberts 2014-02-23 18:52:57 -05:00
parent 4c5cb4d8ec
commit fdd891c7c9
1 changed files with 6 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
tests/testExtendedKalmanFilter.cpp
View File

@ -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<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
const X1& x1 = c.at<X1>(key1());
const X2& x2 = c.at<X2>(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<noiseModel::Constrained>(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<GaussianFactor> linearize(const Values& c, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& c) const {
const X& x1 = c.at<X>(key());
Matrix A1;
Vector b = -evaluateError(x1, A1);
const Key var1 = ordering[key()];
SharedDiagonal constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(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 */