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

Got rid of unnecessary LieVector usage that broke fixed-code

release/4.3a0
dellaert 2014-10-22 01:32:59 +02:00
parent 3b0d2a5f47
commit 113b9d2e74
27 changed files with 424 additions and 391 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
.cproject
View File

@ -806,6 +806,54 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testInertialNavFactor_GlobalVelocity.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInertialNavFactor_GlobalVelocity.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant3.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant1.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant1.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEquivInertialNavFactor_GlobalVel.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEquivInertialNavFactor_GlobalVel.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant2.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRelativeElevationFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRelativeElevationFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2193,6 +2241,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testVelocityConstraint.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVelocityConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments> <buildArguments>-j1</buildArguments>

8
gtsam/geometry/tests/testPoint2.cpp
View File

@ -84,8 +84,8 @@ namespace {
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector norm_proxy(const Point2& point) { double norm_proxy(const Point2& point) {
return LieVector(point.norm()); return point.norm();
} }
} }
TEST( Point2, norm ) { TEST( Point2, norm ) {
@ -112,8 +112,8 @@ TEST( Point2, norm ) {
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
LieVector distance_proxy(const Point2& location, const Point2& point) { double distance_proxy(const Point2& location, const Point2& point) {
return LieVector(location.distance(point)); return location.distance(point);
} }
} }
TEST( Point2, distance ) { TEST( Point2, distance ) {

8
gtsam/geometry/tests/testPose2.cpp
View File

@ -574,8 +574,8 @@ TEST( Pose2, bearing_pose )
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
LieVector range_proxy(const Pose2& pose, const Point2& point) { double range_proxy(const Pose2& pose, const Point2& point) {
return LieVector(pose.range(point)); return pose.range(point);
} }
} }
TEST( Pose2, range ) TEST( Pose2, range )
@ -611,8 +611,8 @@ TEST( Pose2, range )
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) { double range_pose_proxy(const Pose2& pose, const Pose2& point) {
return LieVector(pose.range(point)); return pose.range(point);
} }
} }
TEST( Pose2, range_pose ) TEST( Pose2, range_pose )

46
gtsam/geometry/tests/testPose3.cpp
View File

@ -547,8 +547,8 @@ Pose3
xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
/* ************************************************************************* */ /* ************************************************************************* */
LieVector range_proxy(const Pose3& pose, const Point3& point) { double range_proxy(const Pose3& pose, const Point3& point) {
return LieVector(pose.range(point)); return pose.range(point);
} }
TEST( Pose3, range ) TEST( Pose3, range )
{ {
@ -582,8 +582,8 @@ TEST( Pose3, range )
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector range_pose_proxy(const Pose3& pose, const Pose3& point) { double range_pose_proxy(const Pose3& pose, const Pose3& point) {
return LieVector(pose.range(point)); return pose.range(point);
} }
TEST( Pose3, range_pose ) TEST( Pose3, range_pose )
{ {
@ -674,30 +674,24 @@ TEST(Pose3, align_2) {
/* ************************************************************************* */ /* ************************************************************************* */
/// exp(xi) exp(y) = exp(xi + x) /// exp(xi) exp(y) = exp(xi + x)
/// Hence, y = log (exp(-xi)*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 xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0);
Vector testDerivExpmapInv(const LieVector& dxi) { Vector testDerivExpmapInv(const Vector6& dxi) {
Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi));
return y;
} }
TEST( Pose3, dExpInv_TLN) { TEST( Pose3, dExpInv_TLN) {
Matrix res = Pose3::dExpInv_exp(xi); Matrix res = Pose3::dExpInv_exp(xi);
Matrix numericalDerivExpmapInv = numericalDerivative11( Matrix numericalDerivExpmapInv = numericalDerivative11<Vector, Vector6>(
boost::function<Vector(const LieVector&)>( testDerivExpmapInv, Vector6::Zero(), 1e-5);
boost::bind(testDerivExpmapInv, _1)
),
LieVector(Vector::Zero(6)), 1e-5
);
EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector testDerivAdjoint(const LieVector& xi, const LieVector& v) { Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi)*v; return Pose3::adjointMap(xi) * v;
} }
TEST( Pose3, adjoint) { TEST( Pose3, adjoint) {
@ -706,20 +700,16 @@ TEST( Pose3, adjoint) {
Matrix actualH; Matrix actualH;
Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH);
Matrix numericalH = numericalDerivative21( Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>(
boost::function<Vector(const LieVector&, const LieVector&)>( testDerivAdjoint, screw::xi, screw::xi, 1e-5);
boost::bind(testDerivAdjoint, _1, _2)
),
LieVector(screw::xi), LieVector(screw::xi), 1e-5
);
EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(expected,actual,1e-5));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi).transpose()*v; return Pose3::adjointMap(xi).transpose() * v;
} }
TEST( Pose3, adjointTranspose) { TEST( Pose3, adjointTranspose) {
@ -730,12 +720,8 @@ TEST( Pose3, adjointTranspose) {
Matrix actualH; Matrix actualH;
Vector actual = Pose3::adjointTranspose(xi, v, actualH); Vector actual = Pose3::adjointTranspose(xi, v, actualH);
Matrix numericalH = numericalDerivative21( Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>(
boost::function<Vector(const LieVector&, const LieVector&)>( testDerivAdjointTranspose, xi, v, 1e-5);
boost::bind(testDerivAdjointTranspose, _1, _2)
),
LieVector(xi), LieVector(v), 1e-5
);
EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(expected,actual,1e-15));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5));

2
gtsam/geometry/tests/testRot3M.cpp
View File

@ -217,7 +217,7 @@ TEST( Rot3, rightJacobianExpMapSO3 )
// Linearization point // Linearization point
Vector3 thetahat; thetahat << 0.1, 0, 0; Vector3 thetahat; thetahat << 0.1, 0, 0;
Matrix expectedJacobian = numericalDerivative11<Rot3, LieVector>(boost::bind(&evaluateRotation, _1), LieVector(thetahat)); Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(boost::bind(&evaluateRotation, _1), thetahat);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian)); EXPECT(assert_equal(expectedJacobian, actualJacobian));
} }

