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

Merge remote-tracking branch 'origin/feature/ImuFactorPush2' into manifold

release/4.3a0
dellaert 2016-01-17 21:37:42 -08:00
parent c81f91999d fc1507acb7
commit 9d93ef7d13
20 changed files with 361 additions and 454 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

331
doc/ImuFactor.lyx
View File

@ -1196,7 +1196,7 @@ Even when we assume uncorrelated noise on
\begin_inset Formula $\theta_{k}$
\end_inset
and
and
\begin_inset Formula $v_{k}$
\end_inset
@ -1213,7 +1213,7 @@ reference "eq:euler_theta"
\end_inset
-
\begin_inset CommandInset ref
LatexCommand ref
reference "eq:euler_v"
@ -1227,7 +1227,7 @@ reference "eq:euler_v"
\begin_inset Formula
\[
\zeta_{k+1}=f\left(\zeta_{k},\omega_{k}^{b},a_{k}^{b}\right)
\zeta_{k+1}=f\left(\zeta_{k},a_{k}^{b},\omega_{k}^{b}\right)
\]
\end_inset
@ -1283,6 +1283,15 @@ where
.
\end_layout
\begin_layout Standard
We start with the noise propagation on
\begin_inset Formula $\theta$
\end_inset
, which is independent of the other quantities.
Taking the derivative, we have
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
@ -1314,7 +1323,7 @@ For the derivatives of
we need the derivative
\begin_inset Formula
\[
\deriv{R_{k}a_{k}^{b}}{\theta_{k}}=-R_{k}\Skew{a_{k}^{b}}\deriv{R_{k}}{\theta_{k}}=-R_{k}\Skew{a_{k}^{b}}H(\theta_{k})
\deriv{R_{k}a_{k}^{b}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}\deriv{R_{k}}{\theta_{k}}=R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})
\]
\end_inset
@ -1322,7 +1331,7 @@ For the derivatives of
where we used
\begin_inset Formula
\[
\deriv{\left(Ra\right)}R\approx-R\Skew a
\deriv{\left(Ra\right)}R\approx R\Skew{-a}
\]
\end_inset
@ -1349,8 +1358,8 @@ Putting all this together, we finally obtain
\[
A_{k}\approx\left[\begin{array}{ccc}
I_{3\times3}-\frac{\Delta_{t}}{2}\Skew{\omega_{k}^{b}}\\
-R_{k}\Skew{a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
-R_{k}\Skew{a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3}
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\frac{\Delta_{t}}{2}^{2} & I_{3\times3} & I_{3\times3}\Delta_{t}\\
R_{k}\Skew{-a_{k}^{b}}H(\theta_{k})\Delta_{t} & & I_{3\times3}
\end{array}\right]
\]
@ -1373,314 +1382,6 @@ H(\theta_{k})^{-1}\Delta_{t}\\
\end_inset
\end_layout
\begin_layout Standard
A more accurate partial derivative of
\begin_inset Formula $H(\theta_{k})^{-1}$
\end_inset
can be used, as well.
\end_layout
\begin_layout Section
Old Stuff:
\end_layout
\begin_layout Standard
We only measure
\begin_inset Formula $\omega$
\end_inset
and
\begin_inset Formula $a$
\end_inset
at discrete instants of time, and hence we need to make choices about how
to integrate the equations above numerically.
For a vehicle such as a quadrotor UAV, it is not a bad assumption to model
\begin_inset Formula $\omega$
\end_inset
and
\begin_inset Formula $a$
\end_inset
as piecewise constant in the body frame, as the actuation is fixed to the
body.
\end_layout
\begin_layout Standard
\begin_inset Note Note
status collapsed
\begin_layout Plain Layout
The group operation inherited from
\begin_inset Formula $GL(7)$
\end_inset
yields the following result,
\begin_inset Formula
\[
\left[\begin{array}{ccc}
R_{1} & & p_{1}\\
& R_{1} & v_{1}\\
& & 1
\end{array}\right]\left[\begin{array}{ccc}
R_{2} & & p_{2}\\
& R_{2} & v_{2}\\
& & 1
\end{array}\right]=\left[\begin{array}{ccc}
R_{1}R_{2} & & p_{1}+R_{1}p_{2}\\
& R_{1}R_{2} & v_{1}+R_{1}v_{2}\\
& & 1
\end{array}\right]
\]
\end_inset
i.e.,
\begin_inset Formula $R_{2}$
\end_inset
,
\begin_inset Formula $p_{2}$
\end_inset
, and
\begin_inset Formula $v_{2}$
\end_inset
are to interpreted as quantities in the body frame.
How can we achieve a constant angular velocity/constant acceleration operation
with this representation? For an infinitesimal interval
\begin_inset Formula $\delta$
\end_inset
, we expect the result to be
\begin_inset Formula
\[
\left[\begin{array}{ccc}
R+R\hat{\omega}\delta & & p+v\delta\\
& R+R\hat{\omega}\delta & v+Ra\delta\\
& & 1
\end{array}\right]
\]
\end_inset
This can NOT be achieved by
\begin_inset Formula
\[
\left[\begin{array}{ccc}
R & & p\\
& R & v\\
& & 1
\end{array}\right]\left[\begin{array}{ccc}
I+\hat{\omega}\delta & \delta & 0\\
& I+\hat{\omega}\delta & a\delta\\
& & 1
\end{array}\right]
\]
\end_inset
because it is not closed.
Hence, the exponential map as defined below does not really do it for us
\begin_inset Formula
\[
\left[\begin{array}{ccc}
R & & p\\
& R & v\\
& & 1
\end{array}\right]=\lim_{n\rightarrow\infty}\left(I+\left[\begin{array}{ccc}
\hat{\omega} & & v^{b}\\
& \hat{\omega} & a\\
& & 1
\end{array}\right]\frac{\Delta t}{n}\right)^{n}=\left[\begin{array}{ccc}
R+R\hat{\omega}\delta & & p+v\delta\\
& R+R\hat{\omega}\delta & v+Ra\delta\\
& & 1
\end{array}\right]
\]
\end_inset
\end_layout
\end_inset
\end_layout
\begin_layout Standard
For a quadrotor, forces induced by wind effects and drag are arguably better
approximated over short intervals as constant in the navigation frame.
\end_layout
\begin_layout Standard
Let us examine what we obtain using a constant angular velocity in the body,
but with a zero-order hold on acceleration in the navigation frame instead.
While
\begin_inset Formula $a$
\end_inset
is still measured in the body frame, we rotate it once by
\begin_inset Formula $R_{0}$
\end_inset
at
\begin_inset Formula $t=0$
\end_inset
, and we obtain a much simplified picture:
\begin_inset Formula
\begin{eqnarray*}
R(t) & = & R_{0}\exp\hat{\omega}t\\
v(t) & = & v_{0}+\left(g+R_{0}a\right)t
\end{eqnarray*}
\end_inset
Plugging this into position now yields a much simpler equation, as well:
\begin_inset Formula
\begin{eqnarray*}
p(t) & = & p_{0}+v_{0}t+\left(g+R_{0}a\right)\frac{t^{2}}{2}
\end{eqnarray*}
\end_inset
\end_layout
\begin_layout Standard
The goal of the IMU factor is to integrate IMU measurements between two
successive frames and create a binary factor between two NavStates.
Integrating successive gyro and accelerometer readings using the above
equations over each constant interval yields
\begin_inset Formula
\begin{eqnarray}
R_{k+1} & = & R_{k}\exp\hat{\omega}_{k}\Delta t_{k}\label{eq:iter_Rk}\\
p_{k+1} & = & p_{k}+v_{k}\Delta t_{k}+\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}=p_{k}+\frac{v_{k}+v_{k+1}}{2}\Delta t_{k}\nonumber \\
v_{k+1} & = & v_{k}+(g+R_{k}a_{k})\Delta t_{k}\nonumber
\end{eqnarray}
\end_inset
Starting
\begin_inset Formula $X_{i}=(R_{i},p_{i},v_{i})$
\end_inset
, we then obtain an expression for
\begin_inset Formula $X_{j}$
\end_inset
,
\begin_inset Formula
\begin{eqnarray*}
R_{j} & = & R_{i}\prod_{k}\exp\hat{\omega}_{k}\Delta t_{k}\\
p_{j} & = & p_{i}+\sum_{k}v_{k}\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}\\
v_{j} & = & v_{i}+\sum_{k}(g+R_{k}a_{k})\Delta t_{k}
\end{eqnarray*}
\end_inset
where
\begin_inset Formula $R_{k}$
\end_inset
has to be updated as in
\begin_inset CommandInset ref
LatexCommand formatted
reference "eq:iter_Rk"
\end_inset
.
Note that
\begin_inset Formula
\[
v_{k}=v_{i}+\sum_{l}(g+R_{l}a_{l})\Delta t_{l}
\]
\end_inset
and hence
\begin_inset Formula
\[
p_{j}=p_{i}+\sum_{k}\left(v_{i}+\sum_{l}(g+R_{l}a_{l})\Delta t_{l}\right)\Delta t_{k}+\sum_{k}\left(g+R_{k}a_{k}\right)\frac{\left(\Delta t_{k}\right)^{2}}{2}
\]
\end_inset
\end_layout
\begin_layout Standard
[Is there not a 3/2 power here as in the RSS paper?]
\end_layout
\begin_layout Standard
A crucial problem here is that we depend on a good estimate of
\begin_inset Formula $R_{k}$
\end_inset
at all times, which includes the potentially wrong estimate for the initial
attitude
\begin_inset Formula $R_{i}$
\end_inset
.
\end_layout
\begin_layout Standard
The idea behind the preintegrated IMU factor is two-fold: (a) treat gravity
separately, in the navigation frame, and (b) instead of integrating in
absolute coordinates, we want the IMU integration to happen in the frame
\begin_inset Formula $(R_{i},p_{i},v_{i})$
\end_inset
.
The first idea is easily incorporated by separating out all gravity-related
components:
\begin_inset Formula
\begin{eqnarray*}
p_{j} & = & p_{i}+\sum_{k}\left(\sum_{l}g\Delta t_{l}\right)\Delta t_{k}+\sum_{k}\left(v_{i}+\sum_{l}R_{l}a_{l}\Delta t_{l}\right)\Delta t_{k}+\sum_{k}g\frac{\left(\Delta t_{k}\right)^{2}}{2}+\sum_{k}R_{k}a_{k}\frac{\left(\Delta t_{k}\right)^{2}}{2}\\
v_{j} & = & v_{i}+g\sum_{k}\Delta t_{k}+\sum_{k}R_{k}a_{k}\Delta t_{k}
\end{eqnarray*}
\end_inset
\end_layout
\begin_layout Standard
The binary factor is set up as a prediction:
\begin_inset Formula
\[
X_{j}\approx X_{i}\oplus dX_{ij}
\]
\end_inset
where
\begin_inset Formula $dX_{ij}$
\end_inset
is a tangent vector in the tangent space
\begin_inset Formula $T_{i}$
\end_inset
to the manifold at
\begin_inset Formula $X_{i}$
\end_inset
.
\end_layout
\begin_layout Standard

9
gtsam/base/Matrix.cpp
View File

@ -580,15 +580,6 @@ Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask) {
return M;
}
/* ************************************************************************* */
Matrix3 skewSymmetric(double wx, double wy, double wz)
{
return (Matrix3() <<
0.0, -wz, +wy,
+wz, 0.0, -wx,
-wy, +wx, 0.0).finished();
}
/* ************************************************************************* */
Matrix LLt(const Matrix& A)
{

12
gtsam/base/Matrix.h
View File

@ -477,9 +477,15 @@ GTSAM_EXPORT Matrix vector_scale(const Matrix& A, const Vector& v, bool inf_mask
* @param wz
* @return a 3*3 skew symmetric matrix
*/
GTSAM_EXPORT Matrix3 skewSymmetric(double wx, double wy, double wz);
template<class Derived>
inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) { return skewSymmetric(w(0),w(1),w(2));}
inline Matrix3 skewSymmetric(double wx, double wy, double wz) {
return (Matrix3() << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0).finished();
}
template <class Derived>
inline Matrix3 skewSymmetric(const Eigen::MatrixBase<Derived>& w) {
return skewSymmetric(w(0), w(1), w(2));
}
/** Use Cholesky to calculate inverse square root of a matrix */
GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A);

10
gtsam/geometry/Rot3Q.cpp
View File

