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

Made GaussianConditionalUnordered unit tests pass (fixed null model checking in JF and GC, matrix initialization in unit test)

release/4.3a0
Richard Roberts 2013-07-16 17:37:10 +00:00
parent 333903c4aa
commit 13e56d1d2a
3 changed files with 22 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/linear/GaussianConditionalUnordered.cpp
View File

@ -167,7 +167,8 @@ namespace gtsam {
gtsam::transposeMultiplyAdd(-1.0, Matrix(getA(it)), frontalVec, gy[*it]);
// Scale by sigmas
frontalVec.array() *= model_->sigmas().array();
if(model_)
frontalVec.array() *= model_->sigmas().array();
// Write frontal solution into a VectorValues
DenseIndex vectorPosition = 0;

20
gtsam/linear/JacobianFactorUnordered.cpp
View File

@ -389,16 +389,24 @@ namespace gtsam {
/* ************************************************************************* */
Matrix JacobianFactorUnordered::augmentedInformation() const {
Matrix AbWhitened = Ab_.full();
model_->WhitenInPlace(AbWhitened);
return AbWhitened.transpose() * AbWhitened;
if(model_) {
Matrix AbWhitened = Ab_.full();
model_->WhitenInPlace(AbWhitened);
return AbWhitened.transpose() * AbWhitened;
} else {
return Ab_.full().transpose() * Ab_.full();
}
}
/* ************************************************************************* */
Matrix JacobianFactorUnordered::information() const {
Matrix AWhitened = this->getA();
model_->WhitenInPlace(AWhitened);
return AWhitened.transpose() * AWhitened;
if(model_) {
Matrix AWhitened = this->getA();
model_->WhitenInPlace(AWhitened);
return AWhitened.transpose() * AWhitened;
} else {
return this->getA().transpose() * this->getA();
}
}
/* ************************************************************************* */

12
gtsam/linear/tests/testGaussianConditionalUnordered.cpp
View File

@ -187,8 +187,8 @@ TEST( GaussianConditionalUnordered, solve_simple )
(1, Vector_(4, -3.1,-3.4,-11.9,-13.2));
// verify indices/size
EXPECT_LONGS_EQUAL(2, cg.size());
EXPECT_LONGS_EQUAL(4, cg.rows());
EXPECT_LONGS_EQUAL(2, (long)cg.size());
EXPECT_LONGS_EQUAL(4, (long)cg.rows());
// solve and verify
actual.insert(cg.solve(actual));
@ -224,8 +224,8 @@ TEST( GaussianConditionalUnordered, solve_multifrontal )
(10, sl1);
// verify indices/size
EXPECT_LONGS_EQUAL(3, cg.size());
EXPECT_LONGS_EQUAL(4, cg.rows());
EXPECT_LONGS_EQUAL(3, (long)cg.size());
EXPECT_LONGS_EQUAL(4, (long)cg.rows());
// solve and verify
actual.insert(cg.solve(actual));
@ -272,7 +272,7 @@ TEST( GaussianConditionalUnordered, solveTranspose ) {
TEST( GaussianConditionalUnordered, information ) {
// Create R matrix
Matrix R; R <<
Matrix R(4,4); R <<
1, 2, 3, 4,
0, 5, 6, 7,
0, 0, 8, 9,
@ -293,7 +293,7 @@ TEST( GaussianConditionalUnordered, information ) {
TEST( GaussianConditionalUnordered, isGaussianFactor ) {
// Create R matrix
Matrix R; R <<
Matrix R(4,4); R <<
1, 2, 3, 4,
0, 5, 6, 7,
0, 0, 8, 9,