Commented out repeated arguments
							parent
							
								
									58bbce482d
								
							
						
					
					
						commit
						ae93dd9869
					
				|  | @ -184,139 +184,139 @@ TEST(ExpressionFactor, test2) { | |||
|   EXPECT( assert_equal(*expected, *gf3, 1e-9)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| TEST(ExpressionFactor, compose1) { | ||||
| 
 | ||||
|   // Create expression
 | ||||
|   Rot3_ R1(1), R2(2); | ||||
|   Rot3_ R3 = R1 * R2; | ||||
| 
 | ||||
|   // Create factor
 | ||||
|   ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| 
 | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(1, Rot3()); | ||||
|   values.insert(2, Rot3()); | ||||
| 
 | ||||
|   // Check unwhitenedError
 | ||||
|   std::vector<Matrix> H(2); | ||||
|   Vector actual = f.unwhitenedError(values, H); | ||||
|   EXPECT( assert_equal(eye(3), H[0],1e-9)); | ||||
|   EXPECT( assert_equal(eye(3), H[1],1e-9)); | ||||
| 
 | ||||
|   // Check linearization
 | ||||
|   JacobianFactor expected(1, eye(3), 2, eye(3), zero(3)); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|   EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Test compose with arguments referring to the same rotation
 | ||||
| TEST(ExpressionFactor, compose2) { | ||||
| 
 | ||||
|   // Create expression
 | ||||
|   Rot3_ R1(1), R2(1); | ||||
|   Rot3_ R3 = R1 * R2; | ||||
| 
 | ||||
|   // Create factor
 | ||||
|   ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| 
 | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(1, Rot3()); | ||||
| 
 | ||||
|   // Check unwhitenedError
 | ||||
|   std::vector<Matrix> H(1); | ||||
|   Vector actual = f.unwhitenedError(values, H); | ||||
|   EXPECT_LONGS_EQUAL(1, H.size()); | ||||
|   EXPECT( assert_equal(2*eye(3), H[0],1e-9)); | ||||
| 
 | ||||
|   // Check linearization
 | ||||
|   JacobianFactor expected(1, 2 * eye(3), zero(3)); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|   EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Test compose with one arguments referring to a constant same rotation
 | ||||
| TEST(ExpressionFactor, compose3) { | ||||
| 
 | ||||
|   // Create expression
 | ||||
|   Rot3_ R1(Rot3::identity()), R2(3); | ||||
|   Rot3_ R3 = R1 * R2; | ||||
| 
 | ||||
|   // Create factor
 | ||||
|   ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3); | ||||
| 
 | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(3, Rot3()); | ||||
| 
 | ||||
|   // Check unwhitenedError
 | ||||
|   std::vector<Matrix> 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<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|   EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| // Test compose with three arguments
 | ||||
| Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, | ||||
|     boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2, | ||||
|     boost::optional<Matrix3&> H3) { | ||||
|   // return dummy derivatives (not correct, but that's ok for testing here)
 | ||||
|   if (H1) | ||||
|     *H1 = eye(3); | ||||
|   if (H2) | ||||
|     *H2 = eye(3); | ||||
|   if (H3) | ||||
|     *H3 = eye(3); | ||||
|   return R1 * (R2 * R3); | ||||
| } | ||||
| 
 | ||||
| TEST(ExpressionFactor, composeTernary) { | ||||
| 
 | ||||
|   // Create expression
 | ||||
|   Rot3_ A(1), B(2), C(3); | ||||
|   Rot3_ ABC(composeThree, A, B, C); | ||||
| 
 | ||||
|   // Create factor
 | ||||
|   ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC); | ||||
| 
 | ||||
|   // Create some values
 | ||||
|   Values values; | ||||
|   values.insert(1, Rot3()); | ||||
|   values.insert(2, Rot3()); | ||||
|   values.insert(3, Rot3()); | ||||
| 
 | ||||
|   // Check unwhitenedError
 | ||||
|   std::vector<Matrix> H(3); | ||||
|   Vector actual = f.unwhitenedError(values, H); | ||||
|   EXPECT_LONGS_EQUAL(3, H.size()); | ||||
|   EXPECT( assert_equal(eye(3), H[0],1e-9)); | ||||
|   EXPECT( assert_equal(eye(3), H[1],1e-9)); | ||||
|   EXPECT( assert_equal(eye(3), H[2],1e-9)); | ||||
| 
 | ||||
|   // Check linearization
 | ||||
|   JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3)); | ||||
|   boost::shared_ptr<GaussianFactor> gf = f.linearize(values); | ||||
|   boost::shared_ptr<JacobianFactor> jf = //
 | ||||
|       boost::dynamic_pointer_cast<JacobianFactor>(gf); | ||||
|   EXPECT( assert_equal(expected, *jf,1e-9)); | ||||
| } | ||||
| ///* ************************************************************************* */
 | ||||
| //
 | ||||
| //TEST(ExpressionFactor, compose1) {
 | ||||
| //
 | ||||
| //  // Create expression
 | ||||
| //  Rot3_ R1(1), R2(2);
 | ||||
| //  Rot3_ R3 = R1 * R2;
 | ||||
| //
 | ||||
| //  // Create factor
 | ||||
| //  ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
 | ||||
| //
 | ||||
| //  // Create some values
 | ||||
| //  Values values;
 | ||||
| //  values.insert(1, Rot3());
 | ||||
| //  values.insert(2, Rot3());
 | ||||
| //
 | ||||
| //  // Check unwhitenedError
 | ||||
| //  std::vector<Matrix> H(2);
 | ||||
| //  Vector actual = f.unwhitenedError(values, H);
 | ||||
| //  EXPECT( assert_equal(eye(3), H[0],1e-9));
 | ||||
| //  EXPECT( assert_equal(eye(3), H[1],1e-9));
 | ||||
| //
 | ||||
| //  // Check linearization
 | ||||
| //  JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
 | ||||
| //  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
 | ||||
| //  boost::shared_ptr<JacobianFactor> jf = //
 | ||||
| //      boost::dynamic_pointer_cast<JacobianFactor>(gf);
 | ||||
| //  EXPECT( assert_equal(expected, *jf,1e-9));
 | ||||
| //}
 | ||||
| //
 | ||||
| ///* ************************************************************************* */
 | ||||
| //// Test compose with arguments referring to the same rotation
 | ||||
| //TEST(ExpressionFactor, compose2) {
 | ||||
| //
 | ||||
| //  // Create expression
 | ||||
| //  Rot3_ R1(1), R2(1);
 | ||||
| //  Rot3_ R3 = R1 * R2;
 | ||||
| //
 | ||||
| //  // Create factor
 | ||||
| //  ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
 | ||||
| //
 | ||||
| //  // Create some values
 | ||||
| //  Values values;
 | ||||
| //  values.insert(1, Rot3());
 | ||||
| //
 | ||||
| //  // Check unwhitenedError
 | ||||
| //  std::vector<Matrix> H(1);
 | ||||
| //  Vector actual = f.unwhitenedError(values, H);
 | ||||
| //  EXPECT_LONGS_EQUAL(1, H.size());
 | ||||
| //  EXPECT( assert_equal(2*eye(3), H[0],1e-9));
 | ||||
| //
 | ||||
| //  // Check linearization
 | ||||
| //  JacobianFactor expected(1, 2 * eye(3), zero(3));
 | ||||
| //  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
 | ||||
| //  boost::shared_ptr<JacobianFactor> jf = //
 | ||||
| //      boost::dynamic_pointer_cast<JacobianFactor>(gf);
 | ||||
| //  EXPECT( assert_equal(expected, *jf,1e-9));
 | ||||
| //}
 | ||||
| //
 | ||||
| ///* ************************************************************************* */
 | ||||
| //// Test compose with one arguments referring to a constant same rotation
 | ||||
| //TEST(ExpressionFactor, compose3) {
 | ||||
| //
 | ||||
| //  // Create expression
 | ||||
| //  Rot3_ R1(Rot3::identity()), R2(3);
 | ||||
| //  Rot3_ R3 = R1 * R2;
 | ||||
| //
 | ||||
| //  // Create factor
 | ||||
| //  ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), R3);
 | ||||
| //
 | ||||
| //  // Create some values
 | ||||
| //  Values values;
 | ||||
| //  values.insert(3, Rot3());
 | ||||
| //
 | ||||
| //  // Check unwhitenedError
 | ||||
| //  std::vector<Matrix> 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<GaussianFactor> gf = f.linearize(values);
 | ||||
| //  boost::shared_ptr<JacobianFactor> jf = //
 | ||||
| //      boost::dynamic_pointer_cast<JacobianFactor>(gf);
 | ||||
| //  EXPECT( assert_equal(expected, *jf,1e-9));
 | ||||
| //}
 | ||||
| //
 | ||||
| ///* ************************************************************************* */
 | ||||
| //// Test compose with three arguments
 | ||||
| //Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3,
 | ||||
| //    boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2,
 | ||||
| //    boost::optional<Matrix3&> H3) {
 | ||||
| //  // return dummy derivatives (not correct, but that's ok for testing here)
 | ||||
| //  if (H1)
 | ||||
| //    *H1 = eye(3);
 | ||||
| //  if (H2)
 | ||||
| //    *H2 = eye(3);
 | ||||
| //  if (H3)
 | ||||
| //    *H3 = eye(3);
 | ||||
| //  return R1 * (R2 * R3);
 | ||||
| //}
 | ||||
| //
 | ||||
| //TEST(ExpressionFactor, composeTernary) {
 | ||||
| //
 | ||||
| //  // Create expression
 | ||||
| //  Rot3_ A(1), B(2), C(3);
 | ||||
| //  Rot3_ ABC(composeThree, A, B, C);
 | ||||
| //
 | ||||
| //  // Create factor
 | ||||
| //  ExpressionFactor<Rot3> f(noiseModel::Unit::Create(3), Rot3(), ABC);
 | ||||
| //
 | ||||
| //  // Create some values
 | ||||
| //  Values values;
 | ||||
| //  values.insert(1, Rot3());
 | ||||
| //  values.insert(2, Rot3());
 | ||||
| //  values.insert(3, Rot3());
 | ||||
| //
 | ||||
| //  // Check unwhitenedError
 | ||||
| //  std::vector<Matrix> H(3);
 | ||||
| //  Vector actual = f.unwhitenedError(values, H);
 | ||||
| //  EXPECT_LONGS_EQUAL(3, H.size());
 | ||||
| //  EXPECT( assert_equal(eye(3), H[0],1e-9));
 | ||||
| //  EXPECT( assert_equal(eye(3), H[1],1e-9));
 | ||||
| //  EXPECT( assert_equal(eye(3), H[2],1e-9));
 | ||||
| //
 | ||||
| //  // Check linearization
 | ||||
| //  JacobianFactor expected(1, eye(3), 2, eye(3), 3, eye(3), zero(3));
 | ||||
| //  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
 | ||||
| //  boost::shared_ptr<JacobianFactor> jf = //
 | ||||
| //      boost::dynamic_pointer_cast<JacobianFactor>(gf);
 | ||||
| //  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 GenerateTrace<Point2, MyTypes>::type Generated; | ||||
| //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 >)); | ||||
| 
 | ||||
| Generated generated; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue