diff --git a/gtsam/linear/GaussianConditionalUnordered.cpp b/gtsam/linear/GaussianConditionalUnordered.cpp index 7af208b46..e4837eb34 100644 --- a/gtsam/linear/GaussianConditionalUnordered.cpp +++ b/gtsam/linear/GaussianConditionalUnordered.cpp @@ -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; diff --git a/gtsam/linear/JacobianFactorUnordered.cpp b/gtsam/linear/JacobianFactorUnordered.cpp index c01113bf5..3d0dfa6e2 100644 --- a/gtsam/linear/JacobianFactorUnordered.cpp +++ b/gtsam/linear/JacobianFactorUnordered.cpp @@ -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(); + } } /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp b/gtsam/linear/tests/testGaussianConditionalUnordered.cpp index f858c3d5b..a53d8c308 100644 --- a/gtsam/linear/tests/testGaussianConditionalUnordered.cpp +++ b/gtsam/linear/tests/testGaussianConditionalUnordered.cpp @@ -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,