diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp index b38cf8518..7e2aa507e 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp @@ -22,26 +22,21 @@ using namespace std; using namespace gtsam; -// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below -// to reenable the test. -//#if 0 /* ************************************************************************* */ -LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor& factor){ +Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor& factor){ gtsam::Values values; values.insert(key, org1_T_org2); - // LieVector err = factor.whitenedError(values); - // return err; - return LieVector::Expmap(factor.whitenedError(values)); + return factor.whitenedError(values); } /* ************************************************************************* */ -//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ +//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ // gtsam::Values values; // values.insert(keyA, p1); // values.insert(keyB, p2); -// // LieVector err = factor.whitenedError(values); +// // Vector err = factor.whitenedError(values); // // return err; -// return LieVector::Expmap(factor.whitenedError(values)); +// return Vector::Expmap(factor.whitenedError(values)); //} /* ************************************************************************* */ @@ -236,7 +231,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) Matrix H1_actual = H_actual[0]; double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } @@ -290,12 +285,12 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); // // double stepsize = 1.0e-9; -// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); // // // // try to check numerical derivatives of a standard between factor -// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // @@ -304,8 +299,6 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian) // //} -//#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp index 68671d0bd..146cdc06f 100644 --- a/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp +++ b/gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp @@ -22,26 +22,21 @@ using namespace std; using namespace gtsam; -// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below -// to reenable the test. -//#if 0 /* ************************************************************************* */ -LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM& factor){ +Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM& factor){ gtsam::Values values; values.insert(key, org1_T_org2); - // LieVector err = factor.whitenedError(values); - // return err; - return LieVector::Expmap(factor.whitenedError(values)); + return factor.whitenedError(values); } /* ************************************************************************* */ -//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ +//Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor& factor){ // gtsam::Values values; // values.insert(keyA, p1); // values.insert(keyB, p2); -// // LieVector err = factor.whitenedError(values); +// // Vector err = factor.whitenedError(values); // // return err; -// return LieVector::Expmap(factor.whitenedError(values)); +// return Vector::Expmap(factor.whitenedError(values)); //} /* ************************************************************************* */ @@ -265,7 +260,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) Matrix H1_actual = H_actual[0]; double stepsize = 1.0e-9; - Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); + Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); } /////* ************************************************************************** */ @@ -315,12 +310,12 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); // // double stepsize = 1.0e-9; -// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); -// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); +// Matrix H1_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); +// Matrix H2_expected = gtsam::numericalDerivative11(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); // // // // try to check numerical derivatives of a standard between factor -// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); +// Matrix H1_expected_stnd = gtsam::numericalDerivative11(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // // @@ -329,8 +324,6 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian) // //} -//#endif - /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */