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

Get rid of LieVector

release/4.3a0
dellaert 2014-11-03 11:02:15 +01:00
parent e2c8e2620b
commit b5327673fb
21 changed files with 55 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

1
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>

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

@ -155,11 +155,11 @@ TEST( Values, update_element )
cfg.insert(key1, v1); cfg.insert(key1, v1);
CHECK(cfg.size() == 1); CHECK(cfg.size() == 1);
CHECK(assert_equal(v1, cfg.at<Vector3>(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<Vector3>(key1))); CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -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(assert_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(assert_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);
@ -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(assert_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));

9
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>
@ -89,9 +88,9 @@ public:
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<Vector3, 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<Vector3, 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 Vector3 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
Vector hx(9); Vector hx(9);
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 Vector3(hx);
} }
}; };

1
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>

2
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>

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 {

15
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
@ -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

1
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

1
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 {

1
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 {

1
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 {

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>

1
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>

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

@ -23,7 +23,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 <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
using namespace std; using namespace std;

1
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>

1
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>

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;

1
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>