11
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -148,8 +148,9 @@ TEST( GaussianBayesNet, DeterminantTest )
} }
/* ************************************************************************* */ /* ************************************************************************* */
typedef Eigen::Matrix<double,10,1> Vector10;
namespace { namespace {
double computeError(const GaussianBayesNet& gbn, const Vector& values) double computeError(const GaussianBayesNet& gbn, const Vector10& values)
{ {
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian(); pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); 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))); 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0)));
// Compute the Hessian numerically // Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector>(boost::bind(&computeError, gbn, _1), Matrix hessian = numericalHessian<Vector10>(
Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); boost::bind(&computeError, gbn, _1), Vector10::Zero());
// Compute the gradient numerically // Compute the gradient numerically
Vector gradient = numericalGradient<Vector>(boost::bind(&computeError, gbn, _1), Vector gradient = numericalGradient<Vector10>(
Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())); boost::bind(&computeError, gbn, _1), Vector10::Zero());
// Compute the gradient using dense matrices // Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();

21
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -24,7 +24,6 @@
using namespace boost::assign; using namespace boost::assign;
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/GaussianJunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
@ -175,13 +174,13 @@ TEST(GaussianBayesTree, complicatedMarginal) {
EXPECT(assert_equal(expectedCov, actualCov, 1e1)); EXPECT(assert_equal(expectedCov, actualCov, 1e1));
} }
/* ************************************************************************* */
typedef Eigen::Matrix<double, 10, 1> Vector10;
namespace { namespace {
/* ************************************************************************* */ double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
double computeError(const GaussianBayesTree& gbt, const LieVector& values) pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();
{
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbt).jacobian();
return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); 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)))))); 2, (Vector(4) << 1.0,2.0,15.0,16.0))))));
// Compute the Hessian numerically // Compute the Hessian numerically
Matrix hessian = numericalHessian( Matrix hessian = numericalHessian<Vector10>(
boost::function<double(const LieVector&)>(boost::bind(&computeError, bt, _1)), boost::bind(&computeError, bt, _1), Vector10::Zero());
LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols())));
// Compute the gradient numerically // Compute the gradient numerically
Vector gradient = numericalGradient( Vector gradient = numericalGradient<Vector10>(
boost::function<double(const LieVector&)>(boost::bind(&computeError, bt, _1)), boost::bind(&computeError, bt, _1), Vector10::Zero());
LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols())));
// Compute the gradient using dense matrices // Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();

