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

Most of LieXXX is gone, greatly simplifies code. All tests pass. 

Merge remote-tracking branch 'origin/feature/BAD_noLieXX' into feature/BAD
release/4.3a0
dellaert 2014-11-04 15:50:33 +01:00
parent 6a08370a8a 3824fe5f90
commit 7d57d91fec
48 changed files with 787 additions and 785 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
.cproject
View File

@ -736,14 +736,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testImuFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testImuFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2265,6 +2257,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testVelocityConstraint3.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVelocityConstraint3.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>
@ -2766,10 +2766,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPriorFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget> <buildTarget>testPriorFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>

7
gtsam/geometry/tests/testPoint3.cpp
View File

@ -16,7 +16,6 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -90,8 +89,8 @@ TEST (Point3, normalize) {
} }
//************************************************************************* //*************************************************************************
LieScalar norm_proxy(const Point3& point) { double norm_proxy(const Point3& point) {
return LieScalar(point.norm()); return double(point.norm());
} }
TEST (Point3, norm) { TEST (Point3, norm) {
@ -99,7 +98,7 @@ TEST (Point3, norm) {
Point3 point(3,4,5); // arbitrary point Point3 point(3,4,5); // arbitrary point
double expected = sqrt(50); double expected = sqrt(50);
EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8); EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8);
Matrix expectedH = numericalDerivative11<LieScalar, Point3>(norm_proxy, point); Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }

16
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -276,8 +276,8 @@ TEST(HessianFactor, CombineAndEliminate)
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); 0.0, 0.0, 1.0);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); Vector3 b0(1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3,3) << Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0, 2.0, 0.0, 0.0,
@ -287,15 +287,15 @@ TEST(HessianFactor, CombineAndEliminate)
-2.0, 0.0, 0.0, -2.0, 0.0, 0.0,
0.0, -2.0, 0.0, 0.0, -2.0, 0.0,
0.0, 0.0, -2.0); 0.0, 0.0, -2.0);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); Vector3 b1(2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3,3) << Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0, 3.0, 0.0, 0.0,
0.0, 3.0, 0.0, 0.0, 3.0, 0.0,
0.0, 0.0, 3.0); 0.0, 0.0, 3.0);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); Vector3 b2(3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -305,8 +305,8 @@ TEST(HessianFactor, CombineAndEliminate)
Matrix zero3x3 = zeros(3,3); Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); Vector9 b; b << b1, b0, b2;
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); Vector9 sigmas; sigmas << s1, s0, s2;
// create a full, uneliminated version of the factor // create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));

16
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -369,8 +369,8 @@ TEST(JacobianFactor, eliminate)
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); 0.0, 0.0, 1.0);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); Vector3 b0(1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3, 3) << Matrix A10 = (Matrix(3, 3) <<
2.0, 0.0, 0.0, 2.0, 0.0, 0.0,
@ -380,15 +380,15 @@ TEST(JacobianFactor, eliminate)
-2.0, 0.0, 0.0, -2.0, 0.0, 0.0,
0.0, -2.0, 0.0, 0.0, -2.0, 0.0,
0.0, 0.0, -2.0); 0.0, 0.0, -2.0);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); Vector3 b1(2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3, 3) << Matrix A21 = (Matrix(3, 3) <<
3.0, 0.0, 0.0, 3.0, 0.0, 0.0,
0.0, 3.0, 0.0, 0.0, 3.0, 0.0,
0.0, 0.0, 3.0); 0.0, 0.0, 3.0);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); Vector3 b2(3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -398,8 +398,8 @@ TEST(JacobianFactor, eliminate)
Matrix zero3x3 = zeros(3,3); Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); Vector9 b; b << b1, b0, b2;
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); Vector9 sigmas; sigmas << s1, s0, s2;
JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0)); GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0));

19
gtsam/navigation/CombinedImuFactor.h
View File

@ -17,11 +17,10 @@
#pragma once #pragma once
/* GTSAM includes */ /* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.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/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
/* External or standard includes */ /* External or standard includes */
@ -46,7 +45,7 @@ namespace gtsam {
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/ */
class CombinedImuFactor: public NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> { class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
public: public:
@ -214,7 +213,7 @@ namespace gtsam {
Matrix3 H_vel_vel = I_3x3; Matrix3 H_vel_vel = I_3x3;
Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; Matrix3 H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT; Matrix3 H_vel_biasacc = - deltaRij.matrix() * deltaT;
Matrix3 H_angles_pos = Z_3x3; Matrix3 H_angles_pos = Z_3x3;
@ -222,7 +221,7 @@ namespace gtsam {
Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15); Matrix F(15,15);
@ -322,7 +321,7 @@ namespace gtsam {
private: private:
typedef CombinedImuFactor This; typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> Base; typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements preintegratedMeasurements_; CombinedPreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_; Vector3 gravity_;
@ -412,7 +411,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
@ -626,7 +625,7 @@ namespace gtsam {
/** predicted states from IMU */ /** predicted states from IMU */
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
@ -649,7 +648,7 @@ namespace gtsam {
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij; + 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term

2
gtsam/navigation/ImuBias.h
View File

@ -144,7 +144,7 @@ namespace imuBias {
/// return dimensionality of tangent space /// return dimensionality of tangent space
inline size_t dim() const { return dimension; } inline size_t dim() const { return dimension; }
/** Update the LieVector with a tangent space update */ /** Update the bias with a tangent space update */
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** @return the local coordinates of another object */ /** @return the local coordinates of another object */

15
gtsam/navigation/ImuFactor.h
View File

@ -21,7 +21,6 @@
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.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/debug.h> #include <gtsam/base/debug.h>
/* External or standard includes */ /* External or standard includes */
@ -46,7 +45,7 @@ namespace gtsam {
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/ */
class ImuFactor: public NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> { class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
public: public:
@ -203,13 +202,13 @@ namespace gtsam {
Matrix H_vel_vel = I_3x3; Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; Matrix H_vel_angles = - deltaRij.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix H_angles_pos = Z_3x3; Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3; Matrix H_angles_vel = Z_3x3;
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(9,9); Matrix F(9,9);
@ -285,7 +284,7 @@ namespace gtsam {
private: private:
typedef ImuFactor This; typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> Base; typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_; PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_; Vector3 gravity_;
@ -373,7 +372,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
@ -525,7 +524,7 @@ namespace gtsam {
/** predicted states from IMU */ /** predicted states from IMU */
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false) const bool use2ndOrderCoriolis = false)
@ -547,7 +546,7 @@ namespace gtsam {
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij; + 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term

7
gtsam/navigation/MagFactor.h
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/LieScalar.h>
namespace gtsam { namespace gtsam {
@ -165,7 +164,7 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias * This version uses model measured bM = scale * bRn * direction + bias
* and optimizes for both scale, direction, and the bias. * and optimizes for both scale, direction, and the bias.
*/ */
class MagFactor3: public NoiseModelFactor3<LieScalar, Unit3, Point3> { class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> {
const Point3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const Rot3 bRn_; ///< The assumed known rotation from nav to body const Rot3 bRn_; ///< The assumed known rotation from nav to body
@ -175,7 +174,7 @@ public:
/** Constructor */ /** Constructor */
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
const Rot3& nRb, const SharedNoiseModel& model) : const Rot3& nRb, const SharedNoiseModel& model) :
NoiseModelFactor3<LieScalar, Unit3, Point3>(model, key1, key2, key3), // NoiseModelFactor3<double, Unit3, Point3>(model, key1, key2, key3), //
measured_(measured), bRn_(nRb.inverse()) { measured_(measured), bRn_(nRb.inverse()) {
} }
@ -190,7 +189,7 @@ public:
* @param nM (unknown) local earth magnetic field vector, in nav frame * @param nM (unknown) local earth magnetic field vector, in nav frame
* @param bias (unknown) 3D bias * @param bias (unknown) 3D bias
*/ */
Vector evaluateError(const LieScalar& scale, const Unit3& direction, Vector evaluateError(double scale, const Unit3& direction,
const Point3& bias, boost::optional<Matrix&> H1 = boost::none, const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const { boost::none) const {

5
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.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>
@ -143,9 +142,9 @@ TEST( CombinedImuFactor, ErrorWithBiases )
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)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 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(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(0.5, 0.0, 0.0);
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;

25
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -18,6 +18,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/format.hpp>
namespace gtsam { namespace gtsam {
@ -62,7 +63,8 @@ NonlinearFactor::shared_ptr NonlinearFactor::rekey(
void NoiseModelFactor::print(const std::string& s, void NoiseModelFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
Base::print(s, keyFormatter); Base::print(s, keyFormatter);
noiseModel_->print(" noise model: "); if (noiseModel_)
noiseModel_->print(" noise model: ");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -76,18 +78,19 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
/* ************************************************************************* */ /* ************************************************************************* */
static void check(const SharedNoiseModel& noiseModel, size_t m) { static void check(const SharedNoiseModel& noiseModel, size_t m) {
if (!noiseModel) if (noiseModel && m != noiseModel->dim())
throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
if (m != noiseModel->dim())
throw std::invalid_argument( throw std::invalid_argument(
"NoiseModelFactor was created with a NoiseModel of incorrect dimension."); boost::str(
boost::format(
"NoiseModelFactor: NoiseModel has dimension %1% instead of %2%.")
% noiseModel->dim() % m));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector NoiseModelFactor::whitenedError(const Values& c) const { Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector b = unwhitenedError(c); const Vector b = unwhitenedError(c);
check(noiseModel_, b.size()); check(noiseModel_, b.size());
return noiseModel_->whiten(b); return noiseModel_ ? noiseModel_->whiten(b) : b;
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -95,7 +98,10 @@ double NoiseModelFactor::error(const Values& c) const {
if (active(c)) { if (active(c)) {
const Vector b = unwhitenedError(c); const Vector b = unwhitenedError(c);
check(noiseModel_, b.size()); check(noiseModel_, b.size());
return 0.5 * noiseModel_->distance(b); if (noiseModel_)
return 0.5 * noiseModel_->distance(b);
else
return 0.5 * b.squaredNorm();
} else { } else {
return 0.0; return 0.0;
} }
@ -115,7 +121,8 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
check(noiseModel_, b.size()); check(noiseModel_, b.size());
// Whiten the corresponding system now // Whiten the corresponding system now
noiseModel_->WhitenSystem(A, b); if (noiseModel_)
noiseModel_->WhitenSystem(A, b);
// Fill in terms, needed to create JacobianFactor below // Fill in terms, needed to create JacobianFactor below
std::vector<std::pair<Key, Matrix> > terms(size()); std::vector<std::pair<Key, Matrix> > terms(size());
@ -125,7 +132,7 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
} }
// TODO pass unwhitened + noise model to Gaussian factor // TODO pass unwhitened + noise model to Gaussian factor
if (noiseModel_->is_constrained()) if (noiseModel_ && noiseModel_->is_constrained())
return GaussianFactor::shared_ptr( return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b, new JacobianFactor(terms, b,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit())); boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));

7
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/LieScalar.h>
#include <cmath> #include <cmath>
namespace gtsam { namespace gtsam {
@ -126,7 +125,7 @@ namespace gtsam {
/// Calculate the error of the factor, typically equal to log-likelihood /// Calculate the error of the factor, typically equal to log-likelihood
inline double error(const Values& x) const { inline double error(const Values& x) const {
return f(z_, x.at<LieScalar>(meanKey_), x.at<LieScalar>(precisionKey_)); return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
} }
/** /**
@ -155,8 +154,8 @@ namespace gtsam {
/// linearize returns a Hessianfactor that is an approximation of error(p) /// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
double u = x.at<LieScalar>(meanKey_); double u = x.at<double>(meanKey_);
double p = x.at<LieScalar>(precisionKey_); double p = x.at<double>(precisionKey_);
Key j1 = meanKey_; Key j1 = meanKey_;
Key j2 = precisionKey_; Key j2 = precisionKey_;
return linearize(z_, u, p, j1, j2); return linearize(z_, u, p, j1, j2);

101
gtsam/nonlinear/tests/testValues.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/LieVector.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
@ -74,7 +73,7 @@ struct dimension<TestValue> : public boost::integral_constant<int, 0> {};
TEST( Values, equals1 ) TEST( Values, equals1 )
{ {
Values expected; Values expected;
LieVector v((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v(5.0, 6.0, 7.0);
expected.insert(key1, v); expected.insert(key1, v);
Values actual; Values actual;
@ -86,8 +85,8 @@ TEST( Values, equals1 )
TEST( Values, equals2 ) TEST( Values, equals2 )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << 5.0, 6.0, 8.0)); Vector3 v2(5.0, 6.0, 8.0);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg2.insert(key1, v2); cfg2.insert(key1, v2);
@ -99,8 +98,8 @@ TEST( Values, equals2 )
TEST( Values, equals_nan ) TEST( Values, equals_nan )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << inf, inf, inf)); Vector3 v2(inf, inf, inf);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg2.insert(key1, v2); cfg2.insert(key1, v2);
@ -112,10 +111,10 @@ TEST( Values, equals_nan )
TEST( Values, insert_good ) TEST( Values, insert_good )
{ {
Values cfg1, cfg2, expected; Values cfg1, cfg2, expected;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); Vector3 v2(8.0, 9.0, 1.0);
LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); Vector3 v3(2.0, 4.0, 3.0);
LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); Vector3 v4(8.0, 3.0, 7.0);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg1.insert(key2, v2); cfg1.insert(key2, v2);
@ -134,10 +133,10 @@ TEST( Values, insert_good )
TEST( Values, insert_bad ) TEST( Values, insert_bad )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); Vector3 v2(8.0, 9.0, 1.0);
LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); Vector3 v3(2.0, 4.0, 3.0);
LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); Vector3 v4(8.0, 3.0, 7.0);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg1.insert(key2, v2); cfg1.insert(key2, v2);
@ -151,16 +150,16 @@ TEST( Values, insert_bad )
TEST( Values, update_element ) TEST( Values, update_element )
{ {
Values cfg; Values cfg;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); Vector3 v2(8.0, 9.0, 1.0);
cfg.insert(key1, v1); cfg.insert(key1, v1);
CHECK(cfg.size() == 1); CHECK(cfg.size() == 1);
CHECK(assert_equal(v1, cfg.at<LieVector>(key1))); CHECK(assert_equal((Vector)v1, cfg.at<Vector3>(key1)));
cfg.update(key1, v2); cfg.update(key1, v2);
CHECK(cfg.size() == 1); CHECK(cfg.size() == 1);
CHECK(assert_equal(v2, cfg.at<LieVector>(key1))); CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -168,10 +167,10 @@ TEST(Values, basic_functions)
{ {
Values values; Values values;
const Values& values_c = values; const Values& values_c = values;
values.insert(2, LieVector()); values.insert(2, Vector3());
values.insert(4, LieVector()); values.insert(4, Vector3());
values.insert(6, LieVector()); values.insert(6, Vector3());
values.insert(8, LieVector()); values.insert(8, Vector3());
// find // find
EXPECT_LONGS_EQUAL(4, values.find(4)->key); EXPECT_LONGS_EQUAL(4, values.find(4)->key);
@ -195,8 +194,8 @@ TEST(Values, basic_functions)
//TEST(Values, dim_zero) //TEST(Values, dim_zero)
//{ //{
// Values config0; // Values config0;
// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0)); // config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0));
// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)); // config0.insert(key2, Vector3(5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim()); // LONGS_EQUAL(5, config0.dim());
// //
// VectorValues expected; // VectorValues expected;
@ -209,16 +208,16 @@ TEST(Values, basic_functions)
TEST(Values, expmap_a) TEST(Values, expmap_a)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); config0.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector> VectorValues increment = pair_list_of<Key, Vector>
(key1, (Vector(3) << 1.0, 1.1, 1.2)) (key1, (Vector(3) << 1.0, 1.1, 1.2))
(key2, (Vector(3) << 1.3, 1.4, 1.5)); (key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected; Values expected;
expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); expected.insert(key1, Vector3(2.0, 3.1, 4.2));
expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); expected.insert(key2, Vector3(6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
@ -227,15 +226,15 @@ TEST(Values, expmap_a)
TEST(Values, expmap_b) TEST(Values, expmap_b)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); config0.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector> VectorValues increment = pair_list_of<Key, Vector>
(key2, (Vector(3) << 1.3, 1.4, 1.5)); (key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected; Values expected;
expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); expected.insert(key1, Vector3(1.0, 2.0, 3.0));
expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); expected.insert(key2, Vector3(6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
@ -244,16 +243,16 @@ TEST(Values, expmap_b)
//TEST(Values, expmap_c) //TEST(Values, expmap_c)
//{ //{
// Values config0; // Values config0;
// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); // config0.insert(key1, Vector3(1.0, 2.0, 3.0));
// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); // config0.insert(key2, Vector3(5.0, 6.0, 7.0));
// //
// Vector increment = LieVector(6, // Vector increment = Vector6(
// 1.0, 1.1, 1.2, // 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5); // 1.3, 1.4, 1.5);
// //
// Values expected; // Values expected;
// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); // expected.insert(key1, Vector3(2.0, 3.1, 4.2));
// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); // expected.insert(key2, Vector3(6.3, 7.4, 8.5));
// //
// CHECK(assert_equal(expected, config0.retract(increment))); // CHECK(assert_equal(expected, config0.retract(increment)));
//} //}
@ -262,8 +261,8 @@ TEST(Values, expmap_b)
TEST(Values, expmap_d) TEST(Values, expmap_d)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); config0.insert(key2, Vector3(5.0, 6.0, 7.0));
//config0.print("config0"); //config0.print("config0");
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
CHECK(config0.equals(config0)); CHECK(config0.equals(config0));
@ -280,8 +279,8 @@ TEST(Values, expmap_d)
TEST(Values, localCoordinates) TEST(Values, localCoordinates)
{ {
Values valuesA; Values valuesA;
valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); valuesA.insert(key1, Vector3(1.0, 2.0, 3.0));
valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); valuesA.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues expDelta = pair_list_of<Key, Vector> VectorValues expDelta = pair_list_of<Key, Vector>
(key1, (Vector(3) << 0.1, 0.2, 0.3)) (key1, (Vector(3) << 0.1, 0.2, 0.3))
@ -317,28 +316,28 @@ TEST(Values, extract_keys)
TEST(Values, exists_) TEST(Values, exists_)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(1) << 1.))); config0.insert(key1, 1.0);
config0.insert(key2, LieVector((Vector(1) << 2.))); config0.insert(key2, 2.0);
boost::optional<const LieVector&> v = config0.exists<LieVector>(key1); boost::optional<const double&> v = config0.exists<double>(key1);
CHECK(assert_equal((Vector(1) << 1.),*v)); DOUBLES_EQUAL(1.0,*v,1e-9);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Values, update) TEST(Values, update)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(1) << 1.))); config0.insert(key1, 1.0);
config0.insert(key2, LieVector((Vector(1) << 2.))); config0.insert(key2, 2.0);
Values superset; Values superset;
superset.insert(key1, LieVector((Vector(1) << -1.))); superset.insert(key1, -1.0);
superset.insert(key2, LieVector((Vector(1) << -2.))); superset.insert(key2, -2.0);
config0.update(superset); config0.update(superset);
Values expected; Values expected;
expected.insert(key1, LieVector((Vector(1) << -1.))); expected.insert(key1, -1.0);
expected.insert(key2, LieVector((Vector(1) << -2.))); expected.insert(key2, -2.0);
CHECK(assert_equal(expected, config0)); CHECK(assert_equal(expected, config0));
} }

13
gtsam/slam/EssentialMatrixFactor.h
View File

@ -10,7 +10,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h> #include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/base/LieScalar.h>
#include <iostream> #include <iostream>
namespace gtsam { namespace gtsam {
@ -85,14 +84,13 @@ public:
* Binary factor that optimizes for E and inverse depth d: assumes measurement * Binary factor that optimizes for E and inverse depth d: assumes measurement
* in image 2 is perfect, and returns re-projection error in image 1 * in image 2 is perfect, and returns re-projection error in image 1
*/ */
class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, double> {
LieScalar> {
Point3 dP1_; ///< 3D point corresponding to measurement in image 1 Point3 dP1_; ///< 3D point corresponding to measurement in image 1
Point2 pn_; ///< Measurement in image 2, in ideal coordinates Point2 pn_; ///< Measurement in image 2, in ideal coordinates
double f_; ///< approximate conversion factor for error scaling double f_; ///< approximate conversion factor for error scaling
typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base; typedef NoiseModelFactor2<EssentialMatrix, double> Base;
typedef EssentialMatrixFactor2 This; typedef EssentialMatrixFactor2 This;
public: public:
@ -149,7 +147,7 @@ public:
* @param E essential matrix * @param E essential matrix
* @param d inverse depth d * @param d inverse depth d
*/ */
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd = boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const { boost::none) const {
@ -237,7 +235,8 @@ public:
*/ */
template<class CALIBRATION> template<class CALIBRATION>
EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
const Rot3& cRb, const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) : const Rot3& cRb, const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K) :
EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) { EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {
} }
@ -259,7 +258,7 @@ public:
* @param E essential matrix * @param E essential matrix
* @param d inverse depth d * @param d inverse depth d
*/ */
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd = boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const { boost::none) const {
if (!DE) { if (!DE) {

66
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -36,7 +36,7 @@ SfM_data data;
bool readOK = readBAL(filename, data); bool readOK = readBAL(filename, data);
Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation(); Point3 c1Tc2 = data.cameras[1].pose().translation();
PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(),Cal3_S2()); PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), Cal3_S2());
EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
double baseline = 0.1; // actual baseline of the camera double baseline = 0.1; // actual baseline of the camera
@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected; Matrix Hexpected;
Hexpected = numericalDerivative11<Vector,EssentialMatrix>( Hexpected = numericalDerivative11<Vector, EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE); boost::none), trueE);
@ -164,17 +164,17 @@ TEST (EssentialMatrixFactor2, factor) {
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2; Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
boost::none, boost::none); boost::none);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d); Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d); Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -197,7 +197,7 @@ TEST (EssentialMatrixFactor2, minimization) {
truth.insert(100, trueE); truth.insert(100, trueE);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p; Point3 P1 = data.tracks[i].p;
truth.insert(i, LieScalar(baseline / P1.z())); truth.insert(i, double(baseline / P1.z()));
} }
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
@ -211,7 +211,7 @@ TEST (EssentialMatrixFactor2, minimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100); EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1)); EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1)); EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result // Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -238,17 +238,17 @@ TEST (EssentialMatrixFactor3, factor) {
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2; Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none, boost::none); boost::none);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d); Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d); Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -270,7 +270,7 @@ TEST (EssentialMatrixFactor3, minimization) {
truth.insert(100, bodyE); truth.insert(100, bodyE);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p; Point3 P1 = data.tracks[i].p;
truth.insert(i, LieScalar(baseline / P1.z())); truth.insert(i, double(baseline / P1.z()));
} }
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
@ -284,7 +284,7 @@ TEST (EssentialMatrixFactor3, minimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100); EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(bodyE, actual, 1e-1)); EXPECT(assert_equal(bodyE, actual, 1e-1));
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1)); EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result // Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -314,7 +314,7 @@ Point2 pB(size_t i) {
boost::shared_ptr<Cal3Bundler> // boost::shared_ptr<Cal3Bundler> //
K = boost::make_shared<Cal3Bundler>(500, 0, 0); K = boost::make_shared<Cal3Bundler>(500, 0, 0);
PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(),*K); PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), *K);
Vector vA(size_t i) { Vector vA(size_t i) {
Point2 xy = K->calibrate(pA(i)); Point2 xy = K->calibrate(pA(i));
@ -380,17 +380,17 @@ TEST (EssentialMatrixFactor2, extraTest) {
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2; Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
boost::none, boost::none); boost::none);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, trueE, d); Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, trueE, d); Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -413,7 +413,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
truth.insert(100, trueE); truth.insert(100, trueE);
for (size_t i = 0; i < data.number_tracks(); i++) { for (size_t i = 0; i < data.number_tracks(); i++) {
Point3 P1 = data.tracks[i].p; Point3 P1 = data.tracks[i].p;
truth.insert(i, LieScalar(baseline / P1.z())); truth.insert(i, double(baseline / P1.z()));
} }
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
@ -427,7 +427,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100); EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1)); EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.number_tracks(); i++) for (size_t i = 0; i < data.number_tracks(); i++)
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1)); EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result // Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -449,17 +449,17 @@ TEST (EssentialMatrixFactor3, extraTest) {
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2; Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none, boost::none); boost::none);
Hexpected1 = numericalDerivative21<Vector,EssentialMatrix>(f, bodyE, d); Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector,EssentialMatrix>(f, bodyE, d); Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));

