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>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2193,6 +2241,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<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);
/* ************************************************************************* */
LieVector norm_proxy(const Point2& point) {
return LieVector(point.norm());
double norm_proxy(const Point2& point) {
return point.norm();
}
}
TEST( Point2, norm ) {
@ -112,8 +112,8 @@ TEST( Point2, norm ) {
/* ************************************************************************* */
namespace {
LieVector distance_proxy(const Point2& location, const Point2& point) {
return LieVector(location.distance(point));
double distance_proxy(const Point2& location, const Point2& point) {
return location.distance(point);
}
}
TEST( Point2, distance ) {

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

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

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

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

@ -217,7 +217,7 @@ TEST( Rot3, rightJacobianExpMapSO3 )
// Linearization point
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);
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 {
double computeError(const GaussianBayesNet& gbn, const Vector& values)
double computeError(const GaussianBayesNet& gbn, const Vector10& values)
{
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
@ -179,12 +180,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0)));
// Compute the Hessian numerically
Matrix hessian = numericalHessian<Vector>(boost::bind(&computeError, gbn, _1),
Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()));
Matrix hessian = numericalHessian<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
// Compute the gradient numerically
Vector gradient = numericalGradient<Vector>(boost::bind(&computeError, gbn, _1),
Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols()));
Vector gradient = numericalGradient<Vector10>(
boost::bind(&computeError, gbn, _1), Vector10::Zero());
// Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();

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

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

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

