diff --git a/.cproject b/.cproject index b01317650..895e2667a 100644 --- a/.cproject +++ b/.cproject @@ -806,6 +806,54 @@ true true + + make + -j5 + testInertialNavFactor_GlobalVelocity.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant3.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant1.run + true + true + true + + + make + -j5 + testEquivInertialNavFactor_GlobalVel.run + true + true + true + + + make + -j5 + testInvDepthFactorVariant2.run + true + true + true + + + make + -j5 + testRelativeElevationFactor.run + true + true + true + make -j5 @@ -2193,6 +2241,14 @@ true true + + make + -j5 + testVelocityConstraint.run + true + true + true + make -j1 diff --git a/gtsam/geometry/tests/testPoint2.cpp b/gtsam/geometry/tests/testPoint2.cpp index 66ee5a387..215f376f6 100644 --- a/gtsam/geometry/tests/testPoint2.cpp +++ b/gtsam/geometry/tests/testPoint2.cpp @@ -84,8 +84,8 @@ namespace { Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); /* ************************************************************************* */ - LieVector norm_proxy(const Point2& point) { - return LieVector(point.norm()); + double norm_proxy(const Point2& point) { + return point.norm(); } } TEST( Point2, norm ) { @@ -112,8 +112,8 @@ TEST( Point2, norm ) { /* ************************************************************************* */ namespace { - LieVector distance_proxy(const Point2& location, const Point2& point) { - return LieVector(location.distance(point)); + double distance_proxy(const Point2& location, const Point2& point) { + return location.distance(point); } } TEST( Point2, distance ) { diff --git a/gtsam/geometry/tests/testPose2.cpp b/gtsam/geometry/tests/testPose2.cpp index 42b548f5a..12266c16f 100644 --- a/gtsam/geometry/tests/testPose2.cpp +++ b/gtsam/geometry/tests/testPose2.cpp @@ -574,8 +574,8 @@ TEST( Pose2, bearing_pose ) /* ************************************************************************* */ namespace { - LieVector range_proxy(const Pose2& pose, const Point2& point) { - return LieVector(pose.range(point)); + double range_proxy(const Pose2& pose, const Point2& point) { + return pose.range(point); } } TEST( Pose2, range ) @@ -611,8 +611,8 @@ TEST( Pose2, range ) /* ************************************************************************* */ namespace { - LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) { - return LieVector(pose.range(point)); + double range_pose_proxy(const Pose2& pose, const Pose2& point) { + return pose.range(point); } } TEST( Pose2, range_pose ) diff --git a/gtsam/geometry/tests/testPose3.cpp b/gtsam/geometry/tests/testPose3.cpp index 175a11ff1..2a775379d 100644 --- a/gtsam/geometry/tests/testPose3.cpp +++ b/gtsam/geometry/tests/testPose3.cpp @@ -547,8 +547,8 @@ Pose3 xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); /* ************************************************************************* */ -LieVector range_proxy(const Pose3& pose, const Point3& point) { - return LieVector(pose.range(point)); +double range_proxy(const Pose3& pose, const Point3& point) { + return pose.range(point); } TEST( Pose3, range ) { @@ -582,8 +582,8 @@ TEST( Pose3, range ) } /* ************************************************************************* */ -LieVector range_pose_proxy(const Pose3& pose, const Pose3& point) { - return LieVector(pose.range(point)); +double range_pose_proxy(const Pose3& pose, const Pose3& point) { + return pose.range(point); } TEST( Pose3, range_pose ) { @@ -674,30 +674,24 @@ TEST(Pose3, align_2) { /* ************************************************************************* */ /// exp(xi) exp(y) = exp(xi + x) /// Hence, y = log (exp(-xi)*exp(xi+x)) - Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); -Vector testDerivExpmapInv(const LieVector& dxi) { - Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); - return y; +Vector testDerivExpmapInv(const Vector6& dxi) { + return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi)); } TEST( Pose3, dExpInv_TLN) { Matrix res = Pose3::dExpInv_exp(xi); - Matrix numericalDerivExpmapInv = numericalDerivative11( - boost::function( - boost::bind(testDerivExpmapInv, _1) - ), - LieVector(Vector::Zero(6)), 1e-5 - ); + Matrix numericalDerivExpmapInv = numericalDerivative11( + testDerivExpmapInv, Vector6::Zero(), 1e-5); EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); } /* ************************************************************************* */ -Vector testDerivAdjoint(const LieVector& xi, const LieVector& v) { - return Pose3::adjointMap(xi)*v; +Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) { + return Pose3::adjointMap(xi) * v; } TEST( Pose3, adjoint) { @@ -706,20 +700,16 @@ TEST( Pose3, adjoint) { Matrix actualH; Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); - Matrix numericalH = numericalDerivative21( - boost::function( - boost::bind(testDerivAdjoint, _1, _2) - ), - LieVector(screw::xi), LieVector(screw::xi), 1e-5 - ); + Matrix numericalH = numericalDerivative21( + testDerivAdjoint, screw::xi, screw::xi, 1e-5); EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5)); } /* ************************************************************************* */ -Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { - return Pose3::adjointMap(xi).transpose()*v; +Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) { + return Pose3::adjointMap(xi).transpose() * v; } TEST( Pose3, adjointTranspose) { @@ -730,12 +720,8 @@ TEST( Pose3, adjointTranspose) { Matrix actualH; Vector actual = Pose3::adjointTranspose(xi, v, actualH); - Matrix numericalH = numericalDerivative21( - boost::function( - boost::bind(testDerivAdjointTranspose, _1, _2) - ), - LieVector(xi), LieVector(v), 1e-5 - ); + Matrix numericalH = numericalDerivative21( + testDerivAdjointTranspose, xi, v, 1e-5); EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(numericalH,actualH,1e-5)); diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index 7707e9161..08f5a42e9 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -217,7 +217,7 @@ TEST( Rot3, rightJacobianExpMapSO3 ) // Linearization point Vector3 thetahat; thetahat << 0.1, 0, 0; - Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateRotation, _1), LieVector(thetahat)); + Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateRotation, _1), thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); EXPECT(assert_equal(expectedJacobian, actualJacobian)); } diff --git a/gtsam/linear/tests/testGaussianBayesNet.cpp b/gtsam/linear/tests/testGaussianBayesNet.cpp index 7726f2280..cd5231e49 100644 --- a/gtsam/linear/tests/testGaussianBayesNet.cpp +++ b/gtsam/linear/tests/testGaussianBayesNet.cpp @@ -148,8 +148,9 @@ TEST( GaussianBayesNet, DeterminantTest ) } /* ************************************************************************* */ +typedef Eigen::Matrix Vector10; namespace { - double computeError(const GaussianBayesNet& gbn, const Vector& values) + double computeError(const GaussianBayesNet& gbn, const Vector10& values) { pair Rd = GaussianFactorGraph(gbn).jacobian(); return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); @@ -179,12 +180,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) { 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); // Compute the Hessian numerically - Matrix hessian = numericalHessian(boost::bind(&computeError, gbn, _1), - Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); + Matrix hessian = numericalHessian( + boost::bind(&computeError, gbn, _1), Vector10::Zero()); // Compute the gradient numerically - Vector gradient = numericalGradient(boost::bind(&computeError, gbn, _1), - Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); + Vector gradient = numericalGradient( + boost::bind(&computeError, gbn, _1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); diff --git a/gtsam/linear/tests/testGaussianBayesTree.cpp b/gtsam/linear/tests/testGaussianBayesTree.cpp index 8f37cac0c..217fab543 100644 --- a/gtsam/linear/tests/testGaussianBayesTree.cpp +++ b/gtsam/linear/tests/testGaussianBayesTree.cpp @@ -24,7 +24,6 @@ using namespace boost::assign; #include -#include #include #include #include @@ -175,13 +174,13 @@ TEST(GaussianBayesTree, complicatedMarginal) { EXPECT(assert_equal(expectedCov, actualCov, 1e1)); } +/* ************************************************************************* */ +typedef Eigen::Matrix Vector10; namespace { - /* ************************************************************************* */ - double computeError(const GaussianBayesTree& gbt, const LieVector& values) - { - pair Rd = GaussianFactorGraph(gbt).jacobian(); - return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); - } +double computeError(const GaussianBayesTree& gbt, const Vector10& values) { + pair Rd = GaussianFactorGraph(gbt).jacobian(); + return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); +} } /* ************************************************************************* */ @@ -243,14 +242,12 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) { 2, (Vector(4) << 1.0,2.0,15.0,16.0)))))); // Compute the Hessian numerically - Matrix hessian = numericalHessian( - boost::function(boost::bind(&computeError, bt, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + Matrix hessian = numericalHessian( + boost::bind(&computeError, bt, _1), Vector10::Zero()); // Compute the gradient numerically - Vector gradient = numericalGradient( - boost::function(boost::bind(&computeError, bt, _1)), - LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols()))); + Vector gradient = numericalGradient( + boost::bind(&computeError, bt, _1), Vector10::Zero()); // Compute the gradient using dense matrices Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index ad97d9433..823ce8306 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include #include @@ -39,14 +38,14 @@ using symbol_shorthand::B; /* ************************************************************************* */ namespace { Vector callEvaluateError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias) { return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); } Rot3 evaluateRotationError(const ImuFactor& factor, - const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, + const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j, const imuBias::ConstantBias& bias) { return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; @@ -168,9 +167,9 @@ TEST( ImuFactor, Error ) // Linearization point imuBias::ConstantBias bias; // Bias Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -194,11 +193,11 @@ TEST( ImuFactor, Error ) // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); @@ -240,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases ) // Linearization point // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -278,11 +277,11 @@ TEST( ImuFactor, ErrorWithBiases ) // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); @@ -318,8 +317,8 @@ TEST( ImuFactor, PartialDerivativeExpmap ) // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( - &evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); + Matrix expectedDelRdelBiasOmega = numericalDerivative11(boost::bind( + &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega)); const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); @@ -341,8 +340,8 @@ TEST( ImuFactor, PartialDerivativeLogmap ) // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( - &evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); + Matrix expectedDelFdeltheta = numericalDerivative11(boost::bind( + &evaluateLogRotation, thetahat, _1), Vector3(deltatheta)); const Vector3 x = thetahat; // parametrization of so(3) const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ @@ -447,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) //{ // // Linearization point // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); -// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); -// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); +// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // // // Pre-integrator @@ -503,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); - LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v1((Vector(3) << 0.5, 0.0, 0.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); - LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); + Vector3 v2((Vector(3) << 0.5, 0.0, 0.0)); // Measurements Vector3 gravity; gravity << 0, 0, 9.81; @@ -533,11 +532,11 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement ) // Expected Jacobians Matrix H1e = numericalDerivative11( boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); - Matrix H2e = numericalDerivative11( + Matrix H2e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); Matrix H3e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); - Matrix H4e = numericalDerivative11( + Matrix H4e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); Matrix H5e = numericalDerivative11( boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); diff --git a/gtsam/slam/tests/testPoseRotationPrior.cpp b/gtsam/slam/tests/testPoseRotationPrior.cpp index b40da2e2a..3cac377eb 100644 --- a/gtsam/slam/tests/testPoseRotationPrior.cpp +++ b/gtsam/slam/tests/testPoseRotationPrior.cpp @@ -37,13 +37,13 @@ const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); /* ************************************************************************* */ -LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ -LieVector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ @@ -52,8 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) { Pose3RotationPrior factor(poseKey, rot3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -63,8 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) { Pose3RotationPrior factor(poseKey, rot3C, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -74,8 +72,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) { Pose2RotationPrior factor(poseKey, rot2A, model1); Matrix actH1; EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -85,8 +82,7 @@ TEST( testPoseRotationFactor, level2_error ) { Pose2RotationPrior factor(poseKey, rot2B, model1); Matrix actH1; EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testPoseTranslationPrior.cpp b/gtsam/slam/tests/testPoseTranslationPrior.cpp index 36d94c9a3..47ff708d9 100644 --- a/gtsam/slam/tests/testPoseTranslationPrior.cpp +++ b/gtsam/slam/tests/testPoseTranslationPrior.cpp @@ -34,13 +34,13 @@ const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); /* ************************************************************************* */ -LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ -LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { - return LieVector(factor.evaluateError(x)); +Vector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { + return factor.evaluateError(x); } /* ************************************************************************* */ @@ -49,8 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -60,8 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -71,8 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -82,8 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -93,8 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) { Pose3TranslationPrior factor(poseKey, point3A, model3); Matrix actH1; EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -104,8 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) { Pose3TranslationPrior factor(poseKey, point3B, model3); Matrix actH1; EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -115,8 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) { Pose2TranslationPrior factor(poseKey, point2A, model2); Matrix actH1; EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } @@ -126,8 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) { Pose2TranslationPrior factor(poseKey, point2B, model2); Matrix actH1; EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); - Matrix expH1 = numericalDerivative11( - boost::bind(evalFactorError2, factor, _1), pose1, 1e-5); + Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); } diff --git a/gtsam/slam/tests/testRangeFactor.cpp b/gtsam/slam/tests/testRangeFactor.cpp index 4fa6164a1..cba9300f5 100644 --- a/gtsam/slam/tests/testRangeFactor.cpp +++ b/gtsam/slam/tests/testRangeFactor.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -37,12 +36,12 @@ typedef RangeFactor RangeFactor2D; typedef RangeFactor RangeFactor3D; /* ************************************************************************* */ -LieVector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { +Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { return factor.evaluateError(pose, point); } /* ************************************************************************* */ -LieVector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { +Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { return factor.evaluateError(pose, point); } @@ -214,8 +213,8 @@ TEST( RangeFactor, Jacobian2D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -243,8 +242,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError2D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError2D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -269,8 +268,8 @@ TEST( RangeFactor, Jacobian3D ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); @@ -298,8 +297,8 @@ TEST( RangeFactor, Jacobian3DWithTransform ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); - H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); + H1Expected = numericalDerivative11(boost::bind(&factorError3D, _1, point, factor), pose); + H2Expected = numericalDerivative11(boost::bind(&factorError3D, pose, _1, factor), point); // Verify the Jacobians are correct CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); diff --git a/gtsam/slam/tests/testReferenceFrameFactor.cpp b/gtsam/slam/tests/testReferenceFrameFactor.cpp index 078bf85cd..309801ccb 100644 --- a/gtsam/slam/tests/testReferenceFrameFactor.cpp +++ b/gtsam/slam/tests/testReferenceFrameFactor.cpp @@ -53,9 +53,9 @@ TEST( ReferenceFrameFactor, equals ) { } /* ************************************************************************* */ -LieVector evaluateError_(const PointReferenceFrameFactor& c, +Vector evaluateError_(const PointReferenceFrameFactor& c, const Point2& global, const Pose2& trans, const Point2& local) { - return LieVector(c.evaluateError(global, trans, local)); + return Vector(c.evaluateError(global, trans, local)); } TEST( ReferenceFrameFactor, jacobians ) { @@ -68,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) { tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( + numericalDF = numericalDerivative31( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( + numericalDT = numericalDerivative32( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( + numericalDL = numericalDerivative33( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); @@ -100,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) { tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); Matrix numericalDT, numericalDL, numericalDF; - numericalDF = numericalDerivative31( + numericalDF = numericalDerivative31( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDT = numericalDerivative32( + numericalDT = numericalDerivative32( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); - numericalDL = numericalDerivative33( + numericalDL = numericalDerivative33( boost::bind(evaluateError_, tc, _1, _2, _3), global, trans, local, 1e-5); diff --git a/gtsam_unstable/dynamics/VelocityConstraint.h b/gtsam_unstable/dynamics/VelocityConstraint.h index 252f9dbfe..c9db449f9 100644 --- a/gtsam_unstable/dynamics/VelocityConstraint.h +++ b/gtsam_unstable/dynamics/VelocityConstraint.h @@ -83,9 +83,9 @@ public: virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, boost::optional H1=boost::none, boost::optional H2=boost::none) const { - if (H1) *H1 = gtsam::numericalDerivative21( + if (H1) *H1 = gtsam::numericalDerivative21( boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); - if (H2) *H2 = gtsam::numericalDerivative22( + if (H2) *H2 = gtsam::numericalDerivative22( boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); return evaluateError_(x1, x2, dt_, integration_mode_); } @@ -103,7 +103,7 @@ public: } private: - static gtsam::LieVector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, + static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2, double dt, const dynamics::IntegrationMode& mode) { const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t(); diff --git a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp index 82197302b..475d5285c 100644 --- a/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp +++ b/gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp @@ -18,11 +18,11 @@ const double h = 0.01; //const double deg2rad = M_PI/180.0; //Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0)); Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0)); -//LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); -LieVector V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); -LieVector V1_g1 = g1.inverse().Adjoint(V1_w); +//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0)); +Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0)); +Vector6 V1_g1 = g1.inverse().Adjoint(V1_w); Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); -//LieVector v2 = Pose3::Logmap(g1.between(g2)); +//Vector6 v2 = Pose3::Logmap(g1.between(g2)); double mass = 100.0; Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape @@ -46,16 +46,16 @@ Vector computeFu(const Vector& gamma, const Vector& control) { } /* ************************************************************************* */ -Vector testExpmapDeriv(const LieVector& v) { +Vector testExpmapDeriv(const Vector6& v) { return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v)); } TEST(Reconstruction, ExpmapInvDeriv) { Matrix numericalExpmap = numericalDerivative11( - boost::function( + boost::function( boost::bind(testExpmapDeriv, _1) ), - LieVector(Vector::Zero(6)), 1e-5 + Vector6(Vector::Zero(6)), 1e-5 ); Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1); EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); @@ -72,21 +72,21 @@ TEST( Reconstruction, evaluateError) { Matrix numericalH1 = numericalDerivative31( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( + boost::function( boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), g2, g1, V1_g1, 1e-5 @@ -119,7 +119,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { Pose3 g21 = g2.between(g1); Vector V2_g2 = g21.Adjoint(V2_g1); // convert the new velocity to g2's frame - LieVector expectedv2(V2_g2); + Vector6 expectedv2(V2_g2); // hard constraints don't need a noise model DiscreteEulerPoincareHelicopter constraint(V(2), V(1), G(2), h, @@ -130,21 +130,21 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) { EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0)); Matrix numericalH1 = numericalDerivative31( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH2 = numericalDerivative32( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 ); Matrix numericalH3 = numericalDerivative33( - boost::function( + boost::function( boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) ), expectedv2, V1_g1, g2, 1e-5 diff --git a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp index 6e0b92fd7..3385998b0 100644 --- a/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp +++ b/gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp @@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) { Point2 expected_uv = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); + Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); LieScalar inv_depth(1./4); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); EXPECT(assert_equal(expected_uv, actual_uv)); @@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 1., M_PI/4., atan(1.0/sqrt(2.0)))); LieScalar inv_depth(1/sqrt(3.0)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 0., M_PI/4., atan(2./sqrt(2.0)))); LieScalar inv_depth( 1./sqrt(1.0+1+4)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) { Point2 expected = level_camera.project(landmark); InvDepthCamera3 inv_camera(level_pose, K); - LieVector diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); + Vector5 diag_landmark((Vector(5) << 0., 0., 0., atan(4.0/1), atan(2./sqrt(1.+16.)))); LieScalar inv_depth(1./sqrt(1.+16.+4.)); Point2 actual = inv_camera.project(diag_landmark, inv_depth); EXPECT(assert_equal(expected, actual)); @@ -83,12 +83,12 @@ TEST( InvDepthFactor, Project4) { /* ************************************************************************* */ -Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) { +Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) { return InvDepthCamera3(pose,K).project(landmark, inv_depth); } TEST( InvDepthFactor, Dproject_pose) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_landmark) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark) /* ************************************************************************* */ TEST( InvDepthFactor, Dproject_inv_depth) { - LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); + Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2)); LieScalar inv_depth(1./4); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); InvDepthCamera3 inv_camera(level_pose,K); @@ -124,12 +124,12 @@ TEST( InvDepthFactor, Dproject_inv_depth) /* ************************************************************************* */ TEST(InvDepthFactor, backproject) { - LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); + Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2)); LieScalar inv_depth(1./4); InvDepthCamera3 inv_camera(level_pose,K); Point2 z = inv_camera.project(expected, inv_depth); - LieVector actual_vec; + Vector5 actual_vec; LieScalar actual_inv; boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4); EXPECT(assert_equal(expected,actual_vec,1e-7)); @@ -140,12 +140,12 @@ TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject2) { // backwards facing camera - LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); + Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); LieScalar inv_depth(1./10); InvDepthCamera3 inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); Point2 z = inv_camera.project(expected, inv_depth); - LieVector actual_vec; + Vector5 actual_vec; LieScalar actual_inv; boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10); EXPECT(assert_equal(expected,actual_vec,1e-7)); diff --git a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h index b2174f8a9..41f216b77 100644 --- a/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h +++ b/gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h @@ -315,8 +315,9 @@ public: // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H2_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -336,8 +337,9 @@ public: // Jacobian w.r.t. Vel2 if (H5){ - Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H5_Pose = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = numericalDerivative11(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h index 5ca736c01..76f6d02d5 100644 --- a/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h +++ b/gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h @@ -242,8 +242,9 @@ public: // Jacobian w.r.t. Vel1 if (H2){ - Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); - Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H2_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); + Matrix H2_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); *H2 = stack(2, &H2_Pose, &H2_Vel); } @@ -263,8 +264,9 @@ public: // Jacobian w.r.t. Vel2 if (H5){ - Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); - Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3"); + Matrix H5_Pose = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); + Matrix H5_Vel = gtsam::numericalDerivative11(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); *H5 = stack(2, &H5_Pose, &H5_Vel); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant1.h b/gtsam_unstable/slam/InvDepthFactorVariant1.h index 6a8c297b7..d2a784b0f 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant1.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant1.h @@ -110,7 +110,7 @@ public: landmark), pose); } if (H2) { - (*H2) = numericalDerivative11( + (*H2) = numericalDerivative11( boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, _1), landmark); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant2.h b/gtsam_unstable/slam/InvDepthFactorVariant2.h index 8d55d1ce5..3c95a5010 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant2.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant2.h @@ -113,7 +113,7 @@ public: landmark), pose); } if (H2) { - (*H2) = numericalDerivative11( + (*H2) = numericalDerivative11( boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, _1), landmark); } diff --git a/gtsam_unstable/slam/InvDepthFactorVariant3.h b/gtsam_unstable/slam/InvDepthFactorVariant3.h index e4b282550..2f94227dc 100644 --- a/gtsam_unstable/slam/InvDepthFactorVariant3.h +++ b/gtsam_unstable/slam/InvDepthFactorVariant3.h @@ -111,7 +111,7 @@ public: (*H1) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); } if(H2) { - (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); + (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); } return inverseDepthError(pose, landmark); @@ -234,7 +234,7 @@ public: (*H2) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); } if(H3) { - (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); + (*H3) = numericalDerivative11(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); } return inverseDepthError(pose1, pose2, landmark); diff --git a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp index 7756e79d3..57a19ee78 100644 --- a/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp @@ -29,31 +29,37 @@ using namespace std; using namespace gtsam; -gtsam::Rot3 world_R_ECEF( +Rot3 world_R_ECEF( 0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); -gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); +Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); +Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ -gtsam::Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { +Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, + const InertialNavFactor_GlobalVelocity& factor) { return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6)); } -gtsam::LieVector predictionErrorVel(const Pose3& p1, const LieVector& v1, const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const InertialNavFactor_GlobalVelocity& factor) { - return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3)); +Vector predictionErrorVel(const Pose3& p1, const LieVector& v1, + const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, + const InertialNavFactor_GlobalVelocity& factor) { + return factor.evaluateError(p1, v1, b1, p2, v2).tail(3); } /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, Constructor) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); @@ -61,8 +67,8 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor) double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -72,11 +78,11 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, Equals) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); @@ -84,8 +90,8 @@ TEST( InertialNavFactor_GlobalVelocity, Equals) double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -97,17 +103,17 @@ TEST( InertialNavFactor_GlobalVelocity, Equals) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, Predict) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -134,17 +140,17 @@ TEST( InertialNavFactor_GlobalVelocity, Predict) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -170,17 +176,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -205,17 +211,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -262,33 +268,33 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) // -q[1], q[0],0.0); // Matrix J_hyp( -(R1*qx).matrix() ); // -// gtsam::Matrix J_expected; +// Matrix J_expected; // // LieVector v(predictionRq(angles, q)); // -// J_expected = gtsam::numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); +// J_expected = numericalDerivative11(boost::bind(&predictionRq, _1, q), angles); // // cout<<"J_hyp"<(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; + H1_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); - CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); + CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); + CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); + CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); + CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); + CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); // Checking for Vel part in the jacobians // ****** @@ -347,19 +353,19 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; - H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; + H1_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); + CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); + CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); + CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); + CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); + CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } @@ -368,11 +374,11 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); @@ -380,8 +386,8 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -394,11 +400,11 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) { - gtsam::Key Pose1(11); - gtsam::Key Pose2(12); - gtsam::Key Vel1(21); - gtsam::Key Vel2(22); - gtsam::Key Bias1(31); + Key Pose1(11); + Key Pose2(12); + Key Vel1(21); + Key Vel2(22); + Key Bias1(31); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); @@ -406,8 +412,8 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -422,17 +428,17 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -462,17 +468,17 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -501,17 +507,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -540,17 +546,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) /* ************************************************************************* */ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.1); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -589,17 +595,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) /* ************************************************************************* */ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { - gtsam::Key PoseKey1(11); - gtsam::Key PoseKey2(12); - gtsam::Key VelKey1(21); - gtsam::Key VelKey2(22); - gtsam::Key BiasKey1(31); + Key PoseKey1(11); + Key PoseKey2(12); + Key VelKey1(21); + Key VelKey2(22); + Key BiasKey1(31); double measurement_dt(0.01); Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); - gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); + Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); @@ -640,19 +646,19 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; - H1_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedPose = gtsam::numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; + H1_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedPose = numericalDerivative11(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); - CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); + CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); + CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); + CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); + CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); + CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); // Checking for Vel part in the jacobians // ****** @@ -663,19 +669,19 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols())); // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function - gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; - H1_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); - H2_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); - H3_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); - H4_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); - H5_expectedVel = gtsam::numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); + Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; + H1_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); + H2_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); + H3_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); + H4_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); + H5_expectedVel = numericalDerivative11(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); // Verify they are equal for this choice of state - CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); - CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); + CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); + CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); + CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); + CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); + CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); } /* ************************************************************************* */ diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 24535854d..503a4b953 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -24,8 +24,8 @@ using namespace gtsam; TEST( InvDepthFactorVariant1, optimize) { // Create two poses looking in the x-direction - Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0)); - Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5)); + Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0)); + Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5)); // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1)) Point3 landmark(5, 0, 1); @@ -76,18 +76,18 @@ TEST( InvDepthFactorVariant1, optimize) { LieVector actual = result.at(landmarkKey); - values.at(poseKey1).print("Pose1 Before:\n"); - result.at(poseKey1).print("Pose1 After:\n"); - pose1.print("Pose1 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(poseKey2).print("Pose2 Before:\n"); - result.at(poseKey2).print("Pose2 After:\n"); - pose2.print("Pose2 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(landmarkKey).print("Landmark Before:\n"); - result.at(landmarkKey).print("Landmark After:\n"); - expected.print("Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// values.at(poseKey1).print("Pose1 Before:\n"); +// result.at(poseKey1).print("Pose1 After:\n"); +// pose1.print("Pose1 Truth:\n"); +// cout << endl << endl; +// values.at(poseKey2).print("Pose2 Before:\n"); +// result.at(poseKey2).print("Pose2 After:\n"); +// pose2.print("Pose2 Truth:\n"); +// cout << endl << endl; +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(landmarkKey).print("Landmark After:\n"); +// expected.print("Landmark Truth:\n"); +// cout << endl << endl; // Calculate world coordinates of landmark versions Point3 world_landmarkBefore; @@ -105,10 +105,10 @@ TEST( InvDepthFactorVariant1, optimize) { world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - world_landmarkBefore.print("World Landmark Before:\n"); - world_landmarkAfter.print("World Landmark After:\n"); - landmark.print("World Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// world_landmarkBefore.print("World Landmark Before:\n"); +// world_landmarkAfter.print("World Landmark After:\n"); +// landmark.print("World Landmark Truth:\n"); +// cout << endl << endl; diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp index e99c9bcdf..49e5fc2aa 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp @@ -73,19 +73,18 @@ TEST( InvDepthFactorVariant2, optimize) { Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); LieVector actual = result.at(landmarkKey); - - values.at(poseKey1).print("Pose1 Before:\n"); - result.at(poseKey1).print("Pose1 After:\n"); - pose1.print("Pose1 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(poseKey2).print("Pose2 Before:\n"); - result.at(poseKey2).print("Pose2 After:\n"); - pose2.print("Pose2 Truth:\n"); - std::cout << std::endl << std::endl; - values.at(landmarkKey).print("Landmark Before:\n"); - result.at(landmarkKey).print("Landmark After:\n"); - expected.print("Landmark Truth:\n"); - std::cout << std::endl << std::endl; +// values.at(poseKey1).print("Pose1 Before:\n"); +// result.at(poseKey1).print("Pose1 After:\n"); +// pose1.print("Pose1 Truth:\n"); +// std::cout << std::endl << std::endl; +// values.at(poseKey2).print("Pose2 Before:\n"); +// result.at(poseKey2).print("Pose2 After:\n"); +// pose2.print("Pose2 Truth:\n"); +// std::cout << std::endl << std::endl; +// values.at(landmarkKey).print("Landmark Before:\n"); +// result.at(landmarkKey).print("Landmark After:\n"); +// expected.print("Landmark Truth:\n"); +// std::cout << std::endl << std::endl; // Calculate world coordinates of landmark versions Point3 world_landmarkBefore; @@ -101,12 +100,10 @@ TEST( InvDepthFactorVariant2, optimize) { world_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); } - world_landmarkBefore.print("World Landmark Before:\n"); - world_landmarkAfter.print("World Landmark After:\n"); - landmark.print("World Landmark Truth:\n"); - std::cout << std::endl << std::endl; - - +// world_landmarkBefore.print("World Landmark Before:\n"); +// world_landmarkAfter.print("World Landmark After:\n"); +// landmark.print("World Landmark Truth:\n"); +// std::cout << std::endl << std::endl; // Test that the correct landmark parameters have been recovered EXPECT(assert_equal(expected, actual, 1e-9)); diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp index e65b7cacb..11a91f687 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp @@ -39,7 +39,7 @@ TEST( InvDepthFactorVariant3, optimize) { // Create expected landmark Point3 landmark_p1 = pose1.transform_to(landmark); - landmark_p1.print("Landmark in Pose1 Frame:\n"); + // landmark_p1.print("Landmark in Pose1 Frame:\n"); double theta = atan2(landmark_p1.x(), landmark_p1.z()); double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z())); double rho = 1./landmark_p1.norm(); diff --git a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp index 5b572fb69..20490a8e4 100644 --- a/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp +++ b/gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp @@ -136,11 +136,11 @@ TEST( ProjectionFactor, Jacobian ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 and H4 with numerical derivatives - Matrix H2Expected = numericalDerivative11( + Matrix H2Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, *K1, boost::none, boost::none, boost::none, boost::none), Pose3()); - Matrix H4Expected = numericalDerivative11( + Matrix H4Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point, _1, boost::none, boost::none, boost::none, boost::none), *K1); @@ -172,11 +172,11 @@ TEST( ProjectionFactor, JacobianWithTransform ) { CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); // Verify H2 and H4 with numerical derivatives - Matrix H2Expected = numericalDerivative11( + Matrix H2Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); - Matrix H4Expected = numericalDerivative11( + Matrix H4Expected = numericalDerivative11( boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, _1, boost::none, boost::none, boost::none, boost::none), *K1); diff --git a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp index ffc7344cf..0d5cb2e36 100644 --- a/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp +++ b/gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp @@ -24,8 +24,8 @@ const Point3 point1(3.0, 4.0, 2.0); const gtsam::Key poseKey = 1, pointKey = 2; /* ************************************************************************* */ -LieVector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) { - return LieVector(factor.evaluateError(x, p)); +Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) { + return factor.evaluateError(x, p); } /* ************************************************************************* */ @@ -35,9 +35,9 @@ TEST( testRelativeElevationFactor, level_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -50,9 +50,9 @@ TEST( testRelativeElevationFactor, level_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -65,9 +65,9 @@ TEST( testRelativeElevationFactor, level_negative ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -80,9 +80,9 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -95,9 +95,9 @@ TEST( testRelativeElevationFactor, rotated_positive ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -110,9 +110,9 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); @@ -125,9 +125,9 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) { RelativeElevationFactor factor(poseKey, pointKey, measured, model1); Matrix actH1, actH2; EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); - Matrix expH1 = numericalDerivative21( + Matrix expH1 = numericalDerivative21( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); - Matrix expH2 = numericalDerivative22( + Matrix expH2 = numericalDerivative22( boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH2, actH2, tol)); diff --git a/gtsam_unstable/slam/tests/testTSAMFactors.cpp b/gtsam_unstable/slam/tests/testTSAMFactors.cpp index 44dca9b8e..a8a3ae5e9 100644 --- a/gtsam_unstable/slam/tests/testTSAMFactors.cpp +++ b/gtsam_unstable/slam/tests/testTSAMFactors.cpp @@ -44,10 +44,10 @@ TEST( DeltaFactor, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, boost::none), pose); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, boost::none), point); @@ -78,16 +78,16 @@ TEST( DeltaFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, point, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, point, boost::none, boost::none, boost::none, boost::none), pose); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, point, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, _1, boost::none, boost::none, boost::none, boost::none), point); @@ -119,16 +119,16 @@ TEST( OdometryFactorBase, all ) { // Use numerical derivatives to calculate the Jacobians Matrix H1Expected, H2Expected, H3Expected, H4Expected; - H1Expected = numericalDerivative11( + H1Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, pose2, boost::none, boost::none, boost::none, boost::none), base1); - H2Expected = numericalDerivative11( + H2Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, pose2, boost::none, boost::none, boost::none, boost::none), pose1); - H3Expected = numericalDerivative11( + H3Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, pose2, boost::none, boost::none, boost::none, boost::none), base2); - H4Expected = numericalDerivative11( + H4Expected = numericalDerivative11( boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, base2, _1, boost::none, boost::none, boost::none, boost::none), pose2);