28
gtsam/slam/tests/testPriorFactor.cpp Normal file
View File

@ -0,0 +1,28 @@
/**
* @file testPriorFactor.cpp
* @brief Test PriorFactor
* @author Frank Dellaert
* @date Nov 4, 2014
*/
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/base/LieScalar.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
// Constructor
TEST(PriorFactor, Constructor) {
SharedNoiseModel model;
PriorFactor<LieScalar> factor(1, LieScalar(1.0), model);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

29
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -7,7 +7,6 @@
#pragma once #pragma once
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
@ -29,20 +28,20 @@ public:
protected: protected:
/** measurements from the IMU */ /** measurements from the IMU */
Vector accel_, gyro_; Vector3 accel_, gyro_;
double dt_; /// time between measurements double dt_; /// time between measurements
public: public:
/** Standard constructor */ /** Standard constructor */
FullIMUFactor(const Vector& accel, const Vector& gyro, FullIMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) { : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {
assert(model->dim() == 9); assert(model->dim() == 9);
} }
/** Single IMU vector - imu = [accel, gyro] */ /** Single IMU vector - imu = [accel, gyro] */
FullIMUFactor(const Vector& imu, FullIMUFactor(const Vector6& imu,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) { : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) {
assert(imu.size() == 6); assert(imu.size() == 6);
@ -68,15 +67,15 @@ public:
void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
std::string a = "FullIMUFactor: " + s; std::string a = "FullIMUFactor: " + s;
Base::print(a, formatter); Base::print(a, formatter);
gtsam::print(accel_, "accel"); gtsam::print((Vector)accel_, "accel");
gtsam::print(gyro_, "gyro"); gtsam::print((Vector)gyro_, "gyro");
std::cout << "dt: " << dt_ << std::endl; std::cout << "dt: " << dt_ << std::endl;
} }
// access // access
const Vector& gyro() const { return gyro_; } const Vector3& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; } const Vector3& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); } Vector6 z() const { return (Vector6() << accel_, gyro_); }
/** /**
* Error evaluation with optional derivatives - calculates * Error evaluation with optional derivatives - calculates
@ -85,13 +84,13 @@ public:
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
Vector z(9); Vector9 z;
z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang
if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>( if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>( if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
return z - predict_proxy(x1, x2, dt_); return z - predict_proxy(x1, x2, dt_);
} }
@ -107,11 +106,11 @@ public:
private: private:
/** copy of the measurement function formulated for numerical derivatives */ /** copy of the measurement function formulated for numerical derivatives */
static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
Vector hx(9); Vector9 hx;
hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang
return LieVector(hx); return hx;
} }
}; };

31
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -7,7 +7,6 @@
#pragma once #pragma once
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
@ -27,18 +26,18 @@ public:
protected: protected:
/** measurements from the IMU */ /** measurements from the IMU */
Vector accel_, gyro_; Vector3 accel_, gyro_;
double dt_; /// time between measurements double dt_; /// time between measurements
public: public:
/** Standard constructor */ /** Standard constructor */
IMUFactor(const Vector& accel, const Vector& gyro, IMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {} : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {}
/** Full IMU vector specification */ /** Full IMU vector specification */
IMUFactor(const Vector& imu_vector, IMUFactor(const Vector6& imu_vector,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {} : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {}
@ -61,15 +60,15 @@ public:
void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
std::string a = "IMUFactor: " + s; std::string a = "IMUFactor: " + s;
Base::print(a, formatter); Base::print(a, formatter);
gtsam::print(accel_, "accel"); gtsam::print((Vector)accel_, "accel");
gtsam::print(gyro_, "gyro"); gtsam::print((Vector)gyro_, "gyro");
std::cout << "dt: " << dt_ << std::endl; std::cout << "dt: " << dt_ << std::endl;
} }
// access // access
const Vector& gyro() const { return gyro_; } const Vector3& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; } const Vector3& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); } Vector6 z() const { return (Vector6() << accel_, gyro_);}
/** /**
* Error evaluation with optional derivatives - calculates * Error evaluation with optional derivatives - calculates
@ -78,10 +77,10 @@ public:
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
const Vector meas = z(); const Vector6 meas = z();
if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>( if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>( if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
return predict_proxy(x1, x2, dt_, meas); return predict_proxy(x1, x2, dt_, meas);
} }
@ -96,10 +95,10 @@ public:
private: private:
/** copy of the measurement function formulated for numerical derivatives */ /** copy of the measurement function formulated for numerical derivatives */
static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
double dt, const Vector& meas) { double dt, const Vector6& meas) {
Vector hx = x1.imuPrediction(x2, dt); Vector6 hx = x1.imuPrediction(x2, dt);
return LieVector(meas - hx); return meas - hx;
} }
}; };

51
gtsam_unstable/dynamics/Pendulum.h
View File

@ -9,7 +9,6 @@
#pragma once #pragma once
#include <gtsam/base/LieScalar.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -21,11 +20,11 @@ namespace gtsam {
* - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1} * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1}
* - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1} * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
*/ */
class PendulumFactor1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class PendulumFactor1: public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactor1() {} PendulumFactor1() {}
@ -38,7 +37,7 @@ public:
///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0) PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), h_(h) {} : Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -46,15 +45,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
/** q_k + h*v - q_k1 = 0, with optional derivatives */ /** q_k + h*v - q_k1 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v, Vector evaluateError(const double& qk1, const double& qk, const double& v,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
if (H1) *H1 = -eye(p); if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p); if (H2) *H2 = eye(p);
if (H3) *H3 = eye(p)*h_; if (H3) *H3 = eye(p)*h_;
return qk1.localCoordinates(qk.compose(LieScalar(v*h_))); return (Vector(1) << qk+v*h_-qk1);
} }
}; // \PendulumFactor1 }; // \PendulumFactor1
@ -67,11 +66,11 @@ public:
* - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1}) * - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1})
* - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k) * - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
*/ */
class PendulumFactor2: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class PendulumFactor2: public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactor2() {} PendulumFactor2() {}
@ -86,7 +85,7 @@ public:
///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0) PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} : Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -94,15 +93,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q, Vector evaluateError(const double & vk1, const double & vk, const double & q,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
if (H1) *H1 = -eye(p); if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p); if (H2) *H2 = eye(p);
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value()); if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q);
return vk1.localCoordinates(LieScalar(vk - h_*g_/r_*sin(q))); return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1);
} }
}; // \PendulumFactor2 }; // \PendulumFactor2
@ -114,11 +113,11 @@ public:
* p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) * p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
* = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) * = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1})
*/ */
class PendulumFactorPk: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class PendulumFactorPk: public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactorPk() {} PendulumFactorPk() {}
@ -136,7 +135,7 @@ public:
///Constructor ///Constructor
PendulumFactorPk(Key pKey, Key qKey, Key qKey1, PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey, qKey, qKey1), : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -145,11 +144,11 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
/** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
Vector evaluateError(const LieScalar& pk, const LieScalar& qk, const LieScalar& qk1, Vector evaluateError(const double & pk, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_; double mr2_h = 1/h_*m_*r_*r_;
@ -159,7 +158,7 @@ public:
if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
return pk.localCoordinates(LieScalar(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid))); return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk);
} }
}; // \PendulumFactorPk }; // \PendulumFactorPk
@ -170,11 +169,11 @@ public:
* p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) * p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
* = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) * = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1})
*/ */
class PendulumFactorPk1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactorPk1() {} PendulumFactorPk1() {}
@ -192,7 +191,7 @@ public:
///Constructor ///Constructor
PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1, PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey1, qKey, qKey1), : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -201,11 +200,11 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
/** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& pk1, const LieScalar& qk, const LieScalar& qk1, Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_; double mr2_h = 1/h_*m_*r_*r_;
@ -215,7 +214,7 @@ public:
if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
return pk1.localCoordinates(LieScalar(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid))); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1);
} }
}; // \PendulumFactorPk1 }; // \PendulumFactorPk1

29
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -57,18 +57,17 @@ void PoseRTV::print(const string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
PoseRTV PoseRTV::Expmap(const Vector& v) { PoseRTV PoseRTV::Expmap(const Vector9& v) {
assert(v.size() == 9); Pose3 newPose = Pose3::Expmap(v.head<6>());
Pose3 newPose = Pose3::Expmap(sub(v, 0, 6)); Velocity3 newVel = Velocity3::Expmap(v.tail<3>());
Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9));
return PoseRTV(newPose, newVel); return PoseRTV(newPose, newVel);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector PoseRTV::Logmap(const PoseRTV& p) { Vector9 PoseRTV::Logmap(const PoseRTV& p) {
Vector Lx = Pose3::Logmap(p.Rt_); Vector6 Lx = Pose3::Logmap(p.Rt_);
Vector Lv = Velocity3::Logmap(p.v_); Vector3 Lv = Velocity3::Logmap(p.v_);
return concatVectors(2, &Lx, &Lv); return (Vector9() << Lx, Lv);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -84,9 +83,9 @@ PoseRTV PoseRTV::retract(const Vector& v) const {
Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
const Pose3& x0 = pose(), &x1 = p1.pose(); const Pose3& x0 = pose(), &x1 = p1.pose();
// First order approximation // First order approximation
Vector poseLogmap = x0.localCoordinates(x1); Vector6 poseLogmap = x0.localCoordinates(x1);
Vector lv = rotation().unrotate(p1.velocity() - v_).vector(); Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
return concatVectors(2, &poseLogmap, &lv); return (Vector(9) << poseLogmap, lv);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -190,16 +189,16 @@ PoseRTV PoseRTV::generalDynamics(
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// split out states // split out states
const Rot3 &r1 = R(), &r2 = x2.R(); const Rot3 &r1 = R(), &r2 = x2.R();
const Velocity3 &v1 = v(), &v2 = x2.v(); const Velocity3 &v1 = v(), &v2 = x2.v();
Vector imu(6); Vector6 imu;
// acceleration // acceleration
Vector accel = v1.localCoordinates(v2) / dt; Vector accel = v1.localCoordinates(v2) / dt;
imu.head(3) = r2.transpose() * (accel - g); imu.head<3>() = r2.transpose() * (accel - g);
// rotation rates // rotation rates
// just using euler angles based on matlab code // just using euler angles based on matlab code
@ -211,7 +210,7 @@ Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// normalize yaw in difference (as per Mitch's code) // normalize yaw in difference (as per Mitch's code)
dR(2) = Rot2::fromAngle(dR(2)).theta(); dR(2) = Rot2::fromAngle(dR(2)).theta();
dR /= dt; dR /= dt;
imu.tail(3) = Enb * dR; imu.tail<3>() = Enb * dR;
// imu.tail(3) = r1.transpose() * dR; // imu.tail(3) = r1.transpose() * dR;
return imu; return imu;

6
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -86,8 +86,8 @@ public:
* expmap/logmap are poor approximations that assume independence of components * expmap/logmap are poor approximations that assume independence of components
* Currently implemented using the poor retract/unretract approximations * Currently implemented using the poor retract/unretract approximations
*/ */
static PoseRTV Expmap(const Vector& v); static PoseRTV Expmap(const Vector9& v);
static Vector Logmap(const PoseRTV& p); static Vector9 Logmap(const PoseRTV& p);
static PoseRTV identity() { return PoseRTV(); } static PoseRTV identity() { return PoseRTV(); }
@ -129,7 +129,7 @@ public:
/// Dynamics predictor for both ground and flying robots, given states at 1 and 2 /// Dynamics predictor for both ground and flying robots, given states at 1 and 2
/// Always move from time 1 to time 2 /// Always move from time 1 to time 2
/// @return imu measurement, as [accel, gyro] /// @return imu measurement, as [accel, gyro]
Vector imuPrediction(const PoseRTV& x2, double dt) const; Vector6 imuPrediction(const PoseRTV& x2, double dt) const;
/// predict measurement and where Point3 for x2 should be, as a way /// predict measurement and where Point3 for x2 should be, as a way
/// of enforcing a velocity constraint /// of enforcing a velocity constraint

23
gtsam_unstable/dynamics/SimpleHelicopter.h
View File

@ -9,7 +9,6 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -24,10 +23,10 @@ namespace gtsam {
* Note: this factor is necessary if one needs to smooth the entire graph. It's not needed * Note: this factor is necessary if one needs to smooth the entire graph. It's not needed
* in sequential update method. * in sequential update method.
*/ */
class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, LieVector> { class Reconstruction : public NoiseModelFactor3<Pose3, Pose3, Vector6> {
double h_; // time step double h_; // time step
typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base; typedef NoiseModelFactor3<Pose3, Pose3, Vector6> Base;
public: public:
Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) : Reconstruction(Key gKey1, Key gKey, Key xiKey, double h, double mu = 1000.0) :
Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey, Base(noiseModel::Constrained::All(Pose3::Dim(), fabs(mu)), gKey1, gKey,
@ -41,7 +40,7 @@ public:
gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); } gtsam::NonlinearFactor::shared_ptr(new Reconstruction(*this))); }
/** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0, with optional derivatives */ /** \f$ log((g_k\exp(h\xi_k))^{-1}g_{k+1}) = 0, with optional derivatives */
Vector evaluateError(const Pose3& gk1, const Pose3& gk, const LieVector& xik, Vector evaluateError(const Pose3& gk1, const Pose3& gk, const Vector6& xik,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
@ -74,7 +73,7 @@ public:
/** /**
* Implement the Discrete Euler-Poincare' equation: * Implement the Discrete Euler-Poincare' equation:
*/ */
class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<LieVector, LieVector, Pose3> { class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<Vector6, Vector6, Pose3> {
double h_; /// time step double h_; /// time step
Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ] Matrix Inertia_; /// Inertia tensors Inertia = [ J 0; 0 M ]
@ -87,7 +86,7 @@ class DiscreteEulerPoincareHelicopter : public NoiseModelFactor3<LieVector, LieV
// This might be needed in control or system identification problems. // This might be needed in control or system identification problems.
// We treat them as constant here, since the control inputs are to specify. // We treat them as constant here, since the control inputs are to specify.
typedef NoiseModelFactor3<LieVector, LieVector, Pose3> Base; typedef NoiseModelFactor3<Vector6, Vector6, Pose3> Base;
public: public:
DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey, DiscreteEulerPoincareHelicopter(Key xiKey1, Key xiKey_1, Key gKey,
@ -108,7 +107,7 @@ public:
* where pk = CT_TLN(h*xi_k)*Inertia*xi_k * where pk = CT_TLN(h*xi_k)*Inertia*xi_k
* pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1 * pk_1 = CT_TLN(-h*xi_k_1)*Inertia*xi_k_1
* */ * */
Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk, Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
@ -149,7 +148,7 @@ public:
} }
#if 0 #if 0
Vector computeError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk) const { Vector computeError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk) const {
Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik; Vector pk = Pose3::dExpInv_exp(h_*xik).transpose()*Inertia_*xik;
Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1; Vector pk_1 = Pose3::dExpInv_exp(-h_*xik_1).transpose()*Inertia_*xik_1;
@ -161,13 +160,13 @@ public:
return hx; return hx;
} }
Vector evaluateError(const LieVector& xik, const LieVector& xik_1, const Pose3& gk, Vector evaluateError(const Vector6& xik, const Vector6& xik_1, const Pose3& gk,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
if (H1) { if (H1) {
(*H1) = numericalDerivative31( (*H1) = numericalDerivative31(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>( boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
), ),
xik, xik_1, gk, 1e-5 xik, xik_1, gk, 1e-5
@ -175,7 +174,7 @@ public:
} }
if (H2) { if (H2) {
(*H2) = numericalDerivative32( (*H2) = numericalDerivative32(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>( boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
), ),
xik, xik_1, gk, 1e-5 xik, xik_1, gk, 1e-5
@ -183,7 +182,7 @@ public:
} }
if (H3) { if (H3) {
(*H3) = numericalDerivative33( (*H3) = numericalDerivative33(
boost::function<Vector(const LieVector&, const LieVector&, const Pose3&)>( boost::function<Vector(const Vector6&, const Vector6&, const Pose3&)>(
boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3) boost::bind(&DiscreteEulerPoincareHelicopter::computeError, *this, _1, _2, _3)
), ),
xik, xik_1, gk, 1e-5 xik, xik_1, gk, 1e-5

15
gtsam_unstable/dynamics/VelocityConstraint3.h
View File

@ -1,21 +1,20 @@
/** /**
* @file VelocityConstraint3.h * @file VelocityConstraint3.h
* @brief A simple 3-way factor constraining LieScalar poses and velocity * @brief A simple 3-way factor constraining double poses and velocity
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
*/ */
#pragma once #pragma once
#include <gtsam/base/LieScalar.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
class VelocityConstraint3 : public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class VelocityConstraint3 : public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
VelocityConstraint3() {} VelocityConstraint3() {}
@ -28,7 +27,7 @@ public:
///TODO: comment ///TODO: comment
VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0) VelocityConstraint3(Key key1, Key key2, Key velKey, double dt, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), key1, key2, velKey), dt_(dt) {} : Base(noiseModel::Constrained::All(1, fabs(mu)), key1, key2, velKey), dt_(dt) {}
virtual ~VelocityConstraint3() {} virtual ~VelocityConstraint3() {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -37,15 +36,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); } gtsam::NonlinearFactor::shared_ptr(new VelocityConstraint3(*this))); }
/** x1 + v*dt - x2 = 0, with optional derivatives */ /** x1 + v*dt - x2 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& x1, const LieScalar& x2, const LieScalar& v, Vector evaluateError(const double& x1, const double& x2, const double& v,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
if (H1) *H1 = eye(p); if (H1) *H1 = eye(p);
if (H2) *H2 = -eye(p); if (H2) *H2 = -eye(p);
if (H3) *H3 = eye(p)*dt_; if (H3) *H3 = eye(p)*dt_;
return x2.localCoordinates(x1.compose(LieScalar(v*dt_))); return (Vector(1) << x1+v*dt_-x2);
} }
private: private:

14
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -62,10 +62,9 @@ TEST( testIMUSystem, optimize_chain ) {
// create measurements // create measurements
SharedDiagonal model = noiseModel::Unit::Create(6); SharedDiagonal model = noiseModel::Unit::Create(6);
Vector imu12(6), imu23(6), imu34(6); Vector6 imu12 = pose1.imuPrediction(pose2, dt);
imu12 = pose1.imuPrediction(pose2, dt); Vector6 imu23 = pose2.imuPrediction(pose3, dt);
imu23 = pose2.imuPrediction(pose3, dt); Vector6 imu34 = pose3.imuPrediction(pose4, dt);
imu34 = pose3.imuPrediction(pose4, dt);
// assemble simple graph with IMU measurements and velocity constraints // assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
@ -109,10 +108,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// create measurements // create measurements
SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0); SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);
Vector imu12(6), imu23(6), imu34(6); Vector6 imu12 = pose1.imuPrediction(pose2, dt);
imu12 = pose1.imuPrediction(pose2, dt); Vector6 imu23 = pose2.imuPrediction(pose3, dt);
imu23 = pose2.imuPrediction(pose3, dt); Vector6 imu34 = pose3.imuPrediction(pose4, dt);
imu34 = pose3.imuPrediction(pose4, dt);
// assemble simple graph with IMU measurements and velocity constraints // assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph; NonlinearFactorGraph graph;

8
gtsam_unstable/dynamics/tests/testPendulumFactors.cpp
View File

@ -18,8 +18,8 @@ namespace {
const double g = 9.81, l = 1.0; const double g = 9.81, l = 1.0;
const double deg2rad = M_PI/180.0; const double deg2rad = M_PI/180.0;
LieScalar origin, q1(deg2rad*30.0), q2(deg2rad*31.0); double q1(deg2rad*30.0), q2(deg2rad*31.0);
LieScalar v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1))); double v1(deg2rad*1.0/h), v2((v1-h*g/l*sin(q1)));
} }
@ -46,7 +46,7 @@ TEST( testPendulumFactorPk, evaluateError) {
// hard constraints don't need a noise model // hard constraints don't need a noise model
PendulumFactorPk constraint(P(1), Q(1), Q(2), h); PendulumFactorPk constraint(P(1), Q(1), Q(2), h);
LieScalar pk( 1/h * (q2-q1) + h*g*sin(q1) ); double pk( 1/h * (q2-q1) + h*g*sin(q1) );
// verify error function // verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol)); EXPECT(assert_equal(zero(1), constraint.evaluateError(pk, q1, q2), tol));
@ -57,7 +57,7 @@ TEST( testPendulumFactorPk1, evaluateError) {
// hard constraints don't need a noise model // hard constraints don't need a noise model
PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h); PendulumFactorPk1 constraint(P(2), Q(1), Q(2), h);
LieScalar pk1( 1/h * (q2-q1) ); double pk1( 1/h * (q2-q1) );
// verify error function // verify error function
EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol)); EXPECT(assert_equal(zero(1), constraint.evaluateError(pk1, q1, q2), tol));

23
gtsam_unstable/dynamics/tests/testVelocityConstraint3.cpp
View File

@ -8,23 +8,16 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/dynamics/VelocityConstraint3.h> #include <gtsam_unstable/dynamics/VelocityConstraint3.h>
/* ************************************************************************* */
using namespace gtsam; using namespace gtsam;
using namespace gtsam::symbol_shorthand;
namespace {
const double tol=1e-5;
const double dt = 1.0;
LieScalar origin,
x1(1.0), x2(2.0), v(1.0);
}
/* ************************************************************************* */ /* ************************************************************************* */
// evaluateError
TEST( testVelocityConstraint3, evaluateError) { TEST( testVelocityConstraint3, evaluateError) {
using namespace gtsam::symbol_shorthand; const double tol = 1e-5;
const double dt = 1.0;
double x1(1.0), x2(2.0), v(1.0);
// hard constraints don't need a noise model // hard constraints don't need a noise model
VelocityConstraint3 constraint(X(1), X(2), V(1), dt); VelocityConstraint3 constraint(X(1), X(2), V(1), dt);
@ -33,7 +26,9 @@ TEST( testVelocityConstraint3, evaluateError) {
EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol)); EXPECT(assert_equal(zero(1), constraint.evaluateError(x1, x2, v), tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

20
gtsam_unstable/geometry/InvDepthCamera3.h
View File

@ -15,8 +15,6 @@
#include <boost/optional.hpp> #include <boost/optional.hpp>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/LieScalar.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
@ -71,10 +69,9 @@ public:
* @param inv inverse depth * @param inv inverse depth
* @return Point3 * @return Point3
*/ */
static gtsam::Point3 invDepthTo3D(const gtsam::LieVector& pw, const gtsam::LieScalar& inv) { static gtsam::Point3 invDepthTo3D(const Vector5& pw, double rho) {
gtsam::Point3 ray_base(pw.vector().segment(0,3)); gtsam::Point3 ray_base(pw.segment(0,3));
double theta = pw(3), phi = pw(4); double theta = pw(3), phi = pw(4);
double rho = inv.value(); // inverse depth
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
return ray_base + m/rho; return ray_base + m/rho;
} }
@ -84,15 +81,14 @@ public:
* @param H1 is the jacobian w.r.t. [pose3 calibration] * @param H1 is the jacobian w.r.t. [pose3 calibration]
* @param H2 is the jacobian w.r.t. inv_depth_landmark * @param H2 is the jacobian w.r.t. inv_depth_landmark
*/ */
inline gtsam::Point2 project(const gtsam::LieVector& pw, inline gtsam::Point2 project(const Vector5& pw,
const gtsam::LieScalar& inv_depth, double rho,
boost::optional<gtsam::Matrix&> H1 = boost::none, boost::optional<gtsam::Matrix&> H1 = boost::none,
boost::optional<gtsam::Matrix&> H2 = boost::none, boost::optional<gtsam::Matrix&> H2 = boost::none,
boost::optional<gtsam::Matrix&> H3 = boost::none) const { boost::optional<gtsam::Matrix&> H3 = boost::none) const {
gtsam::Point3 ray_base(pw.vector().segment(0,3)); gtsam::Point3 ray_base(pw.segment(0,3));
double theta = pw(3), phi = pw(4); double theta = pw(3), phi = pw(4);
double rho = inv_depth.value(); // inverse depth
gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi)); gtsam::Point3 m(cos(theta)*cos(phi),sin(theta)*cos(phi),sin(phi));
const gtsam::Point3 landmark = ray_base + m/rho; const gtsam::Point3 landmark = ray_base + m/rho;
@ -157,7 +153,7 @@ public:
* useful for point initialization * useful for point initialization
*/ */
inline std::pair<gtsam::LieVector, gtsam::LieScalar> backproject(const gtsam::Point2& pi, const double depth) const { inline std::pair<Vector5, double> backproject(const gtsam::Point2& pi, const double depth) const {
const gtsam::Point2 pn = k_->calibrate(pi); const gtsam::Point2 pn = k_->calibrate(pi);
gtsam::Point3 pc(pn.x(), pn.y(), 1.0); gtsam::Point3 pc(pn.x(), pn.y(), 1.0);
pc = pc/pc.norm(); pc = pc/pc.norm();
@ -166,8 +162,8 @@ public:
gtsam::Point3 ray = pw - pt; gtsam::Point3 ray = pw - pt;
double theta = atan2(ray.y(), ray.x()); // longitude double theta = atan2(ray.y(), ray.x()); // longitude
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
return std::make_pair(gtsam::LieVector((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)), return std::make_pair(Vector5((Vector(5) << pt.x(),pt.y(),pt.z(), theta, phi)),
gtsam::LieScalar(1./depth)); double(1./depth));
} }
private: private:

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

@ -30,9 +30,9 @@ TEST( InvDepthFactor, Project1) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.)); Vector5 inv_landmark((Vector(5) << 1., 0., 1., 0., 0.));
LieScalar inv_depth(1./4); double 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,1e-8));
EXPECT(assert_equal(Point2(640,480), actual_uv)); EXPECT(assert_equal(Point2(640,480), actual_uv));
} }
@ -46,7 +46,7 @@ TEST( InvDepthFactor, Project2) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 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)); double 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));
} }
@ -61,7 +61,7 @@ TEST( InvDepthFactor, Project3) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 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)); double 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));
} }
@ -76,24 +76,24 @@ TEST( InvDepthFactor, Project4) {
InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K); InvDepthCamera3<Cal3_S2> inv_camera(level_pose, K);
Vector5 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.)); double 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));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 project_(const Pose3& pose, const Vector5& landmark, const LieScalar& inv_depth) { Point2 project_(const Pose3& pose, const Vector5& landmark, const double& 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)
{ {
Vector5 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); double 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);
Matrix actual; Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none); inv_camera.project(landmark, inv_depth, actual, boost::none, boost::none);
EXPECT(assert_equal(expected,actual,1e-6)); EXPECT(assert_equal(expected,actual,1e-6));
} }
@ -101,11 +101,11 @@ TEST( InvDepthFactor, Dproject_pose)
TEST( InvDepthFactor, Dproject_landmark) TEST( InvDepthFactor, Dproject_landmark)
{ {
Vector5 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); double 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);
Matrix actual; Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none); inv_camera.project(landmark, inv_depth, boost::none, actual, boost::none);
EXPECT(assert_equal(expected,actual,1e-7)); EXPECT(assert_equal(expected,actual,1e-7));
} }
@ -113,11 +113,11 @@ TEST( InvDepthFactor, Dproject_landmark)
TEST( InvDepthFactor, Dproject_inv_depth) TEST( InvDepthFactor, Dproject_inv_depth)
{ {
Vector5 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); double 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);
Matrix actual; Matrix actual;
Point2 uv = inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual); inv_camera.project(landmark, inv_depth, boost::none, boost::none, actual);
EXPECT(assert_equal(expected,actual,1e-7)); EXPECT(assert_equal(expected,actual,1e-7));
} }
@ -125,15 +125,15 @@ TEST( InvDepthFactor, Dproject_inv_depth)
TEST(InvDepthFactor, backproject) TEST(InvDepthFactor, backproject)
{ {
Vector 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); double 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);
Vector5 actual_vec; Vector5 actual_vec;
LieScalar actual_inv; double 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));
EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -141,15 +141,15 @@ TEST(InvDepthFactor, backproject2)
{ {
// backwards facing camera // backwards facing camera
Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1)); Vector expected((Vector(5) << -5.,-5.,2., 3., -0.1));
LieScalar inv_depth(1./10); double 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);
Vector5 actual_vec; Vector5 actual_vec;
LieScalar actual_inv; double 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));
EXPECT(assert_equal(inv_depth,actual_inv,1e-7)); EXPECT_DOUBLES_EQUAL(inv_depth,actual_inv,1e-7);
} }
/* ************************************************************************* */ /* ************************************************************************* */

1
gtsam_unstable/gtsam_unstable.h
View File

@ -515,7 +515,6 @@ virtual class PendulumFactorPk1 : gtsam::NonlinearFactor {
Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const; Vector evaluateError(const gtsam::LieScalar& pk1, const gtsam::LieScalar& qk, const gtsam::LieScalar& qk1) const;
}; };
#include <gtsam/base/LieVector.h>
#include <gtsam_unstable/dynamics/SimpleHelicopter.h> #include <gtsam_unstable/dynamics/SimpleHelicopter.h>
virtual class Reconstruction : gtsam::NonlinearFactor { virtual class Reconstruction : gtsam::NonlinearFactor {

12
gtsam_unstable/slam/AHRS.cpp
View File

@ -48,12 +48,12 @@ AHRS::AHRS(const Matrix& stationaryU, const Matrix& stationaryF, double g_e,
F_g_ = -I3 / tau_g; F_g_ = -I3 / tau_g;
F_a_ = -I3 / tau_a; F_a_ = -I3 / tau_a;
Vector var_omega_w = 0 * ones(3); // TODO Vector3 var_omega_w = 0 * ones(3); // TODO
Vector var_omega_g = (0.0034 * 0.0034) * ones(3); Vector3 var_omega_g = (0.0034 * 0.0034) * ones(3);
Vector var_omega_a = (0.034 * 0.034) * ones(3); Vector3 var_omega_a = (0.034 * 0.034) * ones(3);
Vector sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g); Vector3 sigmas_v_g_sq = emul(sigmas_v_g, sigmas_v_g);
Vector vars = concatVectors(4, &var_omega_w, &var_omega_g, &sigmas_v_g_sq, Vector vars(12);
&var_omega_a); vars << var_omega_w, var_omega_g, sigmas_v_g_sq, var_omega_a;
var_w_ = diag(vars); var_w_ = diag(vars);
// Quantities needed for aiding // Quantities needed for aiding

21
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

@ -22,7 +22,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
// Using numerical derivative to calculate d(Pose3::Expmap)/dw // Using numerical derivative to calculate d(Pose3::Expmap)/dw
@ -269,7 +268,7 @@ public:
VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12; VelDelta -= 2*skewSymmetric(world_rho + world_omega_earth)*world_V1_body * dt12;
// Predict // Predict
return Vel1.compose( VelDelta ); return Vel1 + VelDelta;
} }
@ -295,7 +294,7 @@ public:
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
// Calculate error // Calculate error
return Vel2.between(Vel2Pred); return Vel2Pred-Vel2;
} }
Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2, Vector evaluateError(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, const POSE& Pose2, const VELOCITY& Vel2,
@ -344,7 +343,7 @@ public:
} }
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2));
return concatVectors(2, &ErrPoseVector, &ErrVelVector); return concatVectors(2, &ErrPoseVector, &ErrVelVector);
} }
@ -438,18 +437,18 @@ public:
Matrix Z_3x3 = zeros(3,3); Matrix Z_3x3 = zeros(3,3);
Matrix I_3x3 = eye(3,3); Matrix I_3x3 = eye(3,3);
Matrix H_pos_pos = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0); Matrix H_pos_pos = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, _1, delta_vel_in_t0), delta_pos_in_t0);
Matrix H_pos_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0); Matrix H_pos_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_pos, msr_dt, delta_pos_in_t0, _1), delta_vel_in_t0);
Matrix H_pos_angles = Z_3x3; Matrix H_pos_angles = Z_3x3;
Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3); Matrix H_pos_bias = collect(2, &Z_3x3, &Z_3x3);
Matrix H_vel_vel = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0); Matrix H_vel_vel = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_vel_in_t0);
Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, _1, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
Matrix H_vel_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); Matrix H_vel_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_vel, msr_gyro_t, msr_acc_t, msr_dt, delta_angles, delta_vel_in_t0, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
Matrix H_vel_pos = Z_3x3; Matrix H_vel_pos = Z_3x3;
Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles); Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, _1, flag_use_body_P_sensor, body_P_sensor, Bias_t0), delta_angles);
Matrix H_angles_bias = numericalDerivative11<LieVector, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0); Matrix H_angles_bias = numericalDerivative11<Vector3, IMUBIAS>(boost::bind(&PreIntegrateIMUObservations_delta_angles, msr_gyro_t, msr_dt, delta_angles, flag_use_body_P_sensor, body_P_sensor, _1), Bias_t0);
Matrix H_angles_pos = Z_3x3; Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3; Matrix H_angles_vel = Z_3x3;

