diff --git a/include/imu/processor/processor_imu.h b/include/imu/processor/processor_imu.h index 766cf7bf5f6460355ef723dc50125d5f37ac2f51..07f06456f291ff014b2466aed1c8230086f92621 100644 --- a/include/imu/processor/processor_imu.h +++ b/include/imu/processor/processor_imu.h @@ -49,7 +49,7 @@ struct ParamsProcessorImu : public ParamsProcessorMotion ParamsProcessorMotion(_n) { bootstrap_enable = _n["bootstrap"]["enable"].as<bool>(); - if (_n["bootstrap"]["method"]) + if (bootstrap_enable) { string str = _n["bootstrap"]["method"].as<string>(); std::transform(str.begin(), str.end(), str.begin(), ::toupper); diff --git a/include/imu/sensor/sensor_imu.h b/include/imu/sensor/sensor_imu.h index 87865841c71d6494756c9ffc27ca0b7ace9607fa..7b207bfc9c5be67b9013b557472871c89de8b0ab 100644 --- a/include/imu/sensor/sensor_imu.h +++ b/include/imu/sensor/sensor_imu.h @@ -80,10 +80,6 @@ class SensorImu : public SensorBase double getGyroNoise() const; double getAccelNoise() const; - double getWbInitialStdev() const; - double getAbInitialStdev() const; - double getWbRateStdev() const; - double getAbRateStdev() const; bool isGravityOrthogonal() const; Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd & _data) const override; @@ -109,7 +105,7 @@ inline double SensorImu<DIM>::getAccelNoise() const template <unsigned int DIM> inline bool SensorImu<DIM>::isGravityOrthogonal() const { - return params_imu_->orthogonal_gravity; + return DIM==2 and params_imu_->orthogonal_gravity; } template <> diff --git a/schema/processor/ProcessorCompass.schema b/schema/processor/ProcessorCompass.schema new file mode 100644 index 0000000000000000000000000000000000000000..3c97f7469fc38b34aa0998e8c96fc47d177e4aa9 --- /dev/null +++ b/schema/processor/ProcessorCompass.schema @@ -0,0 +1 @@ +follow: ProcessorBase.schema \ No newline at end of file diff --git a/schema/processor/ProcessorImu.schema b/schema/processor/ProcessorImu.schema new file mode 100644 index 0000000000000000000000000000000000000000..ee5f2cd9d433ba713f59c492fbb060928484db30 --- /dev/null +++ b/schema/processor/ProcessorImu.schema @@ -0,0 +1,17 @@ +follow: ProcessorMotion.schema +bootstrap: + enable: + _mandatory: true + _type: bool + _doc: If any boostrap mechanism is enabled for the processor. + method: + _mandatory: false + _type: string + _options: ["STATIC", "G", "V0_G"] + _doc: (only if enable==true) Boostrap method. + averaging_length: + _mandatory: false + _type: double + _doc: Averaging length used by the bootstrap mechanism. + + \ No newline at end of file diff --git a/schema/processor/ProcessorImu2d.schema b/schema/processor/ProcessorImu2d.schema new file mode 100644 index 0000000000000000000000000000000000000000..3823a081464db632c04599be4818e27240e98929 --- /dev/null +++ b/schema/processor/ProcessorImu2d.schema @@ -0,0 +1 @@ +follow: ProcessorMotion.schema \ No newline at end of file diff --git a/schema/sensor/SensorCompass.schema b/schema/sensor/SensorCompass.schema new file mode 100644 index 0000000000000000000000000000000000000000..84aeb60585725bfe1073f226c0fa075c3d9fc4d7 --- /dev/null +++ b/schema/sensor/SensorCompass.schema @@ -0,0 +1,58 @@ +follow: SensorBase.schema + +stdev_noise: + _mandatory: true + _type: double + _doc: standard deviation of the compass measurements (square root of the covariance matrix diagonal). + +states: + P: + follow: SpecStateSensorP3d.schema + O: + follow: SpecStateSensorO3d.schema + I: + follow: SpecStateSensor.schema + type: + _type: string + _mandatory: false + _default: StateParams3 + _options: [StateParams3] + _doc: The derived type of the StateBlock for compass biases + state: + _type: Vector3d + _mandatory: true + _doc: The initial values of the compass biases + dynamic: + _type: bool + _mandatory: true + _doc: If the bias are dynamic, i.e. they change along time. + noise_std: + _type: Vector3d + _mandatory: false + _doc: (only if mode==factor) A vector containing the stdev values of the noise of the prior factor, i.e. the sqrt of the diagonal elements of the covariance matrix. + drift_std: + _type: Vector3d + _mandatory: false + _doc: (only if dynamic==true) A vector containing the stdev values of the noise of the drift factor , i.e. the sqrt of the diagonal elements of the covariance matrix. + F: + follow: SpecStateSensor.schema + type: + _type: string + _mandatory: false + _default: StateParams3 + _options: [StateParams3] + _doc: The derived type of the StateBlock for magnetic field + state: + _type: Vector3d + _mandatory: true + _doc: The initial values of the compass biases + dynamic: + _type: bool + _mandatory: false + _default: false + _options: [false] + _doc: The magnetic field is not dynamic. + noise_std: + _type: Vector3d + _mandatory: false + _doc: (only if mode==factor) A vector containing the stdev values of the noise of the prior factor, i.e. the sqrt of the diagonal elements of the covariance matrix. diff --git a/schema/sensor/SensorImu2d.schema b/schema/sensor/SensorImu2d.schema new file mode 100644 index 0000000000000000000000000000000000000000..0be0668ebaba53bfda68ad4499cf7273b7ac5d4a --- /dev/null +++ b/schema/sensor/SensorImu2d.schema @@ -0,0 +1,43 @@ +follow: SensorBase.schema + +w_noise: + _mandatory: true + _type: double + _doc: standard deviation of the gyroscope measurements (square root of the covariance matrix diagonal). + +a_noise: + _mandatory: true + _type: double + _doc: standard deviation of the accelerometer measurements (square root of the covariance matrix diagonal). + +states: + P: + follow: SpecStateSensorP2d.schema + O: + follow: SpecStateSensorO2d.schema + I: + follow: SpecStateSensor.schema + type: + _type: string + _mandatory: false + _default: StateParams3 + _options: [StateParams3] + _doc: The derived type of the StateBlock for IMU biases + state: + _type: Vector3d + _mandatory: false + _default: [0,0,0] + _doc: The initial values of the IMU biases + dynamic: + _type: bool + _mandatory: true + _doc: If the bias are dynamic, i.e. they change along time. + noise_std: + _type: Vector3d + _mandatory: false + _doc: (only if mode==factor) A vector containing the stdev values of the noise of the prior factor, i.e. the sqrt of the diagonal elements of the covariance matrix. + drift_std: + _type: Vector3d + _mandatory: false + _doc: (only if dynamic==true) A vector containing the stdev values of the noise of the drift factor , i.e. the sqrt of the diagonal elements of the covariance matrix. + diff --git a/schema/sensor/SensorImu3d.schema b/schema/sensor/SensorImu3d.schema new file mode 100644 index 0000000000000000000000000000000000000000..e484cdf39935998c1e8acee7aa3145930d1dc03c --- /dev/null +++ b/schema/sensor/SensorImu3d.schema @@ -0,0 +1,43 @@ +follow: SensorBase.schema + +w_noise: + _mandatory: true + _type: double + _doc: standard deviation of the gyroscope measurements (square root of the covariance matrix diagonal). + +a_noise: + _mandatory: true + _type: double + _doc: standard deviation of the accelerometer measurements (square root of the covariance matrix diagonal). + +states: + P: + follow: SpecStateSensorP3d.schema + O: + follow: SpecStateSensorO3d.schema + I: + follow: SpecStateSensor.schema + type: + _type: string + _mandatory: false + _default: StateParams6 + _options: [StateParams6] + _doc: The derived type of the StateBlock for IMU biases + state: + _type: Vector6d + _mandatory: false + _default: [0,0,0,0,0,0] + _doc: The initial values of the IMU biases + dynamic: + _type: bool + _mandatory: true + _doc: If the bias are dynamic, i.e. they change along time. + noise_std: + _type: Vector6d + _mandatory: false + _doc: (only if mode==factor) A vector containing the stdev values of the noise of the prior factor, i.e. the sqrt of the diagonal elements of the covariance matrix. + drift_std: + _type: Vector6d + _mandatory: false + _doc: (only if dynamic==true) A vector containing the stdev values of the noise of the drift factor , i.e. the sqrt of the diagonal elements of the covariance matrix. + diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index d5809731a5e530177267647f312ea0ff0f977852..27559fb4c62cec4f80ba2543170c7038c6ef0d34 100644 --- a/src/processor/processor_imu.cpp +++ b/src/processor/processor_imu.cpp @@ -111,16 +111,9 @@ void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd void ProcessorImu::configure(SensorBasePtr _sensor) { - sensor_imu_ = std::static_pointer_cast<SensorImu3d>(_sensor); - - auto acc_drift_std = sensor_imu_->getAbRateStdev(); - auto gyro_drift_std = sensor_imu_->getAbRateStdev(); - - ArrayXd imu_drift_sigmas(6); - imu_drift_sigmas << acc_drift_std, acc_drift_std, acc_drift_std, gyro_drift_std, gyro_drift_std, gyro_drift_std; - imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal(); - + imu_drift_cov_ = _sensor->getDriftCov('I'); } + void ProcessorImu::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) { // 1. Feature and factor Imu diff --git a/src/processor/processor_imu2d.cpp b/src/processor/processor_imu2d.cpp index 8db279c69c55283a3fcf37904ccc04593690067b..466e6f294320518edf2057ba01597eebdfb1a849 100644 --- a/src/processor/processor_imu2d.cpp +++ b/src/processor/processor_imu2d.cpp @@ -33,281 +33,275 @@ namespace wolf { - ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu) : - ProcessorMotion("ProcessorImu2d", - {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}}, - 2, 5, 5, 5, 6, 3, - _params_motion_imu), - params_motion_Imu_(std::make_shared<ParamsProcessorImu2d>(*_params_motion_imu)) - { - // - } +ProcessorImu2d::ProcessorImu2d(ParamsProcessorImu2dPtr _params_motion_imu) : + ProcessorMotion("ProcessorImu2d", + {{'P',"StatePoint2d"},{'O',"StateAngle"},{'V',"StateVector2d"}}, + 2, 5, 5, 5, 6, 3, + _params_motion_imu), + params_motion_Imu_(std::make_shared<ParamsProcessorImu2d>(*_params_motion_imu)) +{ + // +} - ProcessorImu2d::~ProcessorImu2d() +ProcessorImu2d::~ProcessorImu2d() +{ + // +} + +void ProcessorImu2d::preProcess() +{ + auto cap_ptr = std::dynamic_pointer_cast<CaptureImu>(incoming_ptr_); + assert(cap_ptr != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str()); +} + +bool ProcessorImu2d::voteForKeyFrame() const +{ + // time span + if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span) { - // + WOLF_DEBUG( "PM: vote: time span" ); + return true; } - - void ProcessorImu2d::preProcess() + // buffer length + if (getBuffer().size() > params_motion_Imu_->max_buff_length) { - auto cap_ptr = std::dynamic_pointer_cast<CaptureImu>(incoming_ptr_); - assert(cap_ptr != nullptr && ("Capture type mismatch. Processor " + getName() + " can only process captures of type CaptureImu").c_str()); + WOLF_DEBUG( "PM: vote: buffer length" ); + return true; } - - bool ProcessorImu2d::voteForKeyFrame() const + // angle turned + double angle = std::abs(delta_integrated_(2)); + if (angle > params_motion_Imu_->angle_turned) { - // time span - if (getBuffer().back().ts_ - getBuffer().front().ts_ > params_motion_Imu_->max_time_span) - { - WOLF_DEBUG( "PM: vote: time span" ); - return true; - } - // buffer length - if (getBuffer().size() > params_motion_Imu_->max_buff_length) - { - WOLF_DEBUG( "PM: vote: buffer length" ); - return true; - } - // angle turned - double angle = std::abs(delta_integrated_(2)); - if (angle > params_motion_Imu_->angle_turned) - { - WOLF_DEBUG( "PM: vote: angle turned" ); - return true; - } - //WOLF_DEBUG( "PM: do not vote" ); - return false; + WOLF_DEBUG( "PM: vote: angle turned" ); + return true; } + //WOLF_DEBUG( "PM: do not vote" ); + return false; +} - CaptureMotionPtr ProcessorImu2d::emplaceCapture(const FrameBasePtr& _frame_own, - const SensorBasePtr& _sensor, - const TimeStamp& _ts, - const VectorXd& _data, - const MatrixXd& _data_cov, - const VectorXd& _calib, - const VectorXd& _calib_preint, - const CaptureBasePtr& _capture_origin) - { - auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureImu>(_frame_own, - _ts, - _sensor, - _data, - _data_cov, - _capture_origin)); - setCalibration(cap_motion, _calib); - cap_motion->setCalibrationPreint(_calib_preint); - - return cap_motion; - } +CaptureMotionPtr ProcessorImu2d::emplaceCapture(const FrameBasePtr& _frame_own, + const SensorBasePtr& _sensor, + const TimeStamp& _ts, + const VectorXd& _data, + const MatrixXd& _data_cov, + const VectorXd& _calib, + const VectorXd& _calib_preint, + const CaptureBasePtr& _capture_origin) +{ + auto cap_motion = std::static_pointer_cast<CaptureMotion>(CaptureBase::emplace<CaptureImu>(_frame_own, + _ts, + _sensor, + _data, + _data_cov, + _capture_origin)); + setCalibration(cap_motion, _calib); + cap_motion->setCalibrationPreint(_calib_preint); + + return cap_motion; +} - VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const - { - if (_capture) - return _capture->getStateBlock('I')->getState(); - else - return getSensor()->getStateBlockDynamic('I')->getState(); - } +VectorXd ProcessorImu2d::getCalibration (const CaptureBaseConstPtr _capture) const +{ + if (_capture) + return _capture->getStateBlock('I')->getState(); + else + return getSensor()->getStateBlockDynamic('I')->getState(); +} - void ProcessorImu2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration) - { +void ProcessorImu2d::setCalibration (const CaptureBasePtr _capture, const VectorXd& _calibration) +{ _capture->getSensorIntrinsic()->setState(_calibration); - } +} + void ProcessorImu2d::configure(SensorBasePtr _sensor) { - sensor_imu2d_ = std::static_pointer_cast<SensorImu2d>(_sensor); + imu_drift_cov_ = _sensor->getDriftCov('I'); +} - auto acc_drift_std = sensor_imu2d_->getAbRateStdev(); - auto gyro_drift_std = sensor_imu2d_->getWbRateStdev(); +void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) +{ + auto feature = FeatureBase::emplace<FeatureImu2d>( + _capture_own, _capture_own->getBuffer().back().delta_integr_, + _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_, + _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_); - Array3d imu_drift_sigmas(acc_drift_std, acc_drift_std, gyro_drift_std); - imu_drift_cov_ = imu_drift_sigmas.square().matrix().asDiagonal(); + CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin); + FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(feature); -} + if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal()) + FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), + params_->apply_loss_function); + else + FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), + params_->apply_loss_function); - void ProcessorImu2d::emplaceFeaturesAndFactors(CaptureBasePtr _capture_origin, CaptureMotionPtr _capture_own) - { - auto feature = FeatureBase::emplace<FeatureImu2d>( - _capture_own, _capture_own->getBuffer().back().delta_integr_, - _capture_own->getBuffer().back().delta_integr_cov_ + unmeasured_perturbation_cov_, - _capture_own->getCalibrationPreint(), _capture_own->getBuffer().back().jacobian_calib_); - - CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin); - FeatureImu2dPtr ftr_imu = std::static_pointer_cast<FeatureImu2d>(feature); - - if (std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal()) - FactorBase::emplace<FactorImu2d>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), - params_->apply_loss_function); - else - FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, ftr_imu, cap_imu, shared_from_this(), - params_->apply_loss_function); - - if (getSensor()->isStateBlockDynamic('I')) - { - const auto& sb_imubias_own = _capture_own->getStateBlock('I'); - const auto& sb_imubias_origin = _capture_origin->getStateBlock('I'); - if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case - { - auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp(); - auto ftr_bias = FeatureBase::emplace<FeatureBase>( - _capture_own, "FeatureBase", - Vector3d::Zero(), // mean IMU drift is zero - imu_drift_cov_ * dt); // IMU drift cov specified in continuous time - auto fac_bias = - FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias, - sb_imubias_own, // IMU bias block at t=own - sb_imubias_origin, // IMU bias block at t=origin - nullptr, // frame other - _capture_origin, // origin capture - nullptr, // feature other - nullptr, // landmark other - 0, // take all of first state block - -1, // take all of first state block - 0, // take all of first second block - -1, // take all of first second block - shared_from_this(), // this processor - params_->apply_loss_function); // loss function - - if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() ) - FactorBase::emplace<FactorImu2d>(ftr_imu, - ftr_imu, - cap_imu, - shared_from_this(), - params_->apply_loss_function); - else - FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, - ftr_imu, - cap_imu, - shared_from_this(), - params_->apply_loss_function); - } - } - } + if (getSensor()->isStateBlockDynamic('I')) + { + const auto& sb_imubias_own = _capture_own->getStateBlock('I'); + const auto& sb_imubias_origin = _capture_origin->getStateBlock('I'); + if (sb_imubias_own != sb_imubias_origin) // make sure it's two different state blocks! -- just in case + { + auto dt = _capture_own->getTimeStamp() - _capture_origin->getTimeStamp(); + auto ftr_bias = FeatureBase::emplace<FeatureBase>( + _capture_own, "FeatureBase", + Vector3d::Zero(), // mean IMU drift is zero + imu_drift_cov_ * dt); // IMU drift cov specified in continuous time + auto fac_bias = + FactorBase::emplace<FactorBlockDifference>(ftr_bias, ftr_bias, + sb_imubias_own, // IMU bias block at t=own + sb_imubias_origin, // IMU bias block at t=origin + nullptr, // frame other + _capture_origin, // origin capture + nullptr, // feature other + nullptr, // landmark other + 0, // take all of first state block + -1, // take all of first state block + 0, // take all of first second block + -1, // take all of first second block + shared_from_this(), // this processor + params_->apply_loss_function); // loss function + + if( std::static_pointer_cast<SensorImu2d>(getSensor())->isGravityOrthogonal() ) + FactorBase::emplace<FactorImu2d>(ftr_imu, + ftr_imu, + cap_imu, + shared_from_this(), + params_->apply_loss_function); + else + FactorBase::emplace<FactorImu2dWithGravity>(ftr_imu, + ftr_imu, + cap_imu, + shared_from_this(), + params_->apply_loss_function); + } + } +} - void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data, - const Eigen::MatrixXd& _data_cov, - const Eigen::VectorXd& _calib, - const double _dt, - Eigen::VectorXd& _delta, - Eigen::MatrixXd& _delta_cov, - Eigen::MatrixXd& _jac_delta_calib) const - { - using namespace Eigen; - assert(_data.size() == data_size_ && "Wrong data size!"); - Vector3d data_2d; - data_2d << _data(0), _data(1), _data(5); - Matrix3d data_cov_2d; - data_cov_2d << _data_cov(0,0), _data_cov(0,1), _data_cov(0,5), - _data_cov(1,0), _data_cov(1,1), _data_cov(1,5), - _data_cov(5,0), _data_cov(5,1), _data_cov(5,5); - - - - Matrix<double, 5, 3> jac_delta_data; - /* - * We have the following computing pipeline: - * Input: data, calib, dt - * Output: delta, delta_cov, jac_calib - * - * We do: - * body = data - calib (measurement - bias) - * delta = body2delta(body, dt) --> jac_body - * jac_data = jac_body - * jac_calib = - jac_body - * delta_cov <-- propagate data_cov using jac_data - * - * where body2delta(.) produces a delta from body=(a,w) as follows: - * dp = P * a * dt^2 - * dth = exp(w * dt) = w * dt - * dv = Q * a * dt - * where P and Q are defined in imu2d::exp(), and also a jacobian J^delta_data - */ - - //create delta - imu2d::body2delta(data_2d - _calib, _dt, _delta, jac_delta_data); - - // compute delta_cov - _delta_cov = jac_delta_data * data_cov_2d * jac_delta_data.transpose(); - - // compute jacobian_calib - _jac_delta_calib = - jac_delta_data; - } +void ProcessorImu2d::computeCurrentDelta(const Eigen::VectorXd& _data, + const Eigen::MatrixXd& _data_cov, + const Eigen::VectorXd& _calib, + const double _dt, + Eigen::VectorXd& _delta, + Eigen::MatrixXd& _delta_cov, + Eigen::MatrixXd& _jac_delta_calib) const +{ + using namespace Eigen; + assert(_data.size() == data_size_ && "Wrong data size!"); + Vector3d data_2d; + data_2d << _data(0), _data(1), _data(5); + Matrix3d data_cov_2d; + data_cov_2d << _data_cov(0,0), _data_cov(0,1), _data_cov(0,5), + _data_cov(1,0), _data_cov(1,1), _data_cov(1,5), + _data_cov(5,0), _data_cov(5,1), _data_cov(5,5); + + + + Matrix<double, 5, 3> jac_delta_data; + /* + * We have the following computing pipeline: + * Input: data, calib, dt + * Output: delta, delta_cov, jac_calib + * + * We do: + * body = data - calib (measurement - bias) + * delta = body2delta(body, dt) --> jac_body + * jac_data = jac_body + * jac_calib = - jac_body + * delta_cov <-- propagate data_cov using jac_data + * + * where body2delta(.) produces a delta from body=(a,w) as follows: + * dp = P * a * dt^2 + * dth = exp(w * dt) = w * dt + * dv = Q * a * dt + * where P and Q are defined in imu2d::exp(), and also a jacobian J^delta_data + */ + + //create delta + imu2d::body2delta(data_2d - _calib, _dt, _delta, jac_delta_data); + + // compute delta_cov + _delta_cov = jac_delta_data * data_cov_2d * jac_delta_data.transpose(); + + // compute jacobian_calib + _jac_delta_calib = - jac_delta_data; +} - void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, - const Eigen::VectorXd& _delta, - const double _dt, - Eigen::VectorXd& _delta_preint_plus_delta) const - { - /* MATHS: - * Simple composition, - * (D o d)p = Dp + Dth*dp + Dv*dt - * (D o d)th = Dth+dth - * (D o d)v = Dv + Dth*dv - * where Dth*dp and Dth*dv are rotations and not scalar products. The version with jacobians is not used here. - */ - _delta_preint_plus_delta = imu2d::compose(_delta_preint, _delta, _dt); - } +void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta, + const double _dt, + Eigen::VectorXd& _delta_preint_plus_delta) const +{ + /* MATHS: + * Simple composition, + * (D o d)p = Dp + Dth*dp + Dv*dt + * (D o d)th = Dth+dth + * (D o d)v = Dv + Dth*dv + * where Dth*dp and Dth*dv are rotations and not scalar products. The version with jacobians is not used here. + */ + _delta_preint_plus_delta = imu2d::compose(_delta_preint, _delta, _dt); +} - void ProcessorImu2d::statePlusDelta(const VectorComposite& _x, - const Eigen::VectorXd& _delta, - const double _dt, - VectorComposite& _x_plus_delta) const - { - assert(_x.has("POV") && "Any key missing in _x"); - assert(_delta.size() == 5 && "Wrong _delta vector size"); - assert(_dt >= 0 && "Time interval _dt is negative!"); - - const auto& delta = VectorComposite("POV", _delta, {2,1,2}); - /* - * MATHS: - * - * In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used - */ - if( std::static_pointer_cast<const SensorImu2d>(getSensor())->isGravityOrthogonal() ) - _x_plus_delta = imu2d::composeOverState(_x, delta, _dt); - else - _x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G")); +void ProcessorImu2d::statePlusDelta(const VectorComposite& _x, + const Eigen::VectorXd& _delta, + const double _dt, + VectorComposite& _x_plus_delta) const +{ + assert(_x.has("POV") && "Any key missing in _x"); + assert(_delta.size() == 5 && "Wrong _delta vector size"); + assert(_dt >= 0 && "Time interval _dt is negative!"); + + const auto& delta = VectorComposite("POV", _delta, {2,1,2}); + /* + * MATHS: + * + * In the absence of gravity (2D case) it's the same as deltaPlusDelta(), so the same compose() function is used + */ + if( std::static_pointer_cast<const SensorImu2d>(getSensor())->isGravityOrthogonal() ) + _x_plus_delta = imu2d::composeOverState(_x, delta, _dt); + else + _x_plus_delta = imu2d::composeOverStateWithGravity(_x, delta, _dt, getSensor()->getState("G")); - } +} - void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, - const Eigen::VectorXd& _delta, - const double _dt, - Eigen::VectorXd& _delta_preint_plus_delta, - Eigen::MatrixXd& _jacobian_delta_preint, - Eigen::MatrixXd& _jacobian_delta) const - { - /* - * Expression of the delta integration step, D' = D (+) d: - * - * Dp' = Dp + Dv*dt + Dq*dp - * Dv' = Dv + Dq*dv - * Dq' = Dq * dq - * - * Jacobians for covariance propagation. - * - * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as: - * - * D'_D = [ I DR*skew(1)*dp I*dt - * 0 1 0 - * 0 DR*skew(1)*dv I ] - * - * b. With respect to delta, gives _jacobian_delta = D'_d as: - * - * D'_d = [ DR 0 0 - * 0 1 0 - * 0 0 DR ] - * - * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion. - */ - imu2d::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu2d_tools - } +void ProcessorImu2d::deltaPlusDelta(const Eigen::VectorXd& _delta_preint, + const Eigen::VectorXd& _delta, + const double _dt, + Eigen::VectorXd& _delta_preint_plus_delta, + Eigen::MatrixXd& _jacobian_delta_preint, + Eigen::MatrixXd& _jacobian_delta) const +{ + /* + * Expression of the delta integration step, D' = D (+) d: + * + * Dp' = Dp + Dv*dt + Dq*dp + * Dv' = Dv + Dq*dv + * Dq' = Dq * dq + * + * Jacobians for covariance propagation. + * + * a. With respect to Delta, gives _jacobian_delta_preint = D'_D as: + * + * D'_D = [ I DR*skew(1)*dp I*dt + * 0 1 0 + * 0 DR*skew(1)*dv I ] + * + * b. With respect to delta, gives _jacobian_delta = D'_d as: + * + * D'_d = [ DR 0 0 + * 0 1 0 + * 0 0 DR ] + * + * Note: covariance propagation, i.e., P+ = D_D * P * D_D' + D_d * M * D_d', is done in ProcessorMotion. + */ + imu2d::compose(_delta_preint, _delta, _dt, _delta_preint_plus_delta, _jacobian_delta_preint, _jacobian_delta); // jacobians tested in imu2d_tools +} - Eigen::VectorXd ProcessorImu2d::correctDelta (const Eigen::VectorXd& delta_preint, - const Eigen::VectorXd& delta_step) const - { - return imu2d::plus(delta_preint, delta_step); - } +Eigen::VectorXd ProcessorImu2d::correctDelta (const Eigen::VectorXd& delta_preint, + const Eigen::VectorXd& delta_step) const +{ + return imu2d::plus(delta_preint, delta_step); +} } // namespace wolf diff --git a/test/gtest_factor_imu.cpp b/test/gtest_factor_imu.cpp index 5509a069b9d8ae5d3faeab4977db2763376d65b6..e10a29b62613ed8ad81433e85576041893a44315 100644 --- a/test/gtest_factor_imu.cpp +++ b/test/gtest_factor_imu.cpp @@ -19,12 +19,6 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -/** - * \file gtest_factor_imu.cpp - * - * Created on: Jan 01, 2017 - * \author: Dinesh Atchuthan - */ #include <core/ceres_wrapper/solver_ceres.h> #include <core/utils/utils_gtest.h> @@ -88,8 +82,8 @@ class FactorImu_biasTest_Static_NullBias : public testing::Test solver->getSolverOptions().max_num_iterations = 1e4; // SENSOR + PROCESSOR Imu - SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", "Main Imu", (Vector7d()<<0,0,0,0,0,0,1).finished(), wolf_root + "/test/yaml/sensor_imu.yaml"); - processor_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml"); + SensorBasePtr sen0_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root}); + processor_ = problem->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root}); sen_imu = static_pointer_cast<SensorImu>(sen0_ptr); processor_imu = static_pointer_cast<ProcessorImu>(processor_); diff --git a/test/gtest_factor_imu2d.cpp b/test/gtest_factor_imu2d.cpp index 8409d49f4e8835dc1627615545e2e5ddffb35f79..1c7f90950bc03d7b1c6c15b9bdc2a3ade6c7a266 100644 --- a/test/gtest_factor_imu2d.cpp +++ b/test/gtest_factor_imu2d.cpp @@ -68,11 +68,11 @@ class FactorImu2d_test : public testing::Test solver->getSolverOptions().gradient_tolerance = 1e-8; // Two frames - frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero()); - frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero()); + frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero()); + frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero()); // Imu2d sensor - sensor = problem_ptr->installSensor("SensorImu", "imu odometer", wolf_root + "/test/yaml/sensor_imu2d.yaml"); + sensor = problem_ptr->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root}); //capture from frm1 to frm0 cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr); diff --git a/test/gtest_factor_imu2d_with_gravity.cpp b/test/gtest_factor_imu2d_with_gravity.cpp index 3fab496313fccf2215d35dfa4c4aa32d9ba3f0e4..e0c72559fa4ecd0b4fd6a0e0d0c99cadf6571134 100644 --- a/test/gtest_factor_imu2d_with_gravity.cpp +++ b/test/gtest_factor_imu2d_with_gravity.cpp @@ -68,11 +68,11 @@ class FactorImu2dwithGravity_test : public testing::Test solver->getSolverOptions().gradient_tolerance = 1e-9; // Two frames - frm0 = problem_ptr->emplaceFrame(TimeStamp(0), Vector5d::Zero()); - frm1 = problem_ptr->emplaceFrame(TimeStamp(1), Vector5d::Zero()); + frm0 = problem_ptr->emplaceFrame(TimeStamp(0), "POV", Vector5d::Zero()); + frm1 = problem_ptr->emplaceFrame(TimeStamp(1), "POV", Vector5d::Zero()); // Imu2d sensor - sensor = problem_ptr->installSensor("SensorImu", "imu odometer", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml"); + sensor = problem_ptr->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml", {wolf_root}); //capture from frm1 to frm0 cap0 = CaptureBase::emplace<CaptureImu>(frm0, 0, sensor, Vector6d::Zero(), data_cov, Vector3d::Zero(), nullptr); diff --git a/test/gtest_feature_imu.cpp b/test/gtest_feature_imu.cpp index b803b67c6528c5ddde87f5194e33027ad5c83a82..567a3a37baa58abd613c8b025eb66fb6678eac37 100644 --- a/test/gtest_feature_imu.cpp +++ b/test/gtest_feature_imu.cpp @@ -49,7 +49,6 @@ class FeatureImu_test : public testing::Test FrameBasePtr last_frame; FrameBasePtr origin_frame; MatrixXd dD_db_jacobians; - ProcessorBasePtr processor_ptr_; ProcessorMotionPtr processor_motion_ptr_; //a new of this will be created for each new test @@ -59,17 +58,16 @@ class FeatureImu_test : public testing::Test // Wolf problem problem = Problem::create("POV", 3); SpecSensorComposite imu_priors{{'P',SpecStateSensor("P",Vector3d::Zero())}, - {'O',SpecStateSensor("O",(Vector4d() << 0,0,0,1).finished())}, - {'I',SpecStateSensor("StateBlock",Vector6d::Zero(),"factor",Vector6d::Ones(),true)}}; + {'O',SpecStateSensor("O",(Vector4d() << 0,0,0,1).finished())}, + {'I',SpecStateSensor("StateBlock",Vector6d::Zero(),"factor",Vector6d::Ones(),true)}}; ParamsSensorImuPtr sen_imu_params = make_shared<ParamsSensorImu>(); - SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", sen_imu_params, imu_priors); + SensorBasePtr sensor_ptr = SensorBase::emplace<SensorImu3d>(problem->getHardware(), sen_imu_params, imu_priors); ParamsProcessorImuPtr prc_imu_params = make_shared<ParamsProcessorImu>(); prc_imu_params->max_time_span = 0.5; prc_imu_params->max_buff_length = 10; prc_imu_params->dist_traveled = 5; prc_imu_params->angle_turned = 0.5; - processor_ptr_ = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params); - processor_motion_ptr_ = static_pointer_cast<ProcessorMotion>(processor_ptr_); + processor_motion_ptr_ = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, prc_imu_params); // Time and data variables TimeStamp t; @@ -78,20 +76,23 @@ class FeatureImu_test : public testing::Test t.set(0); // Set the origin - VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - origin_frame = problem->setPriorFactor(x0, s0, t); + // VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // origin_frame = problem->setPriorFactor(x0, s0, t); + SpecComposite problem_prior{{'P', SpecState("StatePoint3d", Vector3d::Zero(), "factor", Vector3d(1,1,1))}, + {'O', SpecState("StateQuaternion",Quaterniond::Identity().coeffs(), "factor", Vector3d(1,1,1))}, + {'V', SpecState("StateVector3d", Vector3d::Zero(), "factor", Vector3d(1,1,1))}}; + origin_frame = problem->setPrior(problem_prior, t); processor_motion_ptr_->setOrigin(origin_frame); // Emplace one capture to store the Imu data arriving from (sensor / callback / file / etc.) // give the capture a big covariance, otherwise it will be so small that it won't pass following assertions - imu_ptr = static_pointer_cast<CaptureImu>( - CaptureBase::emplace<CaptureImu>(origin_frame, - t, - sensor_ptr, - data_, - Matrix6d::Identity(), - Vector6d::Zero().eval()) ); + imu_ptr = CaptureBase::emplace<CaptureImu>(origin_frame, + t, + sensor_ptr, + data_, + Matrix6d::Identity(), + Vector6d::Zero().eval()); //process data data_ << 2, 0, 9.8, 0, 0, 0; @@ -106,8 +107,8 @@ class FeatureImu_test : public testing::Test //emplace Frame ts = problem->getTimeStamp(); - state_vec = problem->getState().vector(problem->getFrameStructure()); - last_frame = problem->emplaceFrame(ts, state_vec); + state_vec = problem->getState().vector("POV"); + last_frame = problem->emplaceFrame(ts, "POV", state_vec); //emplace a feature delta_preint = processor_motion_ptr_->getMotion().delta_integr_; diff --git a/test/gtest_processor_imu.cpp b/test/gtest_processor_imu.cpp index a2e9bdaf855412a0ba54ad2b10c32d27615db8a9..8cd96d2c68bcb76d73e6c9d9f55a67065bc6ee89 100644 --- a/test/gtest_processor_imu.cpp +++ b/test/gtest_processor_imu.cpp @@ -40,6 +40,19 @@ using std::static_pointer_cast; std::string wolf_root = _WOLF_IMU_ROOT_DIR; +SpecComposite problem_prior_{{'P', SpecState("StatePoint3d", + Vector3d::Zero(), + "factor", + Vector3d::Ones())}, + {'O', SpecState("StateQuaternion", + Quaterniond::Identity().coeffs(), + "factor", + Vector3d::Ones())}, + {'V', SpecState("StateVector3d", + Vector3d::Zero(), + "factor", + Vector3d::Ones())}}; + class ProcessorImut : public testing::Test { @@ -62,8 +75,8 @@ class ProcessorImut : public testing::Test { // Wolf problem problem = Problem::create("POV", 3); - sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu.yaml"); - ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml"); + sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root}); + ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root}); processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr); // Time and data variables @@ -108,18 +121,18 @@ TEST(ProcessorImu_constructors, ALL) ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned); //Factory constructor with pointers - std::string wolf_root = _WOLF_IMU_ROOT_DIR; ProblemPtr problem = Problem::create("POV", 3); - SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu.yaml"); + SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root}); ParamsProcessorImuPtr params_default = std::make_shared<ParamsProcessorImu>(); - ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, params_default); + ProcessorBasePtr processor_ptr = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, params_default); + //ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, params_default); ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan(), params_default->max_time_span); ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length); ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getDistTraveled(), params_default->dist_traveled); ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getAngleTurned(), params_default->angle_turned); //Factory constructor with yaml - processor_ptr = problem->installProcessor("ProcessorImu", "Sec Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml"); + processor_ptr = problem->installProcessor("ProcessorImu", "Main Imu", wolf_root + "/test/yaml/processor_imu.yaml", {wolf_root}); ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxTimeSpan(), 2.0); ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getMaxBuffLength(), 20000); ASSERT_EQ(std::static_pointer_cast<ProcessorImu>(processor_ptr)->getDistTraveled(), 2.0); @@ -130,20 +143,19 @@ TEST(ProcessorImu, voteForKeyFrame) { // Wolf problem ProblemPtr problem = Problem::create("POV", 3); - SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu.yaml"); + SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu.yaml", {wolf_root}); ParamsProcessorImuPtr prc_imu_params = std::make_shared<ParamsProcessorImu>(); prc_imu_params->max_time_span = 1; prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass prc_imu_params->dist_traveled = 1000000000; prc_imu_params->angle_turned = 1000000000; prc_imu_params->voting_active = true; - ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params); + auto processor_ptr = ProcessorBase::emplace<ProcessorImu>(sensor_ptr, prc_imu_params); + //ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu", "Imu pre-integrator", sensor_ptr, prc_imu_params); //setting origin TimeStamp t(0); - VectorComposite x0("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - VectorComposite s0("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0, s0, t); + problem->setPrior(problem_prior_, t); //data variable and covariance matrix // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions @@ -205,13 +217,14 @@ TEST(ProcessorImu, voteForKeyFrame) TEST_F(ProcessorImut, acc_x) { t.set(0); // clock in 0,1 ms ticks - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -227,7 +240,7 @@ TEST_F(ProcessorImut, acc_x) x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2 Vector6d b; b << 0,0,0, 0,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL); } @@ -238,11 +251,13 @@ TEST_F(ProcessorImut, acc_y) // difference hier is that we integrate over 1ms periods t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -258,7 +273,7 @@ TEST_F(ProcessorImut, acc_y) x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02 Vector6d b; b<< 0,0,0, 0,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL); @@ -272,7 +287,7 @@ TEST_F(ProcessorImut, acc_y) // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20 x << 0,10,0, 0,0,0,1, 0,20,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS); } @@ -280,11 +295,13 @@ TEST_F(ProcessorImut, acc_y) TEST_F(ProcessorImut, acc_z) { t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -300,7 +317,7 @@ TEST_F(ProcessorImut, acc_z) x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2 Vector6d b; b<< 0,0,0, 0,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV").head(10) , x, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS_SMALL); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS_SMALL); @@ -314,7 +331,7 @@ TEST_F(ProcessorImut, acc_z) // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10 x << 0,0,25, 0,0,0,1, 0,0,10; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getSensorIntrinsic()->getState() , b, Constants::EPS); ASSERT_MATRIX_APPROX(processor_motion->getLast()->getCalibrationPreint() , b, Constants::EPS); } @@ -322,11 +339,13 @@ TEST_F(ProcessorImut, acc_z) TEST_F(ProcessorImut, check_Covariance) { t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -348,11 +367,13 @@ TEST_F(ProcessorImut, gyro_x) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -371,7 +392,7 @@ TEST_F(ProcessorImut, gyro_x) VectorXd x(10); x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL); //do so for 5s const unsigned int iter = 1000; //how many ms @@ -380,7 +401,7 @@ TEST_F(ProcessorImut, gyro_x) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()); cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 @@ -389,7 +410,7 @@ TEST_F(ProcessorImut, gyro_x) } x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } TEST_F(ProcessorImut, gyro_x_biasedAbx) @@ -407,9 +428,11 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx) MatrixXd P0(9,9); P0.setIdentity(); // init things - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -434,7 +457,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx) x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 VectorXd x_est(10); - x_est = problem->getState().vector(problem->getFrameStructure()).head(10); + x_est = problem->getState().vector("POV"); ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), Constants::EPS); @@ -445,7 +468,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()) + acc_bias; cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 @@ -454,7 +477,7 @@ TEST_F(ProcessorImut, gyro_x_biasedAbx) } x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x_true, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x_true, Constants::EPS); } @@ -463,11 +486,13 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy) double dt(0.001); t.set(0); // clock in 0,1 ms ticks double abx(0.002), aby(0.01); - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -492,7 +517,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy) VectorXd x(10); x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL);// << "expected state : " << problem->getState().vector(problem->getFrameStructure()).transpose() + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL);// << "expected state : " << problem->getState().vector(problem->getFrameStructure()).transpose() // << "\n estimated state : " << x.transpose(); //do so for 5s @@ -502,7 +527,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()) + acc_bias; cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 @@ -511,7 +536,7 @@ TEST_F(ProcessorImut, gyro_xy_biasedAbxy) } x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS);// << "estimated state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() << + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS);// << "estimated state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() << // "\n expected state is : \n" << x.transpose() << std::endl; } @@ -519,11 +544,13 @@ TEST_F(ProcessorImut, gyro_z) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -542,7 +569,7 @@ TEST_F(ProcessorImut, gyro_z) VectorXd x(10); x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL); //do so for 5s const unsigned int iter = 1000; //how many ms @@ -556,17 +583,19 @@ TEST_F(ProcessorImut, gyro_z) } x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } TEST_F(ProcessorImut, gyro_xyz) { t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d::Zero()}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -610,7 +639,7 @@ TEST_F(ProcessorImut, gyro_xyz) quat_comp = quat_comp * v2q(tmp_vec*dt); R0 = R0 * v2R(tmp_vec*dt); // use processorImu - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()); //gravity measured data.tail(3) = tmp_vec; @@ -637,28 +666,30 @@ TEST_F(ProcessorImut, gyro_xyz) VectorXd x(10); x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0; - Quaterniond result_quat(problem->getState().vector(problem->getFrameStructure()).data() + 3); + Quaterniond result_quat(problem->getState().vector("O").data()); //std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl; //check position part - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(3) , x.head(3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("P"), x.head(3), Constants::EPS); //check orientation part - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).segment(3,4) , x.segment(3,4) , Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("O"), x.segment(3,4) , Constants::EPS); //check velocity and bias parts - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).segment(7,3) , x.segment(7,3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("V"), x.segment(7,3), Constants::EPS); } TEST_F(ProcessorImut, gyro_z_ConstVelocity) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 2,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 2,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -677,7 +708,7 @@ TEST_F(ProcessorImut, gyro_z_ConstVelocity) VectorXd x(10); x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL); //do so for 1s const unsigned int iter = 1000; //how many ms @@ -691,18 +722,20 @@ TEST_F(ProcessorImut, gyro_z_ConstVelocity) } x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } TEST_F(ProcessorImut, gyro_x_ConstVelocity) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 2,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 2,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -722,7 +755,7 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity) // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002 x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL); //do so for 1s const unsigned int iter = 1000; //how many ms @@ -731,7 +764,7 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()); cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 @@ -740,18 +773,20 @@ TEST_F(ProcessorImut, gyro_x_ConstVelocity) } x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } TEST_F(ProcessorImut, gyro_xy_ConstVelocity) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 2,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 2,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -771,7 +806,7 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity) // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002 x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL); //do so for 1s const unsigned int iter = 1000; //how many ms @@ -780,7 +815,7 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()); cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 @@ -789,18 +824,20 @@ TEST_F(ProcessorImut, gyro_xy_ConstVelocity) } x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } TEST_F(ProcessorImut, gyro_y_ConstVelocity) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 2,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 2,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -820,7 +857,7 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity) // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002 x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS_SMALL); //do so for 1s const unsigned int iter = 1000; //how many ms @@ -829,7 +866,7 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()); cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 @@ -838,17 +875,19 @@ TEST_F(ProcessorImut, gyro_y_ConstVelocity) } x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } TEST_F(ProcessorImut, gyro_xyz_ConstVelocity) { t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 2,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 2,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(2,0,0)}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -892,7 +931,7 @@ TEST_F(ProcessorImut, gyro_xyz_ConstVelocity) quat_comp = quat_comp * v2q(tmp_vec*dt); R0 = R0 * v2R(tmp_vec*dt); // use processorImu - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()); //gravity measured data.tail(3) = tmp_vec; @@ -920,17 +959,17 @@ TEST_F(ProcessorImut, gyro_xyz_ConstVelocity) //rotation + translation due to initial velocity x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0; - Quaterniond result_quat(problem->getState().vector(problem->getFrameStructure()).data() + 3); + Quaterniond result_quat(problem->getState().vector("O").data()); //std::cout << "final orientation : " << q2v(result_quat).transpose() << std::endl; //check position part - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(3) , x.head(3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("P") , x.head(3), Constants::EPS); //check orientation part - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).segment(3,4) , x.segment(3,4) , Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("O") , x.segment(3,4) , Constants::EPS); //check velocity - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).segment(7,3) , x.segment(7,3), Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("V") , x.segment(7,3), Constants::EPS); } @@ -938,11 +977,13 @@ TEST_F(ProcessorImut, gyro_x_acc_x) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -964,7 +1005,7 @@ TEST_F(ProcessorImut, gyro_x_acc_x) // v = a*dt = 0.001 x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() << + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() << // "\n current x is : \n" << x.transpose() << std::endl; //do so for 1s @@ -974,7 +1015,7 @@ TEST_F(ProcessorImut, gyro_x_acc_x) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()) + (Vector3d()<<1,0,0).finished(); cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000 @@ -985,18 +1026,20 @@ TEST_F(ProcessorImut, gyro_x_acc_x) // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis // v = a*dt = 1 x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } TEST_F(ProcessorImut, gyro_y_acc_y) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -1018,7 +1061,7 @@ TEST_F(ProcessorImut, gyro_y_acc_y) // v = a*dt = 0.001 x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() << + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); // << "1. current state is : \n" << problem->getState().vector(problem->getFrameStructure()).transpose() << // "\n current x is : \n" << x.transpose() << std::endl; //do so for 1s @@ -1028,7 +1071,7 @@ TEST_F(ProcessorImut, gyro_y_acc_y) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()) + (Vector3d()<<0,1,0).finished(); cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000 @@ -1039,18 +1082,20 @@ TEST_F(ProcessorImut, gyro_y_acc_y) // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis // v = a*dt = 1 x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } TEST_F(ProcessorImut, gyro_z_acc_z) { double dt(0.001); t.set(0); // clock in 0,1 ms ticks - x0 << 0,0,0, 0,0,0,1, 0,0,0; - MatrixXd P0(9,9); P0.setIdentity(); - x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); - s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); - problem->setPriorFactor(x0c, s0c, t); + // x0 << 0,0,0, 0,0,0,1, 0,0,0; + // MatrixXd P0(9,9); P0.setIdentity(); + // x0c = VectorComposite("POV", {Vector3d::Zero(), Quaterniond::Identity().coeffs(), Vector3d(0,0,0)}); + // s0c = VectorComposite("POV", {Vector3d(1,1,1), Vector3d(1,1,1), Vector3d(1,1,1)}); + // problem->setPriorFactor(x0c, s0c, t); + problem->setPrior(problem_prior_, t); + // process this capture for joining prior KF (t=0) and set it as origin KF cap_imu_ptr->setTimeStamp(t); cap_imu_ptr->process(); @@ -1072,7 +1117,7 @@ TEST_F(ProcessorImut, gyro_z_acc_z) // v = a*dt = 0.001 x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001; - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); //do so for 1s const unsigned int iter = 1000; //how many ms @@ -1081,7 +1126,7 @@ TEST_F(ProcessorImut, gyro_z_acc_z) // quaternion composition quat_comp = quat_comp * v2q(data.tail(3)*dt); - Quaterniond rot(problem->getState().vector(problem->getFrameStructure()).data()+3); + Quaterniond rot(problem->getState().vector("O").data()); data.head(3) = rot.conjugate() * (- gravity()) + (Vector3d()<<0,0,1).finished(); cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000 @@ -1092,8 +1137,8 @@ TEST_F(ProcessorImut, gyro_z_acc_z) // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis // v = a*dt = 1 x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1; - WOLF_DEBUG(problem->getState().vector(problem->getFrameStructure()).head(10).transpose()); - ASSERT_MATRIX_APPROX(problem->getState().vector(problem->getFrameStructure()).head(10) , x, Constants::EPS); + WOLF_DEBUG(problem->getState().vector("POV").transpose()); + ASSERT_MATRIX_APPROX(problem->getState().vector("POV") , x, Constants::EPS); } int main(int argc, char **argv) diff --git a/test/gtest_processor_imu2d.cpp b/test/gtest_processor_imu2d.cpp index ef7224c7d59b2f5a7bf2b86799ba6f9cd1e23be6..a00b943697c35b13129e1d14294022e326d4b780 100644 --- a/test/gtest_processor_imu2d.cpp +++ b/test/gtest_processor_imu2d.cpp @@ -19,12 +19,6 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -/** - * \file gtest_processor_imu2d.cpp - * - * Created on: Nov 24, 2020 - * \author: igeer - */ #include "imu/internal/config.h" #include "imu/capture/capture_imu.h" @@ -66,8 +60,8 @@ class ProcessorImu2dTest : public testing::Test { // Wolf problem problem = Problem::create("POV", 2); - sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu2d.yaml"); - ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml"); + sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root}); + ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml", {wolf_root}); processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr); // Time and data variables @@ -102,17 +96,18 @@ TEST(ProcessorImu2d_constructors, ALL) std::string wolf_root = _WOLF_IMU_ROOT_DIR; ProblemPtr problem = Problem::create("POV", 2); std::cout << "creating sensor_ptr" << std::endl; - SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu2d.yaml"); + SensorBasePtr sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d.yaml", {wolf_root}); std::cout << "created sensor_ptr" << std::endl; ParamsProcessorImu2dPtr params_default = std::make_shared<ParamsProcessorImu2d>(); - ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", sensor_ptr, params_default); + // ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", sensor_ptr, params_default); + ProcessorBasePtr processor_ptr = ProcessorBase::emplace<ProcessorImu2d>(sensor_ptr, params_default); ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(), params_default->max_time_span); ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), params_default->max_buff_length); ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(), params_default->dist_traveled); ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getAngleTurned(), params_default->angle_turned); //Factory constructor with yaml - processor_ptr = problem->installProcessor("ProcessorImu2d", "Sec Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml"); + processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml", {wolf_root}); ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxTimeSpan(), 2.0); ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getMaxBuffLength(), 20000); ASSERT_EQ(std::static_pointer_cast<ProcessorImu2d>(processor_ptr)->getDistTraveled(), 2.0); @@ -122,10 +117,14 @@ TEST(ProcessorImu2d_constructors, ALL) TEST_F(ProcessorImu2dTest, Prior) { // Set the origin - x0c['P'] = Vector2d(1,2); - x0c['O'] = Vector1d(0); - x0c['V'] = Vector2d(4,5); - auto KF0 = problem->setPriorFix(x0c, t0); + // x0c['P'] = Vector2d(1,2); + // x0c['O'] = Vector1d(0); + // x0c['V'] = Vector2d(4,5); + // auto KF0 = problem->setPriorFix(x0c, t0); + SpecComposite problem_prior{{'P', SpecState("StatePoint2d", Vector2d(1,2))}, + {'O', SpecState("StateAngle", Vector1d::Zero())}, + {'V', SpecState("StateVector2d", Vector2d(4,5))}}; + auto KF0 = problem->setPrior(problem_prior, t0); processor_motion->setOrigin(KF0); //WOLF_DEBUG("x0 =", x0c); //WOLF_DEBUG("KF0 state =", problem->getTrajectory()->getFrameMap().at(t0)->getState("POV")); @@ -133,126 +132,132 @@ TEST_F(ProcessorImu2dTest, Prior) TEST_F(ProcessorImu2dTest, MRUA) { - data << 1, 0, 0, 0, 0, 0; - data_cov *= 1e-3; - // Set the origin - x0c['P'] = Vector2d(1,2); - x0c['O'] = Vector1d(0); - x0c['V'] = Vector2d(4,5); - WOLF_INFO("1"); - auto KF0 = problem->setPriorFix(x0c, t0); - WOLF_INFO("2"); - processor_motion->setOrigin(KF0); - WOLF_INFO("3"); - - problem->print(4,1,1,1); - WOLF_INFO("Current State: ", problem->getState()); - for(t = t0; t <= t0 + 1.0 + dt/2; t+=dt){ - WOLF_INFO("4"); - C->setTimeStamp(t); - WOLF_INFO("5"); - C->setData(data); - WOLF_INFO("6"); - C->setDataCovariance(data_cov); - WOLF_INFO("7"); - C->process(); - WOLF_INFO("8"); - WOLF_INFO("Current State: ", problem->getState()['P'].transpose()); - WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose()); - ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9); - } + data << 1, 0, 0, 0, 0, 0; + data_cov *= 1e-3; + // Set the origin + // x0c['P'] = Vector2d(1,2); + // x0c['O'] = Vector1d(0); + // x0c['V'] = Vector2d(4,5); + // auto KF0 = problem->setPriorFix(x0c, t0); + SpecComposite problem_prior{{'P', SpecState("StatePoint2d", Vector2d(1,2))}, + {'O', SpecState("StateAngle", Vector1d::Zero())}, + {'V', SpecState("StateVector2d", Vector2d(4,5))}}; + auto KF0 = problem->setPrior(problem_prior, t0); + processor_motion->setOrigin(KF0); + + problem->print(4,1,1,1); + WOLF_INFO("Current State: ", problem->getState()); + for(t = t0; t <= t0 + 1.0 + dt/2; t+=dt){ + C->setTimeStamp(t); + C->setData(data); + C->setDataCovariance(data_cov); + C->process(); + WOLF_INFO("Current State: ", problem->getState()['P'].transpose()); + WOLF_INFO((x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2)).transpose()); + ASSERT_MATRIX_APPROX(x0c['P'] + x0c['V']*(t-t0) + 0.5*data.head(2)*std::pow(t-t0, 2), problem->getState()['P'], 1e-9); + } } TEST_F(ProcessorImu2dTest, parabola) { - double v0 = 10; - double a = 1; - - Vector2d pos; - // Set the origin - x0c['P'] = Vector2d(0, 0); - x0c['O'] = Vector1d(0); - x0c['V'] = Vector2d(v0, 0); - - data_cov *= 1e-3; - auto KF0 = problem->setPriorFix(x0c, t0); - processor_motion->setOrigin(KF0); - - for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ - C->setTimeStamp(t); - data << 0, a, 0, 0, 0, 0; - C->setData(data); - C->setDataCovariance(data_cov); - C->process(); - //WOLF_DEBUG("Current State: ", problem->getState()); - pos << v0*(t-t0), - 0.5*a*std::pow(t-t0, 2); - //WOLF_DEBUG("Calculated State: ", pos.transpose()); - EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); - EXPECT_MATRIX_APPROX(Vector2d(v0, a*(t-t0)), problem->getState()['V'], 1e-9); - } + double v0 = 10; + double a = 1; + data_cov *= 1e-3; + + Vector2d pos; + // Set the origin + // x0c['P'] = Vector2d(0, 0); + // x0c['O'] = Vector1d(0); + // x0c['V'] = Vector2d(v0, 0); + // auto KF0 = problem->setPriorFix(x0c, t0); + SpecComposite problem_prior{{'P', SpecState("StatePoint2d", Vector2d(1,2))}, + {'O', SpecState("StateAngle", Vector1d::Zero())}, + {'V', SpecState("StateVector2d", Vector2d(4,5))}}; + auto KF0 = problem->setPrior(problem_prior, t0); + processor_motion->setOrigin(KF0); + + for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ + C->setTimeStamp(t); + data << 0, a, 0, 0, 0, 0; + C->setData(data); + C->setDataCovariance(data_cov); + C->process(); + //WOLF_DEBUG("Current State: ", problem->getState()); + pos << v0*(t-t0), + 0.5*a*std::pow(t-t0, 2); + //WOLF_DEBUG("Calculated State: ", pos.transpose()); + EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); + EXPECT_MATRIX_APPROX(Vector2d(v0, a*(t-t0)), problem->getState()['V'], 1e-9); + } } TEST_F(ProcessorImu2dTest, parabola_deluxe) { - Vector2d v0(13, 56); - Vector2d a(1, 2); - - Vector2d pos; - // Set the origin - x0c['P'] = Vector2d(0, 0); - x0c['O'] = Vector1d(0); - x0c['V'] = v0; - - data_cov *= 1e-3; - auto KF0 = problem->setPriorFix(x0c, t0); - processor_motion->setOrigin(KF0); - - for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ - C->setTimeStamp(t); - data << a(0), a(1), 0, 0, 0, 0; - C->setData(data); - C->setDataCovariance(data_cov); - C->process(); - //WOLF_DEBUG("Current State: ", problem->getState()); - pos = v0*(t-t0) + 0.5*a*std::pow(t-t0, 2); - //WOLF_DEBUG("Calculated State: ", pos.transpose()); - EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); - EXPECT_MATRIX_APPROX(v0 + a*(t-t0), problem->getState()['V'], 1e-9); - } + Vector2d v0(13, 56); + Vector2d a(1, 2); + data_cov *= 1e-3; + + Vector2d pos; + // Set the origin + // x0c['P'] = Vector2d(0, 0); + // x0c['O'] = Vector1d(0); + // x0c['V'] = v0; + // auto KF0 = problem->setPriorFix(x0c, t0); + SpecComposite problem_prior{{'P', SpecState("StatePoint2d", Vector2d(1,2))}, + {'O', SpecState("StateAngle", Vector1d::Zero())}, + {'V', SpecState("StateVector2d", Vector2d(4,5))}}; + auto KF0 = problem->setPrior(problem_prior, t0); + processor_motion->setOrigin(KF0); + + for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ + C->setTimeStamp(t); + data << a(0), a(1), 0, 0, 0, 0; + C->setData(data); + C->setDataCovariance(data_cov); + C->process(); + //WOLF_DEBUG("Current State: ", problem->getState()); + pos = v0*(t-t0) + 0.5*a*std::pow(t-t0, 2); + //WOLF_DEBUG("Calculated State: ", pos.transpose()); + EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); + EXPECT_MATRIX_APPROX(v0 + a*(t-t0), problem->getState()['V'], 1e-9); + } } TEST_F(ProcessorImu2dTest, Circular_move) { - double pi = 3.14159265358993; - double r = 1; - double w = 1; - double alpha = pi/4.; - Vector2d pos; - // Set the origin - x0c['P'] = Vector2d(r, 0); - pos = x0c['P']; - x0c['O'] = Vector1d(alpha); - x0c['V'] = Vector2d(0, w*r); - - data_cov *= 1e-3; - //dt = 0.0001; - auto KF0 = problem->setPriorFix(x0c, t0); - processor_motion->setOrigin(KF0); - - WOLF_DEBUG("Current State: ", problem->getState()); - for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ - C->setTimeStamp(t); - data << -std::cos(alpha)*w*w*r, std::sin(alpha)*w*w*r, 0, 0, 0, w; - C->setData(data); - C->setDataCovariance(data_cov); - C->process(); - WOLF_DEBUG("Current State: ", problem->getState()); - pos << r*std::cos( w*(t-t0) ), - r*std::sin( w*(t-t0) ); - WOLF_DEBUG("Calculated State: ", pos.transpose()); - EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); - } + double pi = 3.14159265358993; + double r = 1; + double w = 1; + double alpha = pi/4.; + Vector2d pos(r, 0); + data_cov *= 1e-3; + //dt = 0.0001; + + // Set the origin + // x0c['P'] = pos; + // x0c['O'] = Vector1d(alpha); + // x0c['V'] = Vector2d(0, w*r); + //auto KF0 = problem->setPriorFix(x0c, t0); + SpecComposite problem_prior{{'P', SpecState("StatePoint2d", pos)}, + {'O', SpecState("StateAngle", Vector1d(alpha))}, + {'V', SpecState("StateVector2d", Vector2d(0, w*r))}}; + auto KF0 = problem->setPrior(problem_prior, t0); + + processor_motion->setOrigin(KF0); + + WOLF_DEBUG("Current State: ", problem->getState()); + for(t = t0+dt; t <= t0 + 0.5 + dt/2; t+=dt){ + C->setTimeStamp(t); + data << -std::cos(alpha)*w*w*r, std::sin(alpha)*w*w*r, 0, 0, 0, w; + C->setData(data); + C->setDataCovariance(data_cov); + C->process(); + WOLF_DEBUG("Current State: ", problem->getState()); + pos << r*std::cos( w*(t-t0) ), + r*std::sin( w*(t-t0) ); + WOLF_DEBUG("Calculated State: ", pos.transpose()); + EXPECT_MATRIX_APPROX(pos , problem->getState()['P'], 1e-9); + } } int main(int argc, char **argv) diff --git a/test/gtest_processor_imu2d_with_gravity.cpp b/test/gtest_processor_imu2d_with_gravity.cpp index d947eb48bc60f50094cf1df8cc3d958f6c1bfb9a..d09460d634b441a0436d1f6b953e574b763e867b 100644 --- a/test/gtest_processor_imu2d_with_gravity.cpp +++ b/test/gtest_processor_imu2d_with_gravity.cpp @@ -19,12 +19,6 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. // //--------LICENSE_END-------- -/** - * \file gtest_processor_imu2d.cpp - * - * Created on: Nov 24, 2020 - * \author: igeer - */ #include "imu/internal/config.h" #include "imu/capture/capture_imu.h" @@ -74,8 +68,8 @@ class ProcessorImu2dTest : public testing::Test // Wolf problem problem = Problem::create("POV", 2); - sensor_ptr = problem->installSensor("SensorImu", "Main Imu", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml"); - ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Imu pre-integrator", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml"); + sensor_ptr = problem->installSensor("SensorImu", wolf_root + "/test/yaml/sensor_imu2d_with_gravity.yaml", {wolf_root}); + ProcessorBasePtr processor_ptr = problem->installProcessor("ProcessorImu2d", "Main Imu", wolf_root + "/test/yaml/processor_imu2d.yaml", {wolf_root}); processor_motion = std::static_pointer_cast<ProcessorMotion>(processor_ptr); // Time and data variables @@ -97,10 +91,14 @@ TEST_F(ProcessorImu2dTest, MUA_Only_G) data << 0, 0, 0, 0, 0, 0; data_cov *= 1e-3; // Set the origin - x0c['P'] = Vector2d(1,2); - x0c['O'] = Vector1d(0); - x0c['V'] = Vector2d(4,5); - auto KF0 = problem->setPriorFix(x0c, t0); +// x0c['P'] = Vector2d(1,2); +// x0c['O'] = Vector1d(0); +// x0c['V'] = Vector2d(4,5); +// auto KF0 = problem->setPriorFix(x0c, t0); + SpecComposite problem_prior{{'P', SpecState("StatePoint2d", Vector2d(1,2))}, + {'O', SpecState("StateAngle", Vector1d::Zero())}, + {'V', SpecState("StateVector2d", Vector2d(4,5))}}; + auto KF0 = problem->setPrior(problem_prior, t0); processor_motion->setOrigin(KF0); // Set the gravity @@ -125,10 +123,14 @@ TEST_F(ProcessorImu2dTest, MUA) data << 1, 2, 0, 0, 0, 0; data_cov *= 1e-3; // Set the origin - x0c['P'] = Vector2d(1,2); - x0c['O'] = Vector1d(0); - x0c['V'] = Vector2d(4,5); - auto KF0 = problem->setPriorFix(x0c, t0); +// x0c['P'] = Vector2d(1,2); +// x0c['O'] = Vector1d(0); +// x0c['V'] = Vector2d(4,5); +// auto KF0 = problem->setPriorFix(x0c, t0); + SpecComposite problem_prior{{'P', SpecState("StatePoint2d", Vector2d(1,2))}, + {'O', SpecState("StateAngle", Vector1d::Zero())}, + {'V', SpecState("StateVector2d", Vector2d(4,5))}}; + auto KF0 = problem->setPrior(problem_prior, t0); processor_motion->setOrigin(KF0); // Set the gravity @@ -154,10 +156,14 @@ TEST_F(ProcessorImu2dTest, Spinning) data << 0, 0, 0, 0, 0, w; data_cov *= 1e-3; // Set the origin - x0c['P'] = Vector2d(1,2); - x0c['O'] = Vector1d(0); - x0c['V'] = Vector2d(4,5); - auto KF0 = problem->setPriorFix(x0c, t0); +// x0c['P'] = Vector2d(1,2); +// x0c['O'] = Vector1d(0); +// x0c['V'] = Vector2d(4,5); +// auto KF0 = problem->setPriorFix(x0c, t0); + SpecComposite problem_prior{{'P', SpecState("StatePoint2d", Vector2d(1,2))}, + {'O', SpecState("StateAngle", Vector1d::Zero())}, + {'V', SpecState("StateVector2d", Vector2d(4,5))}}; + auto KF0 = problem->setPrior(problem_prior, t0); processor_motion->setOrigin(KF0); // Set the gravity diff --git a/test/gtest_sensor_compass.cpp b/test/gtest_sensor_compass.cpp index 15e223cd84c513a660ee72fb3da6d0f6eef003d4..9a919f2f93c7f0d3afef0b2d31d6dd796a4edcee 100644 --- a/test/gtest_sensor_compass.cpp +++ b/test/gtest_sensor_compass.cpp @@ -26,12 +26,11 @@ #include "imu/sensor/sensor_compass.h" #include "core/sensor/factory_sensor.h" -// #include "core/math/covariance.h" - #include <cstdio> using namespace wolf; using namespace Eigen; +using namespace yaml_schema_cpp; std::string wolf_root = _WOLF_IMU_ROOT_DIR; @@ -66,28 +65,27 @@ class sensor_compass_test : public testing::Test void LoadYamlGroundtruth(int dim) { // load from yaml - ParserYaml parser = ParserYaml(wolf_root + (dim == 2 ? - "/test/yaml/sensor_compass_2d.yaml" : - "/test/yaml/sensor_compass_3d.yaml"), - true); - ParamsServer server = ParamsServer(parser.getParams()); - - p_state = server.getParam<VectorXd>("states/P/state"); - o_state = server.getParam<VectorXd>("states/O/state"); - i_state = server.getParam<VectorXd>("states/I/state"); - f_state = server.getParam<VectorXd>("states/F/state"); - - p_mode = server.getParam<std::string>("states/P/mode"); - o_mode = server.getParam<std::string>("states/O/mode"); - i_mode = server.getParam<std::string>("states/I/mode"); - f_mode = server.getParam<std::string>("states/F/mode"); - - p_dynamic = server.getParam<bool>("states/P/dynamic"); - o_dynamic = server.getParam<bool>("states/O/dynamic"); - i_dynamic = server.getParam<bool>("states/I/dynamic"); - f_dynamic = server.getParam<bool>("states/F/dynamic"); - - stdev_noise = server.getParam<double>("stdev_noise"); + YamlServer server({wolf_root}, wolf_root + (dim == 2 ? + "/test/yaml/sensor_compass_2d.yaml" : + "/test/yaml/sensor_compass_3d.yaml")); + auto node = server.getNode(); + + p_state = node["states"]["P"]["state"].as<VectorXd>(); + o_state = node["states"]["O"]["state"].as<VectorXd>(); + i_state = node["states"]["I"]["state"].as<VectorXd>(); + f_state = node["states"]["F"]["state"].as<VectorXd>(); + + p_mode = node["states"]["P"]["mode"].as<std::string>(); + o_mode = node["states"]["O"]["mode"].as<std::string>(); + i_mode = node["states"]["I"]["mode"].as<std::string>(); + f_mode = node["states"]["F"]["mode"].as<std::string>(); + + p_dynamic = node["states"]["P"]["dynamic"].as<bool>(); + o_dynamic = node["states"]["O"]["dynamic"].as<bool>(); + i_dynamic = node["states"]["I"]["dynamic"].as<bool>(); + f_dynamic = node["states"]["F"]["dynamic"].as<bool>(); + + stdev_noise = node["stdev_noise"].as<double>(); noise_cov = VectorXd::Constant(dim, stdev_noise*stdev_noise).asDiagonal(); } @@ -112,43 +110,6 @@ class sensor_compass_test : public testing::Test noise_cov = VectorXd::Constant(dim, stdev_noise*stdev_noise).asDiagonal(); } }; - -TEST_F(sensor_compass_test, constructor_priors_2d) -{ - RandomGroundtruth(2); - - auto params = std::make_shared<ParamsSensorCompass>(); - params->stdev_noise = stdev_noise; - - SpecSensorComposite priors{{'P',SpecStateSensor("P", p_state, p_mode, VectorXd(0), p_dynamic)}, - {'O',SpecStateSensor("O", o_state, o_mode, VectorXd(0), o_dynamic)}, - {'I',SpecStateSensor("StateBlock", i_state, i_mode, VectorXd(0), i_dynamic)}, - {'F',SpecStateSensor("StateBlock", f_state, f_mode, VectorXd(0), f_dynamic)}}; - - auto sen = std::make_shared<SensorCompass>("sensor_1", 2, params, priors); - - ASSERT_NE(sen, nullptr); - - ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix"); - ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix"); - - ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic); - - ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS); - - ASSERT_EQ(sen->getParams(), params); - ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS); - - ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); -} TEST_F(sensor_compass_test, constructor_priors_3d) { @@ -157,12 +118,12 @@ TEST_F(sensor_compass_test, constructor_priors_3d) auto params = std::make_shared<ParamsSensorCompass>(); params->stdev_noise = stdev_noise; - SpecComposite priors{{'P',SpecStateSensor("P", p_state, p_mode, VectorXd(0), p_dynamic)}, - {'O',SpecStateSensor("O", o_state, o_mode, VectorXd(0), o_dynamic)}, - {'I',SpecStateSensor("StateBlock", i_state, i_mode, VectorXd(0), i_dynamic)}, - {'F',SpecStateSensor("StateBlock", f_state, f_mode, VectorXd(0), f_dynamic)}}; + SpecSensorComposite priors{{'P',SpecStateSensor("StatePoint3d", p_state, p_mode, VectorXd(0), p_dynamic)}, + {'O',SpecStateSensor("StateQuaternion", o_state, o_mode, VectorXd(0), o_dynamic)}, + {'I',SpecStateSensor("StateBlock", i_state, i_mode, VectorXd(0), i_dynamic)}, + {'F',SpecStateSensor("StateBlock", f_state, f_mode, VectorXd(0), f_dynamic)}}; - auto sen = std::make_shared<SensorCompass>("sensor_1", 3, params, priors); + auto sen = std::make_shared<SensorCompass>(params, priors); ASSERT_NE(sen, nullptr); @@ -187,141 +148,15 @@ TEST_F(sensor_compass_test, constructor_priors_3d) ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); } -TEST_F(sensor_compass_test, constructor_server_2d) -{ - LoadYamlGroundtruth(2); - - ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_compass_2d.yaml", true); - ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1"); - - auto params = std::make_shared<ParamsSensorCompass>("sensor_1", server); - auto sen = std::make_shared<SensorCompass>("sensor_1", 2, params, server); - - ASSERT_NE(sen, nullptr); - - ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix"); - ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix"); - - ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic); - - ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS); - - ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS); - - ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); -} - -TEST_F(sensor_compass_test, constructor_server_3d) -{ - LoadYamlGroundtruth(3); - - ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_compass_3d.yaml", true); - ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1"); - - auto params = std::make_shared<ParamsSensorCompass>("sensor_1", server); - auto sen = std::make_shared<SensorCompass>("sensor_1", 3, params, server); - - ASSERT_NE(sen, nullptr); - - ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix"); - ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix"); - - ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic); - - ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS); - - ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS); - - ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); -} - -TEST_F(sensor_compass_test, factory_2d) -{ - LoadYamlGroundtruth(2); - - ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_compass_2d.yaml", true); - ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1"); - - auto sb = FactorySensor::create("SensorCompass","sensor_1", 2, server); - - SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb); - - ASSERT_NE(sen, nullptr); - - ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix"); - ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix"); - - ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic); - - ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS); - - ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS); - - ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); -} - -TEST_F(sensor_compass_test, factory_3d) +TEST_F(sensor_compass_test, factory) { LoadYamlGroundtruth(3); - ParserYaml parser = ParserYaml(wolf_root + "/test/yaml/sensor_compass_3d.yaml", true); - ParamsServer server = ParamsServer(parser.getParams(), "/sensor/sensor_1"); - - auto sb = FactorySensor::create("SensorCompass","sensor_1", 3, server); - - SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb); - - ASSERT_NE(sen, nullptr); - - ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix"); - ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix"); - - ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic); - - ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS); - - ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS); - - ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); -} - -TEST_F(sensor_compass_test, factory_yaml_2d) -{ - LoadYamlGroundtruth(2); + YamlServer server({wolf_root}, wolf_root + "/test/yaml/sensor_compass_3d.yaml"); + + ASSERT_TRUE(server.applySchema("SensorCompass")); - auto sb = FactorySensorYaml::create("SensorCompass","sensor_1", 2, wolf_root + "/test/yaml/sensor_compass_2d.yaml"); + auto sb = FactorySensor::create("SensorCompass",server.getNode()); SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb); @@ -347,49 +182,11 @@ TEST_F(sensor_compass_test, factory_yaml_2d) ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); } -TEST_F(sensor_compass_test, factory_yaml_3d) +TEST_F(sensor_compass_test, factory_yaml) { LoadYamlGroundtruth(3); - auto sb = FactorySensorYaml::create("SensorCompass","sensor_1", 3, wolf_root + "/test/yaml/sensor_compass_3d.yaml"); - - SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb); - - ASSERT_NE(sen, nullptr); - - ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix"); - ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix"); - - ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic); - - ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS); - - ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS); - - ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); -} - -TEST_F(sensor_compass_test, factory_priors_2d) -{ - RandomGroundtruth(2); - - auto params = std::make_shared<ParamsSensorCompass>(); - params->stdev_noise = stdev_noise; - - SpecComposite priors{{'P',SpecStateSensor("P", p_state, p_mode, VectorXd(0), p_dynamic)}, - {'O',SpecStateSensor("O", o_state, o_mode, VectorXd(0), o_dynamic)}, - {'I',SpecStateSensor("StateBlock", i_state, i_mode, VectorXd(0), i_dynamic)}, - {'F',SpecStateSensor("StateBlock", f_state, f_mode, VectorXd(0), f_dynamic)}}; - - auto sb = FactorySensorPriorSensorMap::create("SensorCompass","sensor_1", 2, params, priors); + auto sb = FactorySensorYaml::create("SensorCompass", "SensorCompass", wolf_root + "/test/yaml/sensor_compass_3d.yaml", {wolf_root}); SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb); @@ -410,46 +207,6 @@ TEST_F(sensor_compass_test, factory_priors_2d) ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS); ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS); - ASSERT_EQ(sen->getParams(), params); - ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS); - - ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS); -} - -TEST_F(sensor_compass_test, factory_priors_3d) -{ - RandomGroundtruth(3); - - auto params = std::make_shared<ParamsSensorCompass>(); - params->stdev_noise = stdev_noise; - - SpecComposite priors{{'P',SpecStateSensor("P", p_state, p_mode, VectorXd(0), p_dynamic)}, - {'O',SpecStateSensor("O", o_state, o_mode, VectorXd(0), o_dynamic)}, - {'I',SpecStateSensor("StateBlock", i_state, i_mode, VectorXd(0), i_dynamic)}, - {'F',SpecStateSensor("StateBlock", f_state, f_mode, VectorXd(0), f_dynamic)}}; - - auto sb = FactorySensorPriorSensorMap::create("SensorCompass","sensor_1", 3, params, priors); - - SensorCompassPtr sen = std::dynamic_pointer_cast<SensorCompass>(sb); - - ASSERT_NE(sen, nullptr); - - ASSERT_EQ(sen->getP()->isFixed(), p_mode == "fix"); - ASSERT_EQ(sen->getO()->isFixed(), o_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('I')->isFixed(), i_mode == "fix"); - ASSERT_EQ(sen->getStateBlockDynamic('F')->isFixed(), f_mode == "fix"); - - ASSERT_EQ(sen->isStateBlockDynamic('P'), p_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('O'), o_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('I'), i_dynamic); - ASSERT_EQ(sen->isStateBlockDynamic('F'), f_dynamic); - - ASSERT_MATRIX_APPROX(sen->getP()->getState(), p_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getO()->getState(), o_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('I')->getState(), i_state, Constants::EPS); - ASSERT_MATRIX_APPROX(sen->getStateBlockDynamic('F')->getState(), f_state, Constants::EPS); - - ASSERT_EQ(sen->getParams(), params); ASSERT_NEAR(sen->getParams()->stdev_noise, stdev_noise, Constants::EPS); ASSERT_MATRIX_APPROX(sen->computeNoiseCov(VectorXd(0)), noise_cov, Constants::EPS);