45
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -39,14 +38,14 @@ using symbol_shorthand::B;
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
Vector callEvaluateError(const ImuFactor& factor, 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) const imuBias::ConstantBias& bias)
{ {
return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
} }
Rot3 evaluateRotationError(const ImuFactor& factor, 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) const imuBias::ConstantBias& bias)
{ {
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; 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 // Linearization point
imuBias::ConstantBias bias; // Bias 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)); 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)); 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 // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -194,11 +193,11 @@ TEST( ImuFactor, Error )
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<Vector,LieVector>( Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Vector,Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<Vector,LieVector>( Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
@ -240,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point // Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // 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)); // 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)); // 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) 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)); 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)); 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 // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -278,11 +277,11 @@ TEST( ImuFactor, ErrorWithBiases )
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<Vector,LieVector>( Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Vector,Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<Vector,LieVector>( Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
@ -318,8 +317,8 @@ TEST( ImuFactor, PartialDerivativeExpmap )
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind( Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
@ -341,8 +340,8 @@ TEST( ImuFactor, PartialDerivativeLogmap )
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<Vector,LieVector>(boost::bind( Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind(
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); &evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
const Vector3 x = thetahat; // parametrization of so(3) const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
@ -447,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{ //{
// // Linearization point // // 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)); // 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)); // 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)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
// //
// // Pre-integrator // // 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) 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)); 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)); 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 // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -533,11 +532,11 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<Vector,LieVector>( Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Vector,Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<Vector,LieVector>( Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);

20
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -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); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x)); return factor.evaluateError(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { Vector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x)); return factor.evaluateError(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -52,8 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) {
Pose3RotationPrior factor(poseKey, rot3A, model3); Pose3RotationPrior factor(poseKey, rot3A, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -63,8 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3RotationPrior factor(poseKey, rot3C, model3); Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -74,8 +72,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) {
Pose2RotationPrior factor(poseKey, rot2A, model1); Pose2RotationPrior factor(poseKey, rot2A, model1);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -85,8 +82,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2RotationPrior factor(poseKey, rot2B, model1); Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }

32
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -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); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { Vector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x)); return factor.evaluateError(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { Vector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x)); return factor.evaluateError(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -49,8 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3); Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -60,8 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -71,8 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3); Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -82,8 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -93,8 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3); Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -104,8 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -115,8 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) {
Pose2TranslationPrior factor(poseKey, point2A, model2); Pose2TranslationPrior factor(poseKey, point2A, model2);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -126,8 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2TranslationPrior factor(poseKey, point2B, model2); Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }

21
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp> #include <boost/bind.hpp>
@ -37,12 +36,12 @@ typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D; typedef RangeFactor<Pose3, Point3> 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); 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); return factor.evaluateError(pose, point);
} }
@ -214,8 +213,8 @@ TEST( RangeFactor, Jacobian2D ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point); H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -243,8 +242,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point); H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -269,8 +268,8 @@ TEST( RangeFactor, Jacobian3D ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point); H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -298,8 +297,8 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point); H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));

16
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -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) { 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 ) { TEST( ReferenceFrameFactor, jacobians ) {
@ -68,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) {
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF; Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>( numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>( numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>( numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
@ -100,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) {
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF; Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>( numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>( numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>( numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);

6
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -83,9 +83,9 @@ public:
virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, virtual gtsam::Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<gtsam::Matrix&> H1=boost::none, boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none) const { boost::optional<gtsam::Matrix&> H2=boost::none) const {
if (H1) *H1 = gtsam::numericalDerivative21<gtsam::LieVector,PoseRTV,PoseRTV>( if (H1) *H1 = gtsam::numericalDerivative21<gtsam::Vector,PoseRTV,PoseRTV>(
boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
if (H2) *H2 = gtsam::numericalDerivative22<gtsam::LieVector,PoseRTV,PoseRTV>( if (H2) *H2 = gtsam::numericalDerivative22<gtsam::Vector,PoseRTV,PoseRTV>(
boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5); boost::bind(VelocityConstraint::evaluateError_, _1, _2, dt_, integration_mode_), x1, x2, 1e-5);
return evaluateError_(x1, x2, dt_, integration_mode_); return evaluateError_(x1, x2, dt_, integration_mode_);
} }
@ -103,7 +103,7 @@ public:
} }
private: 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) { double dt, const dynamics::IntegrationMode& mode) {
const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t(); const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t();

28
gtsam_unstable/dynamics/tests/testSimpleHelicopter.cpp
View File

@ -18,11 +18,11 @@ const double h = 0.01;
//const double deg2rad = M_PI/180.0; //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::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)); 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)); //Vector6 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)); Vector6 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_g1 = g1.inverse().Adjoint(V1_w);
Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP)); 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; double mass = 100.0;
Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape 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)); return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v));
} }
TEST(Reconstruction, ExpmapInvDeriv) { TEST(Reconstruction, ExpmapInvDeriv) {
Matrix numericalExpmap = numericalDerivative11( Matrix numericalExpmap = numericalDerivative11(
boost::function<Vector(const LieVector&)>( boost::function<Vector(const Vector6&)>(
boost::bind(testExpmapDeriv, _1) boost::bind(testExpmapDeriv, _1)
), ),
LieVector(Vector::Zero(6)), 1e-5 Vector6(Vector::Zero(6)), 1e-5
); );
Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1); Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1);
EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2)); EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2));
@ -72,21 +72,21 @@ TEST( Reconstruction, evaluateError) {
Matrix numericalH1 = numericalDerivative31( Matrix numericalH1 = numericalDerivative31(
boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>( boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
), ),
g2, g1, V1_g1, 1e-5 g2, g1, V1_g1, 1e-5
); );
Matrix numericalH2 = numericalDerivative32( Matrix numericalH2 = numericalDerivative32(
boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>( boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
), ),
g2, g1, V1_g1, 1e-5 g2, g1, V1_g1, 1e-5
); );
Matrix numericalH3 = numericalDerivative33( Matrix numericalH3 = numericalDerivative33(
boost::function<Vector(const Pose3&, const Pose3&, const LieVector&)>( boost::function<Vector(const Pose3&, const Pose3&, const Vector6&)>(
boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) boost::bind(&Reconstruction::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
), ),
g2, g1, V1_g1, 1e-5 g2, g1, V1_g1, 1e-5
@ -119,7 +119,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Pose3 g21 = g2.between(g1); Pose3 g21 = g2.between(g1);
Vector V2_g2 = g21.Adjoint(V2_g1); // convert the new velocity to g2's frame 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 // hard constraints don't need a noise model
DiscreteEulerPoincareHelicopter constraint(V(2), V(1), G(2), h, 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)); EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
Matrix numericalH1 = numericalDerivative31( Matrix numericalH1 = numericalDerivative31(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>( boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
), ),
expectedv2, V1_g1, g2, 1e-5 expectedv2, V1_g1, g2, 1e-5
); );
Matrix numericalH2 = numericalDerivative32( Matrix numericalH2 = numericalDerivative32(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>( boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
), ),
expectedv2, V1_g1, g2, 1e-5 expectedv2, V1_g1, g2, 1e-5
); );
Matrix numericalH3 = numericalDerivative33( Matrix numericalH3 = numericalDerivative33(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>( boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none) boost::bind(&DiscreteEulerPoincareHelicopter::evaluateError, constraint, _1, _2, _3, boost::none, boost::none, boost::none)
), ),
expectedv2, V1_g1, g2, 1e-5 expectedv2, V1_g1, g2, 1e-5