1
gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

@ -22,7 +22,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
// Using numerical derivative to calculate d(Pose3::Expmap)/dw // Using numerical derivative to calculate d(Pose3::Expmap)/dw

7
gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

@ -21,7 +21,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/NoiseModel.h> #include <gtsam/linear/NoiseModel.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
// Using numerical derivative to calculate d(Pose3::Expmap)/dw // Using numerical derivative to calculate d(Pose3::Expmap)/dw
@ -200,7 +199,7 @@ public:
VELOCITY VelDelta(world_a_body*dt_); VELOCITY VelDelta(world_a_body*dt_);
// Predict // Predict
return Vel1.compose(VelDelta); return Vel1 + VelDelta;
} }
void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const { void predict(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1, POSE& Pose2, VELOCITY& Vel2) const {
@ -221,7 +220,7 @@ public:
VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1); VELOCITY Vel2Pred = predictVelocity(Pose1, Vel1, Bias1);
// Calculate error // Calculate error
return Vel2.between(Vel2Pred); return Vel2Pred - Vel2;
} }
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
@ -271,7 +270,7 @@ public:
} }
Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2))); Vector ErrPoseVector(POSE::Logmap(evaluatePoseError(Pose1, Vel1, Bias1, Pose2, Vel2)));
Vector ErrVelVector(VELOCITY::Logmap(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2))); Vector ErrVelVector(evaluateVelocityError(Pose1, Vel1, Bias1, Pose2, Vel2));
return concatVectors(2, &ErrPoseVector, &ErrVelVector); return concatVectors(2, &ErrPoseVector, &ErrVelVector);
} }

2
gtsam_unstable/slam/InvDepthFactor3.h
View File