@ -27,14 +27,12 @@ using namespace std;
namespace gtsam {
static const Matrix I3 = eye(3);
/* ************************************************************************* */
Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}
/* ************************************************************************* */
Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) :
quaternion_((Eigen::Matrix3d() <<
quaternion_((Matrix3() <<
col1.x(), col2.x(), col3.x(),
col1.y(), col2.y(), col3.y(),
col1.z(), col2.z(), col3.z()).finished()) {}
@ -43,7 +41,7 @@ namespace gtsam {
Rot3::Rot3(double R11, double R12, double R13,
double R21, double R22, double R23,
double R31, double R32, double R33) :
quaternion_((Eigen::Matrix3d() <<
quaternion_((Matrix3() <<
R11, R12, R13,
R21, R22, R23,
R31, R32, R33).finished()) {}
@ -91,10 +89,10 @@ namespace gtsam {
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
Matrix R = matrix();
const Matrix3 R = matrix();
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R;
Eigen::Vector3d r = R * p.vector();
const Vector3 r = R * p.vector();
return Point3(r.x(), r.y(), r.z());
}

15
gtsam/geometry/SO3.cpp
View File

@ -132,9 +132,9 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
using std::sin;
double theta2 = omega.dot(omega);
const double theta2 = omega.dot(omega);
if (theta2 <= std::numeric_limits<double>::epsilon()) return I_3x3;
double theta = std::sqrt(theta2); // rotation angle
const double theta = std::sqrt(theta2); // rotation angle
#ifdef DUY_VERSION
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 X = skewSymmetric(omega);
@ -153,10 +153,15 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
* a perturbation on the manifold (expmap(Jr * omega))
*/
// element of Lie algebra so(3): X = omega^, normalized by normx
const Matrix3 Y = skewSymmetric(omega) / theta;
const double wx = omega.x(), wy = omega.y(), wz = omega.z();
Matrix3 Y;
Y << 0.0, -wz, +wy,
+wz, 0.0, -wx,
-wy, +wx, 0.0;
const double s2 = sin(theta / 2.0);
return I_3x3 - (2.0 * s2 * s2 / theta) * Y +
(1.0 - sin(theta) / theta) * Y * Y; // right Jacobian
const double a = (2.0 * s2 * s2 / theta2);
const double b = (1.0 - sin(theta) / theta) / theta2;
return I_3x3 - a * Y + b * Y * Y;
#endif
}

9
gtsam/geometry/triangulation.h
View File

@ -376,6 +376,15 @@ class TriangulationResult: public boost::optional<Point3> {
status_(s) {
}
public:
/**
* Default constructor, only for serialization
*/
TriangulationResult() {}
/**
* Constructor
*/
TriangulationResult(const Point3& p) :
status_(VALID) {
reset(p);

1
gtsam/navigation/ImuBias.h
View File

@ -201,7 +201,6 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("imuBias::ConstantBias", *this);
ar & BOOST_SERIALIZATION_NVP(biasAcc_);
ar & BOOST_SERIALIZATION_NVP(biasGyro_);
}

20
gtsam/navigation/ImuFactor.cpp
View File

@ -73,8 +73,8 @@ void PreintegratedImuMeasurements::integrateMeasurement(
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;
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()
@ -91,7 +91,7 @@ PreintegratedImuMeasurements::PreintegratedImuMeasurements(
const Matrix3& measuredOmegaCovariance,
const Matrix3& integrationErrorCovariance, bool use2ndOrderIntegration) {
if (!use2ndOrderIntegration)
throw("PreintegratedImuMeasurements no longer supports first-order integration: it incorrectly compensated for gravity");
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;
@ -141,8 +141,9 @@ void ImuFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
//------------------------------------------------------------------------------
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)
&& Base::equals(*e, tol);
const bool base = Base::equals(*e, tol);
const bool pim = _PIM_.equals(e->_PIM_, tol);
return e != nullptr && base && pim;
}
//------------------------------------------------------------------------------
@ -161,10 +162,10 @@ ImuFactor::ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedImuMeasurements& 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) {
Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
pose_j, vel_j, bias), _PIM_(pim) {
boost::shared_ptr<PreintegratedImuMeasurements::Params> p = boost::make_shared<
PreintegratedImuMeasurements::Params>(pim.p());
PreintegratedImuMeasurements::Params>(pim.p());
p->n_gravity = n_gravity;
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
@ -185,4 +186,5 @@ void ImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
#endif
//------------------------------------------------------------------------------
} // namespace gtsam
}
// namespace gtsam

7
gtsam/navigation/ImuFactor.h
View File

@ -71,7 +71,9 @@ protected:
///< (first-order propagation from *measurementCovariance*).
/// Default constructor for serialization
PreintegratedImuMeasurements() {}
PreintegratedImuMeasurements() {
preintMeasCov_.setZero();
}
public:
@ -140,8 +142,9 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegrationBase);
ar & BOOST_SERIALIZATION_NVP(preintMeasCov_);
ar & bs::make_nvp("preintMeasCov_", bs::make_array(preintMeasCov_.data(), preintMeasCov_.size()));
}
};