24
gtsam_unstable/geometry/tests/testInvDepthCamera3.cpp
View File

@ -29,7 +29,7 @@ TEST( InvDepthFactor, Project1) {
Point2 expected_uv = level_camera.project(landmark); Point2 expected_uv = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> 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); LieScalar inv_depth(1./4);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth); Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv)); EXPECT(assert_equal(expected_uv, actual_uv));
@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> 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)); LieScalar inv_depth(1/sqrt(3.0));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> 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)); LieScalar inv_depth( 1./sqrt(1.0+1+4));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); EXPECT(assert_equal(expected, actual));
@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) {
Point2 expected = level_camera.project(landmark); Point2 expected = level_camera.project(landmark);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> 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.)); LieScalar inv_depth(1./sqrt(1.+16.+4.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth); Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual)); 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<Cal3_S2>(pose,K).project(landmark, inv_depth); } return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
TEST( InvDepthFactor, Dproject_pose) 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); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InvDepthFactor, Dproject_landmark) 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); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InvDepthFactor, Dproject_inv_depth) 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); LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth); Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -124,12 +124,12 @@ TEST( InvDepthFactor, Dproject_inv_depth)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(InvDepthFactor, backproject) 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); LieScalar inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth); Point2 z = inv_camera.project(expected, inv_depth);
LieVector actual_vec; Vector5 actual_vec;
LieScalar actual_inv; LieScalar actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4); boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
EXPECT(assert_equal(expected,actual_vec,1e-7)); EXPECT(assert_equal(expected,actual_vec,1e-7));
@ -140,12 +140,12 @@ TEST(InvDepthFactor, backproject)
TEST(InvDepthFactor, backproject2) TEST(InvDepthFactor, backproject2)
{ {
// backwards facing camera // 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); LieScalar inv_depth(1./10);
InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K); InvDepthCamera3<Cal3_S2> inv_camera(Pose3(Rot3::ypr(1.5,0.1, -1.5), Point3(-5, -5, 2)),K);
Point2 z = inv_camera.project(expected, inv_depth); Point2 z = inv_camera.project(expected, inv_depth);
LieVector actual_vec; Vector5 actual_vec;
LieScalar actual_inv; LieScalar actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10); boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
EXPECT(assert_equal(expected,actual_vec,1e-7)); EXPECT(assert_equal(expected,actual_vec,1e-7));

10
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -315,8 +315,9 @@ public:
// Jacobian w.r.t. Vel1 // Jacobian w.r.t. Vel1
if (H2){ if (H2){
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, 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_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); Matrix H2_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel); *H2 = stack(2, &H2_Pose, &H2_Vel);
} }
@ -336,8 +337,9 @@ public:
// Jacobian w.r.t. Vel2 // Jacobian w.r.t. Vel2
if (H5){ if (H5){
Matrix H5_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, 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_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); Matrix H5_Pose = numericalDerivative11<POSE, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
*H5 = stack(2, &H5_Pose, &H5_Vel); *H5 = stack(2, &H5_Pose, &H5_Vel);
} }

10
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -242,8 +242,9 @@ public:
// Jacobian w.r.t. Vel1 // Jacobian w.r.t. Vel1
if (H2){ if (H2){
Matrix H2_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, 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_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1); Matrix H2_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
*H2 = stack(2, &H2_Pose, &H2_Vel); *H2 = stack(2, &H2_Pose, &H2_Vel);
} }
@ -263,8 +264,9 @@ public:
// Jacobian w.r.t. Vel2 // Jacobian w.r.t. Vel2
if (H5){ if (H5){
Matrix H5_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, 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_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2); Matrix H5_Pose = gtsam::numericalDerivative11<POSE, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = gtsam::numericalDerivative11<Vector3, Vector3>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
*H5 = stack(2, &H5_Pose, &H5_Vel); *H5 = stack(2, &H5_Pose, &H5_Vel);
} }