@ -80,7 +80,7 @@ public:
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
gtsam::Vector evaluateError(const POSE& pose, const gtsam::LieVector& point, const INVDEPTH& invDepth, gtsam::Vector evaluateError(const POSE& pose, const Vector5& point, const INVDEPTH& invDepth,
boost::optional<gtsam::Matrix&> H1=boost::none, boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none, boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const { boost::optional<gtsam::Matrix&> H3=boost::none) const {

9
gtsam_unstable/slam/InvDepthFactorVariant1.h
View File

@ -15,7 +15,6 @@
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
namespace gtsam { namespace gtsam {
@ -23,7 +22,7 @@ namespace gtsam {
/** /**
* Binary factor representing a visual measurement using an inverse-depth parameterization * Binary factor representing a visual measurement using an inverse-depth parameterization
*/ */
class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, LieVector> { class InvDepthFactorVariant1: public NoiseModelFactor2<Pose3, Vector6> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -33,7 +32,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor2<Pose3, LieVector> Base; typedef NoiseModelFactor2<Pose3, Vector6> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant1 This; typedef InvDepthFactorVariant1 This;
@ -79,7 +78,7 @@ public:
&& this->K_->equals(*e->K_, tol); && this->K_->equals(*e->K_, tol);
} }
Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { Vector inverseDepthError(const Pose3& pose, const Vector6& landmark) const {
try { try {
// Calculate the 3D coordinates of the landmark in the world frame // Calculate the 3D coordinates of the landmark in the world frame
double x = landmark(0), y = landmark(1), z = landmark(2); double x = landmark(0), y = landmark(1), z = landmark(2);
@ -100,7 +99,7 @@ public:
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const LieVector& landmark, Vector evaluateError(const Pose3& pose, const Vector6& landmark,
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 {

9
gtsam_unstable/slam/InvDepthFactorVariant2.h
View File

@ -16,7 +16,6 @@
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
namespace gtsam { namespace gtsam {
@ -24,7 +23,7 @@ namespace gtsam {
/** /**
* Binary factor representing a visual measurement using an inverse-depth parameterization * Binary factor representing a visual measurement using an inverse-depth parameterization
*/ */
class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, LieVector> { class InvDepthFactorVariant2: public NoiseModelFactor2<Pose3, Vector3> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -35,7 +34,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor2<Pose3, LieVector> Base; typedef NoiseModelFactor2<Pose3, Vector3> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant2 This; typedef InvDepthFactorVariant2 This;
@ -83,7 +82,7 @@ public:
&& this->referencePoint_.equals(e->referencePoint_, tol); && this->referencePoint_.equals(e->referencePoint_, tol);
} }
Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
try { try {
// Calculate the 3D coordinates of the landmark in the world frame // Calculate the 3D coordinates of the landmark in the world frame
double theta = landmark(0), phi = landmark(1), rho = landmark(2); double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@ -103,7 +102,7 @@ public:
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const LieVector& landmark, Vector evaluateError(const Pose3& pose, const Vector3& landmark,
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 {

28
gtsam_unstable/slam/InvDepthFactorVariant3.h
View File

@ -15,7 +15,6 @@
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
namespace gtsam { namespace gtsam {
@ -23,7 +22,7 @@ namespace gtsam {
/** /**
* Binary factor representing the first visual measurement using an inverse-depth parameterization * Binary factor representing the first visual measurement using an inverse-depth parameterization
*/ */
class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, LieVector> { class InvDepthFactorVariant3a: public NoiseModelFactor2<Pose3, Vector3> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -33,7 +32,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor2<Pose3, LieVector> Base; typedef NoiseModelFactor2<Pose3, Vector3> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant3a This; typedef InvDepthFactorVariant3a This;
@ -81,7 +80,7 @@ public:
&& this->K_->equals(*e->K_, tol); && this->K_->equals(*e->K_, tol);
} }
Vector inverseDepthError(const Pose3& pose, const LieVector& landmark) const { Vector inverseDepthError(const Pose3& pose, const Vector3& landmark) const {
try { try {
// Calculate the 3D coordinates of the landmark in the Pose frame // Calculate the 3D coordinates of the landmark in the Pose frame
double theta = landmark(0), phi = landmark(1), rho = landmark(2); double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@ -103,7 +102,7 @@ public:
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const LieVector& landmark, Vector evaluateError(const Pose3& pose, const Vector3& landmark,
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 {
@ -142,7 +141,7 @@ private:
/** /**
* Ternary factor representing a visual measurement using an inverse-depth parameterization * Ternary factor representing a visual measurement using an inverse-depth parameterization
*/ */
class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, LieVector> { class InvDepthFactorVariant3b: public NoiseModelFactor3<Pose3, Pose3, Vector3> {
protected: protected:
// Keep a copy of measurement and calibration for I/O // Keep a copy of measurement and calibration for I/O
@ -152,7 +151,7 @@ protected:
public: public:
/// shorthand for base class type /// shorthand for base class type
typedef NoiseModelFactor3<Pose3, Pose3, LieVector> Base; typedef NoiseModelFactor3<Pose3, Pose3, Vector3> Base;
/// shorthand for this class /// shorthand for this class
typedef InvDepthFactorVariant3b This; typedef InvDepthFactorVariant3b This;
@ -200,7 +199,7 @@ public:
&& this->K_->equals(*e->K_, tol); && this->K_->equals(*e->K_, tol);
} }
Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark) const { Vector inverseDepthError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark) const {
try { try {
// Calculate the 3D coordinates of the landmark in the Pose1 frame // Calculate the 3D coordinates of the landmark in the Pose1 frame
double theta = landmark(0), phi = landmark(1), rho = landmark(2); double theta = landmark(0), phi = landmark(1), rho = landmark(2);
@ -222,20 +221,19 @@ public:
} }
/// Evaluate error h(x)-z and optionally derivatives /// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const LieVector& landmark, Vector evaluateError(const Pose3& pose1, const Pose3& pose2, const Vector3& landmark,
boost::optional<gtsam::Matrix&> H1=boost::none, boost::optional<gtsam::Matrix&> H1=boost::none,
boost::optional<gtsam::Matrix&> H2=boost::none, boost::optional<gtsam::Matrix&> H2=boost::none,
boost::optional<gtsam::Matrix&> H3=boost::none) const { boost::optional<gtsam::Matrix&> H3=boost::none) const {
if(H1) { if(H1)
(*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1); (*H1) = numericalDerivative11<Vector,Pose3>(boost::bind(&InvDepthFactorVariant3b::inverseDepthError, this, _1, pose2, landmark), pose1);
}
if(H2) { if(H2)
(*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,Vector3>(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);
} }

1
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -10,7 +10,6 @@
#include <gtsam_unstable/slam/BetweenFactorEM.h> #include <gtsam_unstable/slam/BetweenFactorEM.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>

3
gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <iostream> #include <iostream>
@ -55,7 +54,7 @@ TEST( EquivInertialNavFactor_GlobalVel, Constructor)
SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9)); SharedGaussian imu_model = noiseModel::Gaussian::Covariance(EquivCov_Overall.block(0,0,9,9));
// Constructor // Constructor
EquivInertialNavFactor_GlobalVel<Pose3, LieVector, imuBias::ConstantBias> factor( EquivInertialNavFactor_GlobalVel<Pose3, Vector3, imuBias::ConstantBias> factor(
poseKey1, velKey1, biasKey1, poseKey2, velKey2, poseKey1, velKey1, biasKey1, poseKey2, velKey2,
delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t, delta_pos_in_t0, delta_vel_in_t0, delta_angles, delta_t,
g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1); g, rho, omega_earth, imu_model, Jacobian_wrt_t0_Overall, bias1);

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

@ -23,86 +23,76 @@
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.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>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
Rot3 world_R_ECEF( Rot3 world_R_ECEF(0.31686, 0.51505, 0.79645, 0.85173, -0.52399, 0, 0.41733,
0.31686, 0.51505, 0.79645, 0.67835, -0.60471);
0.85173, -0.52399, 0,
0.41733, 0.67835, -0.60471);
Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); static const Vector3 world_g(0.0, 0.0, 9.81);
Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); static const Vector3 world_rho(0.0, -1.5724e-05, 0.0); // NED system
static const Vector3 ECEF_omega_earth(0.0, 0.0, 7.292115e-5);
static const Vector3 world_omega_earth = world_R_ECEF.matrix()
* ECEF_omega_earth;
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 predictionErrorPose(const Pose3& p1, const LieVector& v1, Pose3 predictionErrorPose(const Pose3& p1, const Vector3& v1,
const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2,
const InertialNavFactor_GlobalVelocity<Pose3, LieVector, const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
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));
} }
Vector predictionErrorVel(const Pose3& p1, const LieVector& v1, Vector predictionErrorVel(const Pose3& p1, const Vector3& v1,
const imuBias::ConstantBias& b1, const Pose3& p2, const LieVector& v2, const imuBias::ConstantBias& b1, const Pose3& p2, const Vector3& v2,
const InertialNavFactor_GlobalVelocity<Pose3, LieVector, const InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias>& factor) {
imuBias::ConstantBias>& factor) {
return factor.evaluateError(p1, v1, b1, p2, v2).tail(3); return factor.evaluateError(p1, v1, b1, p2, v2).tail(3);
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Constructor) {
TEST( InertialNavFactor_GlobalVelocity, Constructor)
{
Key Pose1(11); Key Pose1(11);
Key Pose2(12); Key Pose2(12);
Key Vel1(21); Key Vel1(21);
Key Vel2(22); Key Vel2(22);
Key Bias1(31); Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); Vector3 measurement_acc(0.1, 0.2, 0.4);
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); Vector3 measurement_gyro(-0.2, 0.5, 0.03);
double measurement_dt(0.1); 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
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
measurement_dt, world_g, world_rho, world_omega_earth, model);
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Equals) {
TEST( InertialNavFactor_GlobalVelocity, Equals)
{
Key Pose1(11); Key Pose1(11);
Key Pose2(12); Key Pose2(12);
Key Vel1(21); Key Vel1(21);
Key Vel2(22); Key Vel2(22);
Key Bias1(31); Key Bias1(31);
Vector measurement_acc((Vector(3) << 0.1,0.2,0.4)); Vector3 measurement_acc(0.1, 0.2, 0.4);
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); Vector3 measurement_gyro(-0.2, 0.5, 0.03);
double measurement_dt(0.1); 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
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
measurement_dt, world_g, world_rho, world_omega_earth, model);
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> g(
Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
measurement_dt, world_g, world_rho, world_omega_earth, model);
CHECK(assert_equal(f, g, 1e-5)); CHECK(assert_equal(f, g, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, Predict) {
TEST( InertialNavFactor_GlobalVelocity, Predict)
{
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
Key VelKey1(21); Key VelKey1(21);
@ -110,36 +100,32 @@ TEST( InertialNavFactor_GlobalVelocity, Predict)
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// First test: zero angular motion, some acceleration // First test: zero angular motion, some acceleration
Vector measurement_acc((Vector(3) <<0.1,0.2,0.3-9.81)); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81));
Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model);
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
Pose3 actualPose2; Pose3 actualPose2;
LieVector actualVel2; Vector3 actualVel2;
f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
CHECK(assert_equal(expectedPose2, actualPose2, 1e-5)); CHECK(assert_equal(expectedPose2, actualPose2, 1e-5));
CHECK(assert_equal(expectedVel2, actualVel2, 1e-5)); CHECK(assert_equal((Vector)expectedVel2, actualVel2, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel) {
TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
{
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
Key VelKey1(21); Key VelKey1(21);
@ -147,24 +133,22 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// First test: zero angular motion, some acceleration // First test: zero angular motion, some acceleration
Vector measurement_acc((Vector(3) <<0.1,0.2,0.3-9.81)); Vector measurement_acc((Vector(3) << 0.1, 0.2, 0.3 - 9.81));
Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model);
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -173,9 +157,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVel)
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRot) {
TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
{
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
Key VelKey1(21); Key VelKey1(21);
@ -183,23 +165,23 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration // Second test: zero angular motion, some acceleration
Vector measurement_acc((Vector(3) <<0.0,0.0,0.0-9.81)); Vector measurement_acc((Vector(3) << 0.0, 0.0, 0.0 - 9.81));
Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model);
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0));
Pose3 Pose2(Rot3::Expmap(measurement_gyro*measurement_dt), Point3(2.0,1.0,3.0)); Pose3 Pose2(Rot3::Expmap(measurement_gyro * measurement_dt),
LieVector Vel1((Vector(3) << 0.0, 0.0, 0.0)); Point3(2.0, 1.0, 3.0));
LieVector Vel2((Vector(3) << 0.0, 0.0, 0.0)); Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0));
Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -208,9 +190,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRot)
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel) {
TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
{
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
Key VelKey1(21); Key VelKey1(21);
@ -218,32 +198,30 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
// Second test: zero angular motion, some acceleration - generated in matlab // Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_acc(
(Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model);
Rot3 R1(0.487316618, 0.125253866, 0.86419557, Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
0.580273724, 0.693095498, -0.427669306, -0.427669306, -0.652537293, 0.709880342, 0.265075427);
-0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); Pose3 Pose1(R1, t1);
LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
0.609241153, 0.67099888, -0.422594037, -0.422594037, -0.636011287, 0.731761397, 0.244979388);
-0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt));
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g); Vector dv = measurement_dt * (R1.matrix() * measurement_acc + world_g);
LieVector Vel2 = Vel1.compose( dv ); Vector3 Vel2 = Vel1 + dv;
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -253,16 +231,15 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
} }
///* VADIM - START ************************************************************************* */ ///* VADIM - START ************************************************************************* */
//LieVector predictionRq(const LieVector angles, const LieVector q) { //Vector3 predictionRq(const Vector3 angles, const Vector3 q) {
// return (Rot3().RzRyRx(angles) * q).vector(); // return (Rot3().RzRyRx(angles) * q).vector();
//} //}
// //
//TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) { //TEST (InertialNavFactor_GlobalVelocity, Rotation_Deriv ) {
// LieVector angles((Vector(3) << 3.001, -1.0004, 2.0005)); // Vector3 angles((Vector(3) << 3.001, -1.0004, 2.0005));
// Rot3 R1(Rot3().RzRyRx(angles)); // Rot3 R1(Rot3().RzRyRx(angles));
// LieVector q((Vector(3) << 5.8, -2.2, 4.105)); // Vector3 q((Vector(3) << 5.8, -2.2, 4.105));
// Rot3 qx(0.0, -q[2], q[1], // Rot3 qx(0.0, -q[2], q[1],
// q[2], 0.0, -q[0], // q[2], 0.0, -q[0],
// -q[1], q[0],0.0); // -q[1], q[0],0.0);
@ -270,9 +247,9 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
// //
// Matrix J_expected; // Matrix J_expected;
// //
// LieVector v(predictionRq(angles, q)); // Vector3 v(predictionRq(angles, q));
// //
// J_expected = numericalDerivative11<LieVector, LieVector>(boost::bind(&predictionRq, _1, q), angles); // J_expected = numericalDerivative11<Vector3, Vector3>(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;
@ -281,8 +258,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVel)
//} //}
///* VADIM - END ************************************************************************* */ ///* VADIM - END ************************************************************************* */
/* ************************************************************************* */ /* ************************************************************************* */TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
@ -291,51 +267,63 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); Vector measurement_acc(
Vector measurement_gyro((Vector(3) << 3.14, 3.14/2, -3.14)); (Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343));
Vector measurement_gyro((Vector(3) << 3.14, 3.14 / 2, -3.14));
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> factor(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model);
Rot3 R1(0.487316618, 0.125253866, 0.86419557, Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
0.580273724, 0.693095498, -0.427669306, -0.427669306, -0.652537293, 0.709880342, 0.265075427);
-0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); Pose3 Pose1(R1, t1);
LieVector Vel1((Vector(3) << 0.5, -0.5, 0.4)); Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
0.609241153, 0.67099888, -0.422594037, -0.422594037, -0.636011287, 0.731761397, 0.244979388);
-0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); Vector3 Vel2(
(Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); Vector ActualErr(
factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual,
H2_actual, H3_actual, H4_actual, H5_actual));
// Checking for Pose part in the jacobians // Checking for Pose part in the jacobians
// ****** // ******
Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); Matrix H1_actualPose(H1_actual.block(0, 0, 6, H1_actual.cols()));
Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); Matrix H2_actualPose(H2_actual.block(0, 0, 6, H2_actual.cols()));
Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); Matrix H3_actualPose(H3_actual.block(0, 0, 6, H3_actual.cols()));
Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); Matrix H4_actualPose(H4_actual.block(0, 0, 6, H4_actual.cols()));
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
Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
H1_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H5_expectedPose;
H2_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
H4_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); Pose1);
H5_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); 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 // Verify they are equal for this choice of state
CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
@ -346,19 +334,30 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
// Checking for Vel part in the jacobians // Checking for Vel part in the jacobians
// ****** // ******
Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); Matrix H1_actualVel(H1_actual.block(6, 0, 3, H1_actual.cols()));
Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); Matrix H2_actualVel(H2_actual.block(6, 0, 3, H2_actual.cols()));
Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); Matrix H3_actualVel(H3_actual.block(6, 0, 3, H3_actual.cols()));
Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); Matrix H4_actualVel(H4_actual.block(6, 0, 3, H4_actual.cols()));
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
Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
H1_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H5_expectedVel;
H2_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); H1_expectedVel = numericalDerivative11<Vector, Pose3>(
H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
H4_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); Pose1);
H5_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); 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 // Verify they are equal for this choice of state
CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
@ -368,12 +367,7 @@ TEST (InertialNavFactor_GlobalVelocity, Jacobian ) {
CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5)); CHECK( assert_equal(H5_expectedVel, H5_actualVel, 1e-5));
} }
/* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform) {
/* ************************************************************************* */
TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
{
Key Pose1(11); Key Pose1(11);
Key Pose2(12); Key Pose2(12);
Key Vel1(21); Key Vel1(21);
@ -384,22 +378,18 @@ TEST( InertialNavFactor_GlobalVelocity, ConstructorWithTransform)
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
double measurement_dt(0.1); 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
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
measurement_dt, world_g, world_rho, world_omega_earth, model,
body_P_sensor);
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform) {
TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
{
Key Pose1(11); Key Pose1(11);
Key Pose2(12); Key Pose2(12);
Key Vel1(21); Key Vel1(21);
@ -410,24 +400,23 @@ TEST( InertialNavFactor_GlobalVelocity, EqualsWithTransform)
Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03)); Vector measurement_gyro((Vector(3) << -0.2, 0.5, 0.03));
double measurement_dt(0.1); 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
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(0.0, 0.0, 0.0)); // IMU is in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> g(Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); measurement_dt, world_g, world_rho, world_omega_earth, model,
body_P_sensor);
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> g(
Pose1, Vel1, Bias1, Pose2, Vel2, measurement_acc, measurement_gyro,
measurement_dt, world_g, world_rho, world_omega_earth, model,
body_P_sensor);
CHECK(assert_equal(f, g, 1e-5)); CHECK(assert_equal(f, g, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform) {
TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
{
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
Key VelKey1(21); Key VelKey1(21);
@ -435,39 +424,38 @@ TEST( InertialNavFactor_GlobalVelocity, PredictWithTransform)
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
// First test: zero angular motion, some acceleration // First test: zero angular motion, some acceleration
Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model, body_P_sensor);
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 expectedPose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector expectedVel2((Vector(3) << 0.51, -0.48, 0.43)); Vector3 expectedVel2((Vector(3) << 0.51, -0.48, 0.43));
Pose3 actualPose2; Pose3 actualPose2;
LieVector actualVel2; Vector3 actualVel2;
f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2); f.predict(Pose1, Vel1, Bias1, actualPose2, actualVel2);
CHECK(assert_equal(expectedPose2, actualPose2, 1e-5)); CHECK(assert_equal(expectedPose2, actualPose2, 1e-5));
CHECK(assert_equal(expectedVel2, actualVel2, 1e-5)); CHECK(assert_equal((Vector)expectedVel2, actualVel2, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform) {
TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
{
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
Key VelKey1(21); Key VelKey1(21);
@ -475,27 +463,28 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
// First test: zero angular motion, some acceleration // First test: zero angular motion, some acceleration
Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation Vector measurement_gyro((Vector(3) << 0.0, 0.0, 0.0)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc = (Vector(3) << 0.2, 0.1, -0.3 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model, body_P_sensor);
Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00)); Pose3 Pose1(Rot3(), Point3(2.00, 1.00, 3.00));
Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04)); Pose3 Pose2(Rot3(), Point3(2.05, 0.95, 3.04));
LieVector Vel1((Vector(3) << 0.50, -0.50, 0.40)); Vector3 Vel1((Vector(3) << 0.50, -0.50, 0.40));
LieVector Vel2((Vector(3) << 0.51, -0.48, 0.43)); Vector3 Vel2((Vector(3) << 0.51, -0.48, 0.43));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -504,9 +493,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorPosVelWithTransform)
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform) {
TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
{
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
Key VelKey1(21); Key VelKey1(21);
@ -514,27 +501,31 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
// Second test: zero angular motion, some acceleration // Second test: zero angular motion, some acceleration
Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0+9.81) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc = (Vector(3) << 0.0, 0.0, 0.0 + 9.81)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model, body_P_sensor);
Pose3 Pose1(Rot3(), Point3(2.0,1.0,3.0)); Pose3 Pose1(Rot3(), Point3(2.0, 1.0, 3.0));
Pose3 Pose2(Rot3::Expmap(body_P_sensor.rotation().matrix()*measurement_gyro*measurement_dt), Point3(2.0, 1.0, 3.0)); Pose3 Pose2(
LieVector Vel1((Vector(3) << 0.0,0.0,0.0)); Rot3::Expmap(
LieVector Vel2((Vector(3) << 0.0,0.0,0.0)); body_P_sensor.rotation().matrix() * measurement_gyro
* measurement_dt), Point3(2.0, 1.0, 3.0));
Vector3 Vel1((Vector(3) << 0.0, 0.0, 0.0));
Vector3 Vel2((Vector(3) << 0.0, 0.0, 0.0));
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -543,9 +534,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotWithTransform)
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform) {
TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
{
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
Key VelKey1(21); Key VelKey1(21);
@ -553,36 +542,40 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
// Second test: zero angular motion, some acceleration - generated in matlab // Second test: zero angular motion, some acceleration - generated in matlab
Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation Vector measurement_gyro((Vector(3) << 0.2, 0.1, -0.3)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc =
(Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> f(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model, body_P_sensor);
Rot3 R1(0.487316618, 0.125253866, 0.86419557, Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
0.580273724, 0.693095498, -0.427669306, -0.427669306, -0.652537293, 0.709880342, 0.265075427);
-0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0, 1.0, 3.0);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); Pose3 Pose1(R1, t1);
LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Vector3 Vel1((Vector(3) << 0.5, -0.5, 0.4));
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
0.609241153, 0.67099888, -0.422594037, -0.422594037, -0.636011287, 0.731761397, 0.244979388);
-0.636011287, 0.731761397, 0.244979388); Point3 t2 = t1.compose(Point3(Vel1 * measurement_dt));
Point3 t2 = t1.compose( Point3(Vel1*measurement_dt) );
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
Vector dv = measurement_dt * (R1.matrix() * body_P_sensor.rotation().matrix() * (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + world_g); Vector dv =
LieVector Vel2 = Vel1.compose( dv ); measurement_dt
* (R1.matrix() * body_P_sensor.rotation().matrix()
* (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343)
+ world_g);
Vector3 Vel2 = Vel1 + dv;
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2)); Vector ActualErr(f.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2));
@ -592,8 +585,7 @@ TEST( InertialNavFactor_GlobalVelocity, ErrorRotPosVelWithTransform)
CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5)); CHECK(assert_equal(ExpectedErr, ActualErr, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
Key PoseKey1(11); Key PoseKey1(11);
Key PoseKey2(12); Key PoseKey2(12);
@ -602,56 +594,68 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
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_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system
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)); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1));
Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation Pose3 body_P_sensor(Rot3(0, 1, 0, 1, 0, 0, 0, 0, -1), Point3(1.0, -2.0, 3.0)); // IMU is in ENU orientation
Vector measurement_gyro((Vector(3) << 3.14 / 2, 3.14, +3.14)); // Measured in ENU orientation
Vector measurement_gyro((Vector(3) << 3.14/2, 3.14, +3.14)); // Measured in ENU orientation
Matrix omega__cross = skewSymmetric(measurement_gyro); Matrix omega__cross = skewSymmetric(measurement_gyro);
Vector measurement_acc = (Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343) + omega__cross*omega__cross*body_P_sensor.rotation().inverse().matrix()*body_P_sensor.translation().vector(); // Measured in ENU orientation Vector measurement_acc =
(Vector(3) << -6.763926150509185, 6.501390843381716, +2.300389940090343)
+ omega__cross * omega__cross
* body_P_sensor.rotation().inverse().matrix()
* body_P_sensor.translation().vector(); // Measured in ENU orientation
InertialNavFactor_GlobalVelocity<Pose3, Vector3, imuBias::ConstantBias> factor(
PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc,
measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth,
model, body_P_sensor);
InertialNavFactor_GlobalVelocity<Pose3, LieVector, imuBias::ConstantBias> factor(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model, body_P_sensor); Rot3 R1(0.487316618, 0.125253866, 0.86419557, 0.580273724, 0.693095498,
-0.427669306, -0.652537293, 0.709880342, 0.265075427);
Rot3 R1(0.487316618, 0.125253866, 0.86419557, Point3 t1(2.0, 1.0, 3.0);
0.580273724, 0.693095498, -0.427669306,
-0.652537293, 0.709880342, 0.265075427);
Point3 t1(2.0,1.0,3.0);
Pose3 Pose1(R1, t1); Pose3 Pose1(R1, t1);
LieVector Vel1((Vector(3) << 0.5,-0.5,0.4)); Vector3 Vel1(0.5, -0.5, 0.4);
Rot3 R2(0.473618898, 0.119523052, 0.872582019, Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888,
0.609241153, 0.67099888, -0.422594037, -0.422594037, -0.636011287, 0.731761397, 0.244979388);
-0.636011287, 0.731761397, 0.244979388); Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Point3 t2(2.052670960415706, 0.977252139079380, 2.942482135362800);
Pose3 Pose2(R2, t2); Pose3 Pose2(R2, t2);
LieVector Vel2((Vector(3) << 0.510000000000000, -0.480000000000000, 0.430000000000000)); Vector3 Vel2(0.510000000000000, -0.480000000000000, 0.430000000000000);
imuBias::ConstantBias Bias1; imuBias::ConstantBias Bias1;
Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual; Matrix H1_actual, H2_actual, H3_actual, H4_actual, H5_actual;
Vector ActualErr(factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual, H2_actual, H3_actual, H4_actual, H5_actual)); Vector ActualErr(
factor.evaluateError(Pose1, Vel1, Bias1, Pose2, Vel2, H1_actual,
H2_actual, H3_actual, H4_actual, H5_actual));
// Checking for Pose part in the jacobians // Checking for Pose part in the jacobians
// ****** // ******
Matrix H1_actualPose(H1_actual.block(0,0,6,H1_actual.cols())); Matrix H1_actualPose(H1_actual.block(0, 0, 6, H1_actual.cols()));
Matrix H2_actualPose(H2_actual.block(0,0,6,H2_actual.cols())); Matrix H2_actualPose(H2_actual.block(0, 0, 6, H2_actual.cols()));
Matrix H3_actualPose(H3_actual.block(0,0,6,H3_actual.cols())); Matrix H3_actualPose(H3_actual.block(0, 0, 6, H3_actual.cols()));
Matrix H4_actualPose(H4_actual.block(0,0,6,H4_actual.cols())); Matrix H4_actualPose(H4_actual.block(0, 0, 6, H4_actual.cols()));
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
Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose, H5_expectedPose; Matrix H1_expectedPose, H2_expectedPose, H3_expectedPose, H4_expectedPose,
H1_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H5_expectedPose;
H2_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); H1_expectedPose = numericalDerivative11<Pose3, Pose3>(
H3_expectedPose = numericalDerivative11<Pose3, imuBias::ConstantBias>(boost::bind(&predictionErrorPose, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); boost::bind(&predictionErrorPose, _1, Vel1, Bias1, Pose2, Vel2, factor),
H4_expectedPose = numericalDerivative11<Pose3, Pose3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); Pose1);
H5_expectedPose = numericalDerivative11<Pose3, Vector3>(boost::bind(&predictionErrorPose, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); 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 // Verify they are equal for this choice of state
CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5)); CHECK( assert_equal(H1_expectedPose, H1_actualPose, 1e-5));
@ -662,19 +666,30 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
// Checking for Vel part in the jacobians // Checking for Vel part in the jacobians
// ****** // ******
Matrix H1_actualVel(H1_actual.block(6,0,3,H1_actual.cols())); Matrix H1_actualVel(H1_actual.block(6, 0, 3, H1_actual.cols()));
Matrix H2_actualVel(H2_actual.block(6,0,3,H2_actual.cols())); Matrix H2_actualVel(H2_actual.block(6, 0, 3, H2_actual.cols()));
Matrix H3_actualVel(H3_actual.block(6,0,3,H3_actual.cols())); Matrix H3_actualVel(H3_actual.block(6, 0, 3, H3_actual.cols()));
Matrix H4_actualVel(H4_actual.block(6,0,3,H4_actual.cols())); Matrix H4_actualVel(H4_actual.block(6, 0, 3, H4_actual.cols()));
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
Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel, H5_expectedVel; Matrix H1_expectedVel, H2_expectedVel, H3_expectedVel, H4_expectedVel,
H1_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor), Pose1); H5_expectedVel;
H2_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, _1, Bias1, Pose2, Vel2, factor), Vel1); H1_expectedVel = numericalDerivative11<Vector, Pose3>(
H3_expectedVel = numericalDerivative11<Vector, imuBias::ConstantBias>(boost::bind(&predictionErrorVel, Pose1, Vel1, _1, Pose2, Vel2, factor), Bias1); boost::bind(&predictionErrorVel, _1, Vel1, Bias1, Pose2, Vel2, factor),
H4_expectedVel = numericalDerivative11<Vector, Pose3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, _1, Vel2, factor), Pose2); Pose1);
H5_expectedVel = numericalDerivative11<Vector, Vector3>(boost::bind(&predictionErrorVel, Pose1, Vel1, Bias1, Pose2, _1, factor), Vel2); 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 // Verify they are equal for this choice of state
CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5)); CHECK( assert_equal(H1_expectedVel, H1_actualVel, 1e-5));
@ -685,5 +700,8 @@ TEST (InertialNavFactor_GlobalVelocity, JacobianWithTransform ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
View File

@ -25,7 +25,7 @@ static SharedNoiseModel sigma(noiseModel::Unit::Create(2));
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1)); Pose3 level_pose = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera level_camera(level_pose, *K); SimpleCamera level_camera(level_pose, *K);
typedef InvDepthFactor3<Pose3, LieVector, LieScalar> InverseDepthFactor; typedef InvDepthFactor3<Pose3, Vector5, double> InverseDepthFactor;
typedef NonlinearEquality<Pose3> PoseConstraint; typedef NonlinearEquality<Pose3> PoseConstraint;
/* ************************************************************************* */ /* ************************************************************************* */
@ -38,10 +38,10 @@ TEST( InvDepthFactor, optimize) {
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) << 0., 0., 1., 0., 0.)); Vector5 inv_landmark((Vector(5) << 0., 0., 1., 0., 0.));
// initialize inverse depth with "incorrect" depth of 1/4 // initialize inverse depth with "incorrect" depth of 1/4
// in reality this is 1/5, but initial depth is guessed // in reality this is 1/5, but initial depth is guessed
LieScalar inv_depth(1./4); double inv_depth(1./4);
gtsam::NonlinearFactorGraph graph; gtsam::NonlinearFactorGraph graph;
Values initial; Values initial;
@ -82,8 +82,8 @@ TEST( InvDepthFactor, optimize) {
Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); Values result2 = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize();
Point3 result2_lmk = InvDepthCamera3<Cal3_S2>::invDepthTo3D( Point3 result2_lmk = InvDepthCamera3<Cal3_S2>::invDepthTo3D(
result2.at<LieVector>(Symbol('l',1)), result2.at<Vector5>(Symbol('l',1)),
result2.at<LieScalar>(Symbol('d',1))); result2.at<double>(Symbol('d',1)));
EXPECT(assert_equal(landmark, result2_lmk, 1e-9)); EXPECT(assert_equal(landmark, result2_lmk, 1e-9));
// TODO: need to add priors to make this work with // TODO: need to add priors to make this work with

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

@ -45,7 +45,7 @@ TEST( InvDepthFactorVariant1, optimize) {
double theta = atan2(ray.y(), ray.x()); double theta = atan2(ray.y(), ray.x());
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
double rho = 1./ray.norm(); double rho = 1./ray.norm();
LieVector expected((Vector(6) << x, y, z, theta, phi, rho)); Vector6 expected((Vector(6) << x, y, z, theta, phi, rho));
@ -68,12 +68,12 @@ TEST( InvDepthFactorVariant1, optimize) {
Values values; Values values;
values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract((Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05))); values.insert(landmarkKey, Vector6(expected + (Vector(6) << -0.20, +0.20, -0.35, +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position // Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
LieVector actual = result.at<LieVector>(landmarkKey); Vector6 actual = result.at<Vector6>(landmarkKey);
// values.at<Pose3>(poseKey1).print("Pose1 Before:\n"); // values.at<Pose3>(poseKey1).print("Pose1 Before:\n");
@ -84,22 +84,22 @@ TEST( InvDepthFactorVariant1, optimize) {
// 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");
// cout << endl << endl; // cout << endl << endl;
// values.at<LieVector>(landmarkKey).print("Landmark Before:\n"); // values.at<Vector6>(landmarkKey).print("Landmark Before:\n");
// result.at<LieVector>(landmarkKey).print("Landmark After:\n"); // result.at<Vector6>(landmarkKey).print("Landmark After:\n");
// expected.print("Landmark Truth:\n"); // expected.print("Landmark Truth:\n");
// cout << endl << endl; // cout << endl << endl;
// Calculate world coordinates of landmark versions // Calculate world coordinates of landmark versions
Point3 world_landmarkBefore; Point3 world_landmarkBefore;
{ {
LieVector landmarkBefore = values.at<LieVector>(landmarkKey); Vector6 landmarkBefore = values.at<Vector6>(landmarkKey);
double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2); double x = landmarkBefore(0), y = landmarkBefore(1), z = landmarkBefore(2);
double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5); double theta = landmarkBefore(3), phi = landmarkBefore(4), rho = landmarkBefore(5);
world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); world_landmarkBefore = Point3(x, y, z) + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
} }
Point3 world_landmarkAfter; Point3 world_landmarkAfter;
{ {
LieVector landmarkAfter = result.at<LieVector>(landmarkKey); Vector6 landmarkAfter = result.at<Vector6>(landmarkKey);
double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2); double x = landmarkAfter(0), y = landmarkAfter(1), z = landmarkAfter(2);
double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5); double theta = landmarkAfter(3), phi = landmarkAfter(4), rho = landmarkAfter(5);
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);

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

@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant2, optimize) {
double theta = atan2(ray.y(), ray.x()); double theta = atan2(ray.y(), ray.x());
double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y())); double phi = atan2(ray.z(), sqrt(ray.x()*ray.x()+ray.y()*ray.y()));
double rho = 1./ray.norm(); double rho = 1./ray.norm();
LieVector expected((Vector(3) << theta, phi, rho)); Vector3 expected((Vector(3) << theta, phi, rho));
@ -66,12 +66,12 @@ TEST( InvDepthFactorVariant2, optimize) {
Values values; Values values;
values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position // Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
LieVector actual = result.at<LieVector>(landmarkKey); Vector3 actual = result.at<Vector3>(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");
@ -81,21 +81,21 @@ TEST( InvDepthFactorVariant2, optimize) {
// 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<Vector3>(landmarkKey).print("Landmark Before:\n");
// result.at<LieVector>(landmarkKey).print("Landmark After:\n"); // result.at<Vector3>(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;
{ {
LieVector landmarkBefore = values.at<LieVector>(landmarkKey); Vector3 landmarkBefore = values.at<Vector3>(landmarkKey);
double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2); double theta = landmarkBefore(0), phi = landmarkBefore(1), rho = landmarkBefore(2);
world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho); world_landmarkBefore = referencePoint + Point3(cos(theta)*cos(phi)/rho, sin(theta)*cos(phi)/rho, sin(phi)/rho);
} }
Point3 world_landmarkAfter; Point3 world_landmarkAfter;
{ {
LieVector landmarkAfter = result.at<LieVector>(landmarkKey); Vector3 landmarkAfter = result.at<Vector3>(landmarkKey);
double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2); double theta = landmarkAfter(0), phi = landmarkAfter(1), rho = landmarkAfter(2);
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);
} }
@ -106,7 +106,7 @@ TEST( InvDepthFactorVariant2, optimize) {
// 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((Vector)expected, actual, 1e-9));
} }

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

@ -43,7 +43,7 @@ TEST( InvDepthFactorVariant3, optimize) {
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();
LieVector expected((Vector(3) << theta, phi, rho)); Vector3 expected((Vector(3) << theta, phi, rho));
@ -66,17 +66,17 @@ TEST( InvDepthFactorVariant3, optimize) {
Values values; Values values;
values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30))); values.insert(poseKey1, pose1.retract((Vector(6) << +0.01, -0.02, +0.03, -0.10, +0.20, -0.30)));
values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30))); values.insert(poseKey2, pose2.retract((Vector(6) << +0.01, +0.02, -0.03, -0.10, +0.20, +0.30)));
values.insert(landmarkKey, expected.retract((Vector(3) << +0.02, -0.04, +0.05))); values.insert(landmarkKey, Vector3(expected + (Vector(3) << +0.02, -0.04, +0.05)));
// Optimize the graph to recover the actual landmark position // Optimize the graph to recover the actual landmark position
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize();
LieVector actual = result.at<LieVector>(landmarkKey); Vector3 actual = result.at<Vector3>(landmarkKey);
// 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((Vector)expected, actual, 1e-9));
} }

26
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactor.cpp
View File

@ -10,7 +10,6 @@
#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h> #include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactor.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
@ -23,26 +22,21 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
// to reenable the test.
//#if 0
/* ************************************************************************* */ /* ************************************************************************* */
LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor<gtsam::Pose2>& factor){ Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactor<gtsam::Pose2>& factor){
gtsam::Values values; gtsam::Values values;
values.insert(key, org1_T_org2); values.insert(key, org1_T_org2);
// LieVector err = factor.whitenedError(values); return factor.whitenedError(values);
// return err;
return LieVector::Expmap(factor.whitenedError(values));
} }
/* ************************************************************************* */ /* ************************************************************************* */
//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){ //Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
// gtsam::Values values; // gtsam::Values values;
// values.insert(keyA, p1); // values.insert(keyA, p1);
// values.insert(keyB, p2); // values.insert(keyB, p2);
// // LieVector err = factor.whitenedError(values); // // Vector err = factor.whitenedError(values);
// // return err; // // return err;
// return LieVector::Expmap(factor.whitenedError(values)); // return Vector::Expmap(factor.whitenedError(values));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
@ -237,7 +231,7 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
Matrix H1_actual = H_actual[0]; Matrix H1_actual = H_actual[0];
double stepsize = 1.0e-9; double stepsize = 1.0e-9;
Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize);
// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
} }
@ -291,12 +285,12 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
// //
// double stepsize = 1.0e-9; // double stepsize = 1.0e-9;
// Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); // Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); // Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
// //
// //
// // try to check numerical derivatives of a standard between factor // // try to check numerical derivatives of a standard between factor
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); // Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
// //
// //
@ -305,8 +299,6 @@ TEST( TransformBtwRobotsUnaryFactor, Jacobian)
// //
//} //}
//#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */

26
gtsam_unstable/slam/tests/testTransformBtwRobotsUnaryFactorEM.cpp
View File

@ -10,7 +10,6 @@
#include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h> #include <gtsam_unstable/slam/TransformBtwRobotsUnaryFactorEM.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
@ -23,26 +22,21 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below
// to reenable the test.
//#if 0
/* ************************************************************************* */ /* ************************************************************************* */
LieVector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM<gtsam::Pose2>& factor){ Vector predictionError(const Pose2& org1_T_org2, const gtsam::Key& key, const TransformBtwRobotsUnaryFactorEM<gtsam::Pose2>& factor){
gtsam::Values values; gtsam::Values values;
values.insert(key, org1_T_org2); values.insert(key, org1_T_org2);
// LieVector err = factor.whitenedError(values); return factor.whitenedError(values);
// return err;
return LieVector::Expmap(factor.whitenedError(values));
} }
/* ************************************************************************* */ /* ************************************************************************* */
//LieVector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){ //Vector predictionError_standard(const Pose2& p1, const Pose2& p2, const gtsam::Key& keyA, const gtsam::Key& keyB, const BetweenFactor<gtsam::Pose2>& factor){
// gtsam::Values values; // gtsam::Values values;
// values.insert(keyA, p1); // values.insert(keyA, p1);
// values.insert(keyB, p2); // values.insert(keyB, p2);
// // LieVector err = factor.whitenedError(values); // // Vector err = factor.whitenedError(values);
// // return err; // // return err;
// return LieVector::Expmap(factor.whitenedError(values)); // return Vector::Expmap(factor.whitenedError(values));
//} //}
/* ************************************************************************* */ /* ************************************************************************* */
@ -266,7 +260,7 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
Matrix H1_actual = H_actual[0]; Matrix H1_actual = H_actual[0];
double stepsize = 1.0e-9; double stepsize = 1.0e-9;
Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize); Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, key, g), orgA_T_orgB, stepsize);
// CHECK( assert_equal(H1_expected, H1_actual, 1e-5)); // CHECK( assert_equal(H1_expected, H1_actual, 1e-5));
} }
/////* ************************************************************************** */ /////* ************************************************************************** */
@ -316,12 +310,12 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
//// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8)); //// CHECK( assert_equal(H2_actual_stnd, H2_actual, 1e-8));
// //
// double stepsize = 1.0e-9; // double stepsize = 1.0e-9;
// Matrix H1_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize); // Matrix H1_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, _1, p2, keyA, keyB, f), p1, stepsize);
// Matrix H2_expected = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize); // Matrix H2_expected = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError, p1, _1, keyA, keyB, f), p2, stepsize);
// //
// //
// // try to check numerical derivatives of a standard between factor // // try to check numerical derivatives of a standard between factor
// Matrix H1_expected_stnd = gtsam::numericalDerivative11<LieVector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize); // Matrix H1_expected_stnd = gtsam::numericalDerivative11<Vector, Pose2>(boost::bind(&predictionError_standard, _1, p2, keyA, keyB, h), p1, stepsize);
// CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5)); // CHECK( assert_equal(H1_expected_stnd, H1_actual_stnd, 1e-5));
// //
// //
@ -330,8 +324,6 @@ TEST( TransformBtwRobotsUnaryFactorEM, Jacobian)
// //
//} //}
//#endif
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */

1
gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
using namespace std; using namespace std;

122
tests/testGaussianISAM2.cpp
View File

@ -6,24 +6,24 @@
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/debug.h> #include <tests/smallExample.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/base/LieVector.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/base/treeTraversal-inst.h> #include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/Values.h> #include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/Marginals.h> #include <gtsam/nonlinear/Marginals.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/slam/BearingRangeFactor.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <tests/smallExample.h> #include <gtsam/inference/Ordering.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/base/LieScalar.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
@ -35,6 +35,9 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using boost::shared_ptr; using boost::shared_ptr;
static const SharedNoiseModel model;
static const LieScalar Zero(0);
// SETDEBUG("ISAM2 update", true); // SETDEBUG("ISAM2 update", true);
// SETDEBUG("ISAM2 update verbose", true); // SETDEBUG("ISAM2 update verbose", true);
// SETDEBUG("ISAM2 recalculate", true); // SETDEBUG("ISAM2 recalculate", true);
@ -203,11 +206,11 @@ struct ConsistencyVisitor
consistent(true), isam(isam) {} consistent(true), isam(isam) {}
int operator()(const ISAM2::sharedClique& node, int& parentData) int operator()(const ISAM2::sharedClique& node, int& parentData)
{ {
if(std::find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end()) if(find(isam.roots().begin(), isam.roots().end(), node) == isam.roots().end())
{ {
if(node->parent_.expired()) if(node->parent_.expired())
consistent = false; consistent = false;
if(std::find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end()) if(find(node->parent()->children.begin(), node->parent()->children.end(), node) == node->parent()->children.end())
consistent = false; consistent = false;
} }
BOOST_FOREACH(Key j, node->conditional()->frontals()) BOOST_FOREACH(Key j, node->conditional()->frontals())
@ -223,7 +226,7 @@ struct ConsistencyVisitor
bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) { bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, const ISAM2& isam, Test& test, TestResult& result) {
TestResult& result_ = result; TestResult& result_ = result;
const std::string name_ = test.getName(); const string name_ = test.getName();
Values actual = isam.calculateEstimate(); Values actual = isam.calculateEstimate();
Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize()); Values expected = fullinit.retract(fullgraph.linearize(fullinit)->optimize());
@ -285,19 +288,19 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
TEST(ISAM2, AddFactorsStep1) TEST(ISAM2, AddFactorsStep1)
{ {
NonlinearFactorGraph nonlinearFactors; NonlinearFactorGraph nonlinearFactors;
nonlinearFactors += PriorFactor<LieVector>(10, LieVector(), gtsam::SharedNoiseModel()); nonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
nonlinearFactors += NonlinearFactor::shared_ptr(); nonlinearFactors += NonlinearFactor::shared_ptr();
nonlinearFactors += PriorFactor<LieVector>(11, LieVector(), gtsam::SharedNoiseModel()); nonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
NonlinearFactorGraph newFactors; NonlinearFactorGraph newFactors;
newFactors += PriorFactor<LieVector>(1, LieVector(), gtsam::SharedNoiseModel()); newFactors += PriorFactor<LieScalar>(1, Zero, model);
newFactors += PriorFactor<LieVector>(2, LieVector(), gtsam::SharedNoiseModel()); newFactors += PriorFactor<LieScalar>(2, Zero, model);
NonlinearFactorGraph expectedNonlinearFactors; NonlinearFactorGraph expectedNonlinearFactors;
expectedNonlinearFactors += PriorFactor<LieVector>(10, LieVector(), gtsam::SharedNoiseModel()); expectedNonlinearFactors += PriorFactor<LieScalar>(10, Zero, model);
expectedNonlinearFactors += PriorFactor<LieVector>(1, LieVector(), gtsam::SharedNoiseModel()); expectedNonlinearFactors += PriorFactor<LieScalar>(1, Zero, model);
expectedNonlinearFactors += PriorFactor<LieVector>(11, LieVector(), gtsam::SharedNoiseModel()); expectedNonlinearFactors += PriorFactor<LieScalar>(11, Zero, model);
expectedNonlinearFactors += PriorFactor<LieVector>(2, LieVector(), gtsam::SharedNoiseModel()); expectedNonlinearFactors += PriorFactor<LieScalar>(2, Zero, model);
const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3); const FastVector<size_t> expectedNewFactorIndices = list_of(1)(3);
@ -694,18 +697,17 @@ namespace {
TEST(ISAM2, marginalizeLeaves1) TEST(ISAM2, marginalizeLeaves1)
{ {
ISAM2 isam; ISAM2 isam;
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1)); factors += PriorFactor<LieScalar>(0, Zero, model);
factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
Values values; Values values;
values.insert(0, LieVector(0.0)); values.insert(0, Zero);
values.insert(1, LieVector(0.0)); values.insert(1, Zero);
values.insert(2, LieVector(0.0)); values.insert(2, Zero);
FastMap<Key,int> constrainedKeys; FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0)); constrainedKeys.insert(make_pair(0,0));
@ -724,18 +726,18 @@ TEST(ISAM2, marginalizeLeaves2)
ISAM2 isam; ISAM2 isam;
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1)); factors += PriorFactor<LieScalar>(0, Zero, model);
factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
factors += BetweenFactor<LieVector>(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(2, 3, Zero, model);
Values values; Values values;
values.insert(0, LieVector(0.0)); values.insert(0, Zero);
values.insert(1, LieVector(0.0)); values.insert(1, Zero);
values.insert(2, LieVector(0.0)); values.insert(2, Zero);
values.insert(3, LieVector(0.0)); values.insert(3, Zero);
FastMap<Key,int> constrainedKeys; FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0)); constrainedKeys.insert(make_pair(0,0));
@ -755,25 +757,25 @@ TEST(ISAM2, marginalizeLeaves3)
ISAM2 isam; ISAM2 isam;
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1)); factors += PriorFactor<LieScalar>(0, Zero, model);
factors += BetweenFactor<LieVector>(0, 1, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(0, 1, Zero, model);
factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
factors += BetweenFactor<LieVector>(2, 3, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(2, 3, Zero, model);
factors += BetweenFactor<LieVector>(3, 4, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(3, 4, Zero, model);
factors += BetweenFactor<LieVector>(4, 5, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(4, 5, Zero, model);
factors += BetweenFactor<LieVector>(3, 5, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(3, 5, Zero, model);
Values values; Values values;
values.insert(0, LieVector(0.0)); values.insert(0, Zero);
values.insert(1, LieVector(0.0)); values.insert(1, Zero);
values.insert(2, LieVector(0.0)); values.insert(2, Zero);
values.insert(3, LieVector(0.0)); values.insert(3, Zero);
values.insert(4, LieVector(0.0)); values.insert(4, Zero);
values.insert(5, LieVector(0.0)); values.insert(5, Zero);
FastMap<Key,int> constrainedKeys; FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0)); constrainedKeys.insert(make_pair(0,0));
@ -795,14 +797,14 @@ TEST(ISAM2, marginalizeLeaves4)
ISAM2 isam; ISAM2 isam;
NonlinearFactorGraph factors; NonlinearFactorGraph factors;
factors += PriorFactor<LieVector>(0, LieVector(0.0), noiseModel::Unit::Create(1)); factors += PriorFactor<LieScalar>(0, Zero, model);
factors += BetweenFactor<LieVector>(0, 2, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(0, 2, Zero, model);
factors += BetweenFactor<LieVector>(1, 2, LieVector(0.0), noiseModel::Unit::Create(1)); factors += BetweenFactor<LieScalar>(1, 2, Zero, model);
Values values; Values values;
values.insert(0, LieVector(0.0)); values.insert(0, Zero);
values.insert(1, LieVector(0.0)); values.insert(1, Zero);
values.insert(2, LieVector(0.0)); values.insert(2, Zero);
FastMap<Key,int> constrainedKeys; FastMap<Key,int> constrainedKeys;
constrainedKeys.insert(make_pair(0,0)); constrainedKeys.insert(make_pair(0,0));

45
tests/testNonlinearFactor.cpp
View File

@ -27,7 +27,6 @@
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/LieVector.h>
#include <tests/smallExample.h> #include <tests/smallExample.h>
#include <tests/simulated2D.h> #include <tests/simulated2D.h>
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
@ -235,13 +234,13 @@ TEST( NonlinearFactor, linearize_constraint2 )
} }
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor4 : public NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> { class TestFactor4 : public NoiseModelFactor4<double, double, double, double> {
public: public:
typedef NoiseModelFactor4<LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor4<double, double, double, double> Base;
TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {} TestFactor4() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4)) {}
virtual Vector virtual Vector
evaluateError(const LieVector& x1, const LieVector& x2, const LieVector& x3, const LieVector& x4, evaluateError(const double& x1, const double& x2, const double& x3, const double& x4,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H3 = boost::none,
@ -264,10 +263,10 @@ public:
TEST(NonlinearFactor, NoiseModelFactor4) { TEST(NonlinearFactor, NoiseModelFactor4) {
TestFactor4 tf; TestFactor4 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0))); tv.insert(X(1), double((1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0))); tv.insert(X(2), double((2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0))); tv.insert(X(3), double((3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0))); tv.insert(X(4), double((4.0)));
EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 10.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(25.0/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@ -283,9 +282,9 @@ TEST(NonlinearFactor, NoiseModelFactor4) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor5 : public NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> { class TestFactor5 : public NoiseModelFactor5<double, double, double, double, double> {
public: public:
typedef NoiseModelFactor5<LieVector, LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor5<double, double, double, double, double> Base;
TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {} TestFactor5() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5)) {}
virtual Vector virtual Vector
@ -310,11 +309,11 @@ public:
TEST(NonlinearFactor, NoiseModelFactor5) { TEST(NonlinearFactor, NoiseModelFactor5) {
TestFactor5 tf; TestFactor5 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0))); tv.insert(X(1), double((1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0))); tv.insert(X(2), double((2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0))); tv.insert(X(3), double((3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0))); tv.insert(X(4), double((4.0)));
tv.insert(X(5), LieVector((Vector(1) << 5.0))); tv.insert(X(5), double((5.0)));
EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 15.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(56.25/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));
@ -332,9 +331,9 @@ TEST(NonlinearFactor, NoiseModelFactor5) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
class TestFactor6 : public NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> { class TestFactor6 : public NoiseModelFactor6<double, double, double, double, double, double> {
public: public:
typedef NoiseModelFactor6<LieVector, LieVector, LieVector, LieVector, LieVector, LieVector> Base; typedef NoiseModelFactor6<double, double, double, double, double, double> Base;
TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {} TestFactor6() : Base(noiseModel::Diagonal::Sigmas((Vector(1) << 2.0)), X(1), X(2), X(3), X(4), X(5), X(6)) {}
virtual Vector virtual Vector
@ -362,12 +361,12 @@ public:
TEST(NonlinearFactor, NoiseModelFactor6) { TEST(NonlinearFactor, NoiseModelFactor6) {
TestFactor6 tf; TestFactor6 tf;
Values tv; Values tv;
tv.insert(X(1), LieVector((Vector(1) << 1.0))); tv.insert(X(1), double((1.0)));
tv.insert(X(2), LieVector((Vector(1) << 2.0))); tv.insert(X(2), double((2.0)));
tv.insert(X(3), LieVector((Vector(1) << 3.0))); tv.insert(X(3), double((3.0)));
tv.insert(X(4), LieVector((Vector(1) << 4.0))); tv.insert(X(4), double((4.0)));
tv.insert(X(5), LieVector((Vector(1) << 5.0))); tv.insert(X(5), double((5.0)));
tv.insert(X(6), LieVector((Vector(1) << 6.0))); tv.insert(X(6), double((6.0)));
EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv))); EXPECT(assert_equal((Vector(1) << 21.0), tf.unwhitenedError(tv)));
DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9); DOUBLES_EQUAL(110.25/2.0, tf.error(tv), 1e-9);
JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv))); JacobianFactor jf(*boost::dynamic_pointer_cast<JacobianFactor>(tf.linearize(tv)));