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

Merge branch 'develop' of https://bitbucket.org/gtborg/gtsam into develop

release/4.3a0
Christian Forster 2015-10-02 15:48:37 +02:00
parent 2146eb1b45 4e5a5cc999
commit 8bbf07ebb1
67 changed files with 4438 additions and 3359 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
README.md
View File

@ -31,6 +31,7 @@ Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake:
@ -46,6 +47,6 @@ See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

4
examples/SFMExample_SmartFactor.cpp
View File

@ -58,7 +58,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
smartfactor->add(measurement, i, measurementNoise);
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph

4
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -54,7 +54,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(K));
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
@ -63,7 +63,7 @@ int main(int argc, char* argv[]) {
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
smartfactor->add(measurement, i, measurementNoise);
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph

71
gtsam.h
View File

@ -916,7 +916,7 @@ class StereoCamera {
// Standard Interface
gtsam::Pose3 pose() const;
double baseline() const;
gtsam::Cal3_S2Stereo* calibration() const;
gtsam::Cal3_S2Stereo calibration() const;
// Manifold
gtsam::StereoCamera retract(const Vector& d) const;
@ -2364,15 +2364,17 @@ class SmartProjectionParams {
template<CALIBRATION>
virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const CALIBRATION* K);
SmartProjectionPoseFactor(const CALIBRATION* K,
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(const CALIBRATION* K,
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor,
const gtsam::SmartProjectionParams& params);
void add(const gtsam::Point2& measured_i, size_t poseKey_i,
const gtsam::noiseModel::Base* noise_i);
void add(const gtsam::Point2& measured_i, size_t poseKey_i);
// enabling serialization functionality
//void serialize() const;
@ -2481,18 +2483,18 @@ class ConstantBias {
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class ImuFactorPreintegratedMeasurements {
class PreintegratedImuMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
// ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
PreintegratedImuMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
// PreintegratedImuMeasurements(const gtsam::PreintegratedImuMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol);
double deltaTij() const;
Matrix deltaRij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
@ -2505,25 +2507,24 @@ class ImuFactorPreintegratedMeasurements {
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const;
};
#include <gtsam/navigation/CombinedImuFactor.h>
class CombinedImuFactorPreintegratedMeasurements {
class PreintegratedCombinedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
PreintegratedCombinedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
@ -2531,7 +2532,7 @@ class CombinedImuFactorPreintegratedMeasurements {
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(
PreintegratedCombinedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
@ -2540,14 +2541,14 @@ class CombinedImuFactorPreintegratedMeasurements {
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
// CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// PreintegratedCombinedMeasurements(const gtsam::PreintegratedCombinedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, double tol);
double deltaTij() const;
Matrix deltaRij() const;
gtsam::Rot3 deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
@ -2560,53 +2561,51 @@ class CombinedImuFactorPreintegratedMeasurements {
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const;
};
#include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements {
class PreintegratedAhrsMeasurements {
// Standard Constructor
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance);
PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol);
// get Data
Matrix deltaRij() const;
gtsam::Rot3 deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
// Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
void resetIntegration() ;
};
virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const;
};

62
gtsam/base/ProductLieGroup.h
View File

@ -55,7 +55,7 @@ public:
traits<H>::Compose(this->second,other.second));
}
ProductLieGroup inverse() const {
return ProductLieGroup(this->first.inverse(), this->second.inverse());
return ProductLieGroup(traits<G>::Inverse(this->first), traits<H>::Inverse(this->second));
}
ProductLieGroup compose(const ProductLieGroup& g) const {
return (*this) * g;
@ -74,17 +74,21 @@ public:
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
typedef OptionalJacobian<dimension, dimension> ChartJacobian;
static ProductLieGroup Retract(const TangentVector& v) {
return ProductLieGroup::ChartAtOrigin::Retract(v);
ProductLieGroup retract(const TangentVector& v, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
if (H1||H2) throw std::runtime_error("ProductLieGroup::retract derivatives not implemented yet");
G g = traits<G>::Retract(this->first, v.template head<dimension1>());
H h = traits<H>::Retract(this->second, v.template tail<dimension2>());
return ProductLieGroup(g,h);
}
static TangentVector LocalCoordinates(const ProductLieGroup& g) {
return ProductLieGroup::ChartAtOrigin::Local(g);
}
ProductLieGroup retract(const TangentVector& v) const {
return compose(ProductLieGroup::ChartAtOrigin::Retract(v));
}
TangentVector localCoordinates(const ProductLieGroup& g) const {
return ProductLieGroup::ChartAtOrigin::Local(between(g));
TangentVector localCoordinates(const ProductLieGroup& g, //
ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) const {
if (H1||H2) throw std::runtime_error("ProductLieGroup::localCoordinates derivatives not implemented yet");
typename traits<G>::TangentVector v1 = traits<G>::Local(this->first, g.first);
typename traits<H>::TangentVector v2 = traits<H>::Local(this->second, g.second);
TangentVector v;
v << v1, v2;
return v;
}
/// @}
@ -147,51 +151,19 @@ public:
v << v1, v2;
return v;
}
struct ChartAtOrigin {
static TangentVector Local(const ProductLieGroup& m, ChartJacobian Hm = boost::none) {
return Logmap(m, Hm);
}
static ProductLieGroup Retract(const TangentVector& v, ChartJacobian Hv = boost::none) {
return Expmap(v, Hv);
}
};
ProductLieGroup expmap(const TangentVector& v) const {
return compose(ProductLieGroup::Expmap(v));
}
TangentVector logmap(const ProductLieGroup& g) const {
return ProductLieGroup::Logmap(between(g));
}
static ProductLieGroup Retract(const TangentVector& v, ChartJacobian H1) {
return ProductLieGroup::ChartAtOrigin::Retract(v,H1);
}
static TangentVector LocalCoordinates(const ProductLieGroup& g, ChartJacobian H1) {
return ProductLieGroup::ChartAtOrigin::Local(g,H1);
}
ProductLieGroup retract(const TangentVector& v, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
Jacobian D_g_v;
ProductLieGroup g = ProductLieGroup::ChartAtOrigin::Retract(v,H2 ? &D_g_v : 0);
ProductLieGroup h = compose(g,H1,H2);
if (H2) *H2 = (*H2) * D_g_v;
return h;
}
TangentVector localCoordinates(const ProductLieGroup& g, //
ChartJacobian H1, ChartJacobian H2 = boost::none) const {
ProductLieGroup h = between(g,H1,H2);
Jacobian D_v_h;
TangentVector v = ProductLieGroup::ChartAtOrigin::Local(h, (H1 || H2) ? &D_v_h : 0);
if (H1) *H1 = D_v_h * (*H1);
if (H2) *H2 = D_v_h * (*H2);
return v;
}
/// @}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<typename G, typename H>
struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<
ProductLieGroup<G, H> > {
};
struct traits<ProductLieGroup<G, H> > : internal::LieGroupTraits<ProductLieGroup<G, H> > {};
} // namespace gtsam

4
gtsam/base/Testable.h
View File

@ -123,8 +123,8 @@ namespace gtsam {
double tol_;
equals_star(double tol = 1e-9) : tol_(tol) {}
bool operator()(const boost::shared_ptr<V>& expected, const boost::shared_ptr<V>& actual) {
if (!actual || !expected) return false;
return (traits<V>::Equals(*actual,*expected, tol_));
if (!actual || !expected) return true;
return actual && expected && traits<V>::Equals(*actual,*expected, tol_);
}
};

2
gtsam/geometry/CameraSet.h
View File

@ -161,7 +161,7 @@ public:
for (size_t i = 0; i < m; i++) { // for each camera
const MatrixZD& Fi = Fs[i];
const Eigen::Matrix<double, 2, N> Ei_P = //
const Eigen::Matrix<double, ZDim, N> Ei_P = //
E.block(ZDim * i, 0, ZDim, N) * P;
// D = (Dx2) * ZDim

2
gtsam/geometry/Pose3.cpp
View File

@ -387,7 +387,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = I_3x3;
detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix(V * detWeighting * U.transpose()));
Rot3 R(Matrix3(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);
return Pose3(R, t);
}

47
gtsam/geometry/Rot3.h
View File

@ -85,11 +85,33 @@ namespace gtsam {
double R21, double R22, double R23,
double R31, double R32, double R33);
/** constructor from a rotation matrix */
Rot3(const Matrix3& R);
/**
* Constructor from a rotation matrix
* Version for generic matrices. Need casting to Matrix3
* in quaternion mode, since Eigen's quaternion doesn't
* allow assignment/construction from a generic matrix.
* See: http://stackoverflow.com/questions/27094132/cannot-understand-if-this-is-circular-dependency-or-clang#tab-top
*/
template<typename Derived>
inline Rot3(const Eigen::MatrixBase<Derived>& R) {
#ifdef GTSAM_USE_QUATERNIONS
quaternion_=Matrix3(R);
#else
rot_ = R;
#endif
}
/** constructor from a rotation matrix */
Rot3(const Matrix& R);
/**
* Constructor from a rotation matrix
* Overload version for Matrix3 to avoid casting in quaternion mode.
*/
inline Rot3(const Matrix3& R) {
#ifdef GTSAM_USE_QUATERNIONS
quaternion_=R;
#else
rot_ = R;
#endif
}
/** Constructor from a quaternion. This can also be called using a plain
* Vector, due to implicit conversion from Vector to Quaternion
@ -330,6 +352,17 @@ namespace gtsam {
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
/// operator* for Vector3
inline Vector3 operator*(const Vector3& v) const {
return rotate(Point3(v)).vector();
}
/// rotate for Vector3
Vector3 rotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
return rotate(Point3(v), H1, H2).vector();
}
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
@ -337,6 +370,12 @@ namespace gtsam {
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// unrotate for Vector3
Vector3 unrotate(const Vector3& v, OptionalJacobian<3, 3> H1 = boost::none,
OptionalJacobian<3, 3> H2 = boost::none) const {
return unrotate(Point3(v), H1, H2).vector();
}
/// @}
/// @name Group Action on Unit3
/// @{

18
gtsam/geometry/Rot3M.cpp
View File

@ -50,18 +50,6 @@ Rot3::Rot3(double R11, double R12, double R13,
R31, R32, R33;
}
/* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) {
rot_ = R;
}
/* ************************************************************************* */
Rot3::Rot3(const Matrix& R) {
if (R.rows()!=3 || R.cols()!=3)
throw invalid_argument("Rot3 constructor expects 3*3 matrix");
rot_ = R;
}
/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
}
@ -131,10 +119,8 @@ Matrix3 Rot3::transpose() const {
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1 || H2) {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
}
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
return Point3(rot_ * p.vector());
}

8
gtsam/geometry/Rot3Q.cpp
View File

@ -48,14 +48,6 @@ namespace gtsam {
R21, R22, R23,
R31, R32, R33).finished()) {}
/* ************************************************************************* */
Rot3::Rot3(const Matrix3& R) :
quaternion_(R) {}
/* ************************************************************************* */
Rot3::Rot3(const Matrix& R) :
quaternion_(Matrix3(R)) {}
/* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) :
quaternion_(q) {

4
gtsam/geometry/StereoCamera.h
View File

@ -66,8 +66,8 @@ public:
StereoCamera(const Pose3& leftCamPose, const Cal3_S2Stereo::shared_ptr K);
/// Return shared pointer to calibration
const Cal3_S2Stereo::shared_ptr calibration() const {
return K_;
const Cal3_S2Stereo& calibration() const {
return *K_;
}
/// @}

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

@ -30,7 +30,7 @@ using namespace gtsam;
GTSAM_CONCEPT_MANIFOLD_INST(CalibratedCamera)
// Camera situated at 0.5 meters high, looking down
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
static const Pose3 pose1((Matrix3() <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.

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

@ -182,8 +182,8 @@ TEST(Pose3, expmaps_galore_full)
xi = (Vector(6) << 0.2, 0.3, -0.8, 100.0, 120.0, -60.0).finished();
actual = Pose3::Expmap(xi);
EXPECT(assert_equal(expm<Pose3>(xi,10), actual,1e-5));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-6));
EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-6));
EXPECT(assert_equal(Agrawal06iros(xi), actual,1e-9));
EXPECT(assert_equal(xi, Pose3::Logmap(actual),1e-9));
}
/* ************************************************************************* */

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

@ -28,7 +28,7 @@ using namespace gtsam;
static const Cal3_S2 K(625, 625, 0, 0, 0);
static const Pose3 pose1((Matrix)(Matrix(3,3) <<
static const Pose3 pose1((Matrix3() <<
1., 0., 0.,
0.,-1., 0.,
0., 0.,-1.

106
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -18,9 +18,15 @@
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp>
@ -273,6 +279,106 @@ TEST( triangulation, onePose) {
TriangulationUnderconstrainedException);
}
//******************************************************************************
TEST( triangulation, StereotriangulateNonlinear ) {
Cal3_S2Stereo::shared_ptr stereoK(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612));
// two camera poses m1, m2
Matrix4 m1, m2;
m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779,
0.592783835, -0.77156583, 0.230856632, 66.2186159,
0.116517574, -0.201470143, -0.9725393, -4.28382528,
0, 0, 0, 1;
m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799,
-0.29277519, 0.947083213, 0.131587097, 65.843136,
-0.0206094928, 0.131334858, -0.991123524, -4.3525033,
0, 0, 0, 1;
typedef CameraSet<StereoCamera> Cameras;
Cameras cameras;
cameras.push_back(StereoCamera(Pose3(m1), stereoK));
cameras.push_back(StereoCamera(Pose3(m2), stereoK));
vector<StereoPoint2> measurements;
measurements += StereoPoint2(226.936, 175.212, 424.469);
measurements += StereoPoint2(339.571, 285.547, 669.973);
Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191
Point3 actual = triangulateNonlinear(cameras, measurements, initial);
Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187
EXPECT(assert_equal(expected, actual, 1e-4));
// regular stereo factor comparison - expect very similar result as above
{
typedef GenericStereoFactor<Pose3,Point3> StereoFactor;
Values values;
values.insert(Symbol('x', 1), Pose3(m1));
values.insert(Symbol('x', 2), Pose3(m2));
values.insert(Symbol('l', 1), initial);
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK)));
graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK)));
const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
graph.push_back(PriorFactor<Pose3>(Symbol('x',1), Pose3(m1), posePrior));
graph.push_back(PriorFactor<Pose3>(Symbol('x',2), Pose3(m2), posePrior));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
}
// use Triangulation Factor directly - expect same result as above
{
Values values;
values.insert(Symbol('l', 1), initial);
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.push_back(TriangulationFactor<StereoCamera>(cameras[0], measurements[0], unit, Symbol('l',1)));
graph.push_back(TriangulationFactor<StereoCamera>(cameras[1], measurements[1], unit, Symbol('l',1)));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
}
// use ExpressionFactor - expect same result as above
{
Values values;
values.insert(Symbol('l', 1), initial);
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
Expression<Point3> point_(Symbol('l',1));
Expression<StereoCamera> camera0_(cameras[0]);
Expression<StereoCamera> camera1_(cameras[1]);
Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, point_);
Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, point_);
graph.addExpressionFactor(unit, measurements[0], project0_);
graph.addExpressionFactor(unit, measurements[1], project1_);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4));
}
}
//******************************************************************************
int main() {
TestResult tr;

19
gtsam/geometry/triangulation.h
View File

@ -68,7 +68,7 @@ GTSAM_EXPORT Point3 triangulateDLT(
/**
* Create a factor graph with projection factors from poses and one calibration
* @param poses Camera poses
* @param sharedCal shared pointer to single calibration object
* @param sharedCal shared pointer to single calibration object (monocular only!)
* @param measurements 2D measurements
* @param landmarkKey to refer to landmark
* @param initialEstimate
@ -97,7 +97,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
/**
* Create a factor graph with projection factors from pinhole cameras
* (each camera has a pose and calibration)
* @param cameras pinhole cameras
* @param cameras pinhole cameras (monocular or stereo)
* @param measurements 2D measurements
* @param landmarkKey to refer to landmark
* @param initialEstimate
@ -106,22 +106,21 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
template<class CAMERA>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<CAMERA>& cameras,
const std::vector<Point2>& measurements, Key landmarkKey,
const std::vector<typename CAMERA::Measurement>& measurements, Key landmarkKey,
const Point3& initialEstimate) {
Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
static SharedNoiseModel unit(noiseModel::Unit::Create(CAMERA::Measurement::dimension));
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.push_back(TriangulationFactor<CAMERA> //
(camera_i, measurements[i], unit2, landmarkKey));
(camera_i, measurements[i], unit, landmarkKey));
}
return std::make_pair(graph, values);
}
/// PinholeCamera specific version
/// PinholeCamera specific version // TODO: (chris) why does this exist?
template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,
@ -165,7 +164,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
/**
* Given an initial estimate , refine a point using measurements in several cameras
* @param cameras pinhole cameras
* @param cameras pinhole cameras (monocular or stereo)
* @param measurements 2D measurements
* @param initialEstimate
* @return refined Point3
@ -173,7 +172,7 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
template<class CAMERA>
Point3 triangulateNonlinear(
const std::vector<CAMERA>& cameras,
const std::vector<Point2>& measurements, const Point3& initialEstimate) {
const std::vector<typename CAMERA::Measurement>& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values
Values values;
@ -184,7 +183,7 @@ Point3 triangulateNonlinear(
return optimize(graph, values, Symbol('p', 0));
}
/// PinholeCamera specific version
/// PinholeCamera specific version // TODO: (chris) why does this exist?
template<class CALIBRATION>
Point3 triangulateNonlinear(
const std::vector<PinholeCamera<CALIBRATION> >& cameras,

62
gtsam/linear/NoiseModel.cpp
View File

@ -696,7 +696,7 @@ Huber::Huber(double k, const ReweightScheme reweight)
}
double Huber::weight(double error) const {
return (error < k_) ? (1.0) : (k_ / fabs(error));
return (fabs(error) > k_) ? k_ / fabs(error) : 1.0;
}
void Huber::print(const std::string &s="") const {
@ -799,6 +799,66 @@ Welsh::shared_ptr Welsh::Create(double c, const ReweightScheme reweight) {
return shared_ptr(new Welsh(c, reweight));
}
/* ************************************************************************* */
// GemanMcClure
/* ************************************************************************* */
GemanMcClure::GemanMcClure(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
}
double GemanMcClure::weight(double error) const {
const double c2 = c_*c_;
const double c4 = c2*c2;
const double c2error = c2 + error*error;
return c4/(c2error*c2error);
}
void GemanMcClure::print(const std::string &s="") const {
std::cout << s << ": Geman-McClure (" << c_ << ")" << std::endl;
}
bool GemanMcClure::equals(const Base &expected, double tol) const {
const GemanMcClure* p = dynamic_cast<const GemanMcClure*>(&expected);
if (p == NULL) return false;
return fabs(c_ - p->c_) < tol;
}
GemanMcClure::shared_ptr GemanMcClure::Create(double c, const ReweightScheme reweight) {
return shared_ptr(new GemanMcClure(c, reweight));
}
/* ************************************************************************* */
// DCS
/* ************************************************************************* */
DCS::DCS(double c, const ReweightScheme reweight)
: Base(reweight), c_(c) {
}
double DCS::weight(double error) const {
const double e2 = error*error;
if (e2 > c_)
{
const double w = 2.0*c_/(c_ + e2);
return w*w;
}
return 1.0;
}
void DCS::print(const std::string &s="") const {
std::cout << s << ": DCS (" << c_ << ")" << std::endl;
}
bool DCS::equals(const Base &expected, double tol) const {
const DCS* p = dynamic_cast<const DCS*>(&expected);
if (p == NULL) return false;
return fabs(c_ - p->c_) < tol;
}
DCS::shared_ptr DCS::Create(double c, const ReweightScheme reweight) {
return shared_ptr(new DCS(c, reweight));
}
} // namespace mEstimator
/* ************************************************************************* */

59
gtsam/linear/NoiseModel.h
View File

@ -823,6 +823,65 @@ namespace gtsam {
}
};
/// GemanMcClure implements the "Geman-McClure" robust error model
/// (Zhang97ivc).
///
/// Note that Geman-McClure weight function uses the parameter c == 1.0,
/// but here it's allowed to use different values, so we actually have
/// the generalized Geman-McClure from (Agarwal15phd).
class GTSAM_EXPORT GemanMcClure : public Base {
public:
typedef boost::shared_ptr<GemanMcClure> shared_ptr;
GemanMcClure(double c = 1.0, const ReweightScheme reweight = Block);
virtual ~GemanMcClure() {}
virtual double weight(double error) const;
virtual void print(const std::string &s) const;
virtual bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected:
double c_;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(c_);
}
};
/// DCS implements the Dynamic Covariance Scaling robust error model
/// from the paper Robust Map Optimization (Agarwal13icra).
///
/// Under the special condition of the parameter c == 1.0 and not
/// forcing the output weight s <= 1.0, DCS is similar to Geman-McClure.
class GTSAM_EXPORT DCS : public Base {
public:
typedef boost::shared_ptr<DCS> shared_ptr;
DCS(double c = 1.0, const ReweightScheme reweight = Block);
virtual ~DCS() {}
virtual double weight(double error) const;
virtual void print(const std::string &s) const;
virtual bool equals(const Base& expected, double tol=1e-8) const;
static shared_ptr Create(double k, const ReweightScheme reweight = Block) ;
protected:
double c_;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(c_);
}
};
} ///\namespace mEstimator
/// Base class for robust error models

5
gtsam/linear/Sampler.cpp
View File

@ -14,9 +14,6 @@
* @author Alex Cunningham
*/
#include <boost/random/normal_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <gtsam/linear/Sampler.h>
namespace gtsam {
@ -51,7 +48,7 @@ Vector Sampler::sampleDiagonal(const Vector& sigmas) {
} else {
typedef boost::normal_distribution<double> Normal;
Normal dist(0.0, sigma);
boost::variate_generator<boost::minstd_rand&, Normal> norm(generator_, dist);
boost::variate_generator<boost::mt19937_64&, Normal> norm(generator_, dist);
result(i) = norm();
}
}

4
gtsam/linear/Sampler.h
View File

@ -20,7 +20,7 @@
#include <gtsam/linear/NoiseModel.h>
#include <boost/random/linear_congruential.hpp>
#include <boost/random.hpp>
namespace gtsam {
@ -37,7 +37,7 @@ protected:
noiseModel::Diagonal::shared_ptr model_;
/** generator */
boost::minstd_rand generator_;
boost::mt19937_64 generator_;
public:
typedef boost::shared_ptr<Sampler> shared_ptr;

77
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -322,7 +322,7 @@ TEST(NoiseModel, WhitenInPlace)
}
/* ************************************************************************* */
TEST(NoiseModel, robustFunction)
TEST(NoiseModel, robustFunctionHuber)
{
const double k = 5.0, error1 = 1.0, error2 = 10.0;
const mEstimator::Huber::shared_ptr huber = mEstimator::Huber::Create(k);
@ -332,8 +332,28 @@ TEST(NoiseModel, robustFunction)
DOUBLES_EQUAL(0.5, weight2, 1e-8);
}
TEST(NoiseModel, robustFunctionGemanMcClure)
{
const double k = 1.0, error1 = 1.0, error2 = 10.0;
const mEstimator::GemanMcClure::shared_ptr gmc = mEstimator::GemanMcClure::Create(k);
const double weight1 = gmc->weight(error1),
weight2 = gmc->weight(error2);
DOUBLES_EQUAL(0.25 , weight1, 1e-8);
DOUBLES_EQUAL(9.80296e-5, weight2, 1e-8);
}
TEST(NoiseModel, robustFunctionDCS)
{
const double k = 1.0, error1 = 1.0, error2 = 10.0;
const mEstimator::DCS::shared_ptr dcs = mEstimator::DCS::Create(k);
const double weight1 = dcs->weight(error1),
weight2 = dcs->weight(error2);
DOUBLES_EQUAL(1.0 , weight1, 1e-8);
DOUBLES_EQUAL(0.00039211, weight2, 1e-8);
}
/* ************************************************************************* */
TEST(NoiseModel, robustNoise)
TEST(NoiseModel, robustNoiseHuber)
{
const double k = 10.0, error1 = 1.0, error2 = 100.0;
Matrix A = (Matrix(2, 2) << 1.0, 10.0, 100.0, 1000.0).finished();
@ -342,7 +362,7 @@ TEST(NoiseModel, robustNoise)
mEstimator::Huber::Create(k, mEstimator::Huber::Scalar),
Unit::Create(2));
robust->WhitenSystem(A,b);
robust->WhitenSystem(A, b);
DOUBLES_EQUAL(error1, b(0), 1e-8);
DOUBLES_EQUAL(sqrt(k*error2), b(1), 1e-8);
@ -353,6 +373,57 @@ TEST(NoiseModel, robustNoise)
DOUBLES_EQUAL(sqrt(k/100.0)*1000.0, A(1,1), 1e-8);
}
TEST(NoiseModel, robustNoiseGemanMcClure)
{
const double k = 1.0, error1 = 1.0, error2 = 100.0;
const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
Vector b = Vector2(error1, error2);
const Robust::shared_ptr robust = Robust::Create(
mEstimator::GemanMcClure::Create(k, mEstimator::GemanMcClure::Scalar),
Unit::Create(2));
robust->WhitenSystem(A, b);
const double k2 = k*k;
const double k4 = k2*k2;
const double k2error = k2 + error2*error2;
const double sqrt_weight_error1 = sqrt(0.25);
const double sqrt_weight_error2 = sqrt(k4/(k2error*k2error));
DOUBLES_EQUAL(sqrt_weight_error1*error1, b(0), 1e-8);
DOUBLES_EQUAL(sqrt_weight_error2*error2, b(1), 1e-8);
DOUBLES_EQUAL(sqrt_weight_error1*a00, A(0,0), 1e-8);
DOUBLES_EQUAL(sqrt_weight_error1*a01, A(0,1), 1e-8);
DOUBLES_EQUAL(sqrt_weight_error2*a10, A(1,0), 1e-8);
DOUBLES_EQUAL(sqrt_weight_error2*a11, A(1,1), 1e-8);
}
TEST(NoiseModel, robustNoiseDCS)
{
const double k = 1.0, error1 = 1.0, error2 = 100.0;
const double a00 = 1.0, a01 = 10.0, a10 = 100.0, a11 = 1000.0;
Matrix A = (Matrix(2, 2) << a00, a01, a10, a11).finished();
Vector b = Vector2(error1, error2);
const Robust::shared_ptr robust = Robust::Create(
mEstimator::DCS::Create(k, mEstimator::DCS::Scalar),
Unit::Create(2));
robust->WhitenSystem(A, b);
const double sqrt_weight = 2.0*k/(k + error2*error2);
DOUBLES_EQUAL(error1, b(0), 1e-8);
DOUBLES_EQUAL(sqrt_weight*error2, b(1), 1e-8);
DOUBLES_EQUAL(a00, A(0,0), 1e-8);
DOUBLES_EQUAL(a01, A(0,1), 1e-8);
DOUBLES_EQUAL(sqrt_weight*a10, A(1,0), 1e-8);
DOUBLES_EQUAL(sqrt_weight*a11, A(1,1), 1e-8);
}
/* ************************************************************************* */
#define TEST_GAUSSIAN(gaussian)\
EQUALITY(info, gaussian->information());\

150
gtsam/navigation/AHRSFactor.cpp
View File

@ -27,83 +27,50 @@ namespace gtsam {
//------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const Vector3& bias, const Matrix3& measuredOmegaCovariance) :
PreintegratedRotation(measuredOmegaCovariance), biasHat_(bias)
{
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
AHRSFactor::PreintegratedMeasurements::PreintegratedMeasurements() :
PreintegratedRotation(I_3x3), biasHat_(Vector3())
{
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::print(const string& s) const {
void PreintegratedAhrsMeasurements::print(const string& s) const {
PreintegratedRotation::print(s);
cout << "biasHat [" << biasHat_.transpose() << "]" << endl;
cout << " PreintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}
//------------------------------------------------------------------------------
bool AHRSFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& other, double tol) const {
return PreintegratedRotation::equals(other, tol)
&& equal_with_abs_tol(biasHat_, other.biasHat_, tol);
bool PreintegratedAhrsMeasurements::equals(
const PreintegratedAhrsMeasurements& other, double tol) const {
return PreintegratedRotation::equals(other, tol) &&
equal_with_abs_tol(biasHat_, other.biasHat_, tol);
}
//------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::resetIntegration() {
void PreintegratedAhrsMeasurements::resetIntegration() {
PreintegratedRotation::resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void AHRSFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor) {
void PreintegratedAhrsMeasurements::integrateMeasurement(
const Vector3& measuredOmega, double deltaT) {
// First we compensate the measurements for the bias
Vector3 correctedOmega = measuredOmega - biasHat_;
// Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame
if (body_P_sensor) {
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
// rotation rate vector in the body frame
correctedOmega = body_R_sensor * correctedOmega;
}
// rotation vector describing rotation increment computed from the
// current rotation rate measurement
const Vector3 theta_incr = correctedOmega * deltaT;
Matrix3 D_Rincr_integratedOmega;
const Rot3 incrR = Rot3::Expmap(theta_incr, D_Rincr_integratedOmega); // expensive !!
// Update Jacobian
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
// Update rotation and deltaTij.
Matrix3 Fr; // Jacobian of the update
updateIntegratedRotationAndDeltaT(incrR, deltaT, Fr);
Matrix3 D_incrR_integratedOmega, Fr;
PreintegratedRotation::integrateMeasurement(measuredOmega,
biasHat_, deltaT, &D_incrR_integratedOmega, &Fr);
// first order uncertainty propagation
// the deltaT allows to pass from continuous time noise to discrete time noise
preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose()
+ gyroscopeCovariance() * deltaT;
preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT;
}
//------------------------------------------------------------------------------
Vector3 AHRSFactor::PreintegratedMeasurements::predict(const Vector3& bias,
boost::optional<Matrix&> H) const {
Vector3 PreintegratedAhrsMeasurements::predict(const Vector3& bias,
OptionalJacobian<3,3> H) const {
const Vector3 biasOmegaIncr = bias - biasHat_;
return biascorrectedThetaRij(biasOmegaIncr, H);
const Rot3 biascorrected = biascorrectedDeltaRij(biasOmegaIncr, H);
Matrix3 D_omega_biascorrected;
const Vector3 omega = Rot3::Logmap(biascorrected, H ? &D_omega_biascorrected : 0);
if (H) (*H) = D_omega_biascorrected * (*H);
return omega;
}
//------------------------------------------------------------------------------
Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
Vector PreintegratedAhrsMeasurements::DeltaAngles(
const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles) {
@ -121,22 +88,16 @@ Vector AHRSFactor::PreintegratedMeasurements::DeltaAngles(
//------------------------------------------------------------------------------
// AHRSFactor methods
//------------------------------------------------------------------------------
AHRSFactor::AHRSFactor() :
_PIM_(Vector3(), Z_3x3) {
}
AHRSFactor::AHRSFactor(
Key rot_i, Key rot_j, Key bias,
const PreintegratedAhrsMeasurements& preintegratedMeasurements)
: Base(noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_),
rot_i, rot_j, bias),
_PIM_(preintegratedMeasurements) {}
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), rot_i, rot_j, bias), _PIM_(
preintegratedMeasurements), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor) {
}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr AHRSFactor::clone() const {
//------------------------------------------------------------------------------
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}
@ -147,20 +108,13 @@ void AHRSFactor::print(const string& s,
cout << s << "AHRSFactor(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ",";
_PIM_.print(" preintegrated measurements:");
cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]" << endl;
noiseModel_->print(" noise model: ");
if (body_P_sensor_)
body_P_sensor_->print(" sensor pose in body frame: ");
}
//------------------------------------------------------------------------------
bool AHRSFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_)
|| (body_P_sensor_ && e->body_P_sensor_
&& body_P_sensor_->equals(*e->body_P_sensor_)));
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
}
//------------------------------------------------------------------------------
@ -172,8 +126,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
const Vector3 biascorrectedOmega = _PIM_.predict(bias, H3);
// Coriolis term
const Vector3 coriolis = _PIM_.integrateCoriolis(Ri, omegaCoriolis_);
const Matrix3 coriolisHat = skewSymmetric(coriolis);
const Vector3 coriolis = _PIM_.integrateCoriolis(Ri);
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
// Prediction
@ -191,7 +144,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
if (H1) {
// dfR/dRi
H1->resize(3, 3);
Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat;
Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
(*H1)
<< D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis);
}
@ -199,7 +152,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
if (H2) {
// dfR/dPosej
H2->resize(3, 3);
(*H2) << D_fR_fRrot * Matrix3::Identity();
(*H2) << D_fR_fRrot;
}
if (H3) {
@ -215,15 +168,13 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
}
//------------------------------------------------------------------------------
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor) {
Rot3 AHRSFactor::Predict(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements preintegratedMeasurements) {
const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
// Coriolis term
const Vector3 coriolis = //
preintegratedMeasurements.integrateCoriolis(rot_i, omegaCoriolis);
const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i);
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
@ -231,4 +182,31 @@ Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
return rot_i.compose(correctedDeltaRij);
}
} //namespace gtsam
//------------------------------------------------------------------------------
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias),
_PIM_(pim) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->body_P_sensor = body_P_sensor;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
PreintegratedMeasurements newPim = pim;
newPim.p_ = p;
return Predict(rot_i, bias, newPim);
}
} // namespace gtsam