34
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -29,12 +29,26 @@ 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;
cout << "omegaCoriolis = (" << omegaCoriolis->transpose() << ")" << endl;
if (body_P_sensor)
body_P_sensor->print("body_P_sensor");
}
bool PreintegratedRotation::Params::equals(
const PreintegratedRotation::Params& other, double tol) const {
if (body_P_sensor) {
if (!other.body_P_sensor
|| !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))
return false;
}
if (omegaCoriolis) {
if (!other.omegaCoriolis
|| !equal_with_abs_tol(*omegaCoriolis, *other.omegaCoriolis, tol))
return false;
}
return equal_with_abs_tol(gyroscopeCovariance, other.gyroscopeCovariance, tol);
}
void PreintegratedRotation::resetIntegration() {
deltaTij_ = 0.0;
deltaRij_ = Rot3();
@ -49,8 +63,7 @@ void PreintegratedRotation::print(const string& s) const {
bool PreintegratedRotation::equals(const PreintegratedRotation& other,
double tol) const {
return this->matchesParamsWith(other)
&& deltaRij_.equals(other.deltaRij_, tol)
return p_->equals(*other.p_,tol) && deltaRij_.equals(other.deltaRij_, tol)
&& fabs(deltaTij_ - other.deltaTij_) < tol
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol);
}
@ -76,11 +89,13 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega,
return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
}
void PreintegratedRotation::integrateMeasurement(
const Vector3& measuredOmega, const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) {
void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega,
const Vector3& biasHat, double deltaT,
OptionalJacobian<3, 3> optional_D_incrR_integratedOmega,
OptionalJacobian<3, 3> F) {
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, D_incrR_integratedOmega);
const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT,
D_incrR_integratedOmega);
// If asked, pass first derivative as well
if (optional_D_incrR_integratedOmega) {
@ -93,7 +108,8 @@ void PreintegratedRotation::integrateMeasurement(
// Update Jacobian
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT;
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- D_incrR_integratedOmega * deltaT;
}
Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr,

12
gtsam/navigation/PreintegratedRotation.h
View File

@ -43,15 +43,17 @@ class PreintegratedRotation {
Params() : gyroscopeCovariance(I_3x3) {}
virtual void print(const std::string& s) const;
virtual bool equals(const Params& other, double tol=1e-9) 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);
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & bs::make_nvp("gyroscopeCovariance", bs::make_array(gyroscopeCovariance.data(), gyroscopeCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(omegaCoriolis);
ar & BOOST_SERIALIZATION_NVP(body_P_sensor);
}
};

99
gtsam/navigation/PreintegrationBase.cpp
View File

@ -31,7 +31,7 @@ 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]"
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
<< endl;
if (omegaCoriolis && use2ndOrderCoriolis)
cout << "Using 2nd-order Coriolis" << endl;
@ -40,6 +40,18 @@ void PreintegrationBase::Params::print(const string& s) const {
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
}
//------------------------------------------------------------------------------
bool PreintegrationBase::Params::equals(
const PreintegratedRotation::Params& other, double tol) const {
auto e = dynamic_cast<const PreintegrationBase::Params*>(&other);
return e != nullptr && PreintegratedRotation::Params::equals(other, tol)
&& use2ndOrderCoriolis == e->use2ndOrderCoriolis
&& equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
tol)
&& equal_with_abs_tol(integrationCovariance, e->integrationCovariance,
tol) && equal_with_abs_tol(n_gravity, e->n_gravity, tol);
}
//------------------------------------------------------------------------------
void PreintegrationBase::resetIntegration() {
deltaTij_ = 0.0;
@ -64,8 +76,8 @@ void PreintegrationBase::print(const string& s) const {
//------------------------------------------------------------------------------
bool PreintegrationBase::equals(const PreintegrationBase& other,
double tol) const {
return this->matchesParamsWith(other)
&& fabs(deltaTij_ - other.deltaTij_) < tol
const bool params_match = p_->equals(*other.p_, tol);
return params_match && fabs(deltaTij_ - other.deltaTij_) < tol
&& deltaXij_.equals(other.deltaXij_, tol)
&& biasHat_.equals(other.biasHat_, tol)
&& equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol)
@ -82,13 +94,13 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
// Correct for bias in the sensor frame
// 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
// Equations below assume the "body" frame is the CG
// Compensate for sensor-body displacement if needed: we express the quantities
// (originally in the IMU frame) into 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();
@ -98,9 +110,12 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
j_correctedAcc = bRs * j_correctedAcc;
// 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;
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;
// Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation().vector();
@ -120,7 +135,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
}
}
// Do update in one fell swoop
// Do update in one fell swoop
return make_pair(j_correctedAcc, j_correctedOmega);
}
@ -135,22 +150,27 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
Matrix3 D_correctedAcc_measuredAcc, //
D_correctedAcc_measuredOmega, //
D_correctedOmega_measuredOmega;
bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
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
// 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));
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;
*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;
*D_updated_measuredOmega += D_updated_correctedAcc
* D_correctedAcc_measuredOmega;
}
}
return updated;
@ -162,16 +182,16 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc,
Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
// Save current rotation for updating Jacobians
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();
// Do update
// 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 ?
// 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);
@ -197,7 +217,7 @@ void PreintegrationBase::update(const Vector3& j_measuredAcc,
//------------------------------------------------------------------------------
Vector9 PreintegrationBase::biasCorrectedDelta(
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 6> H) const {
// Correct deltaRij, derivative is delRdelBiasOmega_
// Correct deltaRij, derivative is delRdelBiasOmega_
const imuBias::ConstantBias biasIncr = bias_i - biasHat_;
Matrix3 D_correctedRij_bias;
const Vector3 biasInducedOmega = delRdelBiasOmega_ * biasIncr.gyroscope();
@ -208,8 +228,8 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
Vector9 xi;
Matrix3 D_dR_correctedRij;
// TODO(frank): could line below be simplified? It is equivalent to
// LogMap(deltaRij_.compose(Expmap(biasInducedOmega)))
// 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();
@ -230,18 +250,18 @@ Vector9 PreintegrationBase::biasCorrectedDelta(
NavState PreintegrationBase::predict(const NavState& state_i,
const imuBias::ConstantBias& bias_i, OptionalJacobian<9, 9> H1,
OptionalJacobian<9, 6> H2) const {
// correct for bias
// correct for bias
Matrix96 D_biasCorrected_bias;
Vector9 biasCorrected = biasCorrectedDelta(bias_i,
H2 ? &D_biasCorrected_bias : 0);
// integrate on tangent space
// 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
// 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)
@ -258,12 +278,12 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
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()
// 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
/// Predict state at time j
Matrix99 D_predict_state_i;
Matrix96 D_predict_bias_i;
NavState predictedState_j = predict(state_i, bias_i,
@ -273,11 +293,11 @@ Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
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
// 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)
@ -300,7 +320,7 @@ 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) const {
// NOTE(frank): parameters are supposed to be constant, below is only provided for compatibility
// 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;
@ -311,4 +331,5 @@ PoseVelocityBias PreintegrationBase::predict(const Pose3& pose_i,
#endif
//------------------------------------------------------------------------------
}/// namespace gtsam
}
/// namespace gtsam

23
gtsam/navigation/PreintegrationBase.h
View File

@ -83,18 +83,21 @@ public:
}
void print(const std::string& s) const;
bool equals(const PreintegratedRotation::Params& other, double tol) const;
protected:
/// Default constructor for serialization only: uninitialized!
Params();
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);
namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params",
boost::serialization::base_object<PreintegratedRotation::Params>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);
ar & BOOST_SERIALIZATION_NVP(n_gravity);
}
@ -130,6 +133,7 @@ protected:
/// Default constructor for serialization
PreintegrationBase() {
resetIntegration();
}
public:
@ -300,15 +304,16 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization;
ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(deltaTij_);
ar & BOOST_SERIALIZATION_NVP(deltaXij_);
ar & BOOST_SERIALIZATION_NVP(biasHat_);
ar & BOOST_SERIALIZATION_NVP(delRdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delPdelBiasOmega_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasAcc_);
ar & BOOST_SERIALIZATION_NVP(delVdelBiasOmega_);
ar & bs::make_nvp("delRdelBiasOmega_", bs::make_array(delRdelBiasOmega_.data(), delRdelBiasOmega_.size()));
ar & bs::make_nvp("delPdelBiasAcc_", bs::make_array(delPdelBiasAcc_.data(), delPdelBiasAcc_.size()));
ar & bs::make_nvp("delPdelBiasOmega_", bs::make_array(delPdelBiasOmega_.data(), delPdelBiasOmega_.size()));
ar & bs::make_nvp("delVdelBiasAcc_", bs::make_array(delVdelBiasAcc_.data(), delVdelBiasAcc_.size()));
ar & bs::make_nvp("delVdelBiasOmega_", bs::make_array(delVdelBiasOmega_.data(), delVdelBiasOmega_.size()));
}
};

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

@ -65,12 +65,12 @@ static const double kAccelerometerSigma = 0.1;
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
auto p = PreintegratedImuMeasurements::Params::MakeSharedD(kGravity);
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma * I_3x3;
p->accelerometerCovariance = kAccelerometerSigma * kAccelerometerSigma
* I_3x3;
p->integrationCovariance = 0.0001 * I_3x3;
return p;
}
// Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
/* ************************************************************************* */
@ -123,7 +123,7 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
/* ************************************************************************* */
TEST(ImuFactor, Accelerating) {
const double a = 0.2, v=50;
const double a = 0.2, v = 50;
// Set up body pointing towards y axis, and start at 10,20,0 with velocity going in X
// The body itself has Z axis pointing down
@ -132,9 +132,9 @@ TEST(ImuFactor, Accelerating) {
const Vector3 initial_velocity(v, 0, 0);
const AcceleratingScenario scenario(nRb, initial_position, initial_velocity,
Vector3(a, 0, 0));
Vector3(a, 0, 0));
const double T = 3.0; // seconds
const double T = 3.0; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
AggregateImuReadings pim = runner.integrate(T);
@ -144,17 +144,20 @@ TEST(ImuFactor, Accelerating) {
EXPECT(assert_equal(estimatedCov, pim.preintMeasCov(), 0.1));
// Check G1 and G2 derivatives of pim.update
Matrix93 aG1,aG2;
boost::function<NavState(const Vector3&, const Vector3&)> f =
boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10,
boost::none, boost::none, boost::none);
Matrix93 aG1, aG2;
boost::function<NavState(const Vector3&, const Vector3&)> f = boost::bind(
&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T / 10, boost::none,
boost::none, boost::none);
const Vector3 measuredAcc = runner.actualSpecificForce(T);
const Vector3 measuredOmega = runner.actualAngularVelocity(T);
pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1, aG2);
EXPECT(assert_equal(
numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7), aG1, 1e-7));
EXPECT(assert_equal(
numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7), aG2, 1e-7));
pim.updatedDeltaXij(measuredAcc, measuredOmega, T / 10, boost::none, aG1,
aG2);
EXPECT(
assert_equal(numericalDerivative21(f, measuredAcc, measuredOmega, 1e-7),
aG1, 1e-7));
EXPECT(
assert_equal(numericalDerivative22(f, measuredAcc, measuredOmega, 1e-7),
aG2, 1e-7));
}
/* ************************************************************************* */
@ -268,7 +271,8 @@ TEST(ImuFactor, ErrorAndJacobians) {
Vector expectedError(9);
expectedError << 0, 0, 0, 0, 0, 0, 0, 0, 0;
EXPECT(
assert_equal(expectedError, factor.evaluateError(x1, v1, x2, v2, kZeroBias)));
assert_equal(expectedError,
factor.evaluateError(x1, v1, x2, v2, kZeroBias)));
Values values;
values.insert(X(1), x1);
@ -284,16 +288,19 @@ TEST(ImuFactor, ErrorAndJacobians) {
// Actual Jacobians
Matrix H1a, H2a, H3a, H4a, H5a;
(void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a, H5a);
(void) factor.evaluateError(x1, v1, x2, v2, kZeroBias, H1a, H2a, H3a, H4a,
H5a);
// Make sure rotation part is correct when error is interpreted as axis-angle
// Jacobians are around zero, so the rotation part is the same as:
Matrix H1Rot3 = numericalDerivative11<Rot3, Pose3>(
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias), x1);
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, kZeroBias),
x1);
EXPECT(assert_equal(H1Rot3, H1a.topRows(3)));
Matrix H3Rot3 = numericalDerivative11<Rot3, Pose3>(
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias), x2);
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, kZeroBias),
x2);
EXPECT(assert_equal(H3Rot3, H3a.topRows(3)));
// Evaluate error with wrong values
@ -386,7 +393,6 @@ TEST(ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis) {
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
bool use2ndOrderCoriolis = true;
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
Values values;
@ -533,7 +539,7 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
/* ************************************************************************* */
Vector3 correctedAcc(const PreintegratedImuMeasurements& pim,
const Vector3& measuredAcc, const Vector3& measuredOmega) {
const Vector3& measuredAcc, const Vector3& measuredOmega) {
return pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega).first;
}
@ -544,15 +550,16 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
const Vector3 a = nRb * Vector3(0.2, 0.0, 0.0);
const AcceleratingScenario scenario(nRb, p1, v1, a,
Vector3(0, 0, M_PI / 10.0 + 0.3));
Vector3(0, 0, M_PI / 10.0 + 0.3));
auto p = defaultParams();
p->body_P_sensor =Pose3(Rot3::Expmap(Vector3(0, M_PI/2, 0)), Point3(0.1, 0.05, 0.01));
p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(0, M_PI / 2, 0)),
Point3(0.1, 0.05, 0.01));
p->omegaCoriolis = kNonZeroOmegaCoriolis;
imuBias::ConstantBias biasHat(Vector3(0.2, 0.0, 0.0), Vector3(0.0, 0.0, 0.0));
const double T = 3.0; // seconds
const double T = 3.0; // seconds
ScenarioRunner runner(&scenario, p, T / 10);
// PreintegratedImuMeasurements pim = runner.integrate(T);
@ -575,32 +582,34 @@ TEST(ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement) {
// Check updatedDeltaXij derivatives
Matrix3 D_correctedAcc_measuredOmega = Matrix3::Zero();
pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, boost::none, D_correctedAcc_measuredOmega, boost::none);
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega,
boost::none, D_correctedAcc_measuredOmega, boost::none);
Matrix3 expectedD = numericalDerivative11<Vector3, Vector3>(
boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6);
EXPECT(assert_equal(expectedD, D_correctedAcc_measuredOmega, 1e-5));
Matrix93 G1, G2;
double dt = 0.1;
NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt, boost::none, G1, G2);
NavState preint = pim.updatedDeltaXij(measuredAcc, measuredOmega, dt,
boost::none, G1, G2);
// Matrix9 preintCov = G1*((accNoiseVar2/dt).asDiagonal())*G1.transpose() + G2*((omegaNoiseVar2/dt).asDiagonal())*G2.transpose();
Matrix93 expectedG1 = numericalDerivative21<NavState, Vector3, Vector3>(
boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega,
1e-6);
dt, boost::none, boost::none, boost::none), measuredAcc,
measuredOmega, 1e-6);
EXPECT(assert_equal(expectedG1, G1, 1e-5));
Matrix93 expectedG2 = numericalDerivative22<NavState, Vector3, Vector3>(
boost::bind(&PreintegratedImuMeasurements::updatedDeltaXij, pim, _1, _2,
dt, boost::none, boost::none, boost::none), measuredAcc, measuredOmega,
1e-6);
dt, boost::none, boost::none, boost::none), measuredAcc,
measuredOmega, 1e-6);
EXPECT(assert_equal(expectedG2, G2, 1e-5));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
// EXPECT(MonteCarlo(pim, NavState(x1, initial_velocity), bias, dt, body_P_sensor,
// measuredAcc, measuredOmega, accNoiseVar2, omegaNoiseVar2, 100000));
// integrate at least twice to get position information
// otherwise factor cov noise from preint_cov is not positive definite
pim.integrateMeasurement(measuredAcc, measuredOmega, dt);
@ -687,10 +696,9 @@ TEST(ImuFactor, PredictArbitrary) {
const Vector3 v1(0, 0, 0);
const AcceleratingScenario scenario(x1.rotation(), x1.translation(), v1,
Vector3(0.1, 0.2, 0),
Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
Vector3(0.1, 0.2, 0), Vector3(M_PI / 10, M_PI / 10, M_PI / 10));
const double T = 3.0; // seconds
const double T = 3.0; // seconds
ScenarioRunner runner(&scenario, defaultParams(), T / 10);
//
// PreintegratedImuMeasurements pim = runner.integrate(T);
@ -707,7 +715,7 @@ TEST(ImuFactor, PredictArbitrary) {
Vector3 measuredAcc = runner.actualSpecificForce(0);
auto p = defaultParams();
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
p->integrationCovariance = Z_3x3; // MonteCarlo does not sample integration noise
PreintegratedImuMeasurements pim(p, biasHat);
imuBias::ConstantBias bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
// EXPECT(MonteCarlo(pim, NavState(x1, v1), bias, 0.1, boost::none, measuredAcc, measuredOmega,
@ -724,9 +732,9 @@ TEST(ImuFactor, PredictArbitrary) {
NavState actual = pim.predict(NavState(x1, v1), bias);
// Regression test for Imu Refactor
Rot3 expectedR( //
+0.903715275, -0.250741668, 0.347026393, //
+0.347026393, 0.903715275, -0.250741668, //
Rot3 expectedR( //
+0.903715275, -0.250741668, 0.347026393, //
+0.347026393, 0.903715275, -0.250741668, //
-0.250741668, 0.347026393, 0.903715275);
Point3 expectedT(-0.516077031, 0.57842919, 0.0876478403);
Vector3 expectedV(-1.62337767, 1.57954409, 0.343833571);
@ -740,7 +748,7 @@ TEST(ImuFactor, bodyPSensorNoBias) {
// Rotate sensor (z-down) to body (same as navigation) i.e. z-up
auto p = defaultParams();
p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame
p->n_gravity = Vector3(0, 0, -kGravity); // z-up nav frame
p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3(0, 0, 0));
// Measurements
@ -836,8 +844,8 @@ TEST(ImuFactor, bodyPSensorWithBias) {
// Now add IMU factors and bias noise models
Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0));
for (int i = 1; i < numFactors; i++) {
PreintegratedImuMeasurements pim =
PreintegratedImuMeasurements(p, priorBias);
PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p,
priorBias);
for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
@ -859,6 +867,54 @@ TEST(ImuFactor, bodyPSensorWithBias) {
EXPECT(assert_equal(biasExpected, biasActual, 1e-3));
}
/* ************************************************************************** */
#include <gtsam/base/serializationTestHelpers.h>
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
"gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
"gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
"gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
"gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(ImuFactor, serialization) {
using namespace gtsam::serializationTestHelpers;
auto p = defaultParams();
p->n_gravity = Vector3(0, 0, -9.81);
p->body_P_sensor = Pose3(Rot3::ypr(0, 0, M_PI), Point3());
p->accelerometerCovariance = 1e-7 * I_3x3;
p->gyroscopeCovariance = 1e-8 * I_3x3;
p->integrationCovariance = 1e-9 * I_3x3;
double deltaT = 0.005;
imuBias::ConstantBias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
PreintegratedImuMeasurements pim(p, priorBias);
// measurements are needed for non-inf noise model, otherwise will throw err when deserialize
// Sensor frame is z-down
// Gyroscope measurement is the angular velocity of sensor w.r.t nav frame in sensor frame
Vector3 measuredOmega(0, 0.01, 0);
// Acc measurement is acceleration of sensor in the sensor frame, when stationary,
// table exerts an equal and opposite force w.r.t gravity
Vector3 measuredAcc(0, 0, -9.81);
for (int j = 0; j < 200; ++j)
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pim);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

3
gtsam/slam/SmartFactorBase.h
View File

@ -93,6 +93,9 @@ public:
/// We use the new CameraSte data structure to refer to a set of cameras
typedef CameraSet<CAMERA> Cameras;
/// Default Constructor, for serialization
SmartFactorBase() {}
/// Constructor
SmartFactorBase(const SharedNoiseModel& sharedNoiseModel,
boost::optional<Pose3> body_P_sensor = boost::none) :

7
gtsam/slam/SmartProjectionFactor.h
View File

@ -137,7 +137,7 @@ protected:
/// @name Parameters
/// @{
const SmartProjectionParams params_;
SmartProjectionParams params_;
/// @}
/// @name Caching triangulation
@ -154,6 +154,11 @@ public:
/// shorthand for a set of cameras
typedef CameraSet<CAMERA> Cameras;
/**
* Default constructor, only for serialization
*/
SmartProjectionFactor() {}
/**
* Constructor
* @param body_P_sensor pose of the camera in the body frame

5
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -59,6 +59,11 @@ public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/**
* Default constructor, only for serialization
*/
SmartProjectionPoseFactor() {}
/**
* Constructor
* @param Isotropic measurement noise

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

@ -18,6 +18,7 @@
#include <gtsam/slam/SmartFactorBase.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -29,9 +30,13 @@ static SharedNoiseModel unit3(noiseModel::Unit::Create(3));
/* ************************************************************************* */
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3Bundler.h>
namespace gtsam {
class PinholeFactor: public SmartFactorBase<PinholeCamera<Cal3Bundler> > {
public:
typedef SmartFactorBase<PinholeCamera<Cal3Bundler> > Base;
PinholeFactor() {}
PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
@ -43,6 +48,11 @@ public:
}
};
/// traits
template<>
struct traits<PinholeFactor> : public Testable<PinholeFactor> {};
}
TEST(SmartFactorBase, Pinhole) {
PinholeFactor f= PinholeFactor(unit2);
f.add(Point2(), 1);
@ -52,9 +62,13 @@ TEST(SmartFactorBase, Pinhole) {
/* ************************************************************************* */
#include <gtsam/geometry/StereoCamera.h>
namespace gtsam {
class StereoFactor: public SmartFactorBase<StereoCamera> {
public:
typedef SmartFactorBase<StereoCamera> Base;
StereoFactor() {}
StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) {
}
virtual double error(const Values& values) const {
@ -66,6 +80,11 @@ public:
}
};
/// traits
template<>
struct traits<StereoFactor> : public Testable<StereoFactor> {};
}
TEST(SmartFactorBase, Stereo) {
StereoFactor f(unit3);
f.add(StereoPoint2(), 1);
@ -73,6 +92,24 @@ TEST(SmartFactorBase, Stereo) {
EXPECT_LONGS_EQUAL(2 * 3, f.dim());
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(SmartFactorBase, serialize) {
using namespace gtsam::serializationTestHelpers;
PinholeFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -22,6 +22,7 @@
#include "smartFactorScenarios.h"
#include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
@ -843,6 +844,26 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
EXPECT(assert_equal(yActual, yExpected, 1e-7));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST( SmartProjectionCameraFactor, serialize) {
using namespace vanilla;
using namespace gtsam::serializationTestHelpers;
SmartFactor factor(unit2);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;

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

@ -24,6 +24,7 @@
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/map.hpp>
#include <iostream>
@ -1387,6 +1388,27 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
values.at<Pose3>(x3)));
}
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
TEST(SmartProjectionPoseFactor, serialize) {
using namespace vanillaPose;
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor(model, sharedK, boost::none, params);
EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
EXPECT(equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;