2
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -110,7 +110,7 @@ public:
landmark), pose); landmark), pose);
} }
if (H2) { if (H2) {
(*H2) = numericalDerivative11<Vector, LieVector>( (*H2) = numericalDerivative11<Vector, Vector6>(
boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose, boost::bind(&InvDepthFactorVariant1::inverseDepthError, this, pose,
_1), landmark); _1), landmark);
} }

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -113,7 +113,7 @@ public:
landmark), pose); landmark), pose);
} }
if (H2) { if (H2) {
(*H2) = numericalDerivative11<Vector, LieVector>( (*H2) = numericalDerivative11<Vector, Vector3>(
boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose, boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
_1), landmark); _1), landmark);
} }

4
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -111,7 +111,7 @@ public:
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose); (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, _1, landmark), pose);
} }
if(H2) { if(H2) {
(*H2) = numericalDerivative11<Vector,LieVector>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark); (*H2) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3a::inverseDepthError, this, pose, _1), landmark);
} }
return inverseDepthError(pose, landmark); return inverseDepthError(pose, landmark);
@ -234,7 +234,7 @@ public:
(*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2); (*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
} }
if(H3) { if(H3) {
(*H3) = numericalDerivative11<Vector,LieVector>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark); (*H3) = numericalDerivative11<Vector,Vector3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, pose2, _1), landmark);
} }
return inverseDepthError(pose1, pose2, landmark); return inverseDepthError(pose1, pose2, landmark);