@ -20,7 +20,6 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -39,14 +38,14 @@ using symbol_shorthand::B;
/* ************************************************************************* */
namespace {
Vector callEvaluateError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias)
{
return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
}
Rot3 evaluateRotationError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j,
const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias)
{
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
@ -168,9 +167,9 @@ TEST( ImuFactor, Error )
// Linearization point
imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -194,11 +193,11 @@ TEST( ImuFactor, Error )
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H3e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
@ -240,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -278,11 +277,11 @@ TEST( ImuFactor, ErrorWithBiases )
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H3e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
@ -318,8 +317,8 @@ TEST( ImuFactor, PartialDerivativeExpmap )
// Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega));
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
@ -341,8 +340,8 @@ TEST( ImuFactor, PartialDerivativeLogmap )
// Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<Vector,LieVector>(boost::bind(
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind(
&evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
@ -447,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{
// // Linearization point
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
// Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
//
// // Pre-integrator
@ -503,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0));
Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements
Vector3 gravity; gravity << 0, 0, 9.81;
@ -533,11 +532,11 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H3e = numericalDerivative11<Vector,Pose3>(
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);
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
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);
/* ************************************************************************* */
LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x));
Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
return factor.evaluateError(x);
}
/* ************************************************************************* */
LieVector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x));
Vector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) {
return factor.evaluateError(x);
}
/* ************************************************************************* */
@ -52,8 +52,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) {
Pose3RotationPrior factor(poseKey, rot3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -63,8 +62,7 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -74,8 +72,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) {
Pose2RotationPrior factor(poseKey, rot2A, model1);
Matrix actH1;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -85,8 +82,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
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);
/* ************************************************************************* */
LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x));
Vector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) {
return factor.evaluateError(x);
}
/* ************************************************************************* */
LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x));
Vector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) {
return factor.evaluateError(x);
}
/* ************************************************************************* */
@ -49,8 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -60,8 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -71,8 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -82,8 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -93,8 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -104,8 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -115,8 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) {
Pose2TranslationPrior factor(poseKey, point2A, model2);
Matrix actH1;
EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -126,8 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1;
EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
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/Point2.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp>
@ -37,12 +36,12 @@ typedef RangeFactor<Pose2, Point2> RangeFactor2D;
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);
}
/* ************************************************************************* */
LieVector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
return factor.evaluateError(pose, point);
}
@ -214,8 +213,8 @@ TEST( RangeFactor, Jacobian2D ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -243,8 +242,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -269,8 +268,8 @@ TEST( RangeFactor, Jacobian3D ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -298,8 +297,8 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct
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) {
return LieVector(c.evaluateError(global, trans, local));
return Vector(c.evaluateError(global, trans, local));
}
TEST( ReferenceFrameFactor, jacobians ) {
@ -68,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) {
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
@ -100,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) {
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>(
numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>(
numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>(
numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3),
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,
boost::optional<gtsam::Matrix&> H1=boost::none,
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);
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);
return evaluateError_(x1, x2, dt_, integration_mode_);
}
@ -103,7 +103,7 @@ public:
}
private:
static gtsam::LieVector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
double dt, const dynamics::IntegrationMode& mode) {
const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t();

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;
//Pose3 g1(Rot3::ypr(deg2rad*10.0, deg2rad*20.0, deg2rad*30.0), Point3(100.0, 200.0, 300.0));
Pose3 g1(Rot3(), Point3(100.0, 0.0, 300.0));
//LieVector v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
LieVector V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0));
LieVector V1_g1 = g1.inverse().Adjoint(V1_w);
//Vector6 v1((Vector(6) << 0.1, 0.05, 0.02, 10.0, 20.0, 30.0));
Vector6 V1_w((Vector(6) << 0.0, 0.0, M_PI/3, 0.0, 0.0, 30.0));
Vector6 V1_g1 = g1.inverse().Adjoint(V1_w);
Pose3 g2(g1.retract(h*V1_g1, Pose3::EXPMAP));
//LieVector v2 = Pose3::Logmap(g1.between(g2));
//Vector6 v2 = Pose3::Logmap(g1.between(g2));
double mass = 100.0;
Vector gamma2 = (Vector(2) << 0.0, 0.0); // no shape
@ -46,16 +46,16 @@ Vector computeFu(const Vector& gamma, const Vector& control) {
}
/* ************************************************************************* */
Vector testExpmapDeriv(const LieVector& v) {
Vector testExpmapDeriv(const Vector6& v) {
return Pose3::Logmap(Pose3::Expmap(-h*V1_g1)*Pose3::Expmap(h*V1_g1+v));
}
TEST(Reconstruction, ExpmapInvDeriv) {
Matrix numericalExpmap = numericalDerivative11(
boost::function<Vector(const LieVector&)>(
boost::function<Vector(const Vector6&)>(
boost::bind(testExpmapDeriv, _1)
),
LieVector(Vector::Zero(6)), 1e-5
Vector6(Vector::Zero(6)), 1e-5
);
Matrix dExpInv = Pose3::dExpInv_exp(h*V1_g1);
EXPECT(assert_equal(numericalExpmap, dExpInv, 1e-2));
@ -72,21 +72,21 @@ TEST( Reconstruction, evaluateError) {
Matrix numericalH1 = numericalDerivative31(
boost::function<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)
),
g2, g1, V1_g1, 1e-5
);
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)
),
g2, g1, V1_g1, 1e-5
);
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)
),
g2, g1, V1_g1, 1e-5
@ -119,7 +119,7 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
Pose3 g21 = g2.between(g1);
Vector V2_g2 = g21.Adjoint(V2_g1); // convert the new velocity to g2's frame
LieVector expectedv2(V2_g2);
Vector6 expectedv2(V2_g2);
// hard constraints don't need a noise model
DiscreteEulerPoincareHelicopter constraint(V(2), V(1), G(2), h,
@ -130,21 +130,21 @@ TEST( DiscreteEulerPoincareHelicopter, evaluateError) {
EXPECT(assert_equal(zero(6), constraint.evaluateError(expectedv2, V1_g1, g2, H1, H2, H3), 1e0));
Matrix numericalH1 = numericalDerivative31(
boost::function<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)
),
expectedv2, V1_g1, g2, 1e-5
);
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)
),
expectedv2, V1_g1, g2, 1e-5
);
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)
),
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);
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);
Point2 actual_uv = inv_camera.project(inv_landmark, inv_depth);
EXPECT(assert_equal(expected_uv, actual_uv));
@ -45,7 +45,7 @@ TEST( InvDepthFactor, Project2) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<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));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -60,7 +60,7 @@ TEST( InvDepthFactor, Project3) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<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));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -75,7 +75,7 @@ TEST( InvDepthFactor, Project4) {
Point2 expected = level_camera.project(landmark);
InvDepthCamera3<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.));
Point2 actual = inv_camera.project(diag_landmark, inv_depth);
EXPECT(assert_equal(expected, actual));
@ -83,12 +83,12 @@ TEST( InvDepthFactor, Project4) {
/* ************************************************************************* */
Point2 project_(const Pose3& pose, const LieVector& landmark, const LieScalar& inv_depth) {
Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) {
return InvDepthCamera3<Cal3_S2>(pose,K).project(landmark, inv_depth); }
TEST( InvDepthFactor, Dproject_pose)
{
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative31(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -100,7 +100,7 @@ TEST( InvDepthFactor, Dproject_pose)
/* ************************************************************************* */
TEST( InvDepthFactor, Dproject_landmark)
{
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative32(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -112,7 +112,7 @@ TEST( InvDepthFactor, Dproject_landmark)
/* ************************************************************************* */
TEST( InvDepthFactor, Dproject_inv_depth)
{
LieVector landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
Vector5 landmark((Vector(5) << 0.1,0.2,0.3, 0.1,0.2));
LieScalar inv_depth(1./4);
Matrix expected = numericalDerivative33(project_,level_pose, landmark, inv_depth);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
@ -124,12 +124,12 @@ TEST( InvDepthFactor, Dproject_inv_depth)
/* ************************************************************************* */
TEST(InvDepthFactor, backproject)
{
LieVector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
Vector expected((Vector(5) << 0.,0.,1., 0.1,0.2));
LieScalar inv_depth(1./4);
InvDepthCamera3<Cal3_S2> inv_camera(level_pose,K);
Point2 z = inv_camera.project(expected, inv_depth);
LieVector actual_vec;
Vector5 actual_vec;
LieScalar actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 4);
EXPECT(assert_equal(expected,actual_vec,1e-7));
@ -140,12 +140,12 @@ TEST(InvDepthFactor, backproject)
TEST(InvDepthFactor, backproject2)
{
// backwards facing camera
LieVector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
LieScalar inv_depth(1./10);
InvDepthCamera3<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);
LieVector actual_vec;
Vector5 actual_vec;
LieScalar actual_inv;
boost::tie(actual_vec, actual_inv) = inv_camera.backproject(z, 10);
EXPECT(assert_equal(expected,actual_vec,1e-7));

10
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -315,8 +315,9 @@ public:
// Jacobian w.r.t. Vel1
if (H2){
Matrix H2_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H2_Pose = numericalDerivative11<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);
}
@ -336,8 +337,9 @@ public:
// Jacobian w.r.t. Vel2
if (H5){
Matrix H5_Pose = numericalDerivative11<POSE, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&EquivInertialNavFactor_GlobalVel::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H5_Pose = numericalDerivative11<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);
}

10
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -242,8 +242,9 @@ public:
// Jacobian w.r.t. Vel1
if (H2){
Matrix H2_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
Matrix H2_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, _1, Bias1, Pose2, Vel2), Vel1);
if (Vel1.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H2_Pose = gtsam::numericalDerivative11<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);
}
@ -263,8 +264,9 @@ public:
// Jacobian w.r.t. Vel2
if (H5){
Matrix H5_Pose = gtsam::numericalDerivative11<POSE, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluatePoseError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
Matrix H5_Vel = gtsam::numericalDerivative11<VELOCITY, VELOCITY>(boost::bind(&InertialNavFactor_GlobalVelocity::evaluateVelocityError, this, Pose1, Vel1, Bias1, Pose2, _1), Vel2);
if (Vel2.size()!=3) throw std::runtime_error("Frank's hack to make this compile will not work if size != 3");
Matrix H5_Pose = gtsam::numericalDerivative11<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);
}

2
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

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

2
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -113,7 +113,7 @@ public:
landmark), pose);
}
if (H2) {
(*H2) = numericalDerivative11<Vector, LieVector>(
(*H2) = numericalDerivative11<Vector, Vector3>(
boost::bind(&InvDepthFactorVariant2::inverseDepthError, this, pose,
_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);
}
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);
@ -234,7 +234,7 @@ public:
(*H2) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, pose1, _1, landmark), pose2);
}
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);

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

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

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

@ -24,8 +24,8 @@ using namespace gtsam;
TEST( InvDepthFactorVariant1, optimize) {
// Create two poses looking in the x-direction
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.0));
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1.5));
Pose3 pose1(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.0));
Pose3 pose2(Rot3::ypr(-M_PI/2, 0., -M_PI/2), Point3(0,0,1.5));
// Create a landmark 5 meters in front of pose1 (camera center at (0,0,1))
Point3 landmark(5, 0, 1);
@ -76,18 +76,18 @@ TEST( InvDepthFactorVariant1, optimize) {
LieVector actual = result.at<LieVector>(landmarkKey);
values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
result.at<Pose3>(poseKey1).print("Pose1 After:\n");
pose1.print("Pose1 Truth:\n");
std::cout << std::endl << std::endl;
values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
result.at<Pose3>(poseKey2).print("Pose2 After:\n");
pose2.print("Pose2 Truth:\n");
std::cout << std::endl << std::endl;
values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
result.at<LieVector>(landmarkKey).print("Landmark After:\n");
expected.print("Landmark Truth:\n");
std::cout << std::endl << std::endl;
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
// result.at<Pose3>(poseKey1).print("Pose1 After:\n");
// pose1.print("Pose1 Truth:\n");
// cout << endl << endl;
// values.at<Pose3>(poseKey2).print("Pose2 Before:\n");
// result.at<Pose3>(poseKey2).print("Pose2 After:\n");
// pose2.print("Pose2 Truth:\n");
// cout << endl << endl;
// values.at<LieVector>(landmarkKey).print("Landmark Before:\n");
// result.at<LieVector>(landmarkKey).print("Landmark After:\n");
// expected.print("Landmark Truth:\n");
// cout << endl << endl;
// Calculate world coordinates of landmark versions
Point3 world_landmarkBefore;
@ -105,10 +105,10 @@ TEST( InvDepthFactorVariant1, optimize) {
world_landmarkAfter = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
}
world_landmarkBefore.print("World Landmark Before:\n");
world_landmarkAfter.print("World Landmark After:\n");
landmark.print("World Landmark Truth:\n");
std::cout << std::endl << std::endl;
// world_landmarkBefore.print("World Landmark Before:\n");
// world_landmarkAfter.print("World Landmark After:\n");
// landmark.print("World Landmark Truth:\n");
// cout << endl << endl;

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

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

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

@ -39,7 +39,7 @@ TEST( InvDepthFactorVariant3, optimize) {
// Create expected landmark
Point3 landmark_p1 = pose1.transform_to(landmark);
landmark_p1.print("Landmark in Pose1 Frame:\n");
// landmark_p1.print("Landmark in Pose1 Frame:\n");
double theta = atan2(landmark_p1.x(), landmark_p1.z());
double phi = atan2(landmark_p1.y(), sqrt(landmark_p1.x()*landmark_p1.x()+landmark_p1.z()*landmark_p1.z()));
double rho = 1./landmark_p1.norm();

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

@ -136,11 +136,11 @@ TEST( ProjectionFactor, Jacobian ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// 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,
*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,
_1, boost::none, boost::none, boost::none, boost::none), *K1);
@ -172,11 +172,11 @@ TEST( ProjectionFactor, JacobianWithTransform ) {
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
// Verify H2 and H4 with numerical derivatives
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>(
Matrix H2Expected = numericalDerivative11<Vector, Pose3>(
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
*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,
_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;
/* ************************************************************************* */
LieVector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
return LieVector(factor.evaluateError(x, p));
Vector evalFactorError(const RelativeElevationFactor& factor, const Pose3& x, const Point3& p) {
return factor.evaluateError(x, p);
}
/* ************************************************************************* */
@ -35,9 +35,9 @@ TEST( testRelativeElevationFactor, level_zero_error ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
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);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -50,9 +50,9 @@ TEST( testRelativeElevationFactor, level_positive ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
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);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -65,9 +65,9 @@ TEST( testRelativeElevationFactor, level_negative ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose1, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
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);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -80,9 +80,9 @@ TEST( testRelativeElevationFactor, rotated_zero_error ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
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);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -95,9 +95,9 @@ TEST( testRelativeElevationFactor, rotated_positive ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 2.0), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
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);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -110,9 +110,9 @@ TEST( testRelativeElevationFactor, rotated_negative1 ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose2, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
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);
EXPECT(assert_equal(expH1, actH1, tol));
EXPECT(assert_equal(expH2, actH2, tol));
@ -125,9 +125,9 @@ TEST( testRelativeElevationFactor, rotated_negative2 ) {
RelativeElevationFactor factor(poseKey, pointKey, measured, model1);
Matrix actH1, actH2;
EXPECT(assert_equal((Vector(1) << 3.0), factor.evaluateError(pose3, point1, actH1, actH2)));
Matrix expH1 = numericalDerivative21<LieVector,Pose3,Point3>(
Matrix expH1 = numericalDerivative21<Vector,Pose3,Point3>(
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);
EXPECT(assert_equal(expH1, actH1, 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
Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactor::evaluateError, &factor, _1, point, boost::none,
boost::none), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(
H2Expected = numericalDerivative11<Vector, Point2>(
boost::bind(&DeltaFactor::evaluateError, &factor, pose, _1, boost::none,
boost::none), point);
@ -78,16 +78,16 @@ TEST( DeltaFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&DeltaFactorBase::evaluateError, &factor, _1, pose, base2,
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,
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,
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,
_1, boost::none, boost::none, boost::none, boost::none), point);
@ -119,16 +119,16 @@ TEST( OdometryFactorBase, all ) {
// Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected, H3Expected, H4Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(
H1Expected = numericalDerivative11<Vector, Pose2>(
boost::bind(&OdometryFactorBase::evaluateError, &factor, _1, pose1, base2,
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,
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,
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,
base2, _1, boost::none, boost::none, boost::none, boost::none),
pose2);