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

Commented out repeated arguments

release/4.3a0
dellaert 2014-10-10 13:57:37 +02:00
parent 58bbce482d
commit ae93dd9869
1 changed files with 134 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

268
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -184,139 +184,139 @@ TEST(ExpressionFactor, test2) {
EXPECT( assert_equal(*expected, *gf3, 1e-9)); EXPECT( assert_equal(*expected, *gf3, 1e-9));
} }
/* ************************************************************************* */ ///* ************************************************************************* */
//
TEST(ExpressionFactor, compose1) { //TEST(ExpressionFactor, compose1) {
//
// Create expression // // Create expression
Rot3_ R1(1), R2(2); // Rot3_ R1(1), R2(2);
Rot3_ R3 = R1 * R2; // Rot3_ R3 = R1 * R2;
//
// Create factor // // Create factor
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); // ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
//
// Create some values // // Create some values
Values values; // Values values;
values.insert(1, Rot3()); // values.insert(1, Rot3());
values.insert(2, Rot3()); // values.insert(2, Rot3());
//
// Check unwhitenedError // // Check unwhitenedError
std::vector<Matrix> H(2); // std::vector<Matrix> H(2);
Vector actual = f.unwhitenedError(values, H); // Vector actual = f.unwhitenedError(values, H);
EXPECT( assert_equal(eye(3), H[0],1e-9)); // EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9)); // EXPECT( assert_equal(eye(3), H[1],1e-9));
//
// Check linearization // // Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); // JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); // boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = // // boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf); // boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9)); // EXPECT( assert_equal(expected, *jf,1e-9));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
// Test compose with arguments referring to the same rotation //// Test compose with arguments referring to the same rotation
TEST(ExpressionFactor, compose2) { //TEST(ExpressionFactor, compose2) {
//
// Create expression // // Create expression
Rot3_ R1(1), R2(1); // Rot3_ R1(1), R2(1);
Rot3_ R3 = R1 * R2; // Rot3_ R3 = R1 * R2;
//
// Create factor // // Create factor
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); // ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
//
// Create some values // // Create some values
Values values; // Values values;
values.insert(1, Rot3()); // values.insert(1, Rot3());
//
// Check unwhitenedError // // Check unwhitenedError
std::vector<Matrix> H(1); // std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H); // Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size()); // EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(2*eye(3), H[0],1e-9)); // EXPECT( assert_equal(2*eye(3), H[0],1e-9));
//
// Check linearization // // Check linearization
JacobianFactor expected(1, 2 * eye(3), zero(3)); // JacobianFactor expected(1, 2 * eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); // boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = // // boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf); // boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9)); // EXPECT( assert_equal(expected, *jf,1e-9));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
// Test compose with one arguments referring to a constant same rotation //// Test compose with one arguments referring to a constant same rotation
TEST(ExpressionFactor, compose3) { //TEST(ExpressionFactor, compose3) {
//
// Create expression // // Create expression
Rot3_ R1(Rot3::identity()), R2(3); // Rot3_ R1(Rot3::identity()), R2(3);
Rot3_ R3 = R1 * R2; // Rot3_ R3 = R1 * R2;
//
// Create factor // // Create factor
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); // ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
//
// Create some values // // Create some values
Values values; // Values values;
values.insert(3, Rot3()); // values.insert(3, Rot3());
//
// Check unwhitenedError // // Check unwhitenedError
std::vector<Matrix> H(1); // std::vector<Matrix> H(1);
Vector actual = f.unwhitenedError(values, H); // Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(1, H.size()); // EXPECT_LONGS_EQUAL(1, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9)); // EXPECT( assert_equal(eye(3), H[0],1e-9));
//
// Check linearization // // Check linearization
JacobianFactor expected(3, eye(3), zero(3)); // JacobianFactor expected(3, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); // boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = // // boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf); // boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9)); // EXPECT( assert_equal(expected, *jf,1e-9));
} //}
//
/* ************************************************************************* */ ///* ************************************************************************* */
// Test compose with three arguments //// Test compose with three arguments
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, //Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2, // boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
boost::optional<Matrix3&> H3) { // boost::optional<Matrix3&> H3) {
// return dummy derivatives (not correct, but that's ok for testing here) // // return dummy derivatives (not correct, but that's ok for testing here)
if (H1) // if (H1)
*H1 = eye(3); // *H1 = eye(3);
if (H2) // if (H2)
*H2 = eye(3); // *H2 = eye(3);
if (H3) // if (H3)
*H3 = eye(3); // *H3 = eye(3);
return R1 * (R2 * R3); // return R1 * (R2 * R3);
} //}
//
TEST(ExpressionFactor, composeTernary) { //TEST(ExpressionFactor, composeTernary) {
//
// Create expression // // Create expression
Rot3_ A(1), B(2), C(3); // Rot3_ A(1), B(2), C(3);
Rot3_ ABC(composeThree, A, B, C); // Rot3_ ABC(composeThree, A, B, C);
//
// Create factor // // Create factor
ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC); // ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
//
// Create some values // // Create some values
Values values; // Values values;
values.insert(1, Rot3()); // values.insert(1, Rot3());
values.insert(2, Rot3()); // values.insert(2, Rot3());
values.insert(3, Rot3()); // values.insert(3, Rot3());
//
// Check unwhitenedError // // Check unwhitenedError
std::vector<Matrix> H(3); // std::vector<Matrix> H(3);
Vector actual = f.unwhitenedError(values, H); // Vector actual = f.unwhitenedError(values, H);
EXPECT_LONGS_EQUAL(3, H.size()); // EXPECT_LONGS_EQUAL(3, H.size());
EXPECT( assert_equal(eye(3), H[0],1e-9)); // EXPECT( assert_equal(eye(3), H[0],1e-9));
EXPECT( assert_equal(eye(3), H[1],1e-9)); // EXPECT( assert_equal(eye(3), H[1],1e-9));
EXPECT( assert_equal(eye(3), H[2],1e-9)); // EXPECT( assert_equal(eye(3), H[2],1e-9));
//
// Check linearization // // Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); // JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values); // boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = // // boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf); // boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9)); // EXPECT( assert_equal(expected, *jf,1e-9));
} //}
/* ************************************************************************* */ /* ************************************************************************* */
@ -328,7 +328,7 @@ template<class T> struct Incomplete;
typedef mpl::vector<Pose3, Point3, Cal3_S2> MyTypes; typedef mpl::vector<Pose3, Point3, Cal3_S2> MyTypes;
typedef GenerateTrace<Point2, MyTypes>::type Generated; typedef GenerateTrace<Point2, MyTypes>::type Generated;
//Incomplete<Generated> incomplete; //Incomplete<Generated> incomplete;
BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >)); //BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >));
BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >)); BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >));
Generated generated; Generated generated;