308
gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -29,31 +29,37 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
gtsam::Rot3 world_R_ECEF( Rot3 world_R_ECEF(
0.31686, 0.51505, 0.79645, 0.31686, 0.51505, 0.79645,
0.85173, -0.52399, 0, 0.85173, -0.52399, 0,
0.41733, 0.67835, -0.60471); 0.41733, 0.67835, -0.60471);
gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); 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 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<Pose3, LieVector, imuBias::ConstantBias>& factor) { Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1,
const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2,
const InertialNavFactor_GlobalVelocity<Pose3, LieVector,
imuBias::ConstantBias>& factor) {
return Pose3::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).head(6)); 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<Pose3, LieVector, imuBias::ConstantBias>& factor) { Vector predictionErrorVel(const Pose3& p1, const LieVector& v1,
return LieVector::Expmap(factor.evaluateError(p1, v1, b1, p2, v2).tail(3)); const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2,
const InertialNavFactor_GlobalVelocity<Pose3, LieVector,
imuBias::ConstantBias>& factor) {
return factor.evaluateError(p1, v1, b1, p2, v2).tail(3);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, Constructor) TEST( InertialNavFactor_GlobalVelocity, Constructor)
{ {
gtsam::Key Pose1(11); Key Pose1(11);
gtsam::Key Pose2(12); Key Pose2(12);
gtsam::Key Vel1(21); Key Vel1(21);
gtsam::Key Vel2(22); Key Vel2(22);
gtsam::Key Bias1(31); Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); Vector measurement_acc((Vector(3) << 0.1,0.2,0.4));
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
@ -61,8 +67,8 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor)
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -72,11 +78,11 @@ TEST( InertialNavFactor_GlobalVelocity, Constructor)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, Equals) TEST( InertialNavFactor_GlobalVelocity, Equals)
{ {
gtsam::Key Pose1(11); Key Pose1(11);
gtsam::Key Pose2(12); Key Pose2(12);
gtsam::Key Vel1(21); Key Vel1(21);
gtsam::Key Vel2(22); Key Vel2(22);
gtsam::Key Bias1(31); Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); Vector measurement_acc((Vector(3) << 0.1,0.2,0.4));
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
@ -84,8 +90,8 @@ TEST( InertialNavFactor_GlobalVelocity, Equals)
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -97,17 +103,17 @@ TEST( InertialNavFactor_GlobalVelocity, Equals)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, Predict) TEST( InertialNavFactor_GlobalVelocity, Predict)
{ {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -134,17 +140,17 @@ TEST( InertialNavFactor_GlobalVelocity, Predict)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
{ {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -170,17 +176,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorRot) TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
{ {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -205,17 +211,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
{ {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -262,33 +268,33 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
// -q[1], q[0],0.0); // -q[1], q[0],0.0);
// Matrix J_hyp( -(R1*qx).matrix() ); // Matrix J_hyp( -(R1*qx).matrix() );
// //
// gtsam::Matrix J_expected; // Matrix J_expected;
// //
// LieVector v(predictionRq(angles, q)); // LieVector v(predictionRq(angles, q));
// //
// J_expected = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles); // J_expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles);
// //
// cout<<"J_hyp"<<J_hyp<<endl; // cout<<"J_hyp"<<J_hyp<<endl;
// cout<<"J_expected"<<J_expected<<endl; // cout<<"J_expected"<<J_expected<<endl;
// //
// CHECK( gtsam::assert_equal(J_expected, J_hyp, 1e-6)); // CHECK( assert_equal(J_expected, J_hyp, 1e-6));
//} //}
///* VADIM - END ************************************************************************* */ ///* VADIM - END ************************************************************************* */
/* ************************************************************************* */ /* ************************************************************************* */
TEST (InertialNavFactor_GlobalVelocity, Jacobian ) { TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.01); double measurement_dt(0.01);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -324,19 +330,19 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols())); Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H1_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); H2_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); H4_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); H5_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
// Verify they are equal for this choice of state // Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
// Checking for Vel part in the jacobians // 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())); Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols()));
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H1_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); H2_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); H4_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); H5_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
// Verify they are equal for this choice of state // Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
} }
@ -368,11 +374,11 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
{ {
gtsam::Key Pose1(11); Key Pose1(11);
gtsam::Key Pose2(12); Key Pose2(12);
gtsam::Key Vel1(21); Key Vel1(21);
gtsam::Key Vel2(22); Key Vel2(22);
gtsam::Key Bias1(31); Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4));
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
@ -380,8 +386,8 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -394,11 +400,11 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
{ {
gtsam::Key Pose1(11); Key Pose1(11);
gtsam::Key Pose2(12); Key Pose2(12);
gtsam::Key Vel1(21); Key Vel1(21);
gtsam::Key Vel2(22); Key Vel2(22);
gtsam::Key Bias1(31); Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4)); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.4));
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
@ -406,8 +412,8 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -422,17 +428,17 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
{ {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -462,17 +468,17 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
{ {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -501,17 +507,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
{ {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -540,17 +546,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
/* ************************************************************************* */ /* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
{ {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.1); double measurement_dt(0.1);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
@ -589,17 +595,17 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
/* ************************************************************************* */ /* ************************************************************************* */
TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) { TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
gtsam::Key PoseKey1(11); Key PoseKey1(11);
gtsam::Key PoseKey2(12); Key PoseKey2(12);
gtsam::Key VelKey1(21); Key VelKey1(21);
gtsam::Key VelKey2(22); Key VelKey2(22);
gtsam::Key BiasKey1(31); Key BiasKey1(31);
double measurement_dt(0.01); double measurement_dt(0.01);
Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); 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 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)); 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 world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth);
SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); 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())); Matrix H5_actualPose(H5_actual.block(0,0,6,H5_actual.cols()));
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
gtsam::Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose;
H1_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H1_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); H2_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedPose = gtsam::numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedPose = gtsam::numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); H4_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedPose = gtsam::numericalDerivative11<Pose3, LieVector>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); H5_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
// Verify they are equal for this choice of state // Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedPose, H2_actualPose, 1e-5)); CHECK( assert_equal(H2_expectedPose, H2_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedPose, H3_actualPose, 2e-3)); CHECK( assert_equal(H3_expectedPose, H3_actualPose, 2e-3));
CHECK( gtsam::assert_equal(H4_expectedPose, H4_actualPose, 1e-5)); CHECK( assert_equal(H4_expectedPose, H4_actualPose, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedPose, H5_actualPose, 1e-5)); CHECK( assert_equal(H5_expectedPose, H5_actualPose, 1e-5));
// Checking for Vel part in the jacobians // 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())); Matrix H5_actualVel(H5_actual.block(6,0,3,H5_actual.cols()));
// Calculate the Jacobian matrices H1 until H5 using the numerical derivative function // Calculate the Jacobian matrices H1 until H5 using the numerical derivative function
gtsam::Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel;
H1_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H1_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1);
H2_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); H2_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1);
H3_expectedVel = gtsam::numericalDerivative11<LieVector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1);
H4_expectedVel = gtsam::numericalDerivative11<LieVector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); H4_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2);
H5_expectedVel = gtsam::numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); H5_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2);
// Verify they are equal for this choice of state // Verify they are equal for this choice of state
CHECK( gtsam::assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H2_expectedVel, H2_actualVel, 1e-5)); CHECK( assert_equal(H2_expectedVel, H2_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H3_expectedVel, H3_actualVel, 1e-5)); CHECK( assert_equal(H3_expectedVel, H3_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H4_expectedVel, H4_actualVel, 1e-5)); CHECK( assert_equal(H4_expectedVel, H4_actualVel, 1e-5));
CHECK( gtsam::assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */

36
gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp
View File

@ -24,8 +24,8 @@ using namespace gtsam;
TEST( InvDepthFactorVariant1, optimize) { TEST( InvDepthFactorVariant1, optimize) {
// Create two poses looking in the x-direction // 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 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), gtsam::Point3(0,0,1.5)); 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)) // Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
Point3 landmark(5, 0, 1); Point3 landmark(5, 0, 1);
@ -76,18 +76,18 @@ TEST( InvDepthFactorVariant1, optimize) {
LieVector actual = result.at<LieVector>(landmarkKey); LieVector actual = result.at<LieVector>(landmarkKey);
values.at<Pose3>(poseKey1).print("Pose1 Before:\n"); // values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
result.at<Pose3>(poseKey1).print("Pose1 After:\n"); // result.at<Pose3>(poseKey1).print("Pose1 After:\n");
pose1.print("Pose1 Truth:\n"); // pose1.print("Pose1 Truth:\n");
std::cout << std::endl << std::endl; // cout << endl << endl;
values.at<Pose3>(poseKey2).print("Pose2 Before:\n"); // values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
result.at<Pose3>(poseKey2).print("Pose2 After:\n"); // result.at<Pose3>(poseKey2).print("Pose2 After:\n");
pose2.print("Pose2 Truth:\n"); // pose2.print("Pose2 Truth:\n");
std::cout << std::endl << std::endl; // cout << endl << endl;
values.at<LieVector>(landmarkKey).print("Landmark Before:\n"); // values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
result.at<LieVector>(landmarkKey).print("Landmark After:\n"); // result.at<LieVector>(landmarkKey).print("Landmark After:\n");
expected.print("Landmark Truth:\n"); // expected.print("Landmark Truth:\n");
std::cout << std::endl << std::endl; // cout << endl << endl;
// Calculate world coordinates of landmark versions // Calculate world coordinates of landmark versions
Point3 world_landmarkBefore; 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_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_landmarkBefore.print("World Landmark Before:\n");
world_landmarkAfter.print("World Landmark After:\n"); // world_landmarkAfter.print("World Landmark After:\n");
landmark.print("World Landmark Truth:\n"); // landmark.print("World Landmark Truth:\n");
std::cout << std::endl << std::endl; // cout << endl << endl;

35
gtsam_unstable/slam/tests/testInvDepthFactorVariant2.cpp
View File

@ -73,19 +73,18 @@ TEST( InvDepthFactorVariant2, optimize) {
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
LieVector actual = result.at<LieVector>(landmarkKey); LieVector actual = result.at<LieVector>(landmarkKey);
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
values.at<Pose3>(poseKey1).print("Pose1 Before:\n"); // result.at<Pose3>(poseKey1).print("Pose1 After:\n");
result.at<Pose3>(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n");
pose1.print("Pose1 Truth:\n"); // std::cout << std::endl << std::endl;
std::cout << std::endl << std::endl; // values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
values.at<Pose3>(poseKey2).print("Pose2 Before:\n"); // result.at<Pose3>(poseKey2).print("Pose2 After:\n");
result.at<Pose3>(poseKey2).print("Pose2 After:\n"); // pose2.print("Pose2 Truth:\n");
pose2.print("Pose2 Truth:\n"); // std::cout << std::endl << std::endl;
std::cout << std::endl << std::endl; // values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
values.at<LieVector>(landmarkKey).print("Landmark Before:\n"); // result.at<LieVector>(landmarkKey).print("Landmark After:\n");
result.at<LieVector>(landmarkKey).print("Landmark After:\n"); // expected.print("Landmark Truth:\n");
expected.print("Landmark Truth:\n"); // std::cout << std::endl << std::endl;
std::cout << std::endl << std::endl;
// Calculate world coordinates of landmark versions // Calculate world coordinates of landmark versions
Point3 world_landmarkBefore; 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_landmarkAfter = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
} }
world_landmarkBefore.print("World Landmark Before:\n"); // world_landmarkBefore.print("World Landmark Before:\n");
world_landmarkAfter.print("World Landmark After:\n"); // world_landmarkAfter.print("World Landmark After:\n");
landmark.print("World Landmark Truth:\n"); // landmark.print("World Landmark Truth:\n");
std::cout << std::endl << std::endl; // std::cout << std::endl << std::endl;
// Test that the correct landmark parameters have been recovered // Test that the correct landmark parameters have been recovered
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));

2
gtsam_unstable/slam/tests/testInvDepthFactorVariant3.cpp
View File

@ -39,7 +39,7 @@ TEST( InvDepthFactorVariant3, optimize) {
// Create expected landmark // Create expected landmark
Point3 landmark_p1 = pose1.transform_to(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 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 phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
double rho = 1./landmark_p1.norm(); double rho = 1./landmark_p1.norm();

8
gtsam_unstable/slam/tests/testProjectionFactorPPPC.cpp
View File

@ -136,11 +136,11 @@ TEST( ProjectionFactor, Jacobian ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 and H4 with numerical derivatives // Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>( Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
*K1, boost::none, boost::none, boost::none, boost::none), Pose3()); *K1, boost::none, boost::none, boost::none, boost::none), Pose3());
Matrix H4Expected = numericalDerivative11<LieVector, Cal3_S2>( Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point, boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point,
_1, boost::none, boost::none, boost::none, boost::none), *K1); _1, boost::none, boost::none, boost::none, boost::none), *K1);
@ -172,11 +172,11 @@ TEST( ProjectionFactor, JacobianWithTransform ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 and H4 with numerical derivatives // Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>( Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point, boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
*K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor); *K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);
Matrix H4Expected = numericalDerivative11<LieVector, Cal3_S2>( Matrix H4Expected = numericalDerivative11<Vector, Cal3_S2>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point, boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
_1, boost::none, boost::none, boost::none, boost::none), *K1); _1, boost::none, boost::none, boost::none, boost::none), *K1);

