diff --git a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp index ad176020a..efbafec80 100644 --- a/gtsam_unstable/nonlinear/tests/testBADFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testBADFactor.cpp @@ -134,6 +134,35 @@ TEST(BADFactor, compose2) { EXPECT( assert_equal(expected, *jf,1e-9)); } +/* ************************************************************************* */ +// Test compose with one arguments referring to a constant same rotation +TEST(BADFactor, compose3) { + + // Create expression + Rot3_ R1(Rot3::identity()), R2(3); + Rot3_ R3 = R1 * R2; + + // Create factor + BADFactor f(noiseModel::Unit::Create(3), Rot3(), R3); + + // Create some values + Values values; + values.insert(3, Rot3()); + + // Check unwhitenedError + std::vector H(1); + Vector actual = f.unwhitenedError(values, H); + EXPECT_LONGS_EQUAL(1, H.size()); + EXPECT( assert_equal(eye(3), H[0],1e-9)); + + // Check linearization + JacobianFactor expected(3, eye(3), zero(3)); + boost::shared_ptr gf = f.linearize(values); + boost::shared_ptr jf = // + boost::dynamic_pointer_cast(gf); + EXPECT( assert_equal(expected, *jf,1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr;