diff --git a/doc/ImuFactor.lyx b/doc/ImuFactor.lyx index 7e5ceac33..d9cd35584 100644 --- a/doc/ImuFactor.lyx +++ b/doc/ImuFactor.lyx @@ -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 diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 3cafdd0ba..c6af89486 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -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) { diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 41ffa27b5..e2b40b02b 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -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 -inline Matrix3 skewSymmetric(const Eigen::MatrixBase& 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 +inline Matrix3 skewSymmetric(const Eigen::MatrixBase& 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); diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index b255b082d..2497f6806 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -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()); } diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index e2a514db7..ca80167f4 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -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::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 } diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index c3ab56200..bec901830 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -376,6 +376,15 @@ class TriangulationResult: public boost::optional { status_(s) { } public: + + /** + * Default constructor, only for serialization + */ + TriangulationResult() {} + + /** + * Constructor + */ TriangulationResult(const Point3& p) : status_(VALID) { reset(p); diff --git a/gtsam/navigation/ImuBias.h b/gtsam/navigation/ImuBias.h index 4acccb578..7664c7fd5 100644 --- a/gtsam/navigation/ImuBias.h +++ b/gtsam/navigation/ImuBias.h @@ -201,7 +201,6 @@ private: friend class boost::serialization::access; template 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_); } diff --git a/gtsam/navigation/ImuFactor.cpp b/gtsam/navigation/ImuFactor.cpp index 673706d58..692154c9d 100644 --- a/gtsam/navigation/ImuFactor.cpp +++ b/gtsam/navigation/ImuFactor.cpp @@ -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 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(&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& 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 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 diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index ac7aee797..f52d95b26 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -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 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())); } }; diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 9d623bf38..df38df0b8 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -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, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 7fb734a91..b170ea863 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -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 - 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 + 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); } }; diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index 2372b2ee2..626dcdf70 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -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(&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 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 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 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 q = boost::make_shared(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 diff --git a/gtsam/navigation/PreintegrationBase.h b/gtsam/navigation/PreintegrationBase.h index ce866031a..f5e8c7183 100644 --- a/gtsam/navigation/PreintegrationBase.h +++ b/gtsam/navigation/PreintegrationBase.h @@ -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 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(*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 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())); } }; diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 83ee62ed0..473cc9365 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -65,12 +65,12 @@ static const double kAccelerometerSigma = 0.1; static boost::shared_ptr 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 f = - boost::bind(&PreintegrationBase::updatedDeltaXij, pim, _1, _2, T/10, - boost::none, boost::none, boost::none); + Matrix93 aG1, aG2; + boost::function 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( - 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( - 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(boost::bind(correctedAcc, pim, measuredAcc, _1), measuredOmega, 1e-6); + pim.correctMeasurementsByBiasAndSensorPose(measuredAcc, measuredOmega, + boost::none, D_correctedAcc_measuredOmega, boost::none); + Matrix3 expectedD = numericalDerivative11( + 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( 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( 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 + +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; diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e903d9651..01a8fcf8d 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -93,6 +93,9 @@ public: /// We use the new CameraSte data structure to refer to a set of cameras typedef CameraSet Cameras; + /// Default Constructor, for serialization + SmartFactorBase() {} + /// Constructor SmartFactorBase(const SharedNoiseModel& sharedNoiseModel, boost::optional body_P_sensor = boost::none) : diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 13a4dd38f..09fe84caa 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -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 Cameras; + /** + * Default constructor, only for serialization + */ + SmartProjectionFactor() {} + /** * Constructor * @param body_P_sensor pose of the camera in the body frame diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index c091ff79d..455e0de87 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -59,6 +59,11 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; + /** + * Default constructor, only for serialization + */ + SmartProjectionPoseFactor() {} + /** * Constructor * @param Isotropic measurement noise diff --git a/gtsam/slam/tests/testSmartFactorBase.cpp b/gtsam/slam/tests/testSmartFactorBase.cpp index bf5969d4d..96052bd0f 100644 --- a/gtsam/slam/tests/testSmartFactorBase.cpp +++ b/gtsam/slam/tests/testSmartFactorBase.cpp @@ -18,6 +18,7 @@ #include #include +#include #include using namespace std; @@ -29,9 +30,13 @@ static SharedNoiseModel unit3(noiseModel::Unit::Create(3)); /* ************************************************************************* */ #include #include + +namespace gtsam { + class PinholeFactor: public SmartFactorBase > { public: typedef SmartFactorBase > Base; + PinholeFactor() {} PinholeFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { } virtual double error(const Values& values) const { @@ -43,6 +48,11 @@ public: } }; +/// traits +template<> +struct traits : public Testable {}; +} + TEST(SmartFactorBase, Pinhole) { PinholeFactor f= PinholeFactor(unit2); f.add(Point2(), 1); @@ -52,9 +62,13 @@ TEST(SmartFactorBase, Pinhole) { /* ************************************************************************* */ #include + +namespace gtsam { + class StereoFactor: public SmartFactorBase { public: typedef SmartFactorBase Base; + StereoFactor() {} StereoFactor(const SharedNoiseModel& sharedNoiseModel): Base(sharedNoiseModel) { } virtual double error(const Values& values) const { @@ -66,6 +80,11 @@ public: } }; +/// traits +template<> +struct traits : public Testable {}; +} + 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; diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index d4f60b085..54bbd6c22 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -22,6 +22,7 @@ #include "smartFactorScenarios.h" #include #include +#include #include #include #include @@ -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; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 8796dfe65..0e2429840 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -1387,6 +1388,27 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.at(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;