32
gtsam_unstable/slam/tests/testRelativeElevationFactor.cpp
View File

@ -24,8 +24,8 @@ const Point3 point1(3.0, 4.0, 2.0);
const gtsam::Key poseKey = 1, pointKey = 2; const gtsam::Key poseKey = 1, pointKey = 2;
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) { Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
return LieVector(factor.evaluateError(x, p)); return factor.evaluateError(x, p);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -35,9 +35,9 @@ TEST( testRelativeElevationFactor, level_zero_error ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2))); EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol)); EXPECT(assert_equal(expH2, actH2, tol));
@ -50,9 +50,9 @@ TEST( testRelativeElevationFactor, level_positive ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2))); EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol)); EXPECT(assert_equal(expH2, actH2, tol));
@ -65,9 +65,9 @@ TEST( testRelativeElevationFactor, level_negative ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2))); EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose1, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol)); EXPECT(assert_equal(expH2, actH2, tol));
@ -80,9 +80,9 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2))); EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol)); EXPECT(assert_equal(expH2, actH2, tol));
@ -95,9 +95,9 @@ TEST( testRelativeElevationFactor, rotated_positive ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2))); EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol)); EXPECT(assert_equal(expH2, actH2, tol));
@ -110,9 +110,9 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2))); EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose2, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol)); EXPECT(assert_equal(expH2, actH2, tol));
@ -125,9 +125,9 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1); RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2; Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2))); EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>( Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
Matrix expH2 = numericalDerivative22<LieVector,Pose3,Point3>( Matrix expH2 = numericalDerivative22<Vector,Pose3,Point3>(
boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5); boost::bind(evalFactorError, factor, _1, _2), pose3, point1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol)); EXPECT(assert_equal(expH2, actH2, tol));