177
gtsam/navigation/AHRSFactor.h
View File

@ -26,91 +26,94 @@
namespace gtsam {
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
public:
/**
* PreintegratedAHRSMeasurements accumulates (integrates) the Gyroscope
* measurements (rotation rates) and the corresponding covariance matrix.
* Can be built incrementally so as to avoid costly integration at time of factor construction.
*/
class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation {
protected:
Vector3 biasHat_; ///< Angular rate bias values used during preintegration.
Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
/// Default constructor, only for serialization
PreintegratedAhrsMeasurements() {}
friend class AHRSFactor;
public:
/**
* CombinedPreintegratedMeasurements accumulates (integrates) the Gyroscope
* measurements (rotation rates) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated AHRS factor.
* Can be built incrementally so as to avoid costly integration at time of
* factor construction.
* Default constructor, initialize with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
*/
class GTSAM_EXPORT PreintegratedMeasurements : public PreintegratedRotation {
PreintegratedAhrsMeasurements(const boost::shared_ptr<Params>& p,
const Vector3& biasHat) :
PreintegratedRotation(p), biasHat_(biasHat) {
resetIntegration();
}
friend class AHRSFactor;
const Params& p() const { return *boost::static_pointer_cast<const Params>(p_);}
const Vector3& biasHat() const { return biasHat_; }
const Matrix3& preintMeasCov() const { return preintMeasCov_; }
protected:
Vector3 biasHat_; ///< Acceleration and angular rate bias values used during preintegration. Note that we won't be using the accelerometer
Matrix3 preintMeasCov_; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
/// print
void print(const std::string& s = "Preintegrated Measurements: ") const;
public:
/// equals
bool equals(const PreintegratedAhrsMeasurements&, double tol = 1e-9) const;
/// Default constructor
PreintegratedMeasurements();
/// Reset inetgrated quantities to zero
void resetIntegration();
/**
* Default constructor, initialize with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param measuredOmegaCovariance Covariance matrix of measured angular rate
*/
PreintegratedMeasurements(const Vector3& bias,
const Matrix3& measuredOmegaCovariance);
/**
* Add a single Gyroscope measurement to the preintegration.
* @param measureOmedga Measured angular velocity (in body frame)
* @param deltaT Time step
*/
void integrateMeasurement(const Vector3& measuredOmega, double deltaT);
Vector3 biasHat() const {
return biasHat_;
}
const Matrix3& preintMeasCov() const {
return preintMeasCov_;
}
/// Predict bias-corrected incremental rotation
/// TODO: The matrix Hbias is the derivative of predict? Unit-test?
Vector3 predict(const Vector3& bias, OptionalJacobian<3,3> H = boost::none) const;
/// print
void print(const std::string& s = "Preintegrated Measurements: ") const;
// This function is only used for test purposes
// (compare numerical derivatives wrt analytic ones)
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles);
/// equals
bool equals(const PreintegratedMeasurements&, double tol = 1e-9) const;
/// TODO: Document
void resetIntegration();
/**
* Add a single Gyroscope measurement to the preintegration.
* @param measureOmedga Measured angular velocity (in body frame)
* @param deltaT Time step
* @param body_P_sensor Optional sensor frame
*/
void integrateMeasurement(const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none);
/// Predict bias-corrected incremental rotation
/// TODO: The matrix Hbias is the derivative of predict? Unit-test?
Vector3 predict(const Vector3& bias, boost::optional<Matrix&> H =
boost::none) const;
// This function is only used for test purposes
// (compare numerical derivatives wrt analytic ones)
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
}
};
/// @deprecated constructor
PreintegratedAhrsMeasurements(const Vector3& biasHat,
const Matrix3& measuredOmegaCovariance)
: PreintegratedRotation(boost::make_shared<Params>()),
biasHat_(biasHat) {
p_->gyroscopeCovariance = measuredOmegaCovariance;
resetIntegration();
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
}
};
class GTSAM_EXPORT AHRSFactor: public NoiseModelFactor3<Rot3, Rot3, Vector3> {
typedef AHRSFactor This;
typedef NoiseModelFactor3<Rot3, Rot3, Vector3> Base;
PreintegratedMeasurements _PIM_;
Vector3 gravity_;
Vector3 omegaCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
PreintegratedAhrsMeasurements _PIM_;
/** Default constructor - only use for serialization */
AHRSFactor() {}
public:
@ -121,22 +124,15 @@ public:
typedef boost::shared_ptr<AHRSFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
AHRSFactor();
/**
* Constructor
* @param rot_i previous rot key
* @param rot_j current rot key
* @param bias previous bias key
* @param preintegratedMeasurements preintegrated measurements
* @param omegaCoriolis rotation rate of the inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
*/
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none);
const PreintegratedAhrsMeasurements& preintegratedMeasurements);
virtual ~AHRSFactor() {
}
@ -152,14 +148,10 @@ public:
virtual bool equals(const NonlinearFactor&, double tol = 1e-9) const;
/// Access the preintegrated measurements.
const PreintegratedMeasurements& preintegratedMeasurements() const {
const PreintegratedAhrsMeasurements& preintegratedMeasurements() const {
return _PIM_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
/** implement functions needed to derive from Factor */
/// vector of errors
@ -169,10 +161,25 @@ public:
boost::none) const;
/// predicted states from IMU
/// TODO(frank): relationship with PIM predict ??
static Rot3 Predict(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements preintegratedMeasurements);
/// @deprecated name
typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
/// @deprecated constructor
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
/// @deprecated static function
static Rot3 predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none);
const boost::optional<Pose3>& body_P_sensor = boost::none);
private:
@ -184,13 +191,9 @@ private:
& boost::serialization::make_nvp("NoiseModelFactor3",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
};
// AHRSFactor
typedef AHRSFactor::PreintegratedMeasurements AHRSFactorPreintegratedMeasurements;
} //namespace gtsam

243
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -29,157 +29,125 @@ namespace gtsam {
using namespace std;
//------------------------------------------------------------------------------
// Inner class CombinedPreintegratedMeasurements
// Inner class PreintegratedCombinedMeasurements
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedPreintegratedMeasurements::CombinedPreintegratedMeasurements(
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix& biasAccOmegaInit,
const bool use2ndOrderIntegration) :
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration), biasAccCovariance_(
biasAccCovariance), biasOmegaCovariance_(biasOmegaCovariance), biasAccOmegaInit_(
biasAccOmegaInit) {
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::print(
void PreintegratedCombinedMeasurements::print(
const string& s) const {
PreintegrationBase::print(s);
cout << " biasAccCovariance [ " << biasAccCovariance_ << " ]" << endl;
cout << " biasOmegaCovariance [ " << biasOmegaCovariance_ << " ]" << endl;
cout << " biasAccOmegaInit [ " << biasAccOmegaInit_ << " ]" << endl;
cout << " preintMeasCov [ " << preintMeasCov_ << " ]" << endl;
}
//------------------------------------------------------------------------------
bool CombinedImuFactor::CombinedPreintegratedMeasurements::equals(
const CombinedPreintegratedMeasurements& expected, double tol) const {
return equal_with_abs_tol(biasAccCovariance_, expected.biasAccCovariance_,
tol)
&& equal_with_abs_tol(biasOmegaCovariance_, expected.biasOmegaCovariance_,
tol)
&& equal_with_abs_tol(biasAccOmegaInit_, expected.biasAccOmegaInit_, tol)
&& equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol);
bool PreintegratedCombinedMeasurements::equals(
const PreintegratedCombinedMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol) &&
equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::resetIntegration() {
void PreintegratedCombinedMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor,
boost::optional<Matrix&> F_test, boost::optional<Matrix&> G_test) {
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
#define D_a_a(H) (H)->block<3,3>(9,9)
#define D_g_g(H) (H)->block<3,3>(12,12)
// NOTE: order is important here because each update uses old values, e.g., velocity and position updates are based on previous rotation estimate.
// (i.e., we have to update jacobians and covariances before updating preintegrated measurements).
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) {
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor);
const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr,
deltaT);
// Update preintegrated measurements.
Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 G1,G2;
PreintegrationBase::update(measuredAcc, measuredOmega, deltaT,
&D_incrR_integratedOmega, &F_9x9, &G1, &G2);
// Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
// can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
// consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
const Matrix3 R_i = deltaRij(); // store this
// Update preintegrated measurements. TODO Frank moved from end of this function !!!
Matrix9 F_9x9;
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F_9x9);
// Single Jacobians to propagate covariance
Matrix3 H_vel_biasacc = -R_i * deltaT;
Matrix3 H_angles_biasomega = -D_Rincr_integratedOmega * deltaT;
Matrix3 H_vel_biasacc = -dRij * deltaT;
Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT;
// overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15, 15);
// for documentation:
// F << I_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3,
// Z_3x3, I_3x3, H_vel_angles, H_vel_biasacc, Z_3x3,
// Z_3x3, Z_3x3, H_angles_angles, Z_3x3, H_angles_biasomega,
// Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3,
// Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3;
Eigen::Matrix<double,15,15> F;
F.setZero();
F.block<9, 9>(0, 0) = F_9x9;
F.block<3, 3>(0, 12) = H_angles_biasomega;
F.block<3, 3>(6, 9) = H_vel_biasacc;
F.block<6, 6>(9, 9) = I_6x6;
F.block<3, 3>(3, 9) = H_vel_biasacc;
F.block<3, 3>(6, 12) = H_angles_biasomega;
// first order uncertainty propagation
// Optimized matrix multiplication (1/deltaT) * G * measurementCovariance * G.transpose()
Matrix G_measCov_Gt = Matrix::Zero(15, 15);
Eigen::Matrix<double,15,15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15);
// BLOCK DIAGONAL TERMS
G_measCov_Gt.block<3, 3>(0, 0) = deltaT * integrationCovariance();
G_measCov_Gt.block<3, 3>(3, 3) = (1 / deltaT) * (H_vel_biasacc)
* (accelerometerCovariance() + biasAccOmegaInit_.block<3, 3>(0, 0))
// BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance;
D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc)
* (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0))
* (H_vel_biasacc.transpose());
G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega)
* (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3))
D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega)
* (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
* (H_angles_biasomega.transpose());
G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_;
G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_;
// OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0)
* H_angles_biasomega.transpose();
G_measCov_Gt.block<3, 3>(3, 6) = block23;
G_measCov_Gt.block<3, 3>(6, 3) = block23.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance;
// F_test and G_test are used for testing purposes and are not needed by the factor
if (F_test) {
F_test->resize(15, 15);
(*F_test) << F;
}
if (G_test) {
G_test->resize(15, 21);
// This is for testing & documentation
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
(*G_test) << //
I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
Z_3x3, Z_3x3, -H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
}
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
* H_angles_biasomega.transpose();
D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
}
//------------------------------------------------------------------------------
PreintegratedCombinedMeasurements::PreintegratedCombinedMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, const Matrix3& biasAccCovariance,
const Matrix3& biasOmegaCovariance, const Matrix6& biasAccOmegaInit,
const bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance;
p->biasAccCovariance = biasAccCovariance;
p->biasOmegaCovariance = biasOmegaCovariance;
p->biasAccOmegaInit = biasAccOmegaInit;
p_ = p;
resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
// CombinedImuFactor methods
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor() :
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3, Z_3x3,
Z_3x3, Z_6x6) {
}
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j,
Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
vel_j, bias_i, bias_j), ImuFactorBase(gravity, omegaCoriolis,
body_P_sensor, use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
}
CombinedImuFactor::CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const PreintegratedCombinedMeasurements& pim)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {}
//------------------------------------------------------------------------------
gtsam::NonlinearFactor::shared_ptr CombinedImuFactor::clone() const {
@ -194,17 +162,14 @@ void CombinedImuFactor::print(const string& s,
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5()) << ","
<< keyFormatter(this->key6()) << ")\n";
ImuFactorBase::print("");
_PIM_.print(" preintegrated measurements:");
this->noiseModel_->print(" noise model: ");
}
//------------------------------------------------------------------------------
bool CombinedImuFactor::equals(const NonlinearFactor& expected,
double tol) const {
const This *e = dynamic_cast<const This*>(&expected);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
bool CombinedImuFactor::equals(const NonlinearFactor& other, double tol) const {
const This* e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol);
}
//------------------------------------------------------------------------------
@ -224,8 +189,7 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
Matrix93 D_r_vel_i, D_r_vel_j;
// error wrt preintegrated measurements
Vector9 r_pvR = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j,
bias_i, gravity_, omegaCoriolis_, use2ndOrderCoriolis_,
Vector9 r_Rpv = _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
@ -234,25 +198,25 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
H1->resize(15, 6);
H1->block<9, 6>(0, 0) = D_r_pose_i;
// adding: [dBiasAcc/dPi ; dBiasOmega/dPi]
H1->block<6, 6>(9, 0) = Z_6x6;
H1->block<6, 6>(9, 0).setZero();
}
if (H2) {
H2->resize(15, 3);
H2->block<9, 3>(0, 0) = D_r_vel_i;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H2->block<6, 3>(9, 0) = Matrix::Zero(6, 3);
H2->block<6, 3>(9, 0).setZero();
}
if (H3) {
H3->resize(15, 6);
H3->block<9, 6>(0, 0) = D_r_pose_j;
// adding: [dBiasAcc/dPj ; dBiasOmega/dPj]
H3->block<6, 6>(9, 0) = Z_6x6;
H3->block<6, 6>(9, 0).setZero();
}
if (H4) {
H4->resize(15, 3);
H4->block<9, 3>(0, 0) = D_r_vel_j;
// adding: [dBiasAcc/dVi ; dBiasOmega/dVi]
H4->block<6, 3>(9, 0) = Matrix::Zero(6, 3);
H4->block<6, 3>(9, 0).setZero();
}
if (H5) {
H5->resize(15, 6);
@ -262,15 +226,48 @@ Vector CombinedImuFactor::evaluateError(const Pose3& pose_i,
}
if (H6) {
H6->resize(15, 6);
H6->block<9, 6>(0, 0) = Matrix::Zero(9, 6);
H6->block<9, 6>(0, 0).setZero();
// adding: [dBiasAcc/dBias_j ; dBiasOmega/dBias_j]
H6->block<6, 6>(9, 0) = Hbias_j;
}
// overall error
Vector r(15);
r << r_pvR, fbias; // vector of size 15
r << r_Rpv, fbias; // vector of size 15
return r;
}
//------------------------------------------------------------------------------
CombinedImuFactor::CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias_i, bias_j),
_PIM_(pim) {
boost::shared_ptr<CombinedPreintegratedMeasurements::Params> p =
boost::make_shared<CombinedPreintegratedMeasurements::Params>(pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis);
pose_j = pvb.pose;
vel_j = pvb.velocity;
}
} /// namespace gtsam

286
gtsam/navigation/CombinedImuFactor.h
View File

@ -24,21 +24,17 @@
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ImuFactorBase.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
/*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating
* conditionally independent sets in factor graphs: a unifying perspective based
* on smart factors, Int. Conf. on Robotics and Automation (ICRA), 2014.
*
** REFERENCES:
* REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
* Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
@ -46,14 +42,137 @@ namespace gtsam {
* TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
* Computation of the Jacobian Matrices", Tech. Report, 2013.
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, IMU Preintegration on
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation,
* Robotics: Science and Systems (RSS), 2015.
*/
/**
* PreintegratedCombinedMeasurements integrates the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the CombinedImuFactor. Integration
* is done incrementally (ideally, one integrates the measurement as soon as
* it is received from the IMU) so as to avoid costly integration at time of
* factor construction.
*
* @addtogroup SLAM
*/
class PreintegratedCombinedMeasurements : public PreintegrationBase {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params : PreintegrationBase::Params {
Matrix3 biasAccCovariance; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit; ///< covariance of bias used for pre-integration
/// See two named constructors below for good values of n_gravity in body frame
Params(const Vector3& n_gravity) :
PreintegrationBase::Params(n_gravity), biasAccCovariance(I_3x3), biasOmegaCovariance(
I_3x3), biasAccOmegaInit(I_6x6) {
}
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, g));
}
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, -g));
}
private:
/// Default constructor makes unitialized params struct
Params() {}
/** Serialization function */
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar& BOOST_SERIALIZATION_NVP(biasAccCovariance);
ar& BOOST_SERIALIZATION_NVP(biasOmegaCovariance);
ar& BOOST_SERIALIZATION_NVP(biasAccOmegaInit);
}
};
protected:
/* Covariance matrix of the preintegrated measurements
* COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
* (first-order propagation from *measurementCovariance*).
* PreintegratedCombinedMeasurements also include the biases and keep the correlation
* between the preintegrated measurements and the biases
*/
Eigen::Matrix<double, 15, 15> preintMeasCov_;
PreintegratedCombinedMeasurements() {}
friend class CombinedImuFactor;
public:
/**
* Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
*/
PreintegratedCombinedMeasurements(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat)
: PreintegrationBase(p, biasHat) {
preintMeasCov_.setZero();
}
Params& p() const { return *boost::static_pointer_cast<Params>(p_);}
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const PreintegratedCombinedMeasurements& expected,
double tol = 1e-9) const;
/// Re-initialize PreintegratedCombinedMeasurements
void resetIntegration();
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the
* sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between two consecutive IMU measurements
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body
* frame)
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT);
/// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_; }
/// deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedCombinedMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix6& biasAccOmegaInit, const bool use2ndOrderIntegration = true);
private:
/// Serialization function
friend class boost::serialization::access;
template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar& BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
/**
* CombinedImuFactor is a 6-ways factor involving previous state (pose and
* velocity of the vehicle, as well as bias at previous time step), and current
* state (pose, velocity, bias at current time step). Following the pre-
* integration scheme proposed in [2], the CombinedImuFactor includes many IMU
* measurements, which are "summarized" using the CombinedPreintegratedMeasurements
* measurements, which are "summarized" using the PreintegratedCombinedMeasurements
* class. There are 3 main differences wrpt the ImuFactor class:
* 1) The factor is 6-ways, meaning that it also involves both biases (previous
* and current time step).Therefore, the factor internally imposes the biases
@ -61,105 +180,26 @@ namespace gtsam {
* "biasOmegaCovariance" described the random walk that models bias evolution.
* 2) The preintegration covariance takes into account the noise in the bias
* estimate used for integration.
* 3) The covariance matrix of the CombinedPreintegratedMeasurements preserves
* 3) The covariance matrix of the PreintegratedCombinedMeasurements preserves
* the correlation between the bias uncertainty and the preintegrated
* measurements uncertainty.
*
* @addtogroup SLAM
*/
class CombinedImuFactor: public NoiseModelFactor6<Pose3, Vector3, Pose3,
Vector3, imuBias::ConstantBias, imuBias::ConstantBias>, public ImuFactorBase {
Vector3, imuBias::ConstantBias, imuBias::ConstantBias> {
public:
/**
* CombinedPreintegratedMeasurements integrates the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the CombinedImuFactor. Integration
* is done incrementally (ideally, one integrates the measurement as soon as
* it is received from the IMU) so as to avoid costly integration at time of
* factor construction.
*/
class CombinedPreintegratedMeasurements: public PreintegrationBase {
friend class CombinedImuFactor;
protected:
Matrix3 biasAccCovariance_; ///< continuous-time "Covariance" describing accelerometer bias random walk
Matrix3 biasOmegaCovariance_; ///< continuous-time "Covariance" describing gyroscope bias random walk
Matrix6 biasAccOmegaInit_; ///< covariance of bias used for pre-integration
Eigen::Matrix<double, 15, 15> preintMeasCov_; ///< Covariance matrix of the preintegrated measurements
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
///< (first-order propagation from *measurementCovariance*). CombinedPreintegratedMeasurements also include the biases and keep the correlation
///< between the preintegrated measurements and the biases
public:
/**
* Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param measuredAccCovariance Covariance matrix of measuredAcc
* @param measuredOmegaCovariance Covariance matrix of measured Angular Rate
* @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
* @param biasAccCovariance Covariance matrix of biasAcc (random walk describing BIAS evolution)
* @param biasOmegaCovariance Covariance matrix of biasOmega (random walk describing BIAS evolution)
* @param biasAccOmegaInit Covariance of biasAcc & biasOmega when preintegrating measurements
* @param use2ndOrderIntegration Controls the order of integration
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/
CombinedPreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const Matrix3& biasAccCovariance, const Matrix3& biasOmegaCovariance,
const Matrix& biasAccOmegaInit, const bool use2ndOrderIntegration =
false);
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const CombinedPreintegratedMeasurements& expected, double tol =
1e-9) const;
/// Re-initialize CombinedPreintegratedMeasurements
void resetIntegration();
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between two consecutive IMU measurements
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none,
boost::optional<Matrix&> F_test = boost::none,
boost::optional<Matrix&> G_test = boost::none);
/// methods to access class variables
Matrix preintMeasCov() const {
return preintMeasCov_;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
private:
typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias, imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements _PIM_;
PreintegratedCombinedMeasurements _PIM_;
/** Default constructor - only use for serialization */
CombinedImuFactor() {}
public:
@ -170,9 +210,6 @@ public:
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
CombinedImuFactor();
/**
* Constructor
* @param pose_i Previous pose key
@ -181,21 +218,13 @@ public:
* @param vel_j Current velocity key
* @param bias_i Previous bias key
* @param bias_j Current bias key
* @param CombinedPreintegratedMeasurements CombinedPreintegratedMeasurements IMU measurements
* @param gravity Gravity vector expressed in the global frame
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
* @param PreintegratedCombinedMeasurements Combined IMU measurements
*/
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
CombinedImuFactor(
Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const PreintegratedCombinedMeasurements& preintegratedMeasurements);
virtual ~CombinedImuFactor() {
}
virtual ~CombinedImuFactor() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const;
@ -211,7 +240,7 @@ public:
/** Access the preintegrated measurements. */
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
const PreintegratedCombinedMeasurements& preintegratedMeasurements() const {
return _PIM_;
}
@ -226,20 +255,22 @@ public:
boost::optional<Matrix&> H4 = boost::none, boost::optional<Matrix&> H5 =
boost::none, boost::optional<Matrix&> H6 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
/// @deprecated typename
typedef gtsam::PreintegratedCombinedMeasurements CombinedPreintegratedMeasurements;
/// @deprecated constructor
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i,
Key bias_j, const CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
// @deprecated use PreintegrationBase::predict
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const CombinedPreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
PoseVelocityBias PVB(
PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
use2ndOrderCoriolis, deltaPij_biascorrected_out,
deltaVij_biascorrected_out));
pose_j = PVB.pose;
vel_j = PVB.velocity;
}
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
CombinedPreintegratedMeasurements& pim,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis = false);
private:
@ -250,13 +281,8 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor6",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
};
// class CombinedImuFactor
typedef CombinedImuFactor::CombinedPreintegratedMeasurements CombinedImuFactorPreintegratedMeasurements;
} /// namespace gtsam

10
gtsam/navigation/ImuBias.h
View File

@ -78,20 +78,18 @@ public:
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector3 correctAccelerometer(const Vector3& measurement,
boost::optional<Matrix&> H = boost::none) const {
OptionalJacobian<3, 6> H = boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << -Matrix3::Identity(), Matrix3::Zero();
(*H) << -I_3x3, Z_3x3;
}
return measurement - biasAcc_;
}
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector3 correctGyroscope(const Vector3& measurement,
boost::optional<Matrix&> H = boost::none) const {
OptionalJacobian<3, 6> H = boost::none) const {
if (H) {
H->resize(3, 6);
(*H) << Matrix3::Zero(), -Matrix3::Identity();
(*H) << Z_3x3, -I_3x3;
}
return measurement - biasGyro_;
}

180
gtsam/navigation/ImuFactor.cpp
View File

@ -31,113 +31,91 @@ using namespace std;
//------------------------------------------------------------------------------
// Inner class PreintegratedMeasurements
//------------------------------------------------------------------------------
ImuFactor::PreintegratedMeasurements::PreintegratedMeasurements(
const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration) :
PreintegrationBase(bias, measuredAccCovariance, measuredOmegaCovariance,
integrationErrorCovariance, use2ndOrderIntegration) {
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::print(const string& s) const {
void PreintegratedImuMeasurements::print(const string& s) const {
PreintegrationBase::print(s);
cout << " preintMeasCov \n[" << preintMeasCov_ << "]" << endl;
}
//------------------------------------------------------------------------------
bool ImuFactor::PreintegratedMeasurements::equals(
const PreintegratedMeasurements& expected, double tol) const {
return equal_with_abs_tol(preintMeasCov_, expected.preintMeasCov_, tol)
&& PreintegrationBase::equals(expected, tol);
bool PreintegratedImuMeasurements::equals(
const PreintegratedImuMeasurements& other, double tol) const {
return PreintegrationBase::equals(other, tol)
&& equal_with_abs_tol(preintMeasCov_, other.preintMeasCov_, tol);
}
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::resetIntegration() {
void PreintegratedImuMeasurements::resetIntegration() {
PreintegrationBase::resetIntegration();
preintMeasCov_.setZero();
}
//------------------------------------------------------------------------------
void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor, OptionalJacobian<9, 9> F_test,
OptionalJacobian<9, 9> G_test) {
void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
Vector3 correctedAcc, correctedOmega;
correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
correctedAcc, correctedOmega, body_P_sensor);
const Vector3 integratedOmega = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
Matrix3 D_Rincr_integratedOmega; // Right jacobian computed at theta_incr
const Rot3 Rincr = Rot3::Expmap(integratedOmega, D_Rincr_integratedOmega); // rotation increment computed from the current rotation rate measurement
// Update Jacobians
updatePreintegratedJacobians(correctedAcc, D_Rincr_integratedOmega, Rincr, deltaT);
static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
// Update preintegrated measurements (also get Jacobian)
const Matrix3 R_i = deltaRij(); // store this, which is useful to compute G_test
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
updatePreintegratedMeasurements(correctedAcc, Rincr, deltaT, F);
Matrix93 G1, G2;
Matrix3 D_incrR_integratedOmega;
PreintegrationBase::update(measuredAcc, measuredOmega, dt,
&D_incrR_integratedOmega, &F, &G1, &G2);
// first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a prediction phase in an EKF framework
/* ----------------------------------------------------------------------------------------------------------------------- */
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G.transpose();
// as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
/* --------------------------------------------------------------------------------------------*/
// preintMeasCov = F * preintMeasCov * F.transpose() + G * (1/deltaT) * measurementCovariance * G'
// NOTE 1: (1/deltaT) allows to pass from continuous time noise to discrete time noise
// measurementCovariance_discrete = measurementCovariance_contTime * (1/deltaT)
// NOTE 2: the computation of G * (1/deltaT) * measurementCovariance * G.transpose() is done block-wise,
// as G and measurementCovariance are block-diagonal matrices
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
preintMeasCov_.block<3, 3>(0, 0) += integrationCovariance() * deltaT;
preintMeasCov_.block<3, 3>(3, 3) += R_i * accelerometerCovariance()
* R_i.transpose() * deltaT;
preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega
* gyroscopeCovariance() * D_Rincr_integratedOmega.transpose() * deltaT;
#ifdef OLD_JACOBIAN_CALCULATION
Matrix9 G;
G << G1, Gi, G2;
Matrix9 Cov;
Cov << p().accelerometerCovariance / dt, Z_3x3, Z_3x3,
Z_3x3, p().integrationCovariance * dt, Z_3x3,
Z_3x3, Z_3x3, p().gyroscopeCovariance / dt;
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G * Cov * G.transpose();
#else
preintMeasCov_ = F * preintMeasCov_ * F.transpose()
+ Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
+ G1 * (p().accelerometerCovariance / dt) * G1.transpose()
+ G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
#endif
}
Matrix3 F_pos_noiseacc;
if (use2ndOrderIntegration()) {
F_pos_noiseacc = 0.5 * R_i * deltaT * deltaT;
preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
* accelerometerCovariance() * F_pos_noiseacc.transpose();
Matrix3 temp = F_pos_noiseacc * accelerometerCovariance() * R_i.transpose(); // already includes 1/deltaT
preintMeasCov_.block<3, 3>(0, 3) += temp;
preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
}
//------------------------------------------------------------------------------
PreintegratedImuMeasurements::PreintegratedImuMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
biasHat_ = biasHat;
boost::shared_ptr<Params> p = Params::MakeSharedD();
p->gyroscopeCovariance = measuredOmegaCovariance;
p->accelerometerCovariance = measuredAccCovariance;
p->integrationCovariance = integrationErrorCovariance;
p_ = p;
resetIntegration();
}
// F_test and G_test are given as output for testing purposes and are not needed by the factor
if (F_test) { // This in only for testing
(*F_test) << F;
}
if (G_test) { // This in only for testing & documentation, while the actual computation is done block-wise
if (!use2ndOrderIntegration())
F_pos_noiseacc = Z_3x3;
// intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
}
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
boost::optional<Pose3> body_P_sensor) {
// modify parameters to accommodate deprecated method:-(
p_->body_P_sensor = body_P_sensor;
integrateMeasurement(measuredAcc, measuredOmega, deltaT);
}
//------------------------------------------------------------------------------
// ImuFactor methods
//------------------------------------------------------------------------------
ImuFactor::ImuFactor() :
ImuFactorBase(), _PIM_(imuBias::ConstantBias(), Z_3x3, Z_3x3, Z_3x3) {
}
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis) :
Base(
noiseModel::Gaussian::Covariance(
preintegratedMeasurements.preintMeasCov_), pose_i, vel_i, pose_j,
vel_j, bias), ImuFactorBase(gravity, omegaCoriolis, body_P_sensor,
use2ndOrderCoriolis), _PIM_(preintegratedMeasurements) {
const PreintegratedImuMeasurements& pim) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias), _PIM_(pim) {
}
//------------------------------------------------------------------------------
@ -152,17 +130,18 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
<< keyFormatter(this->key2()) << "," << keyFormatter(this->key3()) << ","
<< keyFormatter(this->key4()) << "," << keyFormatter(this->key5())
<< ")\n";
ImuFactorBase::print("");
Base::print("");
_PIM_.print(" preintegrated measurements:");
// Print standard deviations on covariance only
cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose() << endl;
cout << " noise model sigmas: " << this->noiseModel_->sigmas().transpose()
<< endl;
}
//------------------------------------------------------------------------------
bool ImuFactor::equals(const NonlinearFactor& expected, double tol) const {
const This *e = dynamic_cast<const This*>(&expected);
bool ImuFactor::equals(const NonlinearFactor& other, double tol) const {
const This *e = dynamic_cast<const This*>(&other);
return e != NULL && Base::equals(*e, tol) && _PIM_.equals(e->_PIM_, tol)
&& ImuFactorBase::equals(*e, tol);
&& Base::equals(*e, tol);
}
//------------------------------------------------------------------------------
@ -171,9 +150,36 @@ Vector ImuFactor::evaluateError(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2, boost::optional<Matrix&> H3,
boost::optional<Matrix&> H4, boost::optional<Matrix&> H5) const {
return _PIM_.computeErrorAndJacobians(pose_i, vel_i, pose_j, vel_j, bias_i,
gravity_, omegaCoriolis_, use2ndOrderCoriolis_, H1, H2, H3, H4, H5);
H1, H2, H3, H4, H5);
}
} /// namespace gtsam
//------------------------------------------------------------------------------
ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const boost::optional<Pose3>& body_P_sensor,
const bool use2ndOrderCoriolis) :
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias), _PIM_(pim) {
boost::shared_ptr<PreintegratedMeasurements::Params> p = boost::make_shared<
PreintegratedMeasurements::Params>(pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
p->use2ndOrderCoriolis = use2ndOrderCoriolis;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
Pose3& pose_j, Vector3& vel_j, const imuBias::ConstantBias& bias_i,
PreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis) {
// use deprecated predict
PoseVelocityBias pvb = pim.predict(pose_i, vel_i, bias_i, n_gravity,
omegaCoriolis, use2ndOrderCoriolis);
pose_j = pvb.pose;
vel_j = pvb.velocity;
}
} // namespace gtsam

