test waiting for implementation of constraint_autodiff::evaluate

src/test/gtest_constraint_autodiff.cpp

@@ -12,6 +12,7 @@
 #include "capture_void.h"
 #include "feature_odom_2D.h"
 #include "constraint_odom_2D.h"
+#include "constraint_odom_2D_analytic.h"
 #include "constraint_autodiff.h"
 
 using namespace wolf;
@@ -122,23 +123,27 @@ TEST(CaptureAutodiff, JacobianOdom2d)
     J0 << -cos(f1_pose(2)), -sin(f1_pose(2)),
            sin(f1_pose(2)), -cos(f1_pose(2)),
            0,                0;
-    ASSERT_TRUE((J0-Jauto[0]).maxCoeff() < 1e-9 && (J0-Jauto[0]).minCoeff() > -1e-9);
+    ASSERT_MATRIX_APPROX(J0, Jauto[0], wolf::Constants::EPS);
+    //ASSERT_TRUE((J0-Jauto[0]).maxCoeff() < 1e-9 && (J0-Jauto[0]).minCoeff() > -1e-9);
 
     Eigen::MatrixXs J1(3,1);
     J1 << -(f2_pose(0) - f1_pose(0)) * sin(f1_pose(2)) + (f2_pose(1) - f1_pose(1)) * cos(f1_pose(2)),
           -(f2_pose(0) - f1_pose(0)) * cos(f1_pose(2)) - (f2_pose(1) - f1_pose(1)) * sin(f1_pose(2)),
           -1;
-    ASSERT_TRUE((J1-Jauto[1]).maxCoeff() < 1e-9 && (J1-Jauto[1]).minCoeff() > -1e-9);
+    ASSERT_MATRIX_APPROX(J1, Jauto[1], wolf::Constants::EPS);
+    //ASSERT_TRUE((J1-Jauto[1]).maxCoeff() < 1e-9 && (J1-Jauto[1]).minCoeff() > -1e-9);
 
     Eigen::MatrixXs J2(3,2);
     J2 <<  cos(f1_pose(2)), sin(f1_pose(2)),
            -sin(f1_pose(2)), cos(f1_pose(2)),
            0,               0;
-    ASSERT_TRUE((J2-Jauto[2]).maxCoeff() < 1e-9 && (J2-Jauto[2]).minCoeff() > -1e-9);
+    ASSERT_MATRIX_APPROX(J2, Jauto[2], wolf::Constants::EPS);
+    //ASSERT_TRUE((J2-Jauto[2]).maxCoeff() < 1e-9 && (J2-Jauto[2]).minCoeff() > -1e-9);
 
     Eigen::MatrixXs J3(3,1);
     J3 <<  0, 0, 1;
-    ASSERT_TRUE((J3-Jauto[3]).maxCoeff() < 1e-9 && (J3-Jauto[3]).minCoeff() > -1e-9);
+    ASSERT_MATRIX_APPROX(J3, Jauto[3], wolf::Constants::EPS);
+    //ASSERT_TRUE((J3-Jauto[3]).maxCoeff() < 1e-9 && (J3-Jauto[3]).minCoeff() > -1e-9);
 
 //    std::cout << "autodiff J " << 0 << ":\n" << Jauto[0] << std::endl;
 //    std::cout << "analytic J " << 0 << ":\n" << J0 << std::endl;
@@ -150,6 +155,52 @@ TEST(CaptureAutodiff, JacobianOdom2d)
 //    std::cout << "analytic J " << 3 << ":\n" << J3 << std::endl;
 }
 
+TEST(CaptureAutodiff, AutodiffVsAnalytic)
+{
+    Eigen::Vector3s f1_pose, f2_pose;
+    f1_pose << 2,1,M_PI;
+    f2_pose << 3,5,3*M_PI/2;
+
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+
+    // SENSOR
+    SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), 0.1, 0.1);
+
+    // CAPTURE
+    CaptureVoidPtr capture_ptr = std::make_shared<CaptureVoid>(TimeStamp(0), sensor_ptr);
+    fr2_ptr->addCapture(capture_ptr);
+
+    // FEATURE
+    Eigen::Vector3s d;
+    d << -1, -4, M_PI/2;
+    FeatureBasePtr feature_ptr = std::make_shared<FeatureOdom2D>(d, Eigen::Matrix3s::Identity());
+    capture_ptr->addFeature(feature_ptr);
+
+    // CONSTRAINTS
+    ConstraintOdom2DPtr ctr_autodiff_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
+    feature_ptr->addConstraint(ctr_autodiff_ptr);
+    fr1_ptr->addConstrainedBy(ctr_autodiff_ptr);
+    ConstraintOdom2DAnalyticPtr ctr_analytic_ptr = std::make_shared<ConstraintOdom2DAnalytic>(feature_ptr, fr1_ptr);
+    feature_ptr->addConstraint(ctr_analytic_ptr);
+    fr1_ptr->addConstrainedBy(ctr_analytic_ptr);
+
+    // COMPUTE JACOBIANS
+    std::vector<const Scalar*> states_ptr({fr1_ptr->getPPtr()->getPtr(), fr1_ptr->getOPtr()->getPtr(), fr2_ptr->getPPtr()->getPtr(), fr2_ptr->getOPtr()->getPtr()});
+    std::vector<Eigen::MatrixXs> Jautodiff, Janalytic;
+    Eigen::VectorXs residuals(ctr_autodiff_ptr->getSize());
+    clock_t t = clock();
+    ctr_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
+    std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
+    t = clock();
+    //TODO ConstraintAnalytic::evaluate
+//    ctr_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
+//    std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
+//
+//    for (auto i = 0; i < Jautodiff.size(); i++)
+//        ASSERT_MATRIX_APPROX(Jautodiff[i], Janalytic[i], wolf::Constants::EPS);
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);