20
gtsam_unstable/slam/tests/testTSAMFactors.cpp
View File

@ -44,10 +44,10 @@ TEST( DeltaFactor, all ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>( H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none, boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none,
boost::none), pose); boost::none), pose);
H2Expected = numericalDerivative11<LieVector, Point2>( H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none, boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none,
boost::none), point); boost::none), point);
@ -78,16 +78,16 @@ TEST( DeltaFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected; Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>( H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2, boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2,
point, boost::none, boost::none, boost::none, boost::none), base1); point, boost::none, boost::none, boost::none, boost::none), base1);
H2Expected = numericalDerivative11<LieVector, Pose2>( H2Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2, boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, _1, base2,
point, boost::none, boost::none, boost::none, boost::none), pose); point, boost::none, boost::none, boost::none, boost::none), pose);
H3Expected = numericalDerivative11<LieVector, Pose2>( H3Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1, boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, _1,
point, boost::none, boost::none, boost::none, boost::none), base2); point, boost::none, boost::none, boost::none, boost::none), base2);
H4Expected = numericalDerivative11<LieVector, Point2>( H4Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2, boost::bind(&DeltaFactorBase::evaluateError, &factor, base1, pose, base2,
_1, boost::none, boost::none, boost::none, boost::none), point); _1, boost::none, boost::none, boost::none, boost::none), point);
@ -119,16 +119,16 @@ TEST( OdometryFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected; Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>( H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2, boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2,
pose2, boost::none, boost::none, boost::none, boost::none), base1); pose2, boost::none, boost::none, boost::none, boost::none), base1);
H2Expected = numericalDerivative11<LieVector, Pose2>( H2Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2, boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, _1, base2,
pose2, boost::none, boost::none, boost::none, boost::none), pose1); pose2, boost::none, boost::none, boost::none, boost::none), pose1);
H3Expected = numericalDerivative11<LieVector, Pose2>( H3Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1, boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, _1,
pose2, boost::none, boost::none, boost::none, boost::none), base2); pose2, boost::none, boost::none, boost::none, boost::none), base2);
H4Expected = numericalDerivative11<LieVector, Pose2>( H4Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1, boost::bind(&OdometryFactorBase::evaluateError, &factor, base1, pose1,
base2, _1, boost::none, boost::none, boost::none, boost::none), base2, _1, boost::none, boost::none, boost::none, boost::none),
pose2); pose2);