228
gtsam/navigation/ImuFactor.h
View File

@ -24,21 +24,21 @@
/* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/navigation/ImuFactorBase.h>
#include <gtsam/base/debug.h>
namespace gtsam {
/**
*
* @addtogroup SLAM
*
/*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, "Eliminating
* conditionally independent sets in factor graphs: a unifying perspective based
* on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
*
** REFERENCES:
* C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
* Robotics: Science and Systems (RSS), 2015.
*
* REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups",
* Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for
@ -46,103 +46,116 @@ namespace gtsam {
* TRO, 28(1):61-76, 2012.
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor:
* Computation of the Jacobian Matrices", Tech. Report, 2013.
* [4] C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "IMU Preintegration on
* Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation",
* Robotics: Science and Systems (RSS), 2015.
*/
/**
* PreintegratedIMUMeasurements accumulates (integrates) the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated IMU factor.
* Integration is done incrementally (ideally, one integrates the measurement
* as soon as it is received from the IMU) so as to avoid costly integration
* at time of factor construction.
*
* @addtogroup SLAM
*/
class PreintegratedImuMeasurements: public PreintegrationBase {
friend class ImuFactor;
protected:
Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
///< (first-order propagation from *measurementCovariance*).
/// Default constructor for serialization
PreintegratedImuMeasurements() {}
public:
/**
* Constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param p Parameters, typically fixed in a single application
*/
PreintegratedImuMeasurements(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat) :
PreintegrationBase(p, biasHat) {
preintMeasCov_.setZero();
}
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const PreintegratedImuMeasurements& expected,
double tol = 1e-9) const;
/// Re-initialize PreintegratedIMUMeasurements
void resetIntegration();
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt);
/// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; }
/// @deprecated constructor
/// NOTE(frank): assumes Z-Down convention, only second order integration supported
PreintegratedImuMeasurements(const imuBias::ConstantBias& biasHat,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
bool use2ndOrderIntegration = true);
/// @deprecated version of integrateMeasurement with body_P_sensor
/// Use parameters instead
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt,
boost::optional<Pose3> body_P_sensor);
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
/**
* ImuFactor is a 5-ways factor involving previous state (pose and velocity of
* the vehicle at previous time step), current state (pose and velocity at
* current time step), and the bias estimate. Following the preintegration
* scheme proposed in [2], the ImuFactor includes many IMU measurements, which
* are "summarized" using the PreintegratedMeasurements class.
* are "summarized" using the PreintegratedIMUMeasurements class.
* Note that this factor does not model "temporal consistency" of the biases
* (which are usually slowly varying quantities), which is up to the caller.
* See also CombinedImuFactor for a class that does this for you.
*
* @addtogroup SLAM
*/
class ImuFactor: public NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias>, public ImuFactorBase {
imuBias::ConstantBias> {
public:
/**
* PreintegratedMeasurements accumulates (integrates) the IMU measurements
* (rotation rates and accelerations) and the corresponding covariance matrix.
* The measurements are then used to build the Preintegrated IMU factor.
* Integration is done incrementally (ideally, one integrates the measurement
* as soon as it is received from the IMU) so as to avoid costly integration
* at time of factor construction.
*/
class PreintegratedMeasurements: public PreintegrationBase {
friend class ImuFactor;
protected:
Eigen::Matrix<double, 9, 9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
///< (first-order propagation from *measurementCovariance*).
public:
/**
* Default constructor, initializes the class with no measurements
* @param bias Current estimate of acceleration and rotation rate biases
* @param measuredAccCovariance Covariance matrix of measuredAcc
* @param measuredOmegaCovariance Covariance matrix of measured Angular Rate
* @param integrationErrorCovariance Covariance matrix of integration errors (velocity -> position)
* @param use2ndOrderIntegration Controls the order of integration
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
*/
PreintegratedMeasurements(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance,
const bool use2ndOrderIntegration = false);
/// print
void print(const std::string& s = "Preintegrated Measurements:") const;
/// equals
bool equals(const PreintegratedMeasurements& expected,
double tol = 1e-9) const;
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/**
* Add a single IMU measurement to the preintegration.
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor)
* @param deltaT Time interval between this and the last IMU measurement
* @param body_P_sensor Optional sensor frame (pose of the IMU in the body frame)
* @param Fout, Gout Jacobians used internally (only needed for testing)
*/
void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT,
boost::optional<const Pose3&> body_P_sensor = boost::none,
OptionalJacobian<9, 9> Fout = boost::none, OptionalJacobian<9, 9> Gout =
boost::none);
/// methods to access class variables
Matrix preintMeasCov() const {
return preintMeasCov_;
}
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
}
};
private:
typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3, Vector3, Pose3, Vector3,
imuBias::ConstantBias> Base;
PreintegratedMeasurements _PIM_;
PreintegratedImuMeasurements _PIM_;
public:
@ -163,17 +176,9 @@ public:
* @param pose_j Current pose key
* @param vel_j Current velocity key
* @param bias Previous bias key
* @param preintegratedMeasurements Preintegrated IMU measurements
* @param gravity Gravity vector expressed in the global frame
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
*/
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
const PreintegratedImuMeasurements& preintegratedMeasurements);
virtual ~ImuFactor() {
}
@ -192,7 +197,7 @@ public:
/** Access the preintegrated measurements. */
const PreintegratedMeasurements& preintegratedMeasurements() const {
const PreintegratedImuMeasurements& preintegratedMeasurements() const {
return _PIM_;
}
@ -206,20 +211,22 @@ public:
boost::optional<Matrix&> H3 = boost::none, boost::optional<Matrix&> H4 =
boost::none, boost::optional<Matrix&> H5 = boost::none) const;
/** @deprecated The following function has been deprecated, use PreintegrationBase::predict with the same signature instead */
/// @deprecated typename
typedef PreintegratedImuMeasurements PreintegratedMeasurements;
/// @deprecated constructor, in the new one gravity, coriolis settings are in Params
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
/// @deprecated use PreintegrationBase::predict,
/// in the new one gravity, coriolis settings are in Params
static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j,
Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const PreintegratedMeasurements& PIM, const Vector3& gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) {
PoseVelocityBias PVB(
PIM.predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
use2ndOrderCoriolis, deltaPij_biascorrected_out,
deltaVij_biascorrected_out));
pose_j = PVB.pose;
vel_j = PVB.velocity;
}
PreintegratedMeasurements& pim, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private:
@ -230,13 +237,8 @@ private:
ar & boost::serialization::make_nvp("NoiseModelFactor5",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(_PIM_);
ar & BOOST_SERIALIZATION_NVP(gravity_);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor_);
}
};
// class ImuFactor
typedef ImuFactor::PreintegratedMeasurements ImuFactorPreintegratedMeasurements;
} /// namespace gtsam

94
gtsam/navigation/ImuFactorBase.h
View File

@ -1,94 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PreintegrationBase.h
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#pragma once
/* GTSAM includes */
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/PreintegrationBase.h>
namespace gtsam {
class ImuFactorBase {
protected:
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
/** Default constructor - only use for serialization */
ImuFactorBase() :
gravity_(Vector3(0.0, 0.0, 9.81)), omegaCoriolis_(Vector3(0.0, 0.0, 0.0)), body_P_sensor_(
boost::none), use2ndOrderCoriolis_(false) {
}
/**
* Default constructor, stores basic quantities required by the Imu factors
* @param gravity Gravity vector expressed in the global frame
* @param omegaCoriolis Rotation rate of the global frame w.r.t. an inertial frame
* @param body_P_sensor Optional pose of the sensor frame in the body frame
* @param use2ndOrderCoriolis When true, the second-order term is used in the calculation of the Coriolis Effect
*/
ImuFactorBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false) :
gravity_(gravity), omegaCoriolis_(omegaCoriolis), body_P_sensor_(
body_P_sensor), use2ndOrderCoriolis_(use2ndOrderCoriolis) {
}
/// Methods to access class variables
const Vector3& gravity() const {
return gravity_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
/// Needed for testable
//------------------------------------------------------------------------------
void print(const std::string& /*s*/) const {
std::cout << " gravity: [ " << gravity_.transpose() << " ]" << std::endl;
std::cout << " omegaCoriolis: [ " << omegaCoriolis_.transpose() << " ]"
<< std::endl;
std::cout << " use2ndOrderCoriolis: [ " << use2ndOrderCoriolis_ << " ]"
<< std::endl;
if (this->body_P_sensor_)
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
/// Needed for testable
//------------------------------------------------------------------------------
bool equals(const ImuFactorBase& expected, double tol) const {
return equal_with_abs_tol(gravity_, expected.gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, expected.omegaCoriolis_, tol)
&& (use2ndOrderCoriolis_ == expected.use2ndOrderCoriolis_)
&& ((!body_P_sensor_ && !expected.body_P_sensor_)
|| (body_P_sensor_ && expected.body_P_sensor_
&& body_P_sensor_->equals(*expected.body_P_sensor_)));
}
};
} /// namespace gtsam

358
gtsam/navigation/NavState.cpp Normal file
View File

@ -0,0 +1,358 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file NavState.h
* @brief Navigation state composing of attitude, position, and velocity
* @author Frank Dellaert
* @date July 2015
**/
#include <gtsam/navigation/NavState.h>
using namespace std;
namespace gtsam {
#define TIE(R,t,v,x) const Rot3& R = (x).R_;const Point3& t = (x).t_;const Velocity3& v = (x).v_;
//------------------------------------------------------------------------------
NavState NavState::FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2) {
if (H1)
*H1 << I_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
if (H2)
*H2 << Z_3x3, Z_3x3, pose.rotation().transpose();
return NavState(pose, vel);
}
//------------------------------------------------------------------------------
const Rot3& NavState::attitude(OptionalJacobian<3, 9> H) const {
if (H)
*H << I_3x3, Z_3x3, Z_3x3;
return R_;
}
//------------------------------------------------------------------------------
const Point3& NavState::position(OptionalJacobian<3, 9> H) const {
if (H)
*H << Z_3x3, R(), Z_3x3;
return t_;
}
//------------------------------------------------------------------------------
const Vector3& NavState::velocity(OptionalJacobian<3, 9> H) const {
if (H)
*H << Z_3x3, Z_3x3, R();
return v_;
}
//------------------------------------------------------------------------------
Vector3 NavState::bodyVelocity(OptionalJacobian<3, 9> H) const {
const Rot3& nRb = R_;
const Vector3& n_v = v_;
Matrix3 D_bv_nRb;
Vector3 b_v = nRb.unrotate(n_v, H ? &D_bv_nRb : 0);
if (H)
*H << D_bv_nRb, Z_3x3, I_3x3;
return b_v;
}
//------------------------------------------------------------------------------
Matrix7 NavState::matrix() const {
Matrix3 R = this->R();
Matrix7 T;
T << R, Z_3x3, t(), Z_3x3, R, v(), Vector6::Zero().transpose(), 1.0;
return T;
}
//------------------------------------------------------------------------------
void NavState::print(const string& s) const {
attitude().print(s + ".R");
position().print(s + ".p");
gtsam::print((Vector) v_, s + ".v");
}
//------------------------------------------------------------------------------
bool NavState::equals(const NavState& other, double tol) const {
return R_.equals(other.R_, tol) && t_.equals(other.t_, tol)
&& equal_with_abs_tol(v_, other.v_, tol);
}
//------------------------------------------------------------------------------
NavState NavState::inverse() const {
TIE(nRb, n_t, n_v, *this);
const Rot3 bRn = nRb.inverse();
return NavState(bRn, -(bRn * n_t), -(bRn * n_v));
}
//------------------------------------------------------------------------------
NavState NavState::operator*(const NavState& bTc) const {
TIE(nRb, n_t, n_v, *this);
TIE(bRc, b_t, b_v, bTc);
return NavState(nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v);
}
//------------------------------------------------------------------------------
NavState::PositionAndVelocity //
NavState::operator*(const PositionAndVelocity& b_tv) const {
TIE(nRb, n_t, n_v, *this);
const Point3& b_t = b_tv.first;
const Velocity3& b_v = b_tv.second;
return PositionAndVelocity(nRb * b_t + n_t, nRb * b_v + n_v);
}
//------------------------------------------------------------------------------
Point3 NavState::operator*(const Point3& b_t) const {
return Point3(R_ * b_t + t_);
}
//------------------------------------------------------------------------------
Velocity3 NavState::operator*(const Velocity3& b_v) const {
return Velocity3(R_ * b_v + v_);
}
//------------------------------------------------------------------------------
NavState NavState::ChartAtOrigin::Retract(const Vector9& xi,
OptionalJacobian<9, 9> H) {
Matrix3 D_R_xi;
const Rot3 R = Rot3::Expmap(dR(xi), H ? &D_R_xi : 0);
const Point3 p = Point3(dP(xi));
const Vector v = dV(xi);
const NavState result(R, p, v);
if (H) {
*H << D_R_xi, Z_3x3, Z_3x3, //
Z_3x3, R.transpose(), Z_3x3, //
Z_3x3, Z_3x3, R.transpose();
}
return result;
}
//------------------------------------------------------------------------------
Vector9 NavState::ChartAtOrigin::Local(const NavState& x,
OptionalJacobian<9, 9> H) {
Vector9 xi;
Matrix3 D_xi_R;
xi << Rot3::Logmap(x.R_, H ? &D_xi_R : 0), x.t(), x.v();
if (H) {
*H << D_xi_R, Z_3x3, Z_3x3, //
Z_3x3, x.R(), Z_3x3, //
Z_3x3, Z_3x3, x.R();
}
return xi;
}
//------------------------------------------------------------------------------
NavState NavState::Expmap(const Vector9& xi, OptionalJacobian<9, 9> H) {
if (H)
throw runtime_error("NavState::Expmap derivative not implemented yet");
Eigen::Block<const Vector9, 3, 1> n_omega_nb = dR(xi);
Eigen::Block<const Vector9, 3, 1> v = dP(xi);
Eigen::Block<const Vector9, 3, 1> a = dV(xi);
// NOTE(frank): See Pose3::Expmap
Rot3 nRb = Rot3::Expmap(n_omega_nb);
double theta2 = n_omega_nb.dot(n_omega_nb);
if (theta2 > numeric_limits<double>::epsilon()) {
// Expmap implements a "screw" motion in the direction of n_omega_nb
Vector3 n_t_parallel = n_omega_nb * n_omega_nb.dot(v); // component along rotation axis
Vector3 omega_cross_v = n_omega_nb.cross(v); // points towards axis
Point3 n_t = Point3(omega_cross_v - nRb * omega_cross_v + n_t_parallel)
/ theta2;
Vector3 n_v_parallel = n_omega_nb * n_omega_nb.dot(a); // component along rotation axis
Vector3 omega_cross_a = n_omega_nb.cross(a); // points towards axis
Vector3 n_v = (omega_cross_a - nRb * omega_cross_a + n_v_parallel) / theta2;
return NavState(nRb, n_t, n_v);
} else {
return NavState(nRb, Point3(v), a);
}
}
//------------------------------------------------------------------------------
Vector9 NavState::Logmap(const NavState& nTb, OptionalJacobian<9, 9> H) {
if (H)
throw runtime_error("NavState::Logmap derivative not implemented yet");
TIE(nRb, n_p, n_v, nTb);
Vector3 n_t = n_p.vector();
// NOTE(frank): See Pose3::Logmap
Vector9 xi;
Vector3 n_omega_nb = Rot3::Logmap(nRb);
double theta = n_omega_nb.norm();
if (theta * theta <= numeric_limits<double>::epsilon()) {
xi << n_omega_nb, n_t, n_v;
} else {
Matrix3 W = skewSymmetric(n_omega_nb / theta);
// Formula from Agrawal06iros, equation (14)
// simplified with Mathematica, and multiplying in n_t to avoid matrix math
double factor = (1 - theta / (2. * tan(0.5 * theta)));
Vector3 n_x = W * n_t;
Vector3 v = n_t - (0.5 * theta) * n_x + factor * (W * n_x);
Vector3 n_y = W * n_v;
Vector3 a = n_v - (0.5 * theta) * n_y + factor * (W * n_y);
xi << n_omega_nb, v, a;
}
return xi;
}
//------------------------------------------------------------------------------
Matrix9 NavState::AdjointMap() const {
// NOTE(frank): See Pose3::AdjointMap
const Matrix3 nRb = R();
Matrix3 pAr = skewSymmetric(t()) * nRb;
Matrix3 vAr = skewSymmetric(v()) * nRb;
Matrix9 adj;
// nR/bR nR/bP nR/bV nP/bR nP/bP nP/bV nV/bR nV/bP nV/bV
adj << nRb, Z_3x3, Z_3x3, pAr, nRb, Z_3x3, vAr, Z_3x3, nRb;
return adj;
}
//------------------------------------------------------------------------------
Matrix7 NavState::wedge(const Vector9& xi) {
const Matrix3 Omega = skewSymmetric(dR(xi));
Matrix7 T;
T << Omega, Z_3x3, dP(xi), Z_3x3, Omega, dV(xi), Vector6::Zero().transpose(), 1.0;
return T;
}
//------------------------------------------------------------------------------
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------
NavState NavState::update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const {
Vector9 xi;
Matrix39 D_xiP_state;
Vector3 b_v = bodyVelocity(F ? &D_xiP_state : 0);
double dt22 = 0.5 * dt * dt;
// Integrate on tangent space. TODO(frank): coriolis?
dR(xi) << dt * b_omega;
dP(xi) << dt * b_v + dt22 * b_acceleration;
dV(xi) << dt * b_acceleration;
// Bring back to manifold
Matrix9 D_newState_xi;
NavState newState = retract(xi, F, G1 || G2 ? &D_newState_xi : 0);
// Derivative wrt state is computed by retract directly
// However, as dP(xi) also depends on state, we need to add that contribution
if (F) {
F->middleRows<3>(3) += dt * D_t_t(F) * D_xiP_state;
}
// derivative wrt acceleration
if (G1) {
// D_newState_dPxi = D_newState_xi.middleCols<3>(3)
// D_dPxi_acc = dt22 * I_3x3
// D_newState_dVxi = D_newState_xi.rightCols<3>()
// D_dVxi_acc = dt * I_3x3
// *G2 = D_newState_acc = D_newState_dPxi * D_dPxi_acc + D_newState_dVxi * D_dVxi_acc
*G1 = D_newState_xi.middleCols<3>(3) * dt22
+ D_newState_xi.rightCols<3>() * dt;
}
// derivative wrt omega
if (G2) {
// D_newState_dRxi = D_newState_xi.leftCols<3>()
// D_dRxi_omega = dt * I_3x3
// *G1 = D_newState_omega = D_newState_dRxi * D_dRxi_omega
*G2 = D_newState_xi.leftCols<3>() * dt;
}
return newState;
}
//------------------------------------------------------------------------------
Vector9 NavState::coriolis(double dt, const Vector3& omega, bool secondOrder,
OptionalJacobian<9, 9> H) const {
TIE(nRb, n_t, n_v, *this);
const double dt2 = dt * dt;
const Vector3 omega_cross_vel = omega.cross(n_v);
Vector9 xi;
Matrix3 D_dP_R;
dR(xi) << nRb.unrotate((-dt) * omega, H ? &D_dP_R : 0);
dP(xi) << ((-dt2) * omega_cross_vel); // NOTE(luca): we got rid of the 2 wrt INS paper
dV(xi) << ((-2.0 * dt) * omega_cross_vel);
if (secondOrder) {
const Vector3 omega_cross2_t = omega.cross(omega.cross(n_t.vector()));
dP(xi) -= (0.5 * dt2) * omega_cross2_t;
dV(xi) -= dt * omega_cross2_t;
}
if (H) {
H->setZero();
const Matrix3 Omega = skewSymmetric(omega);
const Matrix3 D_cross_state = Omega * R();
H->setZero();
D_R_R(H) << D_dP_R;
D_t_v(H) << (-dt2) * D_cross_state;
D_v_v(H) << (-2.0 * dt) * D_cross_state;
if (secondOrder) {
const Matrix3 D_cross2_state = Omega * D_cross_state;
D_t_t(H) -= (0.5 * dt2) * D_cross2_state;
D_v_t(H) -= dt * D_cross2_state;
}
}
return xi;
}
//------------------------------------------------------------------------------
Vector9 NavState::correctPIM(const Vector9& pim, double dt,
const Vector3& n_gravity, const boost::optional<Vector3>& omegaCoriolis,
bool use2ndOrderCoriolis, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 9> H2) const {
const Rot3& nRb = R_;
const Velocity3& n_v = v_; // derivative is Ri !
const double dt22 = 0.5 * dt * dt;
Vector9 xi;
Matrix3 D_dP_Ri1, D_dP_Ri2, D_dP_nv, D_dV_Ri;
dR(xi) = dR(pim);
dP(xi) = dP(pim)
+ dt * nRb.unrotate(n_v, H1 ? &D_dP_Ri1 : 0, H2 ? &D_dP_nv : 0)
+ dt22 * nRb.unrotate(n_gravity, H1 ? &D_dP_Ri2 : 0);
dV(xi) = dV(pim) + dt * nRb.unrotate(n_gravity, H1 ? &D_dV_Ri : 0);
if (omegaCoriolis) {
xi += coriolis(dt, *omegaCoriolis, use2ndOrderCoriolis, H1);
}
if (H1 || H2) {
Matrix3 Ri = nRb.matrix();
if (H1) {
if (!omegaCoriolis)
H1->setZero(); // if coriolis H1 is already initialized
D_t_R(H1) += dt * D_dP_Ri1 + dt22 * D_dP_Ri2;
D_t_v(H1) += dt * D_dP_nv * Ri;
D_v_R(H1) += dt * D_dV_Ri;
}
if (H2) {
H2->setIdentity();
}
}
return xi;
}
//------------------------------------------------------------------------------
}/// namespace gtsam

257
gtsam/navigation/NavState.h Normal file
View File

@ -0,0 +1,257 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file NavState.h
* @brief Navigation state composing of attitude, position, and velocity
* @author Frank Dellaert
* @date July 2015
**/
#pragma once
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/ProductLieGroup.h>
namespace gtsam {
/// Velocity is currently typedef'd to Vector3
typedef Vector3 Velocity3;
/**
* Navigation state: Pose (rotation, translation) + velocity
* Implements semi-direct Lie group product of SO(3) and R^6, where R^6 is position/velocity
*/
class NavState: public LieGroup<NavState, 9> {
private:
// TODO(frank):
// - should we rename t_ to p_? if not, we should rename dP do dT
Rot3 R_; ///< Rotation nRb, rotates points/velocities in body to points/velocities in nav
Point3 t_; ///< position n_t, in nav frame
Velocity3 v_; ///< velocity n_v in nav frame
public:
typedef std::pair<Point3, Velocity3> PositionAndVelocity;
/// @name Constructors
/// @{
/// Default constructor
NavState() :
v_(Vector3::Zero()) {
}
/// Construct from attitude, position, velocity
NavState(const Rot3& R, const Point3& t, const Velocity3& v) :
R_(R), t_(t), v_(v) {
}
/// Construct from pose and velocity
NavState(const Pose3& pose, const Velocity3& v) :
R_(pose.rotation()), t_(pose.translation()), v_(v) {
}
/// Construct from Matrix group representation (no checking)
NavState(const Matrix7& T) :
R_(T.block<3, 3>(0, 0)), t_(T.block<3, 1>(0, 6)), v_(T.block<3, 1>(3, 6)) {
}
/// Construct from SO(3) and R^6
NavState(const Matrix3& R, const Vector9 tv) :
R_(R), t_(tv.head<3>()), v_(tv.tail<3>()) {
}
/// Named constructor with derivatives
static NavState FromPoseVelocity(const Pose3& pose, const Vector3& vel,
OptionalJacobian<9, 6> H1, OptionalJacobian<9, 3> H2);
/// @}
/// @name Component Access
/// @{
inline const Rot3& attitude() const {
return R_;
}
inline const Point3& position() const {
return t_;
}
inline const Velocity3& velocity() const {
return v_;
}
const Rot3& attitude(OptionalJacobian<3, 9> H) const;
const Point3& position(OptionalJacobian<3, 9> H) const;
const Velocity3& velocity(OptionalJacobian<3, 9> H) const;
const Pose3 pose() const {
return Pose3(attitude(), position());
}
/// @}
/// @name Derived quantities
/// @{
/// Return rotation matrix. Induces computation in quaternion mode
Matrix3 R() const {
return R_.matrix();
}
/// Return quaternion. Induces computation in matrix mode
Quaternion quaternion() const {
return R_.toQuaternion();
}
/// Return position as Vector3
Vector3 t() const {
return t_.vector();
}
/// Return velocity as Vector3. Computation-free.
const Vector3& v() const {
return v_;
}
// Return velocity in body frame
Velocity3 bodyVelocity(OptionalJacobian<3, 9> H = boost::none) const;
/// Return matrix group representation, in MATLAB notation:
/// nTb = [nRb 0 n_t; 0 nRb n_v; 0 0 1]
/// With this embedding in GL(3), matrix product agrees with compose
Matrix7 matrix() const;
/// @}
/// @name Testable
/// @{
/// print
void print(const std::string& s = "") const;
/// equals
bool equals(const NavState& other, double tol = 1e-8) const;
/// @}
/// @name Group
/// @{
/// identity for group operation
static NavState identity() {
return NavState();
}
/// inverse transformation with derivatives
NavState inverse() const;
/// Group compose is the semi-direct product as specified below:
/// nTc = nTb * bTc = (nRb * bRc, nRb * b_t + n_t, nRb * b_v + n_v)
NavState operator*(const NavState& bTc) const;
/// Native group action is on position/velocity pairs *in the body frame* as follows:
/// nTb * (b_t,b_v) = (nRb * b_t + n_t, nRb * b_v + n_v)
PositionAndVelocity operator*(const PositionAndVelocity& b_tv) const;
/// Act on position alone, n_t = nRb * b_t + n_t
Point3 operator*(const Point3& b_t) const;
/// Act on velocity alone, n_v = nRb * b_v + n_v
Velocity3 operator*(const Velocity3& b_v) const;
/// @}
/// @name Manifold
/// @{
// Tangent space sugar.
// TODO(frank): move to private navstate namespace in cpp
static Eigen::Block<Vector9, 3, 1> dR(Vector9& v) {
return v.segment<3>(0);
}
static Eigen::Block<Vector9, 3, 1> dP(Vector9& v) {
return v.segment<3>(3);
}
static Eigen::Block<Vector9, 3, 1> dV(Vector9& v) {
return v.segment<3>(6);
}
static Eigen::Block<const Vector9, 3, 1> dR(const Vector9& v) {
return v.segment<3>(0);
}
static Eigen::Block<const Vector9, 3, 1> dP(const Vector9& v) {
return v.segment<3>(3);
}
static Eigen::Block<const Vector9, 3, 1> dV(const Vector9& v) {
return v.segment<3>(6);
}
// Chart at origin, constructs components separately (as opposed to Expmap)
struct ChartAtOrigin {
static NavState Retract(const Vector9& xi, //
OptionalJacobian<9, 9> H = boost::none);
static Vector9 Local(const NavState& x, //
OptionalJacobian<9, 9> H = boost::none);
};
/// @}
/// @name Lie Group
/// @{
/// Exponential map at identity - create a NavState from canonical coordinates
static NavState Expmap(const Vector9& xi, //
OptionalJacobian<9, 9> H = boost::none);
/// Log map at identity - return the canonical coordinates for this NavState
static Vector9 Logmap(const NavState& p, //
OptionalJacobian<9, 9> H = boost::none);
/// Calculate Adjoint map, a 9x9 matrix that takes a tangent vector in the body frame, and transforms
/// it to a tangent vector at identity, such that Exmap(AdjointMap()*xi) = (*this) * Exmpap(xi);
Matrix9 AdjointMap() const;
/// wedge creates Lie algebra element from tangent vector
static Matrix7 wedge(const Vector9& xi);
/// @}
/// @name Dynamics
/// @{
/// Integrate forward in time given angular velocity and acceleration in body frame
/// Uses second order integration for position, returns derivatives except dt.
NavState update(const Vector3& b_acceleration, const Vector3& b_omega,
const double dt, OptionalJacobian<9, 9> F, OptionalJacobian<9, 3> G1,
OptionalJacobian<9, 3> G2) const;
/// Compute tangent space contribution due to Coriolis forces
Vector9 coriolis(double dt, const Vector3& omega, bool secondOrder = false,
OptionalJacobian<9, 9> H = boost::none) const;
/// Correct preintegrated tangent vector with our velocity and rotated gravity,
/// taking into account Coriolis forces if omegaCoriolis is given.
Vector9 correctPIM(const Vector9& pim, double dt, const Vector3& n_gravity,
const boost::optional<Vector3>& omegaCoriolis, bool use2ndOrderCoriolis =
false, OptionalJacobian<9, 9> H1 = boost::none,
OptionalJacobian<9, 9> H2 = boost::none) const;
private:
/// @{
/// serialization
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
ar & BOOST_SERIALIZATION_NVP(v_);
}
/// @}
};
// Specialize NavState traits to use a Retract/Local that agrees with IMUFactors
template<>
struct traits<NavState> : Testable<NavState>, internal::LieGroupTraits<NavState> {
};
// Partial specialization of wedge
// TODO: deprecate, make part of traits
template<>
inline Matrix wedge<NavState>(const Vector& xi) {
return NavState::wedge(xi);
}
} // namespace gtsam

111
gtsam/navigation/PreintegratedRotation.cpp Normal file
View File

@ -0,0 +1,111 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PreintegratedRotation.cpp
* @author Luca Carlone
* @author Stephen Williams
* @author Richard Roberts
* @author Vadim Indelman
* @author David Jensen
* @author Frank Dellaert
**/
#include "PreintegratedRotation.h"
using namespace std;
namespace gtsam {
void PreintegratedRotation::Params::print(const string& s) const {
cout << s << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
if (omegaCoriolis)
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")"
<< endl;
if (body_P_sensor)
body_P_sensor->print("body_P_sensor");
}
void PreintegratedRotation::resetIntegration() {
deltaTij_ = 0.0;
deltaRij_ = Rot3();
delRdelBiasOmega_ = Z_3x3;
}
void PreintegratedRotation::print(const string& s) const {
cout << s << endl;
cout << " deltaTij [" << deltaTij_ << "]" << endl;
cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << endl;
}
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
double tol) const {
return deltaRij_.equals(other.deltaRij_, tol)
&& fabs(deltaTij_ - other.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
}
Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> D_incrR_integratedOmega) const {
// First we compensate the measurements for the bias
Vector3 correctedOmega = measuredOmega - biasHat;
// Then compensate for sensor-body displacement: we express the quantities
// (originally in the IMU frame) into the body frame
if (p_->body_P_sensor) {
Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix();
// rotation rate vector in the body frame
correctedOmega = body_R_sensor * correctedOmega;
}
// rotation vector describing rotation increment computed from the
// current rotation rate measurement
const Vector3 integratedOmega = correctedOmega * deltaT;
return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
}
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
Matrix3* F) {
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
D_incrR_integratedOmega);
// Update deltaTij and rotation
deltaTij_ += deltaT;
deltaRij_ = deltaRij_.compose(incrR, F);
// Update Jacobian
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- *D_incrR_integratedOmega * deltaT;
}
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H) const {
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasOmegaIncr;
const Rot3 deltaRij_biascorrected = deltaRij_.expmap(biasInducedOmega,
boost::none, H);
if (H)
(*H) *= delRdelBiasOmega_;
return deltaRij_biascorrected;
}
Vector3 PreintegratedRotation::integrateCoriolis(const Rot3& rot_i) const {
if (!p_->omegaCoriolis)
return Vector3::Zero();
return rot_i.transpose() * (*p_->omegaCoriolis) * deltaTij_;
}
} // namespace gtsam

169
gtsam/navigation/PreintegratedRotation.h
View File

@ -21,7 +21,8 @@
#pragma once
#include <gtsam/geometry/Rot3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
namespace gtsam {
@ -31,124 +32,104 @@ namespace gtsam {
* It includes the definitions of the preintegrated rotation.
*/
class PreintegratedRotation {
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
double deltaTij_; ///< Time interval from i to j
/// Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 delRdelBiasOmega_;
Matrix3 gyroscopeCovariance_; ///< continuous-time "Covariance" of gyroscope measurements
public:
/**
* Default constructor, initializes the variables in the base class
*/
PreintegratedRotation(const Matrix3& measuredOmegaCovariance) :
deltaRij_(Rot3()), deltaTij_(0.0), delRdelBiasOmega_(Z_3x3), gyroscopeCovariance_(
measuredOmegaCovariance) {
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params {
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
Params() :
gyroscopeCovariance(I_3x3) {
}
virtual void print(const std::string& s) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance);
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}
};
protected:
double deltaTij_; ///< Time interval from i to j
Rot3 deltaRij_; ///< Preintegrated relative orientation (in frame i)
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
/// Parameters
boost::shared_ptr<Params> p_;
/// Default constructor for serialization
PreintegratedRotation() {
}
/// methods to access class variables
Matrix3 deltaRij() const {
return deltaRij_.matrix();
} // expensive
Vector3 thetaRij(OptionalJacobian<3, 3> H = boost::none) const {
return Rot3::Logmap(deltaRij_, H);
} // super-expensive
public:
/// Default constructor, resets integration to zero
explicit PreintegratedRotation(const boost::shared_ptr<Params>& p) :
p_(p) {
resetIntegration();
}
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/// @name Access instance variables
/// @{
const double& deltaTij() const {
return deltaTij_;
}
const Rot3& deltaRij() const {
return deltaRij_;
}
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const Matrix3& gyroscopeCovariance() const {
return gyroscopeCovariance_;
}
/// @}
/// Needed for testable
void print(const std::string& s) const {
std::cout << s << std::endl;
std::cout << " deltaTij [" << deltaTij_ << "]" << std::endl;
std::cout << " deltaRij.ypr = (" << deltaRij_.ypr().transpose() << ")" << std::endl;
}
/// @name Testable
/// @{
/// Needed for testable
bool equals(const PreintegratedRotation& expected, double tol) const {
return deltaRij_.equals(expected.deltaRij_, tol)
&& fabs(deltaTij_ - expected.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, expected.delRdelBiasOmega_,
tol)
&& equal_with_abs_tol(gyroscopeCovariance_,
expected.gyroscopeCovariance_, tol);
}
void print(const std::string& s) const;
bool equals(const PreintegratedRotation& other, double tol) const;
/// Re-initialize PreintegratedMeasurements
void resetIntegration() {
deltaRij_ = Rot3();
deltaTij_ = 0.0;
delRdelBiasOmega_ = Z_3x3;
}
/// @}
/// Update preintegrated measurements
void updateIntegratedRotationAndDeltaT(const Rot3& incrR, const double deltaT,
OptionalJacobian<3, 3> H = boost::none) {
deltaRij_ = deltaRij_.compose(incrR, H, boost::none);
deltaTij_ += deltaT;
}
/// Take the gyro measurement, correct it using the (constant) bias estimate
/// and possibly the sensor pose, and then integrate it forward in time to yield
/// an incremental rotation.
Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat,
double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const;
/**
* Update Jacobians to be used during preintegration
* TODO: explain arguments
*/
void update_delRdelBiasOmega(const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- D_Rincr_integratedOmega * deltaT;
}
/// Calculate an incremental rotation given the gyro measurement and a time interval,
/// and update both deltaTij_ and deltaRij_.
void integrateMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT, Matrix3* D_incrR_integratedOmega,
Matrix3* F);
/// Return a bias corrected version of the integrated rotation - expensive
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr) const {
return deltaRij_ * Rot3::Expmap(delRdelBiasOmega_ * biasOmegaIncr);
}
/// Get so<3> version of bias corrected rotation, with optional Jacobian
// Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = boost::none) const {
// First, we correct deltaRij using the biasOmegaIncr, rotated
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
if (H) {
Matrix3 Jrinv_theta_bc;
// This was done via an expmap, now we go *back* to so<3>
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected,
Jrinv_theta_bc);
const Matrix3 Jr_JbiasOmegaIncr = //
Rot3::ExpmapDerivative(delRdelBiasOmega_ * biasOmegaIncr);
(*H) = Jrinv_theta_bc * Jr_JbiasOmegaIncr * delRdelBiasOmega_;
return biascorrectedOmega;
}
//else
return Rot3::Logmap(deltaRij_biascorrected);
}
/// Return a bias corrected version of the integrated rotation, with optional Jacobian
Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = boost::none) const;
/// Integrate coriolis correction in body frame rot_i
Vector3 integrateCoriolis(const Rot3& rot_i,
const Vector3& omegaCoriolis) const {
return rot_i.transpose() * omegaCoriolis * deltaTij();
}
Vector3 integrateCoriolis(const Rot3& rot_i) const;
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { // NOLINT
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaRij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
}
};
} /// namespace gtsam
} // namespace gtsam

567
gtsam/navigation/PreintegrationBase.cpp
View File

@ -20,338 +20,293 @@
**/
#include "PreintegrationBase.h"
#include <boost/make_shared.hpp>
using namespace std;
namespace gtsam {
PreintegrationBase::PreintegrationBase(const imuBias::ConstantBias& bias,
const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3&integrationErrorCovariance,
const bool use2ndOrderIntegration)
: PreintegratedRotation(measuredOmegaCovariance),
biasHat_(bias),
use2ndOrderIntegration_(use2ndOrderIntegration),
deltaPij_(Vector3::Zero()),
deltaVij_(Vector3::Zero()),
delPdelBiasAcc_(Z_3x3),
delPdelBiasOmega_(Z_3x3),
delVdelBiasAcc_(Z_3x3),
delVdelBiasOmega_(Z_3x3),
accelerometerCovariance_(measuredAccCovariance),
integrationCovariance_(integrationErrorCovariance) {
//------------------------------------------------------------------------------
void PreintegrationBase::Params::print(const string& s) const {
PreintegratedRotation::Params::print(s);
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl;
cout << "integrationCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl;
if (omegaCoriolis && use2ndOrderCoriolis)
cout << "Using 2nd-order Coriolis" << endl;
if (body_P_sensor)
body_P_sensor->print(" ");
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
}
/// Needed for testable
void PreintegrationBase::print(const std::string& s) const {
PreintegratedRotation::print(s);
std::cout << " deltaPij [ " << deltaPij_.transpose() << " ]" << std::endl;
std::cout << " deltaVij [ " << deltaVij_.transpose() << " ]" << std::endl;
biasHat_.print(" biasHat");
}
/// Needed for testable
bool PreintegrationBase::equals(const PreintegrationBase& other, double tol) const {
return PreintegratedRotation::equals(other, tol) && biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(deltaPij_, other.deltaPij_, tol)
&& equal_with_abs_tol(deltaVij_, other.deltaVij_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol)
&& equal_with_abs_tol(accelerometerCovariance_, other.accelerometerCovariance_, tol)
&& equal_with_abs_tol(integrationCovariance_, other.integrationCovariance_, tol);
}
/// Re-initialize PreintegratedMeasurements
//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
PreintegratedRotation::resetIntegration();
deltaPij_ = Vector3::Zero();
deltaVij_ = Vector3::Zero();
deltaTij_ = 0.0;
deltaXij_ = NavState();
delRdelBiasOmega_ = Z_3x3;
delPdelBiasAcc_ = Z_3x3;
delPdelBiasOmega_ = Z_3x3;
delVdelBiasAcc_ = Z_3x3;
delVdelBiasOmega_ = Z_3x3;
}
/// Update preintegrated measurements
void PreintegrationBase::updatePreintegratedMeasurements(const Vector3& correctedAcc,
const Rot3& incrR, const double deltaT,
OptionalJacobian<9, 9> F) {
const Matrix3 dRij = deltaRij(); // expensive
const Vector3 temp = dRij * correctedAcc * deltaT;
if (!use2ndOrderIntegration_) {
deltaPij_ += deltaVij_ * deltaT;
} else {
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
}
deltaVij_ += temp;
Matrix3 R_i, F_angles_angles;
if (F)
R_i = deltaRij(); // has to be executed before updateIntegratedRotationAndDeltaT as that updates deltaRij
updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
if (F) {
const Matrix3 F_vel_angles = -R_i * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles;
if (use2ndOrderIntegration_)
F_pos_angles = 0.5 * F_vel_angles * deltaT;
else
F_pos_angles = Z_3x3;
// pos vel angle
*F << //
I_3x3, I_3x3 * deltaT, F_pos_angles, // pos
Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles; // angle
}
//------------------------------------------------------------------------------
void PreintegrationBase::print(const string& s) const {
cout << s << endl;
cout << " deltaTij [" << deltaTij_ << "]" << endl;
cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl;
cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl;
cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl;
biasHat_.print(" biasHat");
}
/// Update Jacobians to be used during preintegration
void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega,
const Rot3& incrR, double deltaT) {
const Matrix3 dRij = deltaRij(); // expensive
const Matrix3 temp = -dRij * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega();
if (!use2ndOrderIntegration_) {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT;
} else {
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * dRij * deltaT * deltaT;
delPdelBiasOmega_ += deltaT * (delVdelBiasOmega_ + temp * 0.5);
}
delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp;
update_delRdelBiasOmega(D_Rincr_integratedOmega, incrR, deltaT);
//------------------------------------------------------------------------------
bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const {
return fabs(deltaTij_ - other.deltaTij_) < tol
&& deltaXij_.equals(other.deltaXij_, tol)
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
&& equal_with_abs_tol(delPdelBiasAcc_, other.delPdelBiasAcc_, tol)
&& equal_with_abs_tol(delPdelBiasOmega_, other.delPdelBiasOmega_, tol)
&& equal_with_abs_tol(delVdelBiasAcc_, other.delVdelBiasAcc_, tol)
&& equal_with_abs_tol(delVdelBiasOmega_, other.delVdelBiasOmega_, tol);
}
void PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& measuredAcc, const Vector3& measuredOmega, Vector3& correctedAcc,
Vector3& correctedOmega, boost::optional<const Pose3&> body_P_sensor) {
correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
correctedOmega = biasHat_.correctGyroscope(measuredOmega);
//------------------------------------------------------------------------------
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
// Then compensate for sensor-body displacement: we express the quantities
// Correct for bias in the sensor frame
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
// Compensate for sensor-body displacement if needed: we express the quantities
// (originally in the IMU frame) into the body frame
if (body_P_sensor) {
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc
- body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
}
// Equations below assume the "body" frame is the CG
if (p().body_P_sensor) {
// Correct omega to rotation rate vector in the body frame
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
j_correctedOmega = bRs * j_correctedOmega;
/// Predict state at time j
//------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict(
const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis,
boost::optional<Vector3&> deltaPij_biascorrected_out,
boost::optional<Vector3&> deltaVij_biascorrected_out) const {
// Correct acceleration
j_correctedAcc = bRs * j_correctedAcc;
const imuBias::ConstantBias biasIncr = bias_i - biasHat();
const Vector3& biasAccIncr = biasIncr.accelerometer();
const Vector3& biasOmegaIncr = biasIncr.gyroscope();
// Jacobians
if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero();
if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs;
const Rot3& Rot_i = pose_i.rotation();
const Matrix3 Rot_i_matrix = Rot_i.matrix();
const Vector3 pos_i = pose_i.translation().vector();
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr
+ delPdelBiasOmega() * biasOmegaIncr;
if (deltaPij_biascorrected_out) // if desired, store this
*deltaPij_biascorrected_out = deltaPij_biascorrected;
const double dt = deltaTij(), dt2 = dt * dt;
Vector3 pos_j = pos_i + Rot_i_matrix * deltaPij_biascorrected + vel_i * dt
- omegaCoriolis.cross(vel_i) * dt2 // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * dt2;
const Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr
+ delVdelBiasOmega() * biasOmegaIncr;
if (deltaVij_biascorrected_out) // if desired, store this
*deltaVij_biascorrected_out = deltaVij_biascorrected;
Vector3 vel_j = Vector3(
vel_i + Rot_i_matrix * deltaVij_biascorrected - 2 * omegaCoriolis.cross(vel_i) * dt // Coriolis term
+ gravity * dt);
if (use2ndOrderCoriolis) {
pos_j += -0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt2; // 2nd order coriolis term for position
vel_j += -omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * dt; // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);
// deltaRij_biascorrected = deltaRij * expmap(delRdelBiasOmega * biasOmegaIncr)
const Vector3 biascorrectedOmega = Rot3::Logmap(deltaRij_biascorrected);
const Vector3 correctedOmega = biascorrectedOmega
- Rot_i_matrix.transpose() * omegaCoriolis * dt; // Coriolis term
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
const Rot3 Rot_j = Rot_i.compose(correctedDeltaRij);
const Pose3 pose_j = Pose3(Rot_j, Point3(pos_j));
return PoseVelocityBias(pose_j, vel_j, bias_i); // bias is predicted as a constant
}
/// Compute errors w.r.t. preintegrated measurements and Jacobians wrpt pose_i, vel_i, bias_i, pose_j, bias_j
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i,
const Vector3& gravity,
const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis,
OptionalJacobian<9, 6> H1,
OptionalJacobian<9, 3> H2,
OptionalJacobian<9, 6> H3,
OptionalJacobian<9, 3> H4,
OptionalJacobian<9, 6> H5) const {
// We need the mismatch w.r.t. the biases used for preintegration
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
// we give some shorter name to rotations and translations
const Rot3& Ri = pose_i.rotation();
const Matrix3 Ri_transpose_matrix = Ri.transpose();
const Rot3& Rj = pose_j.rotation();
const Vector3 pos_j = pose_j.translation().vector();
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, omegaCoriolis,
use2ndOrderCoriolis, deltaPij_biascorrected,
deltaVij_biascorrected);
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fp = Ri_transpose_matrix * (pos_j - predictedState_j.pose.translation().vector());
// Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fv = Ri_transpose_matrix * (vel_j - predictedState_j.velocity);
// fR will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
/* ---------------------------------------------------------------------------------------------------- */
// Get Get so<3> version of bias corrected rotation
// If H5 is asked for, we will need the Jacobian, which we store in H5
// H5 will then be corrected below to take into account the Coriolis effect
Matrix3 D_cThetaRij_biasOmegaIncr;
const Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr,
H5 ? &D_cThetaRij_biasOmegaIncr : 0);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
// Calculate Jacobians, matrices below needed only for some Jacobians...
Vector3 fR;
Matrix3 D_cDeltaRij_cOmega, D_coriolis, D_fR_fRrot, Ritranspose_omegaCoriolisHat;
if (H1 || H2)
Ritranspose_omegaCoriolisHat = Ri_transpose_matrix * skewSymmetric(omegaCoriolis);
// This is done to save computation: we ask for the jacobians only when they are needed
const double dt = deltaTij(), dt2 = dt*dt;
Rot3 fRrot;
const Rot3 RiBetweenRj = Rot3(Ri_transpose_matrix) * Rj;
if (H1 || H2 || H3 || H4 || H5) {
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega, D_cDeltaRij_cOmega);
// Residual rotation error
fRrot = correctedDeltaRij.between(RiBetweenRj);
fR = Rot3::Logmap(fRrot, D_fR_fRrot);
D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
} else {
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
// Residual rotation error
fRrot = correctedDeltaRij.between(RiBetweenRj);
fR = Rot3::Logmap(fRrot);
}
if (H1) {
Matrix3 dfPdPi = -I_3x3, dfVdPi = Z_3x3;
if (use2ndOrderCoriolis) {
// this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
const Matrix3 temp = Ritranspose_omegaCoriolisHat
* (-Ritranspose_omegaCoriolisHat.transpose());
dfPdPi += 0.5 * temp * dt2;
dfVdPi += temp * dt;
// Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation().vector();
if (!b_arm.isZero()) {
// Subtract out the the centripetal acceleration from the measured one
// to get linear acceleration vector in the body frame:
const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
j_correctedAcc -= body_Omega_body * b_velocity_bs;
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (D_correctedAcc_measuredOmega) {
double wdp = j_correctedOmega.dot(b_arm);
*D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
+ j_correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * j_measuredOmega.transpose();
}
}
(*H1) <<
// dfP/dRi
skewSymmetric(fp + deltaPij_biascorrected),
// dfP/dPi
dfPdPi,
// dfV/dRi
skewSymmetric(fv + deltaVij_biascorrected),
// dfV/dPi
dfVdPi,
// dfR/dRi
D_fR_fRrot * (-Rj.between(Ri).matrix() - fRrot.inverse().matrix() * D_coriolis),
// dfR/dPi
Z_3x3;
}
if (H2) {
(*H2) <<
// dfP/dVi
-Ri_transpose_matrix * dt + Ritranspose_omegaCoriolisHat * dt2, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
-Ri_transpose_matrix + 2 * Ritranspose_omegaCoriolisHat * dt, // Coriolis term
// dfR/dVi
Z_3x3;
}
if (H3) {
(*H3) <<
// dfP/dPosej
Z_3x3, Ri_transpose_matrix * Rj.matrix(),
// dfV/dPosej
Matrix::Zero(3, 6),
// dfR/dPosej
D_fR_fRrot, Z_3x3;
}
if (H4) {
(*H4) <<
// dfP/dVj
Z_3x3,
// dfV/dVj
Ri_transpose_matrix,
// dfR/dVj
Z_3x3;
}
if (H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
(*H5) <<
// dfP/dBias
-delPdelBiasAcc(), -delPdelBiasOmega(),
// dfV/dBias
-delVdelBiasAcc(), -delVdelBiasOmega(),
// dfR/dBias
Z_3x3, D_fR_fRrot * (-fRrot.inverse().matrix() * JbiasOmega);
}
Vector9 r;
r << fp, fv, fR;
return r;
// Do update in one fell swoop
return make_pair(j_correctedAcc, j_correctedOmega);
}
ImuBase::ImuBase()
: gravity_(Vector3(0.0, 0.0, 9.81)),
omegaCoriolis_(Vector3(0.0, 0.0, 0.0)),
body_P_sensor_(boost::none),
use2ndOrderCoriolis_(false) {
//------------------------------------------------------------------------------
NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt,
OptionalJacobian<9, 9> D_updated_current,
OptionalJacobian<9, 3> D_updated_measuredAcc,
OptionalJacobian<9, 3> D_updated_measuredOmega) const {
Vector3 j_correctedAcc, j_correctedOmega;
Matrix3 D_correctedAcc_measuredAcc, //
D_correctedAcc_measuredOmega, //
D_correctedOmega_measuredOmega;
bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega,
(needDerivs ? &D_correctedAcc_measuredAcc : 0),
(needDerivs ? &D_correctedAcc_measuredOmega : 0),
(needDerivs ? &D_correctedOmega_measuredOmega : 0));
// Do update in one fell swoop
Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current,
(needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
(needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
if (needDerivs) {
*D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
*D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
if (!p().body_P_sensor->translation().vector().isZero()) {
*D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
}
}
return updated;
}
ImuBase::ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor, const bool use2ndOrderCoriolis)
: gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor),
use2ndOrderCoriolis_(use2ndOrderCoriolis) {
//------------------------------------------------------------------------------
void PreintegrationBase::update(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt,
Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();
// Do update
deltaTij_ += dt;
deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
// Update Jacobians
// TODO(frank): we are repeating some computation here: accessible in F ?
Vector3 j_correctedAcc, j_correctedOmega;
boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
Matrix3 D_acc_R;
oldRij.rotate(j_correctedAcc, D_acc_R);
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
const Vector3 integratedOmega = j_correctedOmega * dt;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- *D_incrR_integratedOmega * dt;
double dt22 = 0.5 * dt * dt;
const Matrix3 dRij = oldRij.matrix(); // expensive
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
delVdelBiasAcc_ += -dRij * dt;
delVdelBiasOmega_ += D_acc_biasOmega * dt;
}
} /// namespace gtsam
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// Correct deltaRij, derivative is delRdelBiasOmega_
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_correctedRij_bias;
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
const Rot3 correctedRij = deltaRij().expmap(biasInducedOmega, boost::none,
H ? &D_correctedRij_bias : 0);
if (H)
D_correctedRij_bias *= delRdelBiasOmega_;
Vector9 xi;
Matrix3 D_dR_correctedRij;
// TODO(frank): could line below be simplified? It is equivalent to
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
NavState::dR(xi) = Rot3::Logmap(correctedRij, H ? &D_dR_correctedRij : 0);
NavState::dP(xi) = deltaPij() + delPdelBiasAcc_ * biasIncr.accelerometer()
+ delPdelBiasOmega_ * biasIncr.gyroscope();
NavState::dV(xi) = deltaVij() + delVdelBiasAcc_ * biasIncr.accelerometer()
+ delVdelBiasOmega_ * biasIncr.gyroscope();
if (H) {
Matrix36 D_dR_bias, D_dP_bias, D_dV_bias;
D_dR_bias << Z_3x3, D_dR_correctedRij * D_correctedRij_bias;
D_dP_bias << delPdelBiasAcc_, delPdelBiasOmega_;
D_dV_bias << delVdelBiasAcc_, delVdelBiasOmega_;
(*H) << D_dR_bias, D_dP_bias, D_dV_bias;
}
return xi;
}
//------------------------------------------------------------------------------
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const {
// correct for bias
Matrix96 D_biasCorrected_bias;
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
H2 ? &D_biasCorrected_bias : 0);
// integrate on tangent space
Matrix9 D_delta_state, D_delta_biasCorrected;
Vector9 xi = state_i.correctPIM(biasCorrected, deltaTij_, p().n_gravity,
p().omegaCoriolis, p().use2ndOrderCoriolis, H1 ? &D_delta_state : 0,
H2 ? &D_delta_biasCorrected : 0);
// Use retract to get back to NavState manifold
Matrix9 D_predict_state, D_predict_delta;
NavState state_j = state_i.retract(xi, D_predict_state, D_predict_delta);
if (H1)
*H1 = D_predict_state + D_predict_delta * D_delta_state;
if (H2)
*H2 = D_predict_delta * D_delta_biasCorrected * D_biasCorrected_bias;
return state_j;
}
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1,
OptionalJacobian<9, 3> H2, OptionalJacobian<9, 6> H3,
OptionalJacobian<9, 3> H4, OptionalJacobian<9, 6> H5) const {
// Note that derivative of constructors below is not identity for velocity, but
// a 9*3 matrix == Z_3x3, Z_3x3, state.R().transpose()
NavState state_i(pose_i, vel_i);
NavState state_j(pose_j, vel_j);
/// Predict state at time j
Matrix99 D_predict_state_i;
Matrix96 D_predict_bias_i;
NavState predictedState_j = predict(state_i, bias_i,
H1 || H2 ? &D_predict_state_i : 0, H5 ? &D_predict_bias_i : 0);
Matrix9 D_error_state_j, D_error_predict;
Vector9 error = state_j.localCoordinates(predictedState_j,
H3 || H4 ? &D_error_state_j : 0, H1 || H2 || H5 ? &D_error_predict : 0);
// Separate out derivatives in terms of 5 arguments
// Note that doing so requires special treatment of velocities, as when treated as
// separate variables the retract applied will not be the semi-direct product in NavState
// Instead, the velocities in nav are updated using a straight addition
// This is difference is accounted for by the R().transpose calls below
if (H1)
*H1 << D_error_predict * D_predict_state_i.leftCols<6>();
if (H2)
*H2
<< D_error_predict * D_predict_state_i.rightCols<3>()
* state_i.R().transpose();
if (H3)
*H3 << D_error_state_j.leftCols<6>();
if (H4)
*H4 << D_error_state_j.rightCols<3>() * state_j.R().transpose();
if (H5)
*H5 << D_error_predict * D_predict_bias_i;
return error;
}
//------------------------------------------------------------------------------
PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Vector3& n_gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis) {
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
boost::shared_ptr<Params> q = boost::make_shared<Params>(p());
q->n_gravity = n_gravity;
q->omegaCoriolis = omegaCoriolis;
q->use2ndOrderCoriolis = use2ndOrderCoriolis;
p_ = q;
return PoseVelocityBias(predict(NavState(pose_i, vel_i), bias_i), bias_i);
}
//------------------------------------------------------------------------------
}/// namespace gtsam

270
gtsam/navigation/PreintegrationBase.h
View File

@ -22,25 +22,26 @@
#pragma once
#include <gtsam/navigation/PreintegratedRotation.h>
#include <gtsam/navigation/NavState.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <boost/make_shared.hpp>
namespace gtsam {
/**
* Struct to hold all state variables of returned by Predict function
*/
/// @deprecated
struct PoseVelocityBias {
Pose3 pose;
Vector3 velocity;
imuBias::ConstantBias bias;
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity, const imuBias::ConstantBias _bias)
: pose(_pose),
velocity(_velocity),
bias(_bias) {
PoseVelocityBias(const Pose3& _pose, const Vector3& _velocity,
const imuBias::ConstantBias _bias) :
pose(_pose), velocity(_velocity), bias(_bias) {
}
PoseVelocityBias(const NavState& navState, const imuBias::ConstantBias _bias) :
pose(navState.pose()), velocity(navState.velocity()), bias(_bias) {
}
NavState navState() const {
return NavState(pose, velocity);
}
};
@ -50,51 +51,126 @@ struct PoseVelocityBias {
* It includes the definitions of the preintegrated variables and the methods
* to access, print, and compare them.
*/
class PreintegrationBase : public PreintegratedRotation {
class PreintegrationBase {
imuBias::ConstantBias biasHat_; ///< Acceleration and angular rate bias values used during preintegration
bool use2ndOrderIntegration_; ///< Controls the order of integration
public:
Vector3 deltaPij_; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij_; ///< Preintegrated relative velocity (in global frame)
/// Parameters for pre-integration:
/// Usage: Create just a single Params and pass a shared pointer to the constructor
struct Params: PreintegratedRotation::Params {
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
Vector3 n_gravity; ///< Gravity vector in nav frame
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
/// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below
Params(const Vector3& n_gravity) :
accelerometerCovariance(I_3x3), integrationCovariance(I_3x3), use2ndOrderCoriolis(
false), n_gravity(n_gravity) {
}
const Matrix3 accelerometerCovariance_; ///< continuous-time "Covariance" of accelerometer measurements
const Matrix3 integrationCovariance_; ///< continuous-time "Covariance" describing integration uncertainty
/// (to compensate errors in Euler integration)
// Default Params for a Z-down navigation frame, such as NED: gravity points along positive Z-axis
static boost::shared_ptr<Params> MakeSharedD(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, g));
}
public:
// Default Params for a Z-up navigation frame, such as ENU: gravity points along negative Z-axis
static boost::shared_ptr<Params> MakeSharedU(double g = 9.81) {
return boost::make_shared<Params>(Vector3(0, 0, -g));
}
void print(const std::string& s) const;
protected:
/// Default constructor for serialization only: uninitialized!
Params();
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation::Params);
ar & BOOST_SERIALIZATION_NVP(accelerometerCovariance);
ar & BOOST_SERIALIZATION_NVP(integrationCovariance);
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
};
protected:
double deltaTij_; ///< Time interval from i to j
/**
* Default constructor, initializes the variables in the base class
* @param bias Current estimate of acceleration and rotation rate biases
* @param use2ndOrderIntegration Controls the order of integration
* (if false: p(t+1) = p(t) + v(t) deltaT ; if true: p(t+1) = p(t) + v(t) deltaT + 0.5 * acc(t) deltaT^2)
* Preintegrated navigation state, from frame i to frame j
* Note: relative position does not take into account velocity at time i, see deltap+, in [2]
* Note: velocity is now also in frame i, as opposed to deltaVij in [2]
*/
PreintegrationBase(const imuBias::ConstantBias& bias, const Matrix3& measuredAccCovariance,
const Matrix3& measuredOmegaCovariance,
const Matrix3&integrationErrorCovariance, const bool use2ndOrderIntegration);
NavState deltaXij_;
/// methods to access class variables
bool use2ndOrderIntegration() const {
return use2ndOrderIntegration_;
/// Parameters
boost::shared_ptr<Params> p_;
/// Acceleration and gyro bias used for preintegration
imuBias::ConstantBias biasHat_;
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
Matrix3 delPdelBiasOmega_; ///< Jacobian of preintegrated position w.r.t. angular rate bias
Matrix3 delVdelBiasAcc_; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega_; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
/// Default constructor for serialization
PreintegrationBase() {
}
const Vector3& deltaPij() const {
return deltaPij_;
public:
/**
* Constructor, initializes the variables in the base class
* @param bias Current estimate of acceleration and rotation rate biases
* @param p Parameters, typically fixed in a single application
*/
PreintegrationBase(const boost::shared_ptr<Params>& p,
const imuBias::ConstantBias& biasHat) :
p_(p), biasHat_(biasHat) {
resetIntegration();
}
const Vector3& deltaVij() const {
return deltaVij_;
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
const Params& p() const {
return *boost::static_pointer_cast<Params>(p_);
}
void set_body_P_sensor(const Pose3& body_P_sensor) {
p_->body_P_sensor = body_P_sensor;
}
/// getters
const NavState& deltaXij() const {
return deltaXij_;
}
const double& deltaTij() const {
return deltaTij_;
}
const Rot3& deltaRij() const {
return deltaXij_.attitude();
}
Vector3 deltaPij() const {
return deltaXij_.position().vector();
}
Vector3 deltaVij() const {
return deltaXij_.velocity();
}
const imuBias::ConstantBias& biasHat() const {
return biasHat_;
}
Vector6 biasHatVector() const {
return biasHat_.vector();
} // expensive
const Matrix3& delRdelBiasOmega() const {
return delRdelBiasOmega_;
}
const Matrix3& delPdelBiasAcc() const {
return delPdelBiasAcc_;
}
@ -108,11 +184,9 @@ class PreintegrationBase : public PreintegratedRotation {
return delVdelBiasOmega_;
}
const Matrix3& accelerometerCovariance() const {
return accelerometerCovariance_;
}
const Matrix3& integrationCovariance() const {
return integrationCovariance_;
// Exposed for MATLAB
Vector6 biasHatVector() const {
return biasHat_.vector();
}
/// print
@ -121,50 +195,62 @@ class PreintegrationBase : public PreintegratedRotation {
/// check equality
bool equals(const PreintegrationBase& other, double tol) const;
/// Re-initialize PreintegratedMeasurements
void resetIntegration();
/// Subtract estimate and correct for sensor pose
/// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero
std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
/// Update preintegrated measurements
void updatePreintegratedMeasurements(const Vector3& correctedAcc, const Rot3& incrR,
const double deltaT, OptionalJacobian<9, 9> F);
/// Calculate the updated preintegrated measurement, does not modify
/// It takes measured quantities in the j frame
NavState updatedDeltaXij(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt,
OptionalJacobian<9, 9> D_updated_current = boost::none,
OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none,
OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const;
/// Update Jacobians to be used during preintegration
void updatePreintegratedJacobians(const Vector3& correctedAcc,
const Matrix3& D_Rincr_integratedOmega, const Rot3& incrR,
double deltaT);
/// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame
void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega);
void correctMeasurementsByBiasAndSensorPose(const Vector3& measuredAcc,
const Vector3& measuredOmega, Vector3& correctedAcc,
Vector3& correctedOmega,
boost::optional<const Pose3&> body_P_sensor);
/// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far
Vector9 biasCorrectedDelta(const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 6> H = boost::none) const;
/// Predict state at time j
PoseVelocityBias predict(
const Pose3& pose_i, const Vector3& vel_i, const imuBias::ConstantBias& bias_i,
const Vector3& gravity, const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const;
NavState predict(const NavState& state_i, const imuBias::ConstantBias& bias_i,
OptionalJacobian<9, 9> H1 = boost::none, OptionalJacobian<9, 6> H2 =
boost::none) const;
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j,
const Vector3& vel_j, const imuBias::ConstantBias& bias_i,
const Vector3& gravity, const Vector3& omegaCoriolis,
const bool use2ndOrderCoriolis, OptionalJacobian<9, 6> H1 =
boost::none,
OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none,
OptionalJacobian<9, 3> H4 = boost::none,
OptionalJacobian<9, 6> H5 = boost::none) const;
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H1 =
boost::none, OptionalJacobian<9, 3> H2 = boost::none,
OptionalJacobian<9, 6> H3 = boost::none, OptionalJacobian<9, 3> H4 =
boost::none, OptionalJacobian<9, 6> H5 = boost::none) const;
private:
/// @deprecated predict
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
const imuBias::ConstantBias& bias_i, const Vector3& n_gravity,
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false);
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotation);
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(deltaPij_);
ar & BOOST_SERIALIZATION_NVP(deltaVij_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
@ -172,32 +258,4 @@ class PreintegrationBase : public PreintegratedRotation {
}
};
class ImuBase {
protected:
Vector3 gravity_;
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
/// Default constructor, with decent gravity and zero coriolis
ImuBase();
/// Fully fledge constructor
ImuBase(const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false);
const Vector3& gravity() const {
return gravity_;
}
const Vector3& omegaCoriolis() const {
return omegaCoriolis_;
}
};
} /// namespace gtsam
} /// namespace gtsam

88
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -35,6 +35,12 @@ using symbol_shorthand::X;
using symbol_shorthand::V;
using symbol_shorthand::B;
Vector3 kZeroOmegaCoriolis(0,0,0);
// Define covariance matrices
double accNoiseVar = 0.01;
const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3;
//******************************************************************************
namespace {
Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i,
@ -50,8 +56,8 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3()) {
AHRSFactor::PreintegratedMeasurements result(bias, Matrix3::Identity());
const Vector3& initialRotationRate = Vector3::Zero()) {
AHRSFactor::PreintegratedMeasurements result(bias, I_3x3);
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin();
@ -95,7 +101,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
double expectedDeltaT1(0.5);
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero());
AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3);
actual1.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
@ -121,18 +127,14 @@ TEST(AHRSFactor, Error) {
Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0));
// Measurements
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
Vector3 measuredOmega;
measuredOmega << M_PI / 100, 0, 0;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3);
pim.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis, boost::none);
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, boost::none);
Vector3 errorActual = factor.evaluateError(x1, x2, bias);
@ -182,18 +184,16 @@ TEST(AHRSFactor, ErrorWithBiases) {
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
// Measurements
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.0, 0.0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3(0,0,0),
Matrix3::Zero());
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0),
Z_3x3);
pim.integrateMeasurement(measuredOmega, deltaT);
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
Vector errorActual = factor.evaluateError(x1, x2, bias);
@ -269,7 +269,7 @@ TEST( AHRSFactor, PartialDerivativeLogmap ) {
const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
double normx = norm_2(x);
const Matrix3 actualDelFdeltheta = Matrix3::Identity() + 0.5 * X
const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X
+ (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X
* X;
@ -359,8 +359,6 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0)));
// Measurements
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.1, 0.1;
Vector3 measuredOmega;
@ -370,13 +368,15 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0));
AHRSFactor::PreintegratedMeasurements pre_int_data(Vector3::Zero(),
Matrix3::Zero());
AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
pim.integrateMeasurement(measuredOmega, deltaT);
// Check preintegrated covariance
EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov()));
// Create factor
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis);
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector, Rot3>(
@ -407,34 +407,35 @@ TEST (AHRSFactor, predictTest) {
Vector3 bias(0,0,0);
// Measurements
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0.0, 0.0;
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.001;
AHRSFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero());
double deltaT = 0.2;
AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance);
for (int i = 0; i < 1000; ++i) {
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
pim.integrateMeasurement(measuredOmega, deltaT);
}
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
// Check preintegrated covariance
Matrix expectedMeasCov(3,3);
expectedMeasCov = 200*kMeasuredAccCovariance;
EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov()));
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
// Predict
Rot3 x;
Rot3 expectedRot = Rot3().ypr(M_PI / 10, 0, 0);
Rot3 actualRot = factor.predict(x, bias, pre_int_data, omegaCoriolis);
Rot3 expectedRot = Rot3().ypr(20*M_PI, 0, 0);
Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
// AHRSFactor::PreintegratedMeasurements::predict
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
boost::bind(&AHRSFactor::PreintegratedMeasurements::predict,
&pre_int_data, _1, boost::none), bias);
&pim, _1, boost::none), bias);
// Actual Jacobians
Matrix H;
(void) pre_int_data.predict(bias,H);
EXPECT(assert_equal(expectedH, H));
(void) pim.predict(bias,H);
EXPECT(assert_equal(expectedH, H, 1e-8));
}
//******************************************************************************
#include <gtsam/linear/GaussianFactorGraph.h>
@ -448,12 +449,7 @@ TEST (AHRSFactor, graphTest) {
// PreIntegrator
Vector3 biasHat(0, 0, 0);
Vector3 gravity;
gravity << 0, 0, 9.81;
Vector3 omegaCoriolis;
omegaCoriolis << 0, 0, 0;
AHRSFactor::PreintegratedMeasurements pre_int_data(biasHat,
Matrix3::Identity());
AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance);
// Pre-integrate measurements
Vector3 measuredOmega(0, M_PI / 20, 0);
@ -461,15 +457,15 @@ TEST (AHRSFactor, graphTest) {
// Create Factor
noiseModel::Base::shared_ptr model = //
noiseModel::Gaussian::Covariance(pre_int_data.preintMeasCov());
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
NonlinearFactorGraph graph;
Values values;
for (size_t i = 0; i < 5; ++i) {
pre_int_data.integrateMeasurement(measuredOmega, deltaT);
pim.integrateMeasurement(measuredOmega, deltaT);
}
// pre_int_data.print("Pre integrated measurementes");
AHRSFactor factor(X(1), X(2), B(1), pre_int_data, omegaCoriolis);
// pim.print("Pre integrated measurementes");
AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis);
values.insert(X(1), x1);
values.insert(X(2), x2);
values.insert(B(1), bias);

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

@ -40,46 +40,6 @@ using symbol_shorthand::V;
using symbol_shorthand::B;
namespace {
/* ************************************************************************* */
// Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration
/* ************************************************************************* */
Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration) {
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
Vector3 deltaPij_new;
if (!use2ndOrderIntegration) {
deltaPij_new = deltaPij_old + deltaVij_old * deltaT;
} else {
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
}
Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old
* Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT);
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
imuBias::ConstantBias bias_new(bias_old);
Vector result(15);
result << deltaPij_new, deltaVij_new, logDeltaRij_new, bias_new.vector();
return result;
}
Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT,
const bool use2ndOrderIntegration) {
Vector result = updatePreintegratedMeasurementsTest(deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega,
deltaT, use2ndOrderIntegration);
return Rot3::Expmap(result.segment<3>(6));
}
// Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
@ -87,9 +47,8 @@ Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
CombinedImuFactor::CombinedPreintegratedMeasurements evaluatePreintegratedMeasurements(
const imuBias::ConstantBias& bias, const list<Vector3>& measuredAccs,
const list<Vector3>& measuredOmegas, const list<double>& deltaTs) {
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias,
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), Matrix3::Identity(), Matrix::Identity(6, 6), false);
CombinedImuFactor::CombinedPreintegratedMeasurements result(bias, I_3x3,
I_3x3, I_3x3, I_3x3, I_3x3, I_6x6);
list<Vector3>::const_iterator itAcc = measuredAccs.begin();
list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
@ -136,25 +95,17 @@ TEST( CombinedImuFactor, PreintegratedMeasurements ) {
double tol = 1e-6;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements expected1(bias, Matrix3::Zero(),
Matrix3::Zero(), Matrix3::Zero());
ImuFactor::PreintegratedMeasurements expected1(bias, Z_3x3, Z_3x3, Z_3x3);
expected1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias,
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), Matrix::Zero(6, 6));
CombinedImuFactor::CombinedPreintegratedMeasurements actual1(bias, Z_3x3,
Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_6x6);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(
assert_equal(Vector(expected1.deltaPij()), Vector(actual1.deltaPij()),
tol));
EXPECT(
assert_equal(Vector(expected1.deltaVij()), Vector(actual1.deltaVij()),
tol));
EXPECT(
assert_equal(Matrix(expected1.deltaRij()), Matrix(actual1.deltaRij()),
tol));
EXPECT(assert_equal(Vector(expected1.deltaPij()), actual1.deltaPij(), tol));
EXPECT(assert_equal(Vector(expected1.deltaVij()), actual1.deltaVij(), tol));
EXPECT(assert_equal(expected1.deltaRij(), actual1.deltaRij(), tol));
DOUBLES_EQUAL(expected1.deltaTij(), actual1.deltaTij(), tol);
}
@ -180,46 +131,39 @@ TEST( CombinedImuFactor, ErrorWithBiases ) {
double deltaT = 1.0;
double tol = 1e-6;
Matrix I6x6(6, 6);
I6x6 = Matrix::Identity(6, 6);
ImuFactor::PreintegratedMeasurements pre_int_data(
ImuFactor::PreintegratedMeasurements pim(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity());
I_3x3, I_3x3, I_3x3);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
CombinedImuFactor::CombinedPreintegratedMeasurements combined_pim(
imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0)),
Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6);
I_3x3, I_3x3, I_3x3, I_3x3, 2 * I_3x3, I_6x6);
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
deltaT);
combined_pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity,
ImuFactor imuFactor(X(1), V(1), X(2), V(2), B(1), pim, gravity,
omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel =
noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
Combined_pre_int_data, gravity, omegaCoriolis);
noiseModel::Gaussian::Covariance(combined_pim.preintMeasCov());
CombinedImuFactor combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
combined_pim, gravity, omegaCoriolis);
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
Vector errorActual = Combinedfactor.evaluateError(x1, v1, x2, v2, bias,
Vector errorExpected = imuFactor.evaluateError(x1, v1, x2, v2, bias);
Vector errorActual = combinedfactor.evaluateError(x1, v1, x2, v2, bias,
bias2);
EXPECT(assert_equal(errorExpected, errorActual.head(9), tol));
// Expected Jacobians
Matrix H1e, H2e, H3e, H4e, H5e;
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
(void) imuFactor.evaluateError(x1, v1, x2, v2, bias, H1e, H2e, H3e, H4e, H5e);
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a, H6a;
(void) Combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
(void) combinedfactor.evaluateError(x1, v1, x2, v2, bias, bias2, H1a, H2a,
H3a, H4a, H5a, H6a);
EXPECT(assert_equal(H1e, H1a.topRows(9)));
@ -253,7 +197,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
}
// Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
CombinedImuFactor::CombinedPreintegratedMeasurements pim =
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas,
deltaTs);
@ -280,16 +224,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) {
Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3);
// Compare Jacobians
EXPECT(assert_equal(expectedDelPdelBiasAcc, preintegrated.delPdelBiasAcc()));
EXPECT(
assert_equal(expectedDelPdelBiasOmega, preintegrated.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc()));
EXPECT(
assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelPdelBiasAcc, pim.delPdelBiasAcc()));
EXPECT(assert_equal(expectedDelPdelBiasOmega, pim.delPdelBiasOmega()));
EXPECT(assert_equal(expectedDelVdelBiasAcc, pim.delVdelBiasAcc()));
EXPECT(assert_equal(expectedDelVdelBiasOmega, pim.delVdelBiasOmega()));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3, 3)));
EXPECT(
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(),
1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(expectedDelRdelBiasOmega, pim.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
/* ************************************************************************* */
@ -307,28 +247,23 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
measuredAcc << 0, 1.1, -9.81;
double deltaT = 0.001;
Matrix I6x6(6, 6);
I6x6 = Matrix::Identity(6, 6);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
I_3x3, I_3x3, 2 * I_3x3, I_6x6);
for (int i = 0; i < 1000; ++i)
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
deltaT);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
noiseModel::Gaussian::shared_ptr Combinedmodel =
noiseModel::Gaussian::Covariance(Combined_pre_int_data.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
Combined_pre_int_data, gravity, omegaCoriolis);
noiseModel::Gaussian::shared_ptr combinedmodel =
noiseModel::Gaussian::Covariance(pim.preintMeasCov());
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
gravity, omegaCoriolis);
// Predict
Pose3 x1;
Vector3 v1(0, 0.0, 0.0);
PoseVelocityBias poseVelocityBias = Combined_pre_int_data.predict(x1, v1,
bias, gravity, omegaCoriolis);
PoseVelocityBias poseVelocityBias = pim.predict(x1, v1, bias, gravity,
omegaCoriolis);
Pose3 expectedPose(Rot3(), Point3(0, 0.5, 0));
Vector3 expectedVelocity;
expectedVelocity << 0, 1, 0;
@ -340,10 +275,8 @@ TEST(CombinedImuFactor, PredictPositionAndVelocity) {
/* ************************************************************************* */
TEST(CombinedImuFactor, PredictRotation) {
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0)); // Biases (acc, rot)
Matrix I6x6(6, 6);
CombinedImuFactor::CombinedPreintegratedMeasurements Combined_pre_int_data(
bias, Matrix3::Identity(), Matrix3::Identity(), Matrix3::Identity(),
Matrix3::Identity(), 2 * Matrix3::Identity(), I6x6, true);
CombinedImuFactor::CombinedPreintegratedMeasurements pim(bias, I_3x3, I_3x3,
I_3x3, I_3x3, 2 * I_3x3, I_6x6);
Vector3 measuredAcc;
measuredAcc << 0, 0, -9.81;
Vector3 gravity;
@ -355,175 +288,18 @@ TEST(CombinedImuFactor, PredictRotation) {
double deltaT = 0.001;
double tol = 1e-4;
for (int i = 0; i < 1000; ++i)
Combined_pre_int_data.integrateMeasurement(measuredAcc, measuredOmega,
deltaT);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2),
Combined_pre_int_data, gravity, omegaCoriolis);
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), pim,
gravity, omegaCoriolis);
// Predict
Pose3 x(Rot3().ypr(0, 0, 0), Point3(0, 0, 0)), x2;
Vector3 v(0, 0, 0), v2;
CombinedImuFactor::Predict(x, v, x2, v2, bias,
Combinedfactor.preintegratedMeasurements(), gravity, omegaCoriolis);
CombinedImuFactor::Predict(x, v, x2, v2, bias, pim, gravity, omegaCoriolis);
Pose3 expectedPose(Rot3().ypr(M_PI / 10, 0, 0), Point3(0, 0, 0));
EXPECT(assert_equal(expectedPose, x2, tol));
}
/* ************************************************************************* */
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
// Linearization point
imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
Pose3 body_P_sensor = Pose3();
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
CombinedImuFactor::CombinedPreintegratedMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas,
deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, body_P_sensor, Factual, Gactual);
bool use2ndOrderIntegration = false;
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected F wrt positions (15,3)
Matrix df_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaPij_old);
// rotation part has to be done properly, on manifold
df_dpos.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaPij_old);
// Compute expected F wrt velocities (15,3)
Matrix df_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaVij_old);
// rotation part has to be done properly, on manifold
df_dvel.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), deltaVij_old);
// Compute expected F wrt angles (15,3)
Matrix df_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), deltaRij_old);
// rotation part has to be done properly, on manifold
df_dangle.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), deltaRij_old);
// Compute expected F wrt biases (15,6)
Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
// rotation part has to be done properly, on manifold
df_dbias.block<3, 6>(6, 0) =
numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
Matrix Fexpected(15, 15);
Fexpected << df_dpos, df_dvel, df_dangle, df_dbias;
EXPECT(assert_equal(Fexpected, Factual));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected G wrt integration noise
Matrix df_dintNoise(15, 3);
df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3;
// Compute expected G wrt acc noise (15,3)
Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// rotation part has to be done properly, on manifold
df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT,
use2ndOrderIntegration), newMeasuredAcc);
// Compute expected G wrt gyro noise (15,3)
Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
use2ndOrderIntegration), newMeasuredOmega);
// rotation part has to be done properly, on manifold
df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT,
use2ndOrderIntegration), newMeasuredOmega);
// Compute expected G wrt bias random walk noise (15,6)
Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries
df_rwBias.setZero();
df_rwBias.block<6, 6>(9, 0) = eye(6);
// Compute expected G wrt gyro noise (15,6)
Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
// rotation part has to be done properly, on manifold
df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11<Rot3,
imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT, use2ndOrderIntegration), bias_old);
df_dinitBias.block<6, 6>(9, 0) = Matrix::Zero(6, 6); // only has to influence first 9 rows
Matrix Gexpected(15, 21);
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

File diff suppressed because it is too large Load Diff

272
gtsam/navigation/tests/testNavState.cpp Normal file
View File

@ -0,0 +1,272 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testNavState.cpp
* @brief Unit tests for NavState
* @author Frank Dellaert
* @date July 2015
*/
#include <gtsam/navigation/NavState.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
static const Rot3 kAttitude = Rot3::RzRyRx(0.1, 0.2, 0.3);
static const Point3 kPosition(1.0, 2.0, 3.0);
static const Pose3 kPose(kAttitude, kPosition);
static const Velocity3 kVelocity(0.4, 0.5, 0.6);
static const NavState kIdentity;
static const NavState kState1(kAttitude, kPosition, kVelocity);
static const Vector3 kOmegaCoriolis(0.02, 0.03, 0.04);
static const Vector3 kGravity(0, 0, 9.81);
static const Vector9 kZeroXi = Vector9::Zero();
/* ************************************************************************* */
TEST(NavState, Constructor) {
boost::function<NavState(const Pose3&, const Vector3&)> construct =
boost::bind(&NavState::FromPoseVelocity, _1, _2, boost::none,
boost::none);
Matrix aH1, aH2;
EXPECT(
assert_equal(kState1,
NavState::FromPoseVelocity(kPose, kVelocity, aH1, aH2)));
EXPECT(assert_equal(numericalDerivative21(construct, kPose, kVelocity), aH1));
EXPECT(assert_equal(numericalDerivative22(construct, kPose, kVelocity), aH2));
}
/* ************************************************************************* */
TEST( NavState, Attitude) {
Matrix39 aH, eH;
Rot3 actual = kState1.attitude(aH);
EXPECT(assert_equal(actual, kAttitude));
eH = numericalDerivative11<Rot3, NavState>(
boost::bind(&NavState::attitude, _1, boost::none), kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
/* ************************************************************************* */
TEST( NavState, Position) {
Matrix39 aH, eH;
Point3 actual = kState1.position(aH);
EXPECT(assert_equal(actual, kPosition));
eH = numericalDerivative11<Point3, NavState>(
boost::bind(&NavState::position, _1, boost::none), kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
/* ************************************************************************* */
TEST( NavState, Velocity) {
Matrix39 aH, eH;
Velocity3 actual = kState1.velocity(aH);
EXPECT(assert_equal(actual, kVelocity));
eH = numericalDerivative11<Velocity3, NavState>(
boost::bind(&NavState::velocity, _1, boost::none), kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
/* ************************************************************************* */
TEST( NavState, BodyVelocity) {
Matrix39 aH, eH;
Velocity3 actual = kState1.bodyVelocity(aH);
EXPECT(assert_equal(actual, kAttitude.unrotate(kVelocity)));
eH = numericalDerivative11<Velocity3, NavState>(
boost::bind(&NavState::bodyVelocity, _1, boost::none), kState1);
EXPECT(assert_equal((Matrix )eH, aH));
}
/* ************************************************************************* */
TEST( NavState, MatrixGroup ) {
// check roundtrip conversion to 7*7 matrix representation
Matrix7 T = kState1.matrix();
EXPECT(assert_equal(kState1, NavState(T)));
// check group product agrees with matrix product
NavState state2 = kState1 * kState1;
Matrix T2 = T * T;
EXPECT(assert_equal(state2, NavState(T2)));
EXPECT(assert_equal(T2, state2.matrix()));
}
/* ************************************************************************* */
TEST( NavState, Manifold ) {
// Check zero xi
EXPECT(assert_equal(kIdentity, kIdentity.retract(kZeroXi)));
EXPECT(assert_equal(kZeroXi, kIdentity.localCoordinates(kIdentity)));
EXPECT(assert_equal(kState1, kState1.retract(kZeroXi)));
EXPECT(assert_equal(kZeroXi, kState1.localCoordinates(kState1)));
// Check definition of retract as operating on components separately
Vector9 xi;
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
Rot3 drot = Rot3::Expmap(xi.head<3>());
Point3 dt = Point3(xi.segment<3>(3));
Velocity3 dvel = Velocity3(-0.1, -0.2, -0.3);
NavState state2 = kState1 * NavState(drot, dt, dvel);
EXPECT(assert_equal(state2, kState1.retract(xi)));
EXPECT(assert_equal(xi, kState1.localCoordinates(state2)));
// roundtrip from state2 to state3 and back
NavState state3 = state2.retract(xi);
EXPECT(assert_equal(xi, state2.localCoordinates(state3)));
// Check derivatives for ChartAtOrigin::Retract
Matrix9 aH;
// For zero xi
boost::function<NavState(const Vector9&)> Retract = boost::bind(
NavState::ChartAtOrigin::Retract, _1, boost::none);
NavState::ChartAtOrigin::Retract(kZeroXi, aH);
EXPECT(assert_equal(numericalDerivative11(Retract, kZeroXi), aH));
// For non-zero xi
NavState::ChartAtOrigin::Retract(xi, aH);
EXPECT(assert_equal(numericalDerivative11(Retract, xi), aH));
// Check derivatives for ChartAtOrigin::Local
// For zero xi
boost::function<Vector9(const NavState&)> Local = boost::bind(
NavState::ChartAtOrigin::Local, _1, boost::none);
NavState::ChartAtOrigin::Local(kIdentity, aH);
EXPECT(assert_equal(numericalDerivative11(Local, kIdentity), aH));
// For non-zero xi
NavState::ChartAtOrigin::Local(kState1, aH);
EXPECT(assert_equal(numericalDerivative11(Local, kState1), aH));
// Check retract derivatives
Matrix9 aH1, aH2;
kState1.retract(xi, aH1, aH2);
boost::function<NavState(const NavState&, const Vector9&)> retract =
boost::bind(&NavState::retract, _1, _2, boost::none, boost::none);
EXPECT(assert_equal(numericalDerivative21(retract, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(retract, kState1, xi), aH2));
// Check localCoordinates derivatives
boost::function<Vector9(const NavState&, const NavState&)> local =
boost::bind(&NavState::localCoordinates, _1, _2, boost::none,
boost::none);
// from state1 to state2
kState1.localCoordinates(state2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, kState1, state2), aH1));
EXPECT(assert_equal(numericalDerivative22(local, kState1, state2), aH2));
// from identity to state2
kIdentity.localCoordinates(state2, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, kIdentity, state2), aH1));
EXPECT(assert_equal(numericalDerivative22(local, kIdentity, state2), aH2));
// from state2 to identity
state2.localCoordinates(kIdentity, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(local, state2, kIdentity), aH1));
EXPECT(assert_equal(numericalDerivative22(local, state2, kIdentity), aH2));
}
/* ************************************************************************* */
TEST( NavState, Lie ) {
// Check zero xi
EXPECT(assert_equal(kIdentity, kIdentity.expmap(kZeroXi)));
EXPECT(assert_equal(kZeroXi, kIdentity.logmap(kIdentity)));
EXPECT(assert_equal(kState1, kState1.expmap(kZeroXi)));
EXPECT(assert_equal(kZeroXi, kState1.logmap(kState1)));
// Expmap/Logmap roundtrip
Vector xi(9);
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
NavState state2 = NavState::Expmap(xi);
EXPECT(assert_equal(xi, NavState::Logmap(state2)));
// roundtrip from state2 to state3 and back
NavState state3 = state2.expmap(xi);
EXPECT(assert_equal(xi, state2.logmap(state3)));
// For the expmap/logmap (not necessarily expmap/local) -xi goes other way
EXPECT(assert_equal(state2, state3.expmap(-xi)));
EXPECT(assert_equal(xi, -state3.logmap(state2)));
}
/* ************************************************************************* */
TEST(NavState, Update) {
Vector3 omega(M_PI / 100.0, 0.0, 0.0);
Vector3 acc(0.1, 0.0, 0.0);
double dt = 10;
Matrix9 aF;
Matrix93 aG1, aG2;
boost::function<NavState(const NavState&, const Vector3&, const Vector3&)> update =
boost::bind(&NavState::update, _1, _2, _3, dt, boost::none,
boost::none, boost::none);
Vector3 b_acc = kAttitude * acc;
NavState expected(kAttitude.expmap(dt * omega),
kPosition + Point3((kVelocity + b_acc * dt / 2) * dt),
kVelocity + b_acc * dt);
NavState actual = kState1.update(acc, omega, dt, aF, aG1, aG2);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7));
EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
// Try different values
omega = Vector3(0.1, 0.2, 0.3);
acc = Vector3(0.4, 0.5, 0.6);
kState1.update(acc, omega, dt, aF, aG1, aG2);
EXPECT(assert_equal(numericalDerivative31(update, kState1, acc, omega, 1e-7), aF, 1e-7));
EXPECT(assert_equal(numericalDerivative32(update, kState1, acc, omega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(numericalDerivative33(update, kState1, acc, omega, 1e-7), aG2, 1e-7));
}
/* ************************************************************************* */
static const double dt = 2.0;
boost::function<Vector9(const NavState&, const bool&)> coriolis = boost::bind(
&NavState::coriolis, _1, dt, kOmegaCoriolis, _2, boost::none);
TEST(NavState, Coriolis) {
Matrix9 aH;
// first-order
kState1.coriolis(dt, kOmegaCoriolis, false, aH);
EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, false), aH));
// second-order
kState1.coriolis(dt, kOmegaCoriolis, true, aH);
EXPECT(assert_equal(numericalDerivative21(coriolis, kState1, true), aH));
}
TEST(NavState, Coriolis2) {
Matrix9 aH;
NavState state2(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0),
Point3(5.0, 1.0, -50.0), Vector3(0.5, 0.0, 0.0));
// first-order
state2.coriolis(dt, kOmegaCoriolis, false, aH);
EXPECT(assert_equal(numericalDerivative21(coriolis, state2, false), aH));
// second-order
state2.coriolis(dt, kOmegaCoriolis, true, aH);
EXPECT(assert_equal(numericalDerivative21(coriolis, state2, true), aH));
}
/* ************************************************************************* */
TEST(NavState, CorrectPIM) {
Vector9 xi;
xi << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4, -0.1, -0.2, -0.3;
double dt = 0.5;
Matrix9 aH1, aH2;
boost::function<Vector9(const NavState&, const Vector9&)> correctPIM =
boost::bind(&NavState::correctPIM, _1, _2, dt, kGravity, kOmegaCoriolis,
false, boost::none, boost::none);
kState1.correctPIM(xi, dt, kGravity, kOmegaCoriolis, false, aH1, aH2);
EXPECT(assert_equal(numericalDerivative21(correctPIM, kState1, xi), aH1));
EXPECT(assert_equal(numericalDerivative22(correctPIM, kState1, xi), aH2));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

64
gtsam/navigation/tests/testPoseVelocityBias.cpp Normal file
View File

@ -0,0 +1,64 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testPoseVelocityBias.cpp
* @brief Unit test for PoseVelocityBias
* @author Frank Dellaert
*/
#include <gtsam/navigation/PreintegrationBase.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
// Should be seen as between(pvb1,pvb2), i.e., written as pvb2 \omin pvb1
Vector9 error(const PoseVelocityBias& pvb1, const PoseVelocityBias& pvb2) {
Matrix3 R1 = pvb1.pose.rotation().matrix();
// Ri.transpose() translate the error from the global frame into pose1's frame
const Vector3 fp = R1.transpose() * (pvb2.pose.translation() - pvb1.pose.translation()).vector();
const Vector3 fv = R1.transpose() * (pvb2.velocity - pvb1.velocity);
const Rot3 R1BetweenR2 = pvb1.pose.rotation().between(pvb2.pose.rotation());
const Vector3 fR = Rot3::Logmap(R1BetweenR2);
Vector9 r;
r << fp, fv, fR;
return r;
}
/* ************************************************************************************************/
TEST(PoseVelocityBias, error) {
Point3 i1(0, 1, 0), j1(-1, 0, 0), k1(0, 0, 1);
Pose3 x1(Rot3(i1, j1, k1), Point3(5.0, 1.0, 0.0));
Vector3 v1(Vector3(0.5, 0.0, 0.0));
imuBias::ConstantBias bias1(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3));
Pose3 x2(Rot3(i1, j1, k1).expmap(Vector3(0.1, 0.2, 0.3)), Point3(5.5, 1.0, 6.0));
Vector3 v2(Vector3(0.5, 4.0, 3.0));
imuBias::ConstantBias bias2(Vector3(0.1, 0.2, -0.3), Vector3(0.2, 0.3, 0.1));
PoseVelocityBias pvb1(x1, v1, bias1), pvb2(x2, v2, bias2);
Vector9 expected, actual = error(pvb1, pvb2);
expected << 0.0, -0.5, 6.0, 4.0, 0.0, 3.0, 0.1, 0.2, 0.3;
EXPECT(assert_equal(expected, actual, 1e-9));
}
/* ************************************************************************************************/
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************************************/

16
gtsam/nonlinear/factorTesting.h
View File

@ -85,9 +85,23 @@ bool testFactorJacobians(TestResult& result_, const std::string& name_,
// Create actual value by linearize
boost::shared_ptr<JacobianFactor> actual = //
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
if (!actual) return false;
// Check cast result and then equality
return actual && assert_equal(expected, *actual, tolerance);
bool equal = assert_equal(expected, *actual, tolerance);
// if not equal, test individual jacobians:
if (!equal) {
for (size_t i = 0; i < actual->size(); i++) {
bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)),
(Matrix) (actual->getA(actual->begin() + i)), tolerance);
if (!i_good) {
std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl;
}
}
}
return equal;
}
}

63
gtsam/slam/SmartFactorBase.h
View File

@ -15,6 +15,7 @@
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
* @author Chris Beall
*/
#pragma once
@ -49,6 +50,7 @@ private:
typedef SmartFactorBase<CAMERA> This;
typedef typename CAMERA::Measurement Z;
protected:
/**
* As of Feb 22, 2015, the noise model is the same for all measurements and
* is isotropic. This allows for moving most calculations of Schur complement
@ -57,7 +59,6 @@ private:
*/
SharedIsotropic noiseModel_;
protected:
/**
* 2D measurement and noise model for each of the m views
* We keep a copy of measurements for I/O and computing the error.
@ -79,22 +80,6 @@ protected:
typedef Eigen::Matrix<double, Dim, 1> VectorD;
typedef Eigen::Matrix<double, ZDim, ZDim> Matrix2;
// check that noise model is isotropic and the same
// TODO, this is hacky, we should just do this via constructor, not add
void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) {
if (!sharedNoiseModel)
return;
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
noiseModel::Isotropic>(sharedNoiseModel);
if (!sharedIsotropic)
throw std::runtime_error("SmartFactorBase: needs isotropic");
if (!noiseModel_)
noiseModel_ = sharedIsotropic;
else if (!sharedIsotropic->equals(*noiseModel_))
throw std::runtime_error(
"SmartFactorBase: cannot add measurements with different noise model");
}
public:
// Definitions for blocks of F, externally visible
@ -109,8 +94,21 @@ public:
typedef CameraSet<CAMERA> Cameras;
/// Constructor
SmartFactorBase(boost::optional<Pose3> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor){}
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none) :
body_P_sensor_(body_P_sensor){
if (!sharedNoiseModel)
throw std::runtime_error("SmartFactorBase: sharedNoiseModel is required");
SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast<
noiseModel::Isotropic>(sharedNoiseModel);
if (!sharedIsotropic)
throw std::runtime_error("SmartFactorBase: needs isotropic");
noiseModel_ = sharedIsotropic;
}
/// Virtual destructor, subclasses from NonlinearFactor
virtual ~SmartFactorBase() {
@ -122,34 +120,18 @@ public:
* @param poseKey is the index corresponding to the camera observing the landmark
* @param sharedNoiseModel is the measurement noise
*/
void add(const Z& measured_i, const Key& cameraKey_i,
const SharedNoiseModel& sharedNoiseModel) {
void add(const Z& measured_i, const Key& cameraKey_i) {
this->measured_.push_back(measured_i);
this->keys_.push_back(cameraKey_i);
maybeSetNoiseModel(sharedNoiseModel);
}
/**
* Add a bunch of measurements, together with the camera keys and noises
* Add a bunch of measurements, together with the camera keys
*/
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
std::vector<SharedNoiseModel>& noises) {
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.at(i));
maybeSetNoiseModel(noises.at(i));
}
}
/**
* Add a bunch of measurements and use the same noise model for all of them
*/
void add(std::vector<Z>& measurements, std::vector<Key>& cameraKeys,
const SharedNoiseModel& noise) {
for (size_t i = 0; i < measurements.size(); i++) {
this->measured_.push_back(measurements.at(i));
this->keys_.push_back(cameraKeys.at(i));
maybeSetNoiseModel(noise);
}
}
@ -158,11 +140,10 @@ public:
* The noise is assumed to be the same for all measurements
*/
template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd, const SharedNoiseModel& noise) {
void add(const SFM_TRACK& trackToAdd) {
for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
this->measured_.push_back(trackToAdd.measurements[k].second);
this->keys_.push_back(trackToAdd.measurements[k].first);
maybeSetNoiseModel(noise);
}
}
@ -198,7 +179,7 @@ public:
}
if(body_P_sensor_)
body_P_sensor_->print("body_P_sensor_:\n");
print("", keyFormatter);
Base::print("", keyFormatter);
}
/// equals

16
gtsam/slam/SmartProjectionFactor.h
View File

@ -44,23 +44,21 @@ enum DegeneracyMode {
/*
* Parameters for the smart projection factors
*/
class GTSAM_EXPORT SmartProjectionParams {
public:
struct GTSAM_EXPORT SmartProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
mutable TriangulationParameters triangulation;
const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
const bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
const bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
// Constructor
@ -161,10 +159,10 @@ public:
* @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors
*/
SmartProjectionFactor(
SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) :
Base(body_P_sensor), params_(params), //
Base(sharedNoiseModel, body_P_sensor), params_(params), //
result_(TriangulationResult::Degenerate()) {
}

6
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -61,14 +61,16 @@ public:
/**
* Constructor
* @param Isotropic measurement noise
* @param K (fixed) calibration, assumed to be the same for all cameras
* @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<CALIBRATION> K,
const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) :
Base(body_P_sensor, params), K_(K) {
Base(sharedNoiseModel, body_P_sensor, params), K_(K) {
}
/** Virtual destructor */

29
gtsam/slam/TriangulationFactor.h
View File

@ -18,6 +18,7 @@
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>
namespace gtsam {
@ -42,9 +43,12 @@ protected:
/// shorthand for this class
typedef TriangulationFactor<CAMERA> This;
/// shorthand for measurement type, e.g. Point2 or StereoPoint2
typedef typename CAMERA::Measurement Measurement;
// Keep a copy of measurement and calibration for I/O
const CAMERA camera_; ///< CAMERA in which this landmark was seen
const Point2 measured_; ///< 2D measurement
const Measurement measured_; ///< 2D measurement
// verbosity handling for Cheirality Exceptions
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
@ -69,14 +73,17 @@ public:
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
*/
TriangulationFactor(const CAMERA& camera, const Point2& measured,
TriangulationFactor(const CAMERA& camera, const Measurement& measured,
const SharedNoiseModel& model, Key pointKey, bool throwCheirality = false,
bool verboseCheirality = false) :
Base(model, pointKey), camera_(camera), measured_(measured), throwCheirality_(
throwCheirality), verboseCheirality_(verboseCheirality) {
if (model && model->dim() != 2)
if (model && model->dim() != Measurement::dimension)
throw std::invalid_argument(
"TriangulationFactor must be created with 2-dimensional noise model.");
"TriangulationFactor must be created with "
+ boost::lexical_cast<std::string>((int) Measurement::dimension)
+ "-dimensional noise model.");
}
/** Virtual destructor */
@ -113,18 +120,18 @@ public:
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const {
try {
Point2 error(camera_.project2(point, boost::none, H2) - measured_);
Measurement error(camera_.project2(point, boost::none, H2) - measured_);
return error.vector();
} catch (CheiralityException& e) {
if (H2)
*H2 = zeros(2, 3);
*H2 = zeros(Measurement::dimension, 3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "
<< DefaultKeyFormatter(this->key()) << " moved behind camera"
<< std::endl;
if (throwCheirality_)
throw e;
return ones(2) * 2.0 * camera_.calibration().fx();
return ones(Measurement::dimension) * 2.0 * camera_.calibration().fx();
}
}
@ -146,9 +153,9 @@ public:
// Allocate memory for Jacobian factor, do only once
if (Ab.rows() == 0) {
std::vector<size_t> dimensions(1, 3);
Ab = VerticalBlockMatrix(dimensions, 2, true);
A.resize(2,3);
b.resize(2);
Ab = VerticalBlockMatrix(dimensions, Measurement::dimension, true);
A.resize(Measurement::dimension,3);
b.resize(Measurement::dimension);
}
// Would be even better if we could pass blocks to project
@ -164,7 +171,7 @@ public:
}
/** return the measurement */
const Point2& measured() const {
const Measurement& measured() const {
return measured_;
}

25
gtsam/slam/tests/testSmartFactorBase.cpp
View File

@ -16,17 +16,24 @@
* @date Feb 2015
*/
#include "../SmartFactorBase.h"
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
return 0.0;
}
@ -37,15 +44,19 @@ class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
};
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f;
f.add(Point2(), 1, SharedNoiseModel());
f.add(Point2(), 2, SharedNoiseModel());
PinholeFactor f= PinholeFactor(unit2);
f.add(Point2(), 1);
f.add(Point2(), 2);
EXPECT_LONGS_EQUAL(2 * 2, f.dim());
}
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
class StereoFactor: public SmartFactorBase<StereoCamera> {
public:
typedef SmartFactorBase<StereoCamera> Base;
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
return 0.0;
}
@ -56,9 +67,9 @@ class StereoFactor: public SmartFactorBase<StereoCamera> {
};
TEST(SmartFactorBase, Stereo) {
StereoFactor f;
f.add(StereoPoint2(), 1, SharedNoiseModel());
f.add(StereoPoint2(), 2, SharedNoiseModel());
StereoFactor f(unit3);
f.add(StereoPoint2(), 1);
f.add(StereoPoint2(), 2);
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
}

160
gtsam/slam/tests/testSmartProjectionCameraFactor.cpp
View File

@ -72,39 +72,39 @@ TEST( SmartProjectionCameraFactor, perturbCameraPose) {
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor2) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(boost::none, params);
SmartFactor factor1(unit2, boost::none, params);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor3) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, x1, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(measurement1, x1);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Constructor4) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(boost::none, params);
factor1.add(measurement1, x1, unit2);
SmartFactor factor1(unit2, boost::none, params);
factor1.add(measurement1, x1);
}
/* ************************************************************************* */
TEST( SmartProjectionCameraFactor, Equals ) {
using namespace vanilla;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, x1, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(measurement1, x1);
SmartFactor::shared_ptr factor2(new SmartFactor());
factor2->add(measurement1, x1, unit2);
SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
factor2->add(measurement1, x1);
}
/* *************************************************************************/
@ -115,9 +115,9 @@ TEST( SmartProjectionCameraFactor, noiseless ) {
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, factor1->error(values), 1e-7);
@ -140,9 +140,9 @@ TEST( SmartProjectionCameraFactor, noisy ) {
Camera perturbed_level_camera_right = perturbCameraPose(level_camera_right);
values.insert(c2, perturbed_level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
// Point is now uninitialized before a triangulation event
EXPECT(!factor1->point());
@ -164,20 +164,16 @@ TEST( SmartProjectionCameraFactor, noisy ) {
Vector actual = factor1->whitenedError(cameras1, point1);
EXPECT(assert_equal(expected, actual, 1));
SmartFactor::shared_ptr factor2(new SmartFactor());
SmartFactor::shared_ptr factor2(new SmartFactor(unit2));
vector<Point2> measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
vector<SharedNoiseModel> noises;
noises.push_back(unit2);
noises.push_back(unit2);
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
factor2->add(measurements, views, noises);
factor2->add(measurements, views);
double actualError2 = factor2->error(values);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1);
@ -195,16 +191,16 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// Create and fill smartfactors
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
vector<Key> views;
views.push_back(c1);
views.push_back(c2);
views.push_back(c3);
smartFactor1->add(measurements_cam1, views, unit2);
smartFactor2->add(measurements_cam2, views, unit2);
smartFactor3->add(measurements_cam3, views, unit2);
smartFactor1->add(measurements_cam1, views);
smartFactor2->add(measurements_cam2, views);
smartFactor3->add(measurements_cam3, views);
// Create factor graph and add priors on two cameras
NonlinearFactorGraph graph;
@ -308,8 +304,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
measures.second = measurements_cam1.at(i);
track1.measurements.push_back(measures);
}
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(track1, unit2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(track1);
SfM_Track track2;
for (size_t i = 0; i < 3; ++i) {
@ -318,11 +314,11 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
measures.second = measurements_cam2.at(i);
track2.measurements.push_back(measures);
}
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(track2, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(track2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
@ -384,20 +380,20 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
smartFactor4->add(measurements_cam4, views);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
smartFactor5->add(measurements_cam5, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
@ -464,20 +460,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
smartFactor4->add(measurements_cam4, views);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
smartFactor5->add(measurements_cam5, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
@ -540,20 +536,20 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
views.push_back(c2);
views.push_back(c3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, unit2);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(unit2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, unit2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(unit2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, unit2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(unit2));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(new SmartFactor());
smartFactor4->add(measurements_cam4, views, unit2);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(unit2));
smartFactor4->add(measurements_cam4, views);
SmartFactor::shared_ptr smartFactor5(new SmartFactor());
smartFactor5->add(measurements_cam5, views, unit2);
SmartFactor::shared_ptr smartFactor5(new SmartFactor(unit2));
smartFactor5->add(measurements_cam5, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(9, 1e-6);
@ -604,9 +600,9 @@ TEST( SmartProjectionCameraFactor, noiselessBundler ) {
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
double actualError = factor1->error(values);
@ -633,9 +629,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor ) {
values.insert(c2, level_camera_right);
NonlinearFactorGraph smartGraph;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
smartGraph.push_back(factor1);
double expectedError = factor1->error(values);
double expectedErrorGraph = smartGraph.error(values);
@ -674,9 +670,9 @@ TEST( SmartProjectionCameraFactor, comparisonGeneralSfMFactor1 ) {
values.insert(c2, level_camera_right);
NonlinearFactorGraph smartGraph;
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
smartGraph.push_back(factor1);
Matrix expectedHessian = smartGraph.linearize(values)->hessian().first;
Vector expectedInfoVector = smartGraph.linearize(values)->hessian().second;
@ -765,9 +761,9 @@ TEST( SmartProjectionCameraFactor, computeImplicitJacobian ) {
values.insert(c1, level_camera);
values.insert(c2, level_camera_right);
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, c1, unit2);
factor1->add(level_uv_right, c2, unit2);
SmartFactor::shared_ptr factor1(new SmartFactor(unit2));
factor1->add(level_uv, c1);
factor1->add(level_uv_right, c2);
Matrix expectedE;
Vector expectedb;
@ -814,9 +810,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
params.setEnableEPI(useEPI);
SmartFactor::shared_ptr explicitFactor(
new SmartFactor(boost::none, params));
explicitFactor->add(level_uv, c1, unit2);
explicitFactor->add(level_uv_right, c2, unit2);
new SmartFactor(unit2, boost::none, params));
explicitFactor->add(level_uv, c1);
explicitFactor->add(level_uv_right, c2);
GaussianFactor::shared_ptr gaussianHessianFactor = explicitFactor->linearize(
values);
@ -826,9 +822,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
// Implicit Schur version
params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
SmartFactor::shared_ptr implicitFactor(
new SmartFactor(boost::none, params));
implicitFactor->add(level_uv, c1, unit2);
implicitFactor->add(level_uv_right, c2, unit2);
new SmartFactor(unit2, boost::none, params));
implicitFactor->add(level_uv, c1);
implicitFactor->add(level_uv_right, c2);
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
implicitFactor->linearize(values);
CHECK(gaussianImplicitSchurFactor);

206
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -53,7 +53,7 @@ LevenbergMarquardtParams lmParams;
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
}
/* ************************************************************************* */
@ -61,14 +61,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
using namespace vanillaPose;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor1(sharedK, boost::none, params);
SmartFactor factor1(model, sharedK, boost::none, params);
}
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor3) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
factor1->add(measurement1, x1, model);
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
factor1->add(measurement1, x1);
}
/* ************************************************************************* */
@ -76,18 +76,18 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
using namespace vanillaPose;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor1(sharedK, boost::none, params);
factor1.add(measurement1, x1, model);
SmartFactor factor1(model, sharedK, boost::none, params);
factor1.add(measurement1, x1);
}
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Equals ) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(sharedK));
factor1->add(measurement1, x1, model);
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
factor1->add(measurement1, x1);
SmartFactor::shared_ptr factor2(new SmartFactor(sharedK));
factor2->add(measurement1, x1, model);
SmartFactor::shared_ptr factor2(new SmartFactor(model,sharedK));
factor2->add(measurement1, x1);
CHECK(assert_equal(*factor1, *factor2));
}
@ -101,9 +101,9 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
SmartFactor factor(sharedK);
factor.add(level_uv, x1, model);
factor.add(level_uv_right, x2, model);
SmartFactor factor(model, sharedK);
factor.add(level_uv, x1);
factor.add(level_uv_right, x2);
Values values; // it's a pose factor, hence these are poses
values.insert(x1, cam1.pose());
@ -166,26 +166,22 @@ TEST( SmartProjectionPoseFactor, noisy ) {
Point3(0.5, 0.1, 0.3));
values.insert(x2, pose_right.compose(noise_pose));
SmartFactor::shared_ptr factor(new SmartFactor((sharedK)));
factor->add(level_uv, x1, model);
factor->add(level_uv_right, x2, model);
SmartFactor::shared_ptr factor(new SmartFactor(model, sharedK));
factor->add(level_uv, x1);
factor->add(level_uv_right, x2);
double actualError1 = factor->error(values);
SmartFactor::shared_ptr factor2(new SmartFactor((sharedK)));
SmartFactor::shared_ptr factor2(new SmartFactor(model, sharedK));
vector<Point2> measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
vector<SharedNoiseModel> noises;
noises.push_back(model);
noises.push_back(model);
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
factor2->add(measurements, views, noises);
factor2->add(measurements, views);
double actualError2 = factor2->error(values);
DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
}
@ -238,14 +234,14 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(false);
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(K, sensor_to_body, params);
smartFactor1.add(measurements_cam1, views, model);
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(model, K, sensor_to_body, params);
smartFactor1.add(measurements_cam1, views);
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(K, sensor_to_body, params);
smartFactor2.add(measurements_cam2, views, model);
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(model, K, sensor_to_body, params);
smartFactor2.add(measurements_cam2, views);
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(K, sensor_to_body, params);
smartFactor3.add(measurements_cam3, views, model);
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(model, K, sensor_to_body, params);
smartFactor3.add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -296,14 +292,14 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
views.push_back(x2);
views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK2));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK2));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK2));
smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK2));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -366,8 +362,8 @@ TEST( SmartProjectionPoseFactor, Factors ) {
views.push_back(x1);
views.push_back(x2);
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(sharedK);
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1 = boost::make_shared<SmartFactor>(model, sharedK);
smartFactor1->add(measurements_cam1, views);
SmartFactor::Cameras cameras;
cameras.push_back(cam1);
@ -524,14 +520,14 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedK));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedK));
smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -586,19 +582,19 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(false);
SmartFactor factor1(sharedK, boost::none, params);
SmartFactor factor1(model, sharedK, boost::none, params);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -650,16 +646,16 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
params.setEnableEPI(false);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -717,20 +713,20 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
SmartFactor::shared_ptr smartFactor4(
new SmartFactor(sharedK, boost::none, params));
smartFactor4->add(measurements_cam4, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor4->add(measurements_cam4, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -775,16 +771,16 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
params.setLinearizationMode(gtsam::JACOBIAN_Q);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -895,16 +891,16 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
params.setRankTolerance(10);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
smartFactor3->add(measurements_cam3, views);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
@ -978,12 +974,12 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
params.setDegeneracyMode(gtsam::HANDLE_INFINITY);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK2, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK2, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK2, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK2, boost::none, params));
smartFactor2->add(measurements_cam2, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3, 0.10);
@ -1040,16 +1036,16 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,
@ -1108,8 +1104,8 @@ TEST( SmartProjectionPoseFactor, Hessian ) {
measurements_cam1.push_back(cam1_uv1);
measurements_cam1.push_back(cam2_uv1);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedK2));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK2));
smartFactor1->add(measurements_cam1, views);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
@ -1140,8 +1136,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotation ) {
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(sharedK));
smartFactorInstance->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor(model, sharedK));
smartFactorInstance->add(measurements_cam1, views);
Values values;
values.insert(x1, cam1.pose());
@ -1196,8 +1192,8 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
vector<Point2> measurements_cam1;
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
SmartFactor::shared_ptr smartFactor(new SmartFactor(sharedK2));
smartFactor->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedK2));
smartFactor->add(measurements_cam1, views);
Values values;
values.insert(x1, cam1.pose());
@ -1239,8 +1235,8 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
using namespace bundlerPose;
SmartProjectionParams params;
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
SmartFactor factor(sharedBundlerK, boost::none, params);
factor.add(measurement1, x1, model);
SmartFactor factor(model, sharedBundlerK, boost::none, params);
factor.add(measurement1, x1);
}
/* *************************************************************************/
@ -1263,14 +1259,14 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
views.push_back(x2);
views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(sharedBundlerK));
smartFactor1->add(measurements_cam1, views, model);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedBundlerK));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(sharedBundlerK));
smartFactor2->add(measurements_cam2, views, model);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedBundlerK));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(sharedBundlerK));
smartFactor3->add(measurements_cam3, views, model);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedBundlerK));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1334,16 +1330,16 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(sharedBundlerK, boost::none, params));
smartFactor1->add(measurements_cam1, views, model);
new SmartFactor(model, sharedBundlerK, boost::none, params));
smartFactor1->add(measurements_cam1, views);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(sharedBundlerK, boost::none, params));
smartFactor2->add(measurements_cam2, views, model);
new SmartFactor(model, sharedBundlerK, boost::none, params));
smartFactor2->add(measurements_cam2, views);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(sharedBundlerK, boost::none, params));
smartFactor3->add(measurements_cam3, views, model);
new SmartFactor(model, sharedBundlerK, boost::none, params));
smartFactor3->add(measurements_cam3, views);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
const SharedDiagonal noisePriorTranslation = noiseModel::Isotropic::Sigma(3,

45
gtsam/slam/tests/testTriangulationFactor.cpp
View File

@ -18,7 +18,10 @@
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/nonlinear/Expression.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -46,7 +49,7 @@ Point2 z1 = camera1.project(landmark);
//******************************************************************************
TEST( triangulation, TriangulationFactor ) {
// Create the factor with a measurement that is 3 pixels off in x
Key pointKey(1);
SharedNoiseModel model;
typedef TriangulationFactor<SimpleCamera> Factor;
@ -63,6 +66,46 @@ TEST( triangulation, TriangulationFactor ) {
CHECK(assert_equal(HExpected, HActual, 1e-3));
}
//******************************************************************************
TEST( triangulation, TriangulationFactorStereo ) {
Key pointKey(1);
SharedNoiseModel model=noiseModel::Isotropic::Sigma(3,0.5);
Cal3_S2Stereo::shared_ptr stereoCal(new Cal3_S2Stereo(1500, 1200, 0, 640, 480, 0.5));
StereoCamera camera2(pose1, stereoCal);
StereoPoint2 z2 = camera2.project(landmark);
typedef TriangulationFactor<StereoCamera> Factor;
Factor factor(camera2, z2, model, pointKey);
// Use the factor to calculate the Jacobians
Matrix HActual;
factor.evaluateError(landmark, HActual);
Matrix HExpected = numericalDerivative11<Vector,Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
// Verify the Jacobians are correct
CHECK(assert_equal(HExpected, HActual, 1e-3));
// compare same problem against expression factor
Expression<StereoPoint2>::UnaryFunction<Point3>::type f = boost::bind(&StereoCamera::project2, camera2, _1, boost::none, _2);
Expression<Point3> point_(pointKey);
Expression<StereoPoint2> project2_(f, point_);
ExpressionFactor<StereoPoint2> eFactor(model, z2, project2_);
Values values;
values.insert(pointKey, landmark);
vector<Matrix> HActual1(1), HActual2(1);
Vector error1 = factor.unwhitenedError(values, HActual1);
Vector error2 = eFactor.unwhitenedError(values, HActual2);
EXPECT(assert_equal(error1, error2));
EXPECT(assert_equal(HActual1[0], HActual2[0]));
}
//******************************************************************************
int main() {
TestResult tr;

26
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -38,13 +38,14 @@ Vector PoseRTV::vector() const {
Vector rtv(9);
rtv.head(3) = rotation().xyz();
rtv.segment(3,3) = translation().vector();
rtv.tail(3) = velocity().vector();
rtv.tail(3) = velocity();
return rtv;
}
/* ************************************************************************* */
bool PoseRTV::equals(const PoseRTV& other, double tol) const {
return pose().equals(other.pose(), tol) && velocity().equals(other.velocity(), tol);
return pose().equals(other.pose(), tol)
&& equal_with_abs_tol(velocity(), other.velocity(), tol);
}
/* ************************************************************************* */
@ -52,7 +53,7 @@ void PoseRTV::print(const string& s) const {
cout << s << ":" << endl;
gtsam::print((Vector)R().xyz(), " R:rpy");
t().print(" T");
velocity().print(" V");
gtsam::print((Vector)velocity(), " V");
}
/* ************************************************************************* */
@ -137,7 +138,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
Vector6 imu;
// acceleration
Vector3 accel = (v2-v1).vector() / dt;
Vector3 accel = (v2-v1) / dt;
imu.head<3>() = r2.transpose() * (accel - kGravity);
// rotation rates
@ -160,7 +161,7 @@ Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, double dt) const {
// predict point for constraint
// NOTE: uses simple Euler approach for prediction
Point3 pred_t2 = t() + v2 * dt;
Point3 pred_t2 = t().retract(v2 * dt);
return pred_t2;
}
@ -187,7 +188,7 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
// Note that we rotate the velocity
Matrix3 D_newvel_R, D_newvel_v;
Velocity3 newvel = trans.rotation().rotate(velocity(), D_newvel_R, D_newvel_v);
Velocity3 newvel = trans.rotation().rotate(Point3(velocity()), D_newvel_R, D_newvel_v).vector();
if (Dglobal) {
Dglobal->setZero();
@ -204,10 +205,10 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal,
}
/* ************************************************************************* */
Matrix PoseRTV::RRTMbn(const Vector& euler) {
Matrix PoseRTV::RRTMbn(const Vector3& euler) {
assert(euler.size() == 3);
const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1));
const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1));
const double s1 = sin(euler.x()), c1 = cos(euler.x());
const double t2 = tan(euler.y()), c2 = cos(euler.y());
Matrix Ebn(3,3);
Ebn << 1.0, s1 * t2, c1 * t2,
0.0, c1, -s1,
@ -221,11 +222,10 @@ Matrix PoseRTV::RRTMbn(const Rot3& att) {
}
/* ************************************************************************* */
Matrix PoseRTV::RRTMnb(const Vector& euler) {
assert(euler.size() == 3);
Matrix PoseRTV::RRTMnb(const Vector3& euler) {
Matrix Enb(3,3);
const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1));
const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1));
const double s1 = sin(euler.x()), c1 = cos(euler.x());
const double s2 = sin(euler.y()), c2 = cos(euler.y());
Enb << 1.0, 0.0, -s2,
0.0, c1, s1*c2,
0.0, -s1, c1*c2;

26
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -13,11 +13,12 @@
namespace gtsam {
/// Syntactic sugar to clarify components
typedef Point3 Velocity3;
typedef Vector3 Velocity3;
/**
* Robot state for use with IMU measurements
* - contains translation, translational velocity and rotation
* TODO(frank): Alex should deprecate/move to project
*/
class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
protected:
@ -34,11 +35,11 @@ public:
PoseRTV(const Rot3& rot, const Point3& t, const Velocity3& vel)
: Base(Pose3(rot, t), vel) {}
explicit PoseRTV(const Point3& t)
: Base(Pose3(Rot3(), t),Velocity3()) {}
: Base(Pose3(Rot3(), t),Vector3::Zero()) {}
PoseRTV(const Pose3& pose, const Velocity3& vel)
: Base(pose, vel) {}
explicit PoseRTV(const Pose3& pose)
: Base(pose,Velocity3()) {}
: Base(pose,Vector3::Zero()) {}
// Construct from Base
PoseRTV(const Base& base)
@ -66,7 +67,7 @@ public:
// and avoidance of Point3
Vector vector() const;
Vector translationVec() const { return pose().translation().vector(); }
Vector velocityVec() const { return velocity().vector(); }
const Velocity3& velocityVec() const { return velocity(); }
// testable
bool equals(const PoseRTV& other, double tol=1e-6) const;
@ -145,14 +146,12 @@ public:
/// RRTMbn - Function computes the rotation rate transformation matrix from
/// body axis rates to euler angle (global) rates
static Matrix RRTMbn(const Vector& euler);
static Matrix RRTMbn(const Vector3& euler);
static Matrix RRTMbn(const Rot3& att);
/// RRTMnb - Function computes the rotation rate transformation matrix from
/// euler angle rates to body axis rates
static Matrix RRTMnb(const Vector& euler);
static Matrix RRTMnb(const Vector3& euler);
static Matrix RRTMnb(const Rot3& att);
/// @}
@ -173,14 +172,7 @@ struct traits<PoseRTV> : public internal::LieGroup<PoseRTV> {};
// Define Range functor specializations that are used in RangeFactor
template <typename A1, typename A2> struct Range;
template <>
struct Range<PoseRTV, PoseRTV> {
typedef double result_type;
double operator()(const PoseRTV& pose1, const PoseRTV& pose2,
OptionalJacobian<1, 9> H1 = boost::none,
OptionalJacobian<1, 9> H2 = boost::none) {
return pose1.range(pose2, H1, H2);
}
};
template<>
struct Range<PoseRTV, PoseRTV> : HasRange<PoseRTV, PoseRTV, double> {};
} // \namespace gtsam

11
gtsam_unstable/dynamics/VelocityConstraint.h
View File

@ -106,12 +106,13 @@ private:
static gtsam::Vector evaluateError_(const PoseRTV& x1, const PoseRTV& x2,
double dt, const dynamics::IntegrationMode& mode) {
const Velocity3& v1 = x1.v(), v2 = x2.v(), p1 = x1.t(), p2 = x2.t();
Velocity3 hx;
const Velocity3& v1 = x1.v(), v2 = x2.v();
const Point3& p1 = x1.t(), p2 = x2.t();
Point3 hx;
switch(mode) {
case dynamics::TRAPEZOIDAL: hx = p1 + (v1 + v2) * dt *0.5; break;
case dynamics::EULER_START: hx = p1 + v1 * dt; break;
case dynamics::EULER_END : hx = p1 + v2 * dt; break;
case dynamics::TRAPEZOIDAL: hx = p1.retract((v1 + v2) * dt *0.5); break;
case dynamics::EULER_START: hx = p1.retract(v1 * dt); break;
case dynamics::EULER_END : hx = p1.retract(v2 * dt); break;
default: assert(false); break;
}
return (p2 - hx).vector();

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

@ -56,9 +56,9 @@ TEST( testIMUSystem, optimize_chain ) {
// create a simple chain of poses to generate IMU measurements
const double dt = 1.0;
PoseRTV pose1,
pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Point3(2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Point3(0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Point3(2.0, 2.0, 0.0));
pose2(Point3(1.0, 1.0, 0.0), Rot3::ypr(0.1, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0)),
pose3(Point3(2.0, 2.0, 0.0), Rot3::ypr(0.2, 0.0, 0.0), Velocity3(0.0, 0.0, 0.0)),
pose4(Point3(3.0, 3.0, 0.0), Rot3::ypr(0.3, 0.0, 0.0), Velocity3(2.0, 2.0, 0.0));
// create measurements
SharedDiagonal model = noiseModel::Unit::Create(6);
@ -102,9 +102,9 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// create a simple chain of poses to generate IMU measurements
const double dt = 1.0;
PoseRTV pose1,
pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Point3(1.0, 0.0, 0.0));
pose2(Point3(1.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
pose3(Point3(2.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0)),
pose4(Point3(3.0, 0.0, 0.0), Rot3::ypr(0.0, 0.0, 0.0), Velocity3(1.0, 0.0, 0.0));
// create measurements
SharedDiagonal model = noiseModel::Isotropic::Sigma(9, 1.0);

160
gtsam_unstable/dynamics/tests/testPoseRTV.cpp
View File

@ -15,44 +15,43 @@ using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(PoseRTV)
GTSAM_CONCEPT_LIE_INST(PoseRTV)
const double tol=1e-5;
Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3);
Point3 pt(1.0, 2.0, 3.0);
Velocity3 vel(0.4, 0.5, 0.6);
static const Rot3 rot = Rot3::RzRyRx(0.1, 0.2, 0.3);
static const Point3 pt(1.0, 2.0, 3.0);
static const Velocity3 vel(0.4, 0.5, 0.6);
static const Vector3 kZero3 = Vector3::Zero();
/* ************************************************************************* */
TEST( testPoseRTV, constructors ) {
PoseRTV state1(pt, rot, vel);
EXPECT(assert_equal(pt, state1.t(), tol));
EXPECT(assert_equal(rot, state1.R(), tol));
EXPECT(assert_equal(vel, state1.v(), tol));
EXPECT(assert_equal(Pose3(rot, pt), state1.pose(), tol));
EXPECT(assert_equal(pt, state1.t()));
EXPECT(assert_equal(rot, state1.R()));
EXPECT(assert_equal(vel, state1.v()));
EXPECT(assert_equal(Pose3(rot, pt), state1.pose()));
PoseRTV state2;
EXPECT(assert_equal(Point3(), state2.t(), tol));
EXPECT(assert_equal(Rot3(), state2.R(), tol));
EXPECT(assert_equal(Velocity3(), state2.v(), tol));
EXPECT(assert_equal(Pose3(), state2.pose(), tol));
EXPECT(assert_equal(Point3(), state2.t()));
EXPECT(assert_equal(Rot3(), state2.R()));
EXPECT(assert_equal(kZero3, state2.v()));
EXPECT(assert_equal(Pose3(), state2.pose()));
PoseRTV state3(Pose3(rot, pt), vel);
EXPECT(assert_equal(pt, state3.t(), tol));
EXPECT(assert_equal(rot, state3.R(), tol));
EXPECT(assert_equal(vel, state3.v(), tol));
EXPECT(assert_equal(Pose3(rot, pt), state3.pose(), tol));
EXPECT(assert_equal(pt, state3.t()));
EXPECT(assert_equal(rot, state3.R()));
EXPECT(assert_equal(vel, state3.v()));
EXPECT(assert_equal(Pose3(rot, pt), state3.pose()));
PoseRTV state4(Pose3(rot, pt));
EXPECT(assert_equal(pt, state4.t(), tol));
EXPECT(assert_equal(rot, state4.R(), tol));
EXPECT(assert_equal(Velocity3(), state4.v(), tol));
EXPECT(assert_equal(Pose3(rot, pt), state4.pose(), tol));
EXPECT(assert_equal(pt, state4.t()));
EXPECT(assert_equal(rot, state4.R()));
EXPECT(assert_equal(kZero3, state4.v()));
EXPECT(assert_equal(Pose3(rot, pt), state4.pose()));
Vector vec_init = (Vector(9) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0, 0.4, 0.5, 0.6).finished();
PoseRTV state5(vec_init);
EXPECT(assert_equal(pt, state5.t(), tol));
EXPECT(assert_equal(rot, state5.R(), tol));
EXPECT(assert_equal(vel, state5.v(), tol));
EXPECT(assert_equal(vec_init, state5.vector(), tol));
EXPECT(assert_equal(pt, state5.t()));
EXPECT(assert_equal(rot, state5.R()));
EXPECT(assert_equal(vel, state5.v()));
EXPECT(assert_equal(vec_init, state5.vector()));
}
/* ************************************************************************* */
@ -65,33 +64,44 @@ TEST( testPoseRTV, dim ) {
/* ************************************************************************* */
TEST( testPoseRTV, equals ) {
PoseRTV state1, state2(pt, rot, vel), state3(state2), state4(Pose3(rot, pt));
EXPECT(assert_equal(state1, state1, tol));
EXPECT(assert_equal(state2, state3, tol));
EXPECT(assert_equal(state3, state2, tol));
EXPECT(assert_inequal(state1, state2, tol));
EXPECT(assert_inequal(state2, state1, tol));
EXPECT(assert_inequal(state2, state4, tol));
EXPECT(assert_equal(state1, state1));
EXPECT(assert_equal(state2, state3));
EXPECT(assert_equal(state3, state2));
EXPECT(assert_inequal(state1, state2));
EXPECT(assert_inequal(state2, state1));
EXPECT(assert_inequal(state2, state4));
}
/* ************************************************************************* */
TEST( testPoseRTV, Lie ) {
// origin and zero deltas
PoseRTV identity;
EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9)), tol));
EXPECT(assert_equal(zero(9), identity.localCoordinates(identity), tol));
EXPECT(assert_equal(identity, (PoseRTV)identity.retract(zero(9))));
EXPECT(assert_equal(zero(9), identity.localCoordinates(identity)));
PoseRTV state1(pt, rot, vel);
EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9)), tol));
EXPECT(assert_equal(zero(9), state1.localCoordinates(state1), tol));
EXPECT(assert_equal(state1, (PoseRTV)state1.retract(zero(9))));
EXPECT(assert_equal(zero(9), state1.localCoordinates(state1)));
Vector delta = (Vector(9) << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3).finished();
Rot3 rot2 = rot.retract(repeat(3, 0.1));
Point3 pt2 = pt + rot * Point3(0.2, 0.3, 0.4);
Velocity3 vel2 = vel + rot * Velocity3(-0.1,-0.2,-0.3);
PoseRTV state2(pt2, rot2, vel2);
EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta), 1e-1));
EXPECT(assert_equal(delta, state1.localCoordinates(state2), 1e-1));
EXPECT(assert_equal(delta, -state2.localCoordinates(state1), 1e-1));
Vector delta(9);
delta << 0.1, 0.1, 0.1, 0.2, 0.3, 0.4,-0.1,-0.2,-0.3;
Pose3 pose2 = Pose3(rot, pt).retract(delta.head<6>());
Velocity3 vel2 = vel + Velocity3(-0.1, -0.2, -0.3);
PoseRTV state2(pose2.translation(), pose2.rotation(), vel2);
EXPECT(assert_equal(state2, (PoseRTV)state1.retract(delta)));
EXPECT(assert_equal(delta, state1.localCoordinates(state2)));
// roundtrip from state2 to state3 and back
PoseRTV state3 = state2.retract(delta);
EXPECT(assert_equal(delta, state2.localCoordinates(state3)));
// roundtrip from state3 to state4 and back, with expmap.
PoseRTV state4 = state3.expmap(delta);
EXPECT(assert_equal(delta, state3.logmap(state4)));
// For the expmap/logmap (not necessarily retract/local) -delta goes other way
EXPECT(assert_equal(state3, (PoseRTV)state4.expmap(-delta)));
EXPECT(assert_equal(delta, -state4.logmap(state3)));
}
/* ************************************************************************* */
@ -108,15 +118,15 @@ TEST( testPoseRTV, dynamics_identities ) {
x3 = x2.generalDynamics(accel, gyro, dt);
x4 = x3.generalDynamics(accel, gyro, dt);
// EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first, tol));
// EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first, tol));
// EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first, tol));
// EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first, tol));
// EXPECT(assert_equal(imu01, x0.imuPrediction(x1, dt).first));
// EXPECT(assert_equal(imu12, x1.imuPrediction(x2, dt).first));
// EXPECT(assert_equal(imu23, x2.imuPrediction(x3, dt).first));
// EXPECT(assert_equal(imu34, x3.imuPrediction(x4, dt).first));
//
// EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second, tol));
// EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second, tol));
// EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second, tol));
// EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second, tol));
// EXPECT(assert_equal(x1.translation(), x0.imuPrediction(x1, dt).second));
// EXPECT(assert_equal(x2.translation(), x1.imuPrediction(x2, dt).second));
// EXPECT(assert_equal(x3.translation(), x2.imuPrediction(x3, dt).second));
// EXPECT(assert_equal(x4.translation(), x3.imuPrediction(x4, dt).second));
}
@ -129,8 +139,8 @@ TEST( testPoseRTV, compose ) {
state1.compose(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(compose_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(compose_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
EXPECT(assert_equal(numericH1, actH1));
EXPECT(assert_equal(numericH2, actH2));
}
/* ************************************************************************* */
@ -142,8 +152,8 @@ TEST( testPoseRTV, between ) {
state1.between(state2, actH1, actH2);
Matrix numericH1 = numericalDerivative21(between_proxy, state1, state2);
Matrix numericH2 = numericalDerivative22(between_proxy, state1, state2);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
EXPECT(assert_equal(numericH1, actH1));
EXPECT(assert_equal(numericH2, actH2));
}
/* ************************************************************************* */
@ -154,7 +164,7 @@ TEST( testPoseRTV, inverse ) {
Matrix actH1;
state1.inverse(actH1);
Matrix numericH1 = numericalDerivative11(inverse_proxy, state1);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH1, actH1));
}
/* ************************************************************************* */
@ -162,16 +172,16 @@ double range_proxy(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
TEST( testPoseRTV, range ) {
Point3 tA(1.0, 2.0, 3.0), tB(3.0, 2.0, 3.0);
PoseRTV rtvA(tA), rtvB(tB);
EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), tol);
EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), tol);
EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), tol);
EXPECT_DOUBLES_EQUAL(0.0, rtvA.range(rtvA), 1e-9);
EXPECT_DOUBLES_EQUAL(2.0, rtvA.range(rtvB), 1e-9);
EXPECT_DOUBLES_EQUAL(2.0, rtvB.range(rtvA), 1e-9);
Matrix actH1, actH2;
rtvA.range(rtvB, actH1, actH2);
Matrix numericH1 = numericalDerivative21(range_proxy, rtvA, rtvB);
Matrix numericH2 = numericalDerivative22(range_proxy, rtvA, rtvB);
EXPECT(assert_equal(numericH1, actH1, tol));
EXPECT(assert_equal(numericH2, actH2, tol));
EXPECT(assert_equal(numericH1, actH1));
EXPECT(assert_equal(numericH2, actH2));
}
/* ************************************************************************* */
@ -187,13 +197,13 @@ TEST( testPoseRTV, transformed_from_1 ) {
Matrix actDTrans, actDGlobal;
PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V));
EXPECT(assert_equal(expected, actual, tol));
PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
EXPECT(assert_equal(expected, actual));
Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size
EXPECT(assert_equal(numDGlobal, actDGlobal, tol));
EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative
EXPECT(assert_equal(numDGlobal, actDGlobal));
EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative
}
/* ************************************************************************* */
@ -206,27 +216,27 @@ TEST( testPoseRTV, transformed_from_2 ) {
Matrix actDTrans, actDGlobal;
PoseRTV actual = start.transformed_from(transform, actDGlobal, actDTrans);
PoseRTV expected(transform.compose(start.pose()), transform.rotation().rotate(V));
EXPECT(assert_equal(expected, actual, tol));
PoseRTV expected(transform.compose(start.pose()), transform.rotation().matrix() * V);
EXPECT(assert_equal(expected, actual));
Matrix numDGlobal = numericalDerivative21(transformed_from_proxy, start, transform, 1e-5); // At 1e-8, fails
Matrix numDTrans = numericalDerivative22(transformed_from_proxy, start, transform, 1e-8); // Sensitive to step size
EXPECT(assert_equal(numDGlobal, actDGlobal, tol));
EXPECT(assert_equal(numDTrans, actDTrans, tol)); // FIXME: still needs analytic derivative
EXPECT(assert_equal(numDGlobal, actDGlobal));
EXPECT(assert_equal(numDTrans, actDTrans, 1e-5)); // FIXME: still needs analytic derivative
}
/* ************************************************************************* */
TEST(testPoseRTV, RRTMbn) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(zero(3)), tol));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3()), tol));
EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3)), tol));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(kZero3)));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMbn(Rot3())));
EXPECT(assert_equal(PoseRTV::RRTMbn(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMbn(Rot3::ypr(0.1, 0.2, 0.3))));
}
/* ************************************************************************* */
TEST(testPoseRTV, RRTMnb) {
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(zero(3)), tol));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3()), tol));
EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3)), tol));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(kZero3)));
EXPECT(assert_equal(Matrix::Identity(3,3), PoseRTV::RRTMnb(Rot3())));
EXPECT(assert_equal(PoseRTV::RRTMnb(Vector3(0.3, 0.2, 0.1)), PoseRTV::RRTMnb(Rot3::ypr(0.1, 0.2, 0.3))));
}
/* ************************************************************************* */

4
gtsam_unstable/dynamics/tests/testVelocityConstraint.cpp
View File

@ -15,9 +15,9 @@ const Key x1 = 1, x2 = 2;
const double dt = 1.0;
PoseRTV origin,
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0)),
pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0)),
pose1a(Point3(0.5, 0.0, 0.0)),
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Point3(1.0, 0.0, 0.0));
pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Velocity3(1.0, 0.0, 0.0));
/* ************************************************************************* */
TEST( testVelocityConstraint, trapezoidal ) {

6
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -87,17 +87,17 @@ int main(int argc, char** argv){
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor(K));
SmartFactor::shared_ptr factor(new SmartFactor(model, K));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> i >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor(K));
factor = SmartFactor::shared_ptr(new SmartFactor(model, K));
current_l = l;
}
factor->add(Point2(uL,v), i, model);
factor->add(Point2(uL,v), i);
}
Pose3 firstPose = initial_estimate.at<Pose3>(1);

8
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -46,7 +46,7 @@ using namespace gtsam;
int main(int argc, char** argv){
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
typedef SmartStereoProjectionPoseFactor SmartFactor;
bool output_poses = true;
string poseOutput("../../../examples/data/optimized_poses.txt");
@ -109,17 +109,17 @@ int main(int argc, char** argv){
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor());
SmartFactor::shared_ptr factor(new SmartFactor(model));
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor());
factor = SmartFactor::shared_ptr(new SmartFactor(model));
current_l = l;
}
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), model, K);
factor->add(StereoPoint2(uL,uR,v), Symbol('x',x), K);
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));

29
gtsam_unstable/geometry/Event.cpp Normal file
View File

@ -0,0 +1,29 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Event
* @brief Space-time event
* @author Frank Dellaert
* @author Jay Chakravarty
* @date December 2014
*/
#include <gtsam_unstable/geometry/Event.h>
namespace gtsam {
const double Event::Speed = 330;
const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished();
}

3
gtsam_unstable/geometry/Event.h
View File

@ -99,9 +99,6 @@ public:
}
};
const double Event::Speed = 330;
const Matrix14 Event::JacobianZ = (Matrix14() << 0,0,0,1).finished();
// Define GTSAM traits
template<>
struct GTSAM_EXPORT traits<Event> : internal::Manifold<Event> {};

8
gtsam_unstable/gtsam_unstable.h
View File

@ -53,9 +53,9 @@ class Dummy {
class PoseRTV {
PoseRTV();
PoseRTV(Vector rtv);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const gtsam::Point3& vel);
PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const gtsam::Point3& vel);
PoseRTV(const gtsam::Pose3& pose, const gtsam::Point3& vel);
PoseRTV(const gtsam::Point3& pt, const gtsam::Rot3& rot, const Vector& vel);
PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel);
PoseRTV(const gtsam::Pose3& pose, const Vector& vel);
PoseRTV(const gtsam::Pose3& pose);
PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz);
@ -66,7 +66,7 @@ class PoseRTV {
// access
gtsam::Point3 translation() const;
gtsam::Rot3 rotation() const;
gtsam::Point3 velocity() const;
Vector velocity() const;
gtsam::Pose3 pose() const;
// Vector interfaces

2
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -101,7 +101,7 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u,
// convert to navigation frame
Rot3 nRb = bRn_.inverse();
Vector3 n_omega_bn = (nRb*b_omega_bn).vector();
Vector3 n_omega_bn = nRb * b_omega_bn;
// integrate bRn using exponential map, assuming constant over dt
Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt);

2
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -42,7 +42,7 @@ public:
/// gravity in the body frame
Vector3 b_g(double g_e) const {
Vector3 n_g(0, 0, g_e);
return (bRn_ * n_g).vector();
return bRn_ * n_g;
}
const Rot3& bRn() const {return bRn_; }

775
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -11,10 +11,11 @@
/**
* @file SmartStereoProjectionFactor.h
* @brief Base class to create smart factors on poses or cameras
* @brief Smart stereo factor on StereoCameras (pose + calibration)
* @author Luca Carlone
* @author Zsolt Kira
* @author Frank Dellaert
* @author Chris Beall
*/
#pragma once
@ -24,6 +25,7 @@
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
@ -33,79 +35,136 @@
namespace gtsam {
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
/// How to manage degeneracy
enum DegeneracyMode {
IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY
};
/*
* Parameters for the smart stereo projection factors
*/
struct GTSAM_EXPORT SmartStereoProjectionParams {
LinearizationMode linearizationMode; ///< How to linearize the factor
DegeneracyMode degeneracyMode; ///< How to linearize the factor
/// @name Parameters governing the triangulation
/// @{
TriangulationParameters triangulation;
double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
/// @}
/// @name Parameters governing how triangulation result is treated
/// @{
bool throwCheirality; ///< If true, re-throws Cheirality exceptions (default: false)
bool verboseCheirality; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
/// Constructor
SmartStereoProjectionParams(LinearizationMode linMode = HESSIAN,
DegeneracyMode degMode = IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false) :
linearizationMode(linMode), degeneracyMode(degMode), retriangulationThreshold(
1e-5), throwCheirality(throwCheirality), verboseCheirality(
verboseCheirality) {
}
virtual ~SmartStereoProjectionParams() {
}
void print(const std::string& str) const {
std::cout << "linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << triangulation << std::endl;
}
LinearizationMode getLinearizationMode() const {
return linearizationMode;
}
DegeneracyMode getDegeneracyMode() const {
return degeneracyMode;
}
TriangulationParameters getTriangulationParameters() const {
return triangulation;
}
bool getVerboseCheirality() const {
return verboseCheirality;
}
bool getThrowCheirality() const {
return throwCheirality;
}
void setLinearizationMode(LinearizationMode linMode) {
linearizationMode = linMode;
}
void setDegeneracyMode(DegeneracyMode degMode) {
degeneracyMode = degMode;
}
void setRankTolerance(double rankTol) {
triangulation.rankTolerance = rankTol;
}
void setEnableEPI(bool enableEPI) {
triangulation.enableEPI = enableEPI;
}
void setLandmarkDistanceThreshold(double landmarkDistanceThreshold) {
triangulation.landmarkDistanceThreshold = landmarkDistanceThreshold;
}
void setDynamicOutlierRejectionThreshold(double dynOutRejectionThreshold) {
triangulation.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
}
};
/**
* SmartStereoProjectionFactor: triangulates point
*/
template<class CALIBRATION>
* SmartStereoProjectionFactor: triangulates point and keeps an estimate of it around.
* This factor operates with StereoCamera. This factor requires that values
* contains the involved StereoCameras. Calibration is assumed to be fixed, as this
* is also assumed in StereoCamera.
* If you'd like to store poses in values instead of cameras, use
* SmartStereoProjectionPoseFactor instead
*/
class SmartStereoProjectionFactor: public SmartFactorBase<StereoCamera> {
private:
typedef SmartFactorBase<StereoCamera> Base;
protected:
/// @name Parameters
/// @{
const SmartStereoProjectionParams params_;
/// @}
/// @name Caching triangulation
/// @{
const TriangulationParameters parameters_;
// TODO:
// mutable TriangulationResult result_; ///< result from triangulateSafe
const double retriangulationThreshold_; ///< threshold to decide whether to re-triangulate
mutable TriangulationResult result_; ///< result from triangulateSafe
mutable std::vector<Pose3> cameraPosesTriangulation_; ///< current triangulation poses
/// @}
const bool manageDegeneracy_; ///< if set to true will use the rotation-only version for degenerate cases
const double linearizationThreshold_; ///< threshold to decide whether to re-linearize
mutable std::vector<Pose3> cameraPosesLinearization_; ///< current linearization poses
mutable Point3 point_; ///< Current estimate of the 3D point
mutable bool degenerate_;
mutable bool cheiralityException_;
/// shorthand for base class type
typedef SmartFactorBase<StereoCamera> Base;
/// shorthand for this class
typedef SmartStereoProjectionFactor<CALIBRATION> This;
enum {
ZDim = 3
}; ///< Dimension trait of measurement type
/// @name Parameters governing how triangulation result is treated
/// @{
const bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
const bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
/// @}
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// shorthand for a StereoCamera // TODO: Get rid of this?
typedef StereoCamera Camera;
typedef boost::shared_ptr<SmartStereoProjectionFactor> shared_ptr;
/// Vector of cameras
typedef CameraSet<Camera> Cameras;
typedef CameraSet<StereoCamera> Cameras;
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param body_P_sensor is the transform from body to sensor frame (default identity)
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionFactor(const double rankTolerance,
const double linThreshold, const bool manageDegeneracy,
const bool enableEPI, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
parameters_(rankTolerance, enableEPI, landmarkDistanceThreshold,
dynamicOutlierRejectionThreshold), //
retriangulationThreshold_(1e-5), manageDegeneracy_(manageDegeneracy), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false) {
SmartStereoProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()) :
Base(sharedNoiseModel), //
params_(params), //
result_(TriangulationResult::Degenerate()) {
}
/** Virtual destructor */
@ -120,14 +179,20 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartStereoProjectionFactor\n";
std::cout << "triangulationParameters:\n" << parameters_ << std::endl;
std::cout << "degenerate_ = " << degenerate_ << std::endl;
std::cout << "cheiralityException_ = " << cheiralityException_ << std::endl;
std::cout << "linearizationThreshold_ = " << linearizationThreshold_
<< std::endl;
std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl;
std::cout << "triangulationParameters:\n" << params_.triangulation << std::endl;
std::cout << "result:\n" << result_ << std::endl;
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const SmartStereoProjectionFactor *e =
dynamic_cast<const SmartStereoProjectionFactor*>(&p);
return e && params_.linearizationMode == e->params_.linearizationMode
&& Base::equals(p, tol);
}
/// Check if the new linearization point_ is the same as the one used for previous triangulation
bool decideIfTriangulate(const Cameras& cameras) const {
// several calls to linearize will be done from the same linearization point_, hence it is not needed to re-triangulate
@ -146,7 +211,7 @@ public:
if (!retriangulate) {
for (size_t i = 0; i < cameras.size(); i++) {
if (!cameras[i].pose().equals(cameraPosesTriangulation_[i],
retriangulationThreshold_)) {
params_.retriangulationThreshold)) {
retriangulate = true; // at least two poses are different, hence we retriangulate
break;
}
@ -164,124 +229,99 @@ public:
return retriangulate; // if we arrive to this point_ all poses are the same and we don't need re-triangulation
}
/// triangulateSafe
size_t triangulateSafe(const Values& values) const {
return triangulateSafe(this->cameras(values));
}
// /// triangulateSafe
// size_t triangulateSafe(const Values& values) const {
// return triangulateSafe(this->cameras(values));
// }
/// triangulateSafe
size_t triangulateSafe(const Cameras& cameras) const {
TriangulationResult triangulateSafe(const Cameras& cameras) const {
size_t m = cameras.size();
if (m < 2) { // if we have a single pose the corresponding factor is uninformative
degenerate_ = true;
return m;
}
bool retriangulate = decideIfTriangulate(cameras);
// if(!retriangulate)
// std::cout << "retriangulate = false" << std::endl;
//
// bool retriangulate = true;
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
// std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
//TODO: Chris will replace this with something else for stereo
// point_ = triangulatePoint3<CALIBRATION>(cameras, this->measured_,
// rankTolerance_, enableEPI_);
// // // Temporary hack to use monocular triangulation
std::vector<Point2> mono_measurements;
BOOST_FOREACH(const StereoPoint2& sp, this->measured_) {
mono_measurements.push_back(sp.point2());
}
std::vector<PinholeCamera<Cal3_S2> > mono_cameras;
BOOST_FOREACH(const Camera& camera, cameras) {
const Pose3& pose = camera.pose();
const Cal3_S2& K = camera.calibration()->calibration();
mono_cameras.push_back(PinholeCamera<Cal3_S2>(pose, K));
}
point_ = triangulatePoint3<PinholeCamera<Cal3_S2> >(mono_cameras, mono_measurements,
parameters_.rankTolerance, parameters_.enableEPI);
// // // End temporary hack
// FIXME: temporary: triangulation using only first camera
// const StereoPoint2& z0 = this->measured_.at(0);
// point_ = cameras[0].backproject(z0);
degenerate_ = false;
cheiralityException_ = false;
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i = 0;
BOOST_FOREACH(const Camera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if (cameraTranslation.distance(point_) > parameters_.landmarkDistanceThreshold) {
degenerate_ = true;
break;
}
const StereoPoint2& zi = this->measured_.at(i);
try {
StereoPoint2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
}
i += 1;
}
//std::cout << "totalReprojError error: " << totalReprojError << std::endl;
// we discard smart factors that have large reprojection error
if (parameters_.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > parameters_.dynamicOutlierRejectionThreshold)
degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
// 2) The rank of the matrix used for triangulation is < 3: rotation-only, parallel cameras (or motion towards the landmark)
// in the second case we want to use a rotation-only smart factor
degenerate_ = true;
cheiralityException_ = false;
} catch (TriangulationCheiralityException&) {
// point is behind one of the cameras: can be the case of close-to-parallel cameras or may depend on outliers
// we manage this case by either discarding the smart factor, or imposing a rotation-only constraint
cheiralityException_ = true;
// std::cout << "Retriangulate " << std::endl;
std::vector<Point3> reprojections;
reprojections.reserve(m);
for(size_t i = 0; i < m; i++) {
reprojections.push_back(cameras[i].backproject(measured_[i]));
}
}
return m;
Point3 pw_sum;
BOOST_FOREACH(const Point3& pw, reprojections) {
pw_sum = pw_sum + pw;
}
// average reprojected landmark
Point3 pw_avg = pw_sum / double(m);
double totalReprojError = 0;
// check if it lies in front of all cameras
for(size_t i = 0; i < m; i++) {
const Pose3& pose = cameras[i].pose();
const Point3& pl = pose.transform_to(pw_avg);
if (pl.z() <= 0) {
result_ = TriangulationResult::BehindCamera();
return result_;
}
// check landmark distance
if (params_.triangulation.landmarkDistanceThreshold > 0 &&
pl.norm() > params_.triangulation.landmarkDistanceThreshold) {
result_ = TriangulationResult::Degenerate();
return result_;
}
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0) {
const StereoPoint2& zi = measured_[i];
StereoPoint2 reprojectionError(cameras[i].project(pw_avg) - zi);
totalReprojError += reprojectionError.vector().norm();
}
} // for
if (params_.triangulation.dynamicOutlierRejectionThreshold > 0
&& totalReprojError / m > params_.triangulation.dynamicOutlierRejectionThreshold) {
result_ = TriangulationResult::Degenerate();
return result_;
}
if(params_.triangulation.enableEPI) {
try {
pw_avg = triangulateNonlinear(cameras, measured_, pw_avg);
} catch(StereoCheiralityException& e) {
if(params_.verboseCheirality)
std::cout << "Cheirality Exception in SmartStereoProjectionFactor" << std::endl;
if(params_.throwCheirality)
throw;
result_ = TriangulationResult::BehindCamera();
return TriangulationResult::BehindCamera();
}
}
result_ = TriangulationResult(pw_avg);
} // if retriangulate
return result_;
}
/// triangulate
bool triangulateForLinearize(const Cameras& cameras) const {
bool isDebug = false;
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug) {
std::cout << "createImplicitSchurFactor: degenerate configuration"
<< std::endl;
}
return false;
} else {
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
}
return true;
}
triangulateSafe(cameras); // imperative, might reset result_
return (result_);
}
/// linearize returns a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<Base::Dim> > createHessianFactor(
const Cameras& cameras, const double lambda = 0.0) const {
const Cameras& cameras, const double lambda = 0.0, bool diagonalDamping =
false) const {
bool isDebug = false;
size_t numKeys = this->keys_.size();
// Create structures for Hessian Factors
std::vector<Key> js;
@ -290,146 +330,142 @@ public:
if (this->measured_.size() != cameras.size()) {
std::cout
<< "SmartProjectionHessianFactor: this->measured_.size() inconsistent with input"
<< "SmartStereoProjectionHessianFactor: this->measured_.size() inconsistent with input"
<< std::endl;
exit(1);
}
triangulateSafe(cameras);
if (isDebug)
std::cout << "point_ = " << point_ << std::endl;
if (numKeys < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (isDebug)
std::cout << "In linearize: exception" << std::endl;
if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) {
// failed: return"empty" Hessian
BOOST_FOREACH(Matrix& m, Gs)
m = zeros(Base::Dim, Base::Dim);
BOOST_FOREACH(Vector& v, gs)
v = zero(Base::Dim);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs,
0.0);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
Gs, gs, 0.0);
}
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) { // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
if (isDebug)
std::cout << "degenerate_ = true" << std::endl;
}
if (this->linearizationThreshold_ >= 0) // if we apply selective relinearization and we need to relinearize
for (size_t i = 0; i < cameras.size(); i++)
this->cameraPosesLinearization_[i] = cameras[i].pose();
// ==================================================================
std::vector<typename Base::MatrixZD> Fblocks;
// Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
std::vector<Base::MatrixZD> Fblocks;
Matrix F, E;
Vector b;
computeJacobians(Fblocks, E, b, cameras);
Base::FillDiagonalF(Fblocks, F); // expensive !!!
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
// Schur complement trick
// Frank says: should be possible to do this more efficiently?
// And we care, as in grouped factors this is called repeatedly
Matrix H(Base::Dim * numKeys, Base::Dim * numKeys);
Vector gs_vector;
// Whiten using noise model
Base::whitenJacobians(Fblocks, E, b);
Matrix3 P = Cameras::PointCov(E, lambda);
H.noalias() = F.transpose() * (F - (E * (P * (E.transpose() * F))));
gs_vector.noalias() = F.transpose() * (b - (E * (P * (E.transpose() * b))));
// build augmented hessian
SymmetricBlockMatrix augmentedHessian = //
Cameras::SchurComplement(Fblocks, E, b, lambda, diagonalDamping);
if (isDebug)
std::cout << "gs_vector size " << gs_vector.size() << std::endl;
if (isDebug)
std::cout << "H:\n" << H << std::endl;
// Populate Gs and gs
int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < (DenseIndex) numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * Base::Dim;
gs.at(i1) = gs_vector.segment<Base::Dim>(i1D);
for (DenseIndex i2 = 0; i2 < (DenseIndex) numKeys; i2++) {
if (i2 >= i1) {
Gs.at(GsCount2) = H.block<Base::Dim, Base::Dim>(i1D, i2 * Base::Dim);
GsCount2++;
}
}
}
// ==================================================================
double f = b.squaredNorm();
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_, Gs, gs, f);
return boost::make_shared<RegularHessianFactor<Base::Dim> >(this->keys_,
augmentedHessian);
}
// // create factor
// boost::shared_ptr<ImplicitSchurFactor<Base::Dim> > createImplicitSchurFactor(
// create factor
// boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > createRegularImplicitSchurFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createImplicitSchurFactor(cameras, point_, lambda);
// return Base::createRegularImplicitSchurFactor(cameras, *result_, lambda);
// else
// return boost::shared_ptr<ImplicitSchurFactor<Base::Dim> >();
// // failed: return empty
// return boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> >();
// }
//
// /// create factor
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
// boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
// const Cameras& cameras, double lambda) const {
// if (triangulateForLinearize(cameras))
// return Base::createJacobianQFactor(cameras, point_, lambda);
// return Base::createJacobianQFactor(cameras, *result_, lambda);
// else
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
// // failed: return empty
// return boost::make_shared<JacobianFactorQ<Base::Dim, Base::ZDim> >(this->keys_);
// }
//
// /// Create a factor, takes values
// boost::shared_ptr<JacobianFactorQ<Base::Dim> > createJacobianQFactor(
// boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > createJacobianQFactor(
// const Values& values, double lambda) const {
// Cameras cameras;
// // TODO triangulate twice ??
// bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
// if (nonDegenerate)
// return createJacobianQFactor(cameras, lambda);
// else
// return boost::make_shared< JacobianFactorQ<Base::Dim> >(this->keys_);
// return createJacobianQFactor(this->cameras(values), lambda);
// }
//
/// different (faster) way to compute Jacobian factor
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
return Base::createJacobianSVDFactor(cameras, *result_, lambda);
else
return boost::make_shared<JacobianFactorSVD<Base::Dim, ZDim> >(this->keys_);
}
/// Returns true if nonDegenerate
bool computeCamerasAndTriangulate(const Values& values,
Cameras& cameras) const {
Values valuesFactor;
// /// linearize to a Hessianfactor
// virtual boost::shared_ptr<RegularHessianFactor<Base::Dim> > linearizeToHessian(
// const Values& values, double lambda = 0.0) const {
// return createHessianFactor(this->cameras(values), lambda);
// }
// Select only the cameras
BOOST_FOREACH(const Key key, this->keys_)
valuesFactor.insert(key, values.at(key));
// /// linearize to an Implicit Schur factor
// virtual boost::shared_ptr<RegularImplicitSchurFactor<StereoCamera> > linearizeToImplicit(
// const Values& values, double lambda = 0.0) const {
// return createRegularImplicitSchurFactor(this->cameras(values), lambda);
// }
//
// /// linearize to a JacobianfactorQ
// virtual boost::shared_ptr<JacobianFactorQ<Base::Dim, Base::ZDim> > linearizeToJacobian(
// const Values& values, double lambda = 0.0) const {
// return createJacobianQFactor(this->cameras(values), lambda);
// }
cameras = this->cameras(valuesFactor);
size_t nrCameras = this->triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_)))
return false;
// instead, if we want to manage the exception..
if (this->cheiralityException_ || this->degenerate_) // if we want to manage the exceptions with rotation-only factors
this->degenerate_ = true;
if (this->degenerate_) {
std::cout << "SmartStereoProjectionFactor: this is not ready"
<< std::endl;
std::cout << "this->cheiralityException_ " << this->cheiralityException_
<< std::endl;
std::cout << "this->degenerate_ " << this->degenerate_ << std::endl;
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(const Cameras& cameras,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
switch (params_.linearizationMode) {
case HESSIAN:
return createHessianFactor(cameras, lambda);
// case IMPLICIT_SCHUR:
// return createRegularImplicitSchurFactor(cameras, lambda);
case JACOBIAN_SVD:
return createJacobianSVDFactor(cameras, lambda);
// case JACOBIAN_Q:
// return createJacobianQFactor(cameras, lambda);
default:
throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
}
return true;
}
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return a Gaussian factor
*/
boost::shared_ptr<GaussianFactor> linearizeDamped(const Values& values,
const double lambda = 0.0) const {
// depending on flag set on construction we may linearize to different linear factors
Cameras cameras = this->cameras(values);
return linearizeDamped(cameras, lambda);
}
/// linearize
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
return linearizeDamped(values);
}
/**
* Triangulate and compute derivative of error with respect to point
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Cameras& cameras) const {
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
cameras.project2(*result_, boost::none, E);
return nonDegenerate;
}
/**
@ -437,87 +473,62 @@ public:
* @return whether triangulation worked
*/
bool triangulateAndComputeE(Matrix& E, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
cameras.project2(point_, boost::none, E);
return nonDegenerate;
Cameras cameras = this->cameras(values);
return triangulateAndComputeE(E, cameras);
}
/// Version that takes values, and creates the point
bool computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
if (nonDegenerate)
computeJacobians(Fblocks, E, b, cameras, 0.0);
return nonDegenerate;
}
/// Compute F, E only (called below in both vanilla and SVD versions)
/// Assumes the point has been computed
/// Note E can be 2m*3 or 2m*2, in case point is degenerate
void computeJacobians(std::vector<typename Base::MatrixZD>& Fblocks,
Matrix& E, Vector& b, const Cameras& cameras) const {
if (this->degenerate_) {
throw("FIXME: computeJacobians degenerate case commented out!");
// std::cout << "manage degeneracy " << manageDegeneracy_ << std::endl;
// std::cout << "point " << point_ << std::endl;
// std::cout
// << "SmartStereoProjectionFactor: Management of degeneracy is disabled - not ready to be used"
// << std::endl;
// if (D > 6) {
// std::cout
// << "Management of degeneracy is not yet ready when one also optimizes for the calibration "
// << std::endl;
// }
void computeJacobiansWithTriangulatedPoint(
std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Cameras& cameras) const {
if (!result_) {
throw ("computeJacobiansWithTriangulatedPoint");
// // Handle degeneracy
// // TODO check flag whether we should do this
// Unit3 backProjected; /* = cameras[0].backprojectPointAtInfinity(
// this->measured_.at(0)); */
//
// int numKeys = this->keys_.size();
// E = zeros(2 * numKeys, 2);
// b = zero(2 * numKeys);
// double f = 0;
// for (size_t i = 0; i < this->measured_.size(); i++) {
// if (i == 0) { // first pose
// this->point_ = cameras[i].backprojectPointAtInfinity(
// this->measured_.at(i));
// // 3D parametrization of point at infinity: [px py 1]
// }
// Matrix Fi, Ei;
// Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei)
// - this->measured_.at(i)).vector();
//
// this->noise_.at(i)->WhitenSystem(Fi, Ei, bi);
// f += bi.squaredNorm();
// Fblocks.push_back(typename Base::MatrixZD(this->keys_[i], Fi));
// E.block < 2, 2 > (2 * i, 0) = Ei;
// subInsert(b, bi, 2 * i);
// }
// return f;
// Base::computeJacobians(Fblocks, E, b, cameras, backProjected);
} else {
// nondegenerate: just return Base version
Base::computeJacobians(Fblocks, E, b, cameras, point_);
} // end else
// valid result: just return Base version
Base::computeJacobians(Fblocks, E, b, cameras, *result_);
}
}
/// Version that takes values, and creates the point
bool triangulateAndComputeJacobians(
std::vector<Base::MatrixZD>& Fblocks, Matrix& E, Vector& b,
const Values& values) const {
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras);
return nonDegenerate;
}
/// takes values
bool triangulateAndComputeJacobiansSVD(
std::vector<typename Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
std::vector<Base::MatrixZD>& Fblocks, Matrix& Enull, Vector& b,
const Values& values) const {
typename Base::Cameras cameras;
double good = computeCamerasAndTriangulate(values, cameras);
if (good)
return Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, point_);
return true;
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
Base::computeJacobiansSVD(Fblocks, Enull, b, cameras, *result_);
return nonDegenerate;
}
/// Calculate vector of re-projection errors, before applying noise model
Vector reprojectionErrorAfterTriangulation(const Values& values) const {
Cameras cameras;
bool nonDegenerate = computeCamerasAndTriangulate(values, cameras);
Cameras cameras = this->cameras(values);
bool nonDegenerate = triangulateForLinearize(cameras);
if (nonDegenerate)
return Base::unwhitenedError(cameras, point_);
return Base::unwhitenedError(cameras, *result_);
else
return zero(cameras.size() * 3);
return zero(cameras.size() * Base::ZDim);
}
/**
@ -529,87 +540,61 @@ public:
double totalReprojectionError(const Cameras& cameras,
boost::optional<Point3> externalPoint = boost::none) const {
size_t nrCameras;
if (externalPoint) {
nrCameras = this->keys_.size();
point_ = *externalPoint;
degenerate_ = false;
cheiralityException_ = false;
} else {
nrCameras = this->triangulateSafe(cameras);
}
if (externalPoint)
result_ = TriangulationResult(*externalPoint);
else
result_ = triangulateSafe(cameras);
if (nrCameras < 2
|| (!this->manageDegeneracy_
&& (this->cheiralityException_ || this->degenerate_))) {
if (result_)
// All good, just use version in base class
return Base::totalReprojectionError(cameras, *result_);
else if (params_.degeneracyMode == HANDLE_INFINITY) {
throw(std::runtime_error("Backproject at infinity not implemented for SmartStereo."));
// // Otherwise, manage the exceptions with rotation-only factors
// const StereoPoint2& z0 = this->measured_.at(0);
// Unit3 backprojected; //= cameras.front().backprojectPointAtInfinity(z0);
//
// return Base::totalReprojectionError(cameras, backprojected);
} else {
// if we don't want to manage the exceptions we discard the factor
// std::cout << "In error evaluation: exception" << std::endl;
return 0.0;
}
if (this->cheiralityException_) { // if we want to manage the exceptions with rotation-only factors
std::cout
<< "SmartProjectionHessianFactor: cheirality exception (this should not happen if CheiralityException is disabled)!"
<< std::endl;
this->degenerate_ = true;
}
if (this->degenerate_) {
return 0.0; // TODO: this maybe should be zero?
// std::cout
// << "SmartProjectionHessianFactor: trying to manage degeneracy (this should not happen is manageDegeneracy is disabled)!"
// << std::endl;
// size_t i = 0;
// double overallError = 0;
// BOOST_FOREACH(const Camera& camera, cameras) {
// const StereoPoint2& zi = this->measured_.at(i);
// if (i == 0) // first pose
// this->point_ = camera.backprojectPointAtInfinity(zi); // 3D parametrization of point at infinity
// StereoPoint2 reprojectionError(
// camera.projectPointAtInfinity(this->point_) - zi);
// overallError += 0.5
// * this->noise_.at(i)->distance(reprojectionError.vector());
// i += 1;
// }
// return overallError;
} else {
// Just use version in base class
return Base::totalReprojectionError(cameras, point_);
}
}
/// Cameras are computed in derived class
virtual Cameras cameras(const Values& values) const = 0;
/// Calculate total reprojection error
virtual double error(const Values& values) const {
if (this->active(values)) {
return totalReprojectionError(Base::cameras(values));
} else { // else of active flag
return 0.0;
}
}
/** return the landmark */
boost::optional<Point3> point() const {
return point_;
}
TriangulationResult point() const {
return result_;
}
/** COMPUTE the landmark */
boost::optional<Point3> point(const Values& values) const {
triangulateSafe(values);
return point_;
}
/** COMPUTE the landmark */
TriangulationResult point(const Values& values) const {
Cameras cameras = this->cameras(values);
return triangulateSafe(cameras);
}
/** return the degenerate state */
inline bool isDegenerate() const {
return (cheiralityException_ || degenerate_);
}
/// Is result valid?
bool isValid() const {
return result_;
}
/** return the cheirality status flag */
inline bool isPointBehindCamera() const {
return cheiralityException_;
}
/** return chirality verbosity */
inline bool verboseCheirality() const {
return verboseCheirality_;
}
/** return the degenerate state */
bool isDegenerate() const {
return result_.degenerate();
}
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const {
return throwCheirality_;
}
/** return the cheirality status flag */
bool isPointBehindCamera() const {
return result_.behindCamera();
}
private:
@ -618,15 +603,15 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
ar & BOOST_SERIALIZATION_NVP(params_.throwCheirality);
ar & BOOST_SERIALIZATION_NVP(params_.verboseCheirality);
}
};
/// traits
template<class CALIBRATION>
struct traits<SmartStereoProjectionFactor<CALIBRATION> > : public Testable<
SmartStereoProjectionFactor<CALIBRATION> > {
template<>
struct traits<SmartStereoProjectionFactor > : public Testable<
SmartStereoProjectionFactor> {
};
} // \ namespace gtsam

135
gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h
View File

@ -11,7 +11,7 @@
/**
* @file SmartStereoProjectionPoseFactor.h
* @brief Produces an Hessian factors on POSES from monocular measurements of a single landmark
* @brief Smart stereo factor on poses, assuming camera calibration is fixed
* @author Luca Carlone
* @author Chris Beall
* @author Zsolt Kira
@ -35,53 +35,40 @@ namespace gtsam {
*/
/**
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
* This factor assumes that camera calibration is fixed, but each camera
* has its own calibration.
* The factor only constrains poses (variable dimension is 6).
* This factor requires that values contains the involved poses (Pose3).
* @addtogroup SLAM
*/
template<class CALIBRATION>
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor<CALIBRATION> {
public:
/// Linearization mode: what factor to linearize to
enum LinearizationMode {
HESSIAN, IMPLICIT_SCHUR, JACOBIAN_Q, JACOBIAN_SVD
};
class SmartStereoProjectionPoseFactor: public SmartStereoProjectionFactor {
protected:
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
std::vector<boost::shared_ptr<Cal3_S2Stereo> > K_all_; ///< shared pointer to calibration object (one for each camera)
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// shorthand for base class type
typedef SmartStereoProjectionFactor<CALIBRATION> Base;
typedef SmartStereoProjectionFactor Base;
/// shorthand for this class
typedef SmartStereoProjectionPoseFactor<CALIBRATION> This;
typedef SmartStereoProjectionPoseFactor This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Constructor
* @param rankTol tolerance used to check if point triangulation is degenerate
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
* otherwise the factor is simply neglected
* @param enableEPI if set to true linear triangulation is refined with embedded LM iterations
* @param Isotropic measurement noise
* @param params internal parameters of the smart factors
*/
SmartStereoProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN,
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(
linearizeTo) {
SmartStereoProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
const SmartStereoProjectionParams& params =
SmartStereoProjectionParams()) :
Base(sharedNoiseModel, params) {
}
/** Virtual destructor */
@ -91,27 +78,23 @@ public:
* add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKey is key corresponding to the camera observing the same landmark
* @param noise_i is the measurement noise
* @param K_i is the (known) camera calibration
* @param K is the (fixed) camera calibration
*/
void add(const StereoPoint2 measured_i, const Key poseKey_i,
const SharedNoiseModel noise_i,
const boost::shared_ptr<CALIBRATION> K_i) {
Base::add(measured_i, poseKey_i, noise_i);
K_all_.push_back(K_i);
void add(const StereoPoint2 measured, const Key poseKey,
const boost::shared_ptr<Cal3_S2Stereo> K) {
Base::add(measured, poseKey);
K_all_.push_back(K);
}
/**
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noises vector of measurement noises
* @param Ks vector of calibration objects
*/
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
std::vector<boost::shared_ptr<CALIBRATION> > Ks) {
Base::add(measurements, poseKeys, noises);
std::vector<boost::shared_ptr<Cal3_S2Stereo> > Ks) {
Base::add(measurements, poseKeys);
for (size_t i = 0; i < measurements.size(); i++) {
K_all_.push_back(Ks.at(i));
}
@ -121,13 +104,12 @@ public:
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noise measurement noise (same for all measurements)
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<StereoPoint2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
const boost::shared_ptr<Cal3_S2Stereo> K) {
for (size_t i = 0; i < measurements.size(); i++) {
Base::add(measurements.at(i), poseKeys.at(i), noise);
Base::add(measurements.at(i), poseKeys.at(i));
K_all_.push_back(K);
}
}
@ -140,57 +122,19 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "SmartStereoProjectionPoseFactor, z = \n ";
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_)
BOOST_FOREACH(const boost::shared_ptr<Cal3_S2Stereo>& K, K_all_)
K->print("calibration = ");
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
const SmartStereoProjectionPoseFactor *e =
dynamic_cast<const SmartStereoProjectionPoseFactor*>(&p);
return e && Base::equals(p, tol);
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of Values
*/
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k);
typename Base::Camera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
return cameras;
}
/**
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return
*/
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
// depending on flag set on construction we may linearize to different linear factors
switch(linearizeTo_){
case JACOBIAN_SVD :
return this->createJacobianSVDFactor(cameras(values), 0.0);
break;
case JACOBIAN_Q :
throw("JacobianQ not working yet!");
// return this->createJacobianQFactor(cameras(values), 0.0);
break;
default:
return this->createHessianFactor(cameras(values));
break;
}
}
/**
* error calculates the error of the factor.
*/
@ -203,10 +147,27 @@ public:
}
/** return the calibration object */
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
inline const std::vector<boost::shared_ptr<Cal3_S2Stereo> > calibration() const {
return K_all_;
}
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of Values
*/
Base::Cameras cameras(const Values& values) const {
Base::Cameras cameras;
size_t i=0;
BOOST_FOREACH(const Key& k, this->keys_) {
const Pose3& pose = values.at<Pose3>(k);
StereoCamera camera(pose, K_all_[i++]);
cameras.push_back(camera);
}
return cameras;
}
private:
/// Serialization function
@ -220,9 +181,9 @@ private:
}; // end of class declaration
/// traits
template<class CALIBRATION>
struct traits<SmartStereoProjectionPoseFactor<CALIBRATION> > : public Testable<
SmartStereoProjectionPoseFactor<CALIBRATION> > {
template<>
struct traits<SmartStereoProjectionPoseFactor> : public Testable<
SmartStereoProjectionPoseFactor> {
};
} // \ namespace gtsam

292
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -40,11 +40,10 @@ static double b = 1;
static Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fov, w, h, b));
static Cal3_S2Stereo::shared_ptr K2(
new Cal3_S2Stereo(1500, 1200, 0, 640, 480, b));
static boost::shared_ptr<Cal3Bundler> Kbundler(
new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000));
static double rankTol = 1.0;
static double linThreshold = -1.0;
static SmartStereoProjectionParams params;
// static bool manageDegeneracy = true;
// Create a noise model for the pixel error
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
@ -63,8 +62,6 @@ static StereoPoint2 measurement1(323.0, 300.0, 240.0); //potentially use more re
static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
Point3(0.25, -0.10, 1.0));
typedef SmartStereoProjectionPoseFactor<Cal3_S2Stereo> SmartFactor;
vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
const StereoCamera& cam2, const StereoCamera& cam3, Point3 landmark) {
@ -80,37 +77,37 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
return measurements_cam;
}
LevenbergMarquardtParams params;
LevenbergMarquardtParams lm_params;
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) {
SmartFactor::shared_ptr factor1(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor2) {
SmartFactor factor1(rankTol, linThreshold);
SmartStereoProjectionPoseFactor factor1(model, params);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor3) {
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, poseKey1, model, K);
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(measurement1, poseKey1, K);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor4) {
SmartFactor factor1(rankTol, linThreshold);
factor1.add(measurement1, poseKey1, model, K);
SmartStereoProjectionPoseFactor factor1(model, params);
factor1.add(measurement1, poseKey1, K);
}
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Equals ) {
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(measurement1, poseKey1, model, K);
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(measurement1, poseKey1, K);
SmartFactor::shared_ptr factor2(new SmartFactor());
factor2->add(measurement1, poseKey1, model, K);
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
factor2->add(measurement1, poseKey1, K);
CHECK(assert_equal(*factor1, *factor2));
}
@ -137,15 +134,15 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
values.insert(x1, level_pose);
values.insert(x2, level_pose_right);
SmartFactor factor1;
factor1.add(level_uv, x1, model, K2);
factor1.add(level_uv_right, x2, model, K2);
SmartStereoProjectionPoseFactor factor1(model);
factor1.add(level_uv, x1, K2);
factor1.add(level_uv_right, x2, K2);
double actualError = factor1.error(values);
double expectedError = 0.0;
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
SmartFactor::Cameras cameras = factor1.cameras(values);
SmartStereoProjectionPoseFactor::Cameras cameras = factor1.cameras(values);
double actualError2 = factor1.totalReprojectionError(cameras);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
@ -179,21 +176,17 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
Point3(0.5, 0.1, 0.3));
values.insert(x2, level_pose_right.compose(noise_pose));
SmartFactor::shared_ptr factor1(new SmartFactor());
factor1->add(level_uv, x1, model, K);
factor1->add(level_uv_right, x2, model, K);
SmartStereoProjectionPoseFactor::shared_ptr factor1(new SmartStereoProjectionPoseFactor(model));
factor1->add(level_uv, x1, K);
factor1->add(level_uv_right, x2, K);
double actualError1 = factor1->error(values);
SmartFactor::shared_ptr factor2(new SmartFactor());
SmartStereoProjectionPoseFactor::shared_ptr factor2(new SmartStereoProjectionPoseFactor(model));
vector<StereoPoint2> measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
vector<SharedNoiseModel> noises;
noises.push_back(model);
noises.push_back(model);
vector<boost::shared_ptr<Cal3_S2Stereo> > Ks; ///< shared pointer to calibration object (one for each camera)
Ks.push_back(K);
Ks.push_back(K);
@ -202,7 +195,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
views.push_back(x1);
views.push_back(x2);
factor2->add(measurements, views, noises, Ks);
factor2->add(measurements, views, Ks);
double actualError2 = factor2->error(values);
@ -211,6 +204,7 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2);
@ -229,11 +223,11 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
Point3 landmark3(3, 0, 3.0);
// 1. Project three landmarks into three cameras and triangulate
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark2);
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
vector<Key> views;
@ -241,17 +235,21 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
views.push_back(x2);
views.push_back(x3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor());
smartFactor1->add(measurements_cam1, views, model, K2);
SmartStereoProjectionParams smart_params;
smart_params.triangulation.enableEPI = true;
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor1->add(measurements_l1, views, K2);
SmartFactor::shared_ptr smartFactor2(new SmartFactor());
smartFactor2->add(measurements_cam2, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor2->add(measurements_l2, views, K2);
SmartFactor::shared_ptr smartFactor3(new SmartFactor());
smartFactor3->add(measurements_cam3, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, smart_params));
smartFactor3->add(measurements_l3, views, K2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
@ -274,16 +272,24 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
-0.000986635786, 0.0314107591, -0.999013364, -0.0313952598),
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1);
// cout << std::setprecision(10) << "\n----SmartStereoFactor graph initial error: " << graph.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph.error(values), 1e-7);
// get triangulated landmarks from smart factors
Point3 landmark1_smart = *smartFactor1->point();
Point3 landmark2_smart = *smartFactor2->point();
Point3 landmark3_smart = *smartFactor3->point();
Values result;
gttic_(SmartStereoProjectionPoseFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
gttoc_(SmartStereoProjectionPoseFactor);
tictoc_finishedIteration_();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-6);
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
// cout << std::setprecision(10) << "SmartStereoFactor graph optimized error: " << graph.error(result) << endl;
GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
VectorValues delta = GFG->optimize();
@ -292,6 +298,51 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// result.print("results of 3 camera, 3 landmark optimization \n");
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
/* ***************************************************************
* Same problem with regular Stereo factors, expect same error!
* ****************************************************************/
// landmark1_smart.print("landmark1_smart");
// landmark2_smart.print("landmark2_smart");
// landmark3_smart.print("landmark3_smart");
// add landmarks to values
values.insert(L(1), landmark1_smart);
values.insert(L(2), landmark2_smart);
values.insert(L(3), landmark3_smart);
// add factors
NonlinearFactorGraph graph2;
graph2.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph2.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
bool verboseCheirality = true;
graph2.push_back(ProjectionFactor(measurements_l1[0], model, x1, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l1[1], model, x2, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l1[2], model, x3, L(1), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[0], model, x1, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[1], model, x2, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l2[2], model, x3, L(2), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l3[0], model, x1, L(3), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l3[1], model, x2, L(3), K2, false, verboseCheirality));
graph2.push_back(ProjectionFactor(measurements_l3[2], model, x3, L(3), K2, false, verboseCheirality));
// cout << std::setprecision(10) << "\n----StereoFactor graph initial error: " << graph2.error(values) << endl;
EXPECT_DOUBLES_EQUAL(797312.95069157204, graph2.error(values), 1e-7);
LevenbergMarquardtOptimizer optimizer2(graph2, values, lm_params);
Values result2 = optimizer2.optimize();
EXPECT_DOUBLES_EQUAL(0, graph2.error(result2), 1e-5);
// cout << std::setprecision(10) << "StereoFactor graph optimized error: " << graph2.error(result2) << endl;
}
/* *************************************************************************/
@ -325,17 +376,17 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model, K);
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1( new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -355,7 +406,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
values.insert(x3, pose3 * noise_pose);
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
}
@ -363,7 +414,7 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
double excludeLandmarksFutherThanDist = 2;
// double excludeLandmarksFutherThanDist = 2;
vector<Key> views;
views.push_back(x1);
@ -393,20 +444,18 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, K);
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
params.setLandmarkDistanceThreshold(2);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -427,7 +476,7 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
// All factors are disabled and pose should remain where it is
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
}
@ -435,9 +484,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
@ -471,25 +517,26 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier
SmartFactor::shared_ptr smartFactor1(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, K);
SmartStereoProjectionParams params;
params.setLinearizationMode(JACOBIAN_SVD);
params.setDynamicOutlierRejectionThreshold(1);
SmartFactor::shared_ptr smartFactor2(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartFactor::shared_ptr smartFactor4(
new SmartFactor(1, -1, false, false, SmartFactor::JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4(new SmartStereoProjectionPoseFactor(model, params));
smartFactor4->add(measurements_cam4, views, K);
// same as factor 4, but dynamic outlier rejection is off
SmartStereoProjectionPoseFactor::shared_ptr smartFactor4b(new SmartStereoProjectionPoseFactor(model));
smartFactor4b->add(measurements_cam4, views, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -508,9 +555,25 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
values.insert(x2, pose2);
values.insert(x3, pose3);
// All factors are disabled and pose should remain where it is
EXPECT_DOUBLES_EQUAL(0, smartFactor1->error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(0, smartFactor2->error(values), 1e-9);
EXPECT_DOUBLES_EQUAL(0, smartFactor3->error(values), 1e-9);
// zero error due to dynamic outlier rejection
EXPECT_DOUBLES_EQUAL(0, smartFactor4->error(values), 1e-9);
// dynamic outlier rejection is off
EXPECT_DOUBLES_EQUAL(6700, smartFactor4b->error(values), 1e-9);
// Factors 1-3 should have valid point, factor 4 should not
EXPECT(smartFactor1->point());
EXPECT(smartFactor2->point());
EXPECT(smartFactor3->point());
EXPECT(smartFactor4->point().degenerate());
EXPECT(smartFactor4b->point());
// Factor 4 is disabled, pose 3 stays put
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
}
@ -545,13 +608,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor1->add(measurements_cam1, views, model, K);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor2->add(measurements_cam2, views, model, K);
//
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(1, -1, false, false, JACOBIAN_Q));
// smartFactor3->add(measurements_cam3, views, model, K);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -571,7 +634,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(x3, pose3*noise_pose);
//
//// Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize();
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
//}
@ -630,7 +693,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(L(2), landmark2);
// values.insert(L(3), landmark3);
//
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// Values result = optimizer.optimize();
//
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
@ -669,17 +732,17 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark3);
// Create smartfactors
double rankTol = 10;
SmartStereoProjectionParams params;
params.setRankTolerance(10);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
smartFactor1->add(measurements_cam1, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(model, params));
smartFactor1->add(measurements_cam1, views, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
smartFactor2->add(measurements_cam2, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(model, params));
smartFactor2->add(measurements_cam2, views, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
smartFactor3->add(measurements_cam3, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(model, params));
smartFactor3->add(measurements_cam3, views, K);
// Create graph to optimize
NonlinearFactorGraph graph;
@ -755,10 +818,10 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
//
// double rankTol = 50;
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor1->add(measurements_cam1, views, model, K2);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor2->add(measurements_cam2, views, model, K2);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -781,7 +844,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
// Values result;
// gttic_(SmartStereoProjectionPoseFactor);
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize();
// gttoc_(SmartStereoProjectionPoseFactor);
// tictoc_finishedIteration_();
@ -824,13 +887,13 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
// double rankTol = 10;
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor1->add(measurements_cam1, views, model, K);
//
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor2(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor2->add(measurements_cam2, views, model, K);
//
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol, linThreshold, manageDegeneracy));
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor3(new SmartStereoProjectionPoseFactor(rankTol, linThreshold, manageDegeneracy));
// smartFactor3->add(measurements_cam3, views, model, K);
//
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -855,7 +918,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
// Values result;
// gttic_(SmartStereoProjectionPoseFactor);
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
// result = optimizer.optimize();
// gttoc_(SmartStereoProjectionPoseFactor);
// tictoc_finishedIteration_();
@ -889,7 +952,7 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// measurements_cam1.push_back(cam1_uv1);
// measurements_cam1.push_back(cam2_uv1);
//
// SmartFactor::shared_ptr smartFactor1(new SmartFactor());
// SmartStereoProjectionPoseFactor::shared_ptr smartFactor1(new SmartStereoProjectionPoseFactor());
// smartFactor1->add(measurements_cam1,views, model, K2);
//
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3));
@ -930,8 +993,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
smartFactorInstance->add(measurements_cam1, views, model, K);
SmartStereoProjectionPoseFactor::shared_ptr smartFactorInstance(new SmartStereoProjectionPoseFactor(model));
smartFactorInstance->add(measurements_cam1, views, K);
Values values;
values.insert(x1, pose1);
@ -977,6 +1040,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
@ -997,8 +1061,8 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1,
cam2, cam3, landmark1);
SmartFactor::shared_ptr smartFactor(new SmartFactor());
smartFactor->add(measurements_cam1, views, model, K2);
SmartStereoProjectionPoseFactor::shared_ptr smartFactor(new SmartStereoProjectionPoseFactor(model));
smartFactor->add(measurements_cam1, views, K2);
Values values;
values.insert(x1, pose1);
@ -1021,7 +1085,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
// Hessian is invariant to rotations in the nondegenerate case
EXPECT(
assert_equal(hessianFactor->information(),
hessianFactorRot->information(), 1e-8));
hessianFactorRot->information(), 1e-6));
Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI / 2, -M_PI / 3, -M_PI / 2),
Point3(10, -4, 5));
@ -1037,7 +1101,7 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
// Hessian is invariant to rotations and translations in the nondegenerate case
EXPECT(
assert_equal(hessianFactor->information(),
hessianFactorRotTran->information(), 1e-8));
hessianFactorRotTran->information(), 1e-6));
}
/* ************************************************************************* */

4
tests/testLie.cpp
View File

@ -54,9 +54,9 @@ TEST(Lie, ProductLieGroup) {
Vector5 d;
d << 1, 2, 0.1, 0.2, 0.3;
Product expected(Point2(1, 2), Pose2::Expmap(Vector3(0.1, 0.2, 0.3)));
Product pair2 = pair1.retract(d);
Product pair2 = pair1.expmap(d);
EXPECT(assert_equal(expected, pair2, 1e-9));
EXPECT(assert_equal(d, pair1.localCoordinates(pair2), 1e-9));
EXPECT(assert_equal(d, pair1.logmap(pair2), 1e-9));
}
/* ************************************************************************* */