diff --git a/src/feature_imu.cpp b/src/feature_imu.cpp index cd1bec5570bfb324be83b80cbae572b32d73b37b..3bc14edc32c1222dc3e4bb387cdd3364dd15c26e 100644 --- a/src/feature_imu.cpp +++ b/src/feature_imu.cpp @@ -10,6 +10,16 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen: //std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl; } +FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians) : + FeatureBase("IMU", _delta_preintegrated, _delta_preintegrated_covariance), + dp_preint_(_delta_preintegrated.head<3>()),dv_preint_(_delta_preintegrated.segment<3>(7)), dq_preint_(_delta_preintegrated.segment<4>(3)), + acc_bias_preint_(_delta_preintegrated.segment<3>(10)), gyro_bias_preint_(_delta_preintegrated.tail<3>()), + dDp_dab_(_dD_db_jacobians.block(3,3,0,0)), dDv_dab_(_dD_db_jacobians.block(3,3,6,0)), + dDp_dwb_(_dD_db_jacobians.block(3,3,0,3)), dDv_dwb_(_dD_db_jacobians.block(3,3,6,3)), dDq_dwb_(_dD_db_jacobians.block(3,3,3,3)) +{ + //std::cout << "New FeatureIMU: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl; +} + FeatureIMU::~FeatureIMU() { // diff --git a/src/feature_imu.h b/src/feature_imu.h index a1933685742a9ac172734f5bbf198ebbc715dcf4..781db5c561d53e6cffa524371168bbd49f31e270 100644 --- a/src/feature_imu.h +++ b/src/feature_imu.h @@ -24,6 +24,15 @@ class FeatureIMU : public FeatureBase */ FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance); + /** \brief Constructor from capture pointer and measure + * + * \param _measurement the measurement + * \param _meas_covariance the noise of the measurement + * \param _dD_db_jacobians Jacobians of preintegrated delta wrt IMU biases + * + */ + FeatureIMU(const Eigen::VectorXs& _delta_preintegrated, const Eigen::MatrixXs& _delta_preintegrated_covariance, const Eigen::Matrix<wolf::Scalar,9,6>& _dD_db_jacobians); + virtual ~FeatureIMU(); public: // TODO eventually produce getters for these and then go private