From c7ceee72c0abd9ce1707e15b02440334651d7ff2 Mon Sep 17 00:00:00 2001 From: Vladyslav Usenko Date: Sat, 17 Apr 2021 18:41:42 +0200 Subject: [PATCH] Switched to templated version of IMU types --- include/basalt/device/rs_t265.h | 2 +- include/basalt/utils/imu_types.h | 46 ++++++++++++--------- include/basalt/vi_estimator/ba_base.h | 11 ++--- include/basalt/vi_estimator/keypoint_vio.h | 24 ++++++----- include/basalt/vi_estimator/keypoint_vo.h | 8 ++-- include/basalt/vi_estimator/nfr_mapper.h | 7 ++-- include/basalt/vi_estimator/vio_estimator.h | 6 +-- src/device/rs_t265.cpp | 4 +- src/mapper_sim.cpp | 4 +- src/mapper_sim_naive.cpp | 7 ++-- src/rs_t265_record.cpp | 4 +- src/rs_t265_vio.cpp | 5 ++- src/vi_estimator/ba_base.cpp | 5 ++- src/vi_estimator/keypoint_vio.cpp | 21 +++++----- src/vi_estimator/keypoint_vio_linearize.cpp | 20 +++++---- src/vi_estimator/keypoint_vo.cpp | 20 +++++---- src/vi_estimator/nfr_mapper.cpp | 8 ++-- src/vio.cpp | 7 ++-- src/vio_sim.cpp | 7 ++-- test/src/test_nfr.cpp | 10 ++--- test/src/test_vio.cpp | 30 ++++++++------ thirdparty/CLI11 | 2 +- thirdparty/basalt-headers | 2 +- 23 files changed, 145 insertions(+), 115 deletions(-) diff --git a/include/basalt/device/rs_t265.h b/include/basalt/device/rs_t265.h index e550e27..4d70d67 100644 --- a/include/basalt/device/rs_t265.h +++ b/include/basalt/device/rs_t265.h @@ -92,7 +92,7 @@ class RsT265Device { OpticalFlowInput::Ptr last_img_data; tbb::concurrent_bounded_queue* image_data_queue = nullptr; - tbb::concurrent_bounded_queue* imu_data_queue = nullptr; + tbb::concurrent_bounded_queue::Ptr>* imu_data_queue = nullptr; tbb::concurrent_bounded_queue* pose_data_queue = nullptr; private: diff --git a/include/basalt/utils/imu_types.h b/include/basalt/utils/imu_types.h index 348db35..b179639 100644 --- a/include/basalt/utils/imu_types.h +++ b/include/basalt/utils/imu_types.h @@ -57,8 +57,13 @@ static const Eigen::Vector3d g(0, 0, -9.81); static const Eigen::Vector3d g_dir(0, 0, -1); } // namespace constants +template +struct PoseStateWithLin; + +template struct PoseVelBiasStateWithLin { - using VecN = PoseVelBiasState::VecN; + using Scalar = Scalar_; + using VecN = typename PoseVelBiasState::VecN; PoseVelBiasStateWithLin() { linearized = false; @@ -75,7 +80,7 @@ struct PoseVelBiasStateWithLin { state_current = state_linearized; } - PoseVelBiasStateWithLin(const PoseVelBiasState& other) + PoseVelBiasStateWithLin(const PoseVelBiasState& other) : linearized(false), state_linearized(other) { delta.setZero(); state_current = other; @@ -102,7 +107,7 @@ struct PoseVelBiasStateWithLin { } } - inline const PoseVelBiasState& getState() const { + inline const PoseVelBiasState& getState() const { if (!linearized) { return state_linearized; } else { @@ -110,7 +115,7 @@ struct PoseVelBiasStateWithLin { } } - inline const PoseVelBiasState& getStateLin() const { + inline const PoseVelBiasState& getStateLin() const { return state_linearized; } @@ -118,7 +123,7 @@ struct PoseVelBiasStateWithLin { inline const VecN& getDelta() const { return delta; } inline int64_t getT_ns() const { return state_linearized.t_ns; } - friend struct PoseStateWithLin; + friend struct PoseStateWithLin; inline void backup() { backup_delta = delta; @@ -136,10 +141,10 @@ struct PoseVelBiasStateWithLin { private: bool linearized; VecN delta; - PoseVelBiasState state_linearized, state_current; + PoseVelBiasState state_linearized, state_current; VecN backup_delta; - PoseVelBiasState backup_state_linearized, backup_state_current; + PoseVelBiasState backup_state_linearized, backup_state_current; friend class cereal::access; @@ -159,8 +164,11 @@ struct PoseVelBiasStateWithLin { } }; +template struct PoseStateWithLin { - using VecN = PoseState::VecN; + using Scalar = Scalar_; + using VecN = typename PoseState::VecN; + using SE3 = typename PoseState::SE3; PoseStateWithLin() { linearized = false; @@ -174,13 +182,13 @@ struct PoseStateWithLin { T_w_i_current = T_w_i; } - PoseStateWithLin(const PoseVelBiasStateWithLin& other) + PoseStateWithLin(const PoseVelBiasStateWithLin& other) : linearized(other.linearized), - delta(other.delta.head<6>()), + delta(other.delta.template head<6>()), pose_linearized(other.state_linearized.t_ns, other.state_linearized.T_w_i) { T_w_i_current = pose_linearized.T_w_i; - PoseState::incPose(delta, T_w_i_current); + PoseState::incPose(delta, T_w_i_current); } void setLinTrue() { @@ -203,11 +211,11 @@ struct PoseStateWithLin { inline void applyInc(const VecN& inc) { if (!linearized) { - PoseState::incPose(inc, pose_linearized.T_w_i); + PoseState::incPose(inc, pose_linearized.T_w_i); } else { delta += inc; T_w_i_current = pose_linearized.T_w_i; - PoseState::incPose(delta, T_w_i_current); + PoseState::incPose(delta, T_w_i_current); } } @@ -231,12 +239,12 @@ struct PoseStateWithLin { private: bool linearized; VecN delta; - PoseState pose_linearized; - Sophus::SE3d T_w_i_current; + PoseState pose_linearized; + SE3 T_w_i_current; VecN backup_delta; - PoseState backup_pose_linearized; - Sophus::SE3d backup_T_w_i_current; + PoseState backup_pose_linearized; + SE3 backup_T_w_i_current; friend class cereal::access; @@ -270,8 +278,8 @@ struct MargData { AbsOrderMap aom; Eigen::MatrixXd abs_H; Eigen::VectorXd abs_b; - Eigen::aligned_map frame_states; - Eigen::aligned_map frame_poses; + Eigen::aligned_map> frame_states; + Eigen::aligned_map> frame_poses; std::set kfs_all; std::set kfs_to_marg; bool use_imu; diff --git a/include/basalt/vi_estimator/ba_base.h b/include/basalt/vi_estimator/ba_base.h index 3b875af..89c2b62 100644 --- a/include/basalt/vi_estimator/ba_base.h +++ b/include/basalt/vi_estimator/ba_base.h @@ -257,8 +257,9 @@ class BundleAdjustmentBase { static Eigen::VectorXd checkNullspace( const Eigen::MatrixXd& marg_H, const Eigen::VectorXd& marg_b, const AbsOrderMap& marg_order, - const Eigen::aligned_map& frame_states, - const Eigen::aligned_map& frame_poses); + const Eigen::aligned_map>& + frame_states, + const Eigen::aligned_map>& frame_poses); /// Triangulates the point and returns homogenous representation. First 3 /// components - unit-length direction vector. Last component inverse @@ -404,7 +405,7 @@ class BundleAdjustmentBase { } // protected: - PoseStateWithLin getPoseStateWithLin(int64_t t_ns) const { + PoseStateWithLin getPoseStateWithLin(int64_t t_ns) const { auto it = frame_poses.find(t_ns); if (it != frame_poses.end()) return it->second; @@ -417,8 +418,8 @@ class BundleAdjustmentBase { return PoseStateWithLin(it2->second); } - Eigen::aligned_map frame_states; - Eigen::aligned_map frame_poses; + Eigen::aligned_map> frame_states; + Eigen::aligned_map> frame_poses; // Point management LandmarkDatabase lmdb; diff --git a/include/basalt/vi_estimator/keypoint_vio.h b/include/basalt/vi_estimator/keypoint_vio.h index e4151e5..80b0eed 100644 --- a/include/basalt/vi_estimator/keypoint_vio.h +++ b/include/basalt/vi_estimator/keypoint_vio.h @@ -75,25 +75,29 @@ class KeypointVioEstimator : public VioEstimatorBase, virtual ~KeypointVioEstimator() { processing_thread->join(); } - void addIMUToQueue(const ImuData::Ptr& data); + void addIMUToQueue(const ImuData::Ptr& data); void addVisionToQueue(const OpticalFlowResult::Ptr& data); bool measure(const OpticalFlowResult::Ptr& data, - const IntegratedImuMeasurement::Ptr& meas); + const IntegratedImuMeasurement::Ptr& meas); static void linearizeAbsIMU( const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, double& imu_error, double& bg_error, double& ba_error, - const Eigen::aligned_map& states, - const Eigen::aligned_map& imu_meas, + const Eigen::aligned_map>& + states, + const Eigen::aligned_map>& + imu_meas, const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g); static void computeImuError( const AbsOrderMap& aom, double& imu_error, double& bg_error, double& ba_error, - const Eigen::aligned_map& states, - const Eigen::aligned_map& imu_meas, + const Eigen::aligned_map>& + states, + const Eigen::aligned_map>& + imu_meas, const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g); @@ -116,11 +120,11 @@ class KeypointVioEstimator : public VioEstimatorBase, return frame_states.at(last_state_t_ns).getState().vel_w_i; } - const PoseVelBiasState& get_state() const { + const PoseVelBiasState& get_state() const { return frame_states.at(last_state_t_ns).getState(); } - PoseVelBiasState get_state(int64_t t_ns) const { - PoseVelBiasState state; + PoseVelBiasState get_state(int64_t t_ns) const { + PoseVelBiasState state; auto it = frame_states.find(t_ns); @@ -187,7 +191,7 @@ class KeypointVioEstimator : public VioEstimatorBase, std::set kf_ids; int64_t last_state_t_ns; - Eigen::aligned_map imu_meas; + Eigen::aligned_map> imu_meas; const Eigen::Vector3d g; diff --git a/include/basalt/vi_estimator/keypoint_vo.h b/include/basalt/vi_estimator/keypoint_vo.h index 4769b2f..7876992 100644 --- a/include/basalt/vi_estimator/keypoint_vo.h +++ b/include/basalt/vi_estimator/keypoint_vo.h @@ -74,7 +74,7 @@ class KeypointVoEstimator : public VioEstimatorBase, virtual ~KeypointVoEstimator() { processing_thread->join(); } - void addIMUToQueue(const ImuData::Ptr& data); + void addIMUToQueue(const ImuData::Ptr& data); void addVisionToQueue(const OpticalFlowResult::Ptr& data); bool measure(const OpticalFlowResult::Ptr& data, bool add_frame); @@ -98,11 +98,11 @@ class KeypointVoEstimator : public VioEstimatorBase, return frame_states.at(last_state_t_ns).getState().vel_w_i; } - const PoseVelBiasState& get_state() const { + const PoseVelBiasState& get_state() const { return frame_states.at(last_state_t_ns).getState(); } - PoseVelBiasState get_state(int64_t t_ns) const { - PoseVelBiasState state; + PoseVelBiasState get_state(int64_t t_ns) const { + PoseVelBiasState state; auto it = frame_states.find(t_ns); diff --git a/include/basalt/vi_estimator/nfr_mapper.h b/include/basalt/vi_estimator/nfr_mapper.h index fba9252..2f745cd 100644 --- a/include/basalt/vi_estimator/nfr_mapper.h +++ b/include/basalt/vi_estimator/nfr_mapper.h @@ -69,7 +69,8 @@ class NfrMapper : public BundleAdjustmentBase { MapperLinearizeAbsReduce( AbsOrderMap& aom, - const Eigen::aligned_map* frame_poses) + const Eigen::aligned_map>* + frame_poses) : BundleAdjustmentBase::LinearizeAbsReduce(aom), frame_poses(frame_poses) { this->accum.reset(aom.total_size); @@ -154,7 +155,7 @@ class NfrMapper : public BundleAdjustmentBase { double roll_pitch_error; double rel_error; - const Eigen::aligned_map* frame_poses; + const Eigen::aligned_map>* frame_poses; }; NfrMapper(const basalt::Calibration& calib, const VioConfig& config); @@ -167,7 +168,7 @@ class NfrMapper : public BundleAdjustmentBase { void optimize(int num_iterations = 10); - Eigen::aligned_map& getFramePoses(); + Eigen::aligned_map>& getFramePoses(); void computeRelPose(double& rel_error); diff --git a/include/basalt/vi_estimator/vio_estimator.h b/include/basalt/vi_estimator/vio_estimator.h index 48a19a0..cf126d8 100644 --- a/include/basalt/vi_estimator/vio_estimator.h +++ b/include/basalt/vi_estimator/vio_estimator.h @@ -77,10 +77,10 @@ class VioEstimatorBase { std::atomic finished; tbb::concurrent_bounded_queue vision_data_queue; - tbb::concurrent_bounded_queue imu_data_queue; + tbb::concurrent_bounded_queue::Ptr> imu_data_queue; - tbb::concurrent_bounded_queue* out_state_queue = - nullptr; + tbb::concurrent_bounded_queue::Ptr>* + out_state_queue = nullptr; tbb::concurrent_bounded_queue* out_marg_queue = nullptr; tbb::concurrent_bounded_queue* out_vis_queue = nullptr; diff --git a/src/device/rs_t265.cpp b/src/device/rs_t265.cpp index 5bd4214..dee3915 100644 --- a/src/device/rs_t265.cpp +++ b/src/device/rs_t265.cpp @@ -135,8 +135,8 @@ void RsT265Device::start() { Eigen::Vector3d accel_interpolated = w0 * prev_accel_data->data + w1 * d.data; - basalt::ImuData::Ptr data; - data.reset(new basalt::ImuData); + basalt::ImuData::Ptr data; + data.reset(new basalt::ImuData); data->t_ns = gyro_data.timestamp * 1e6; data->accel = accel_interpolated; data->gyro = gyro_data.data; diff --git a/src/mapper_sim.cpp b/src/mapper_sim.cpp index 6d7d793..01e5a09 100644 --- a/src/mapper_sim.cpp +++ b/src/mapper_sim.cpp @@ -532,7 +532,7 @@ void randomInc() { for (auto& kv : nrf_mapper->getFramePoses()) { Sophus::SE3d pose = random_inc * kv.second.getPose(); - basalt::PoseStateWithLin p(kv.first, pose); + basalt::PoseStateWithLin p(kv.first, pose); kv.second = p; } @@ -550,7 +550,7 @@ void randomYawInc() { for (auto& kv : nrf_mapper->getFramePoses()) { Sophus::SE3d pose = random_inc * kv.second.getPose(); - basalt::PoseStateWithLin p(kv.first, pose); + basalt::PoseStateWithLin p(kv.first, pose); kv.second = p; } diff --git a/src/mapper_sim_naive.cpp b/src/mapper_sim_naive.cpp index e186f7a..9ef2436 100644 --- a/src/mapper_sim_naive.cpp +++ b/src/mapper_sim_naive.cpp @@ -107,7 +107,8 @@ basalt::KeypointVioEstimator::Ptr vio; // Visualization vars std::unordered_map vis_map; tbb::concurrent_bounded_queue out_vis_queue; -tbb::concurrent_bounded_queue out_state_queue; +tbb::concurrent_bounded_queue::Ptr> + out_state_queue; std::vector images; @@ -182,7 +183,7 @@ int main(int argc, char** argv) { std::thread t0([&]() { for (size_t i = 0; i < gt_imu_t_ns.size(); i++) { - basalt::ImuData::Ptr data(new basalt::ImuData); + basalt::ImuData::Ptr data(new basalt::ImuData); data->t_ns = gt_imu_t_ns[i]; data->accel = noisy_accel[i]; @@ -238,7 +239,7 @@ int main(int argc, char** argv) { }); std::thread t3([&]() { - basalt::PoseVelBiasState::Ptr data; + basalt::PoseVelBiasState::Ptr data; while (true) { out_state_queue.pop(data); diff --git a/src/rs_t265_record.cpp b/src/rs_t265_record.cpp index 1569fda..7889f28 100644 --- a/src/rs_t265_record.cpp +++ b/src/rs_t265_record.cpp @@ -72,7 +72,7 @@ pangolin::Var exposure("ui.exposure", 5.0, 1, 20); tbb::concurrent_bounded_queue image_data_queue, image_data_queue2; -tbb::concurrent_bounded_queue imu_data_queue; +tbb::concurrent_bounded_queue::Ptr> imu_data_queue; tbb::concurrent_bounded_queue pose_data_queue; std::atomic stop_workers; @@ -164,7 +164,7 @@ void image_save_worker() { } void imu_save_worker() { - basalt::ImuData::Ptr data; + basalt::ImuData::Ptr data; while (!stop_workers) { if (imu_data_queue.try_pop(data)) { diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp index 5f012b6..0eab8b5 100644 --- a/src/rs_t265_vio.cpp +++ b/src/rs_t265_vio.cpp @@ -97,7 +97,8 @@ pangolin::Var follow("ui.follow", true, false, true); basalt::VioVisualizationData::Ptr curr_vis_data; tbb::concurrent_bounded_queue out_vis_queue; -tbb::concurrent_bounded_queue out_state_queue; +tbb::concurrent_bounded_queue::Ptr> + out_state_queue; std::vector vio_t_ns; Eigen::aligned_vector vio_t_w_i; @@ -203,7 +204,7 @@ int main(int argc, char** argv) { })); std::thread t4([&]() { - basalt::PoseVelBiasState::Ptr data; + basalt::PoseVelBiasState::Ptr data; while (true) { out_state_queue.pop(data); diff --git a/src/vi_estimator/ba_base.cpp b/src/vi_estimator/ba_base.cpp index bd56b03..34962f0 100644 --- a/src/vi_estimator/ba_base.cpp +++ b/src/vi_estimator/ba_base.cpp @@ -634,8 +634,9 @@ void BundleAdjustmentBase::computeMargPriorError( Eigen::VectorXd BundleAdjustmentBase::checkNullspace( const Eigen::MatrixXd& H, const Eigen::VectorXd& b, const AbsOrderMap& order, - const Eigen::aligned_map& frame_states, - const Eigen::aligned_map& frame_poses) { + const Eigen::aligned_map>& + frame_states, + const Eigen::aligned_map>& frame_poses) { BASALT_ASSERT(size_t(H.cols()) == order.total_size); size_t marg_size = order.total_size; diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index 87b11b9..eca9ea0 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -99,7 +99,7 @@ void KeypointVioEstimator::initialize(int64_t t_ns, const Sophus::SE3d& T_w_i, last_state_t_ns = t_ns; imu_meas[t_ns] = IntegratedImuMeasurement(t_ns, bg, ba); frame_states[t_ns] = - PoseVelBiasStateWithLin(t_ns, T_w_i, vel_w_i, bg, ba, true); + PoseVelBiasStateWithLin(t_ns, T_w_i, vel_w_i, bg, ba, true); marg_order.abs_order_map[t_ns] = std::make_pair(0, POSE_VEL_BIAS_SIZE); marg_order.total_size = POSE_VEL_BIAS_SIZE; @@ -112,14 +112,14 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, const Eigen::Vector3d& ba) { auto proc_func = [&, bg, ba] { OpticalFlowResult::Ptr prev_frame, curr_frame; - IntegratedImuMeasurement::Ptr meas; + IntegratedImuMeasurement::Ptr meas; const Eigen::Vector3d accel_cov = calib.dicrete_time_accel_noise_std().array().square(); const Eigen::Vector3d gyro_cov = calib.dicrete_time_gyro_noise_std().array().square(); - ImuData::Ptr data; + ImuData::Ptr data; imu_data_queue.pop(data); data->accel = calib.calib_accel_bias.getCalibrated(data->accel); data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro); @@ -157,7 +157,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, last_state_t_ns = curr_frame->t_ns; imu_meas[last_state_t_ns] = IntegratedImuMeasurement(last_state_t_ns, bg, ba); - frame_states[last_state_t_ns] = PoseVelBiasStateWithLin( + frame_states[last_state_t_ns] = PoseVelBiasStateWithLin( last_state_t_ns, T_w_i_init, vel_w_i_init, bg, ba, true); marg_order.abs_order_map[last_state_t_ns] = @@ -177,7 +177,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, auto last_state = frame_states.at(last_state_t_ns); - meas.reset(new IntegratedImuMeasurement( + meas.reset(new IntegratedImuMeasurement( prev_frame->t_ns, last_state.getState().bias_gyro, last_state.getState().bias_accel)); @@ -221,7 +221,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, processing_thread.reset(new std::thread(proc_func)); } -void KeypointVioEstimator::addIMUToQueue(const ImuData::Ptr& data) { +void KeypointVioEstimator::addIMUToQueue(const ImuData::Ptr& data) { imu_data_queue.emplace(data); } @@ -230,8 +230,9 @@ void KeypointVioEstimator::addVisionToQueue( vision_data_queue.push(data); } -bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas, - const IntegratedImuMeasurement::Ptr& meas) { +bool KeypointVioEstimator::measure( + const OpticalFlowResult::Ptr& opt_flow_meas, + const IntegratedImuMeasurement::Ptr& meas) { if (meas.get()) { BASALT_ASSERT(frame_states[last_state_t_ns].getState().t_ns == meas->get_start_t_ns()); @@ -392,7 +393,7 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas, if (out_state_queue) { PoseVelBiasStateWithLin p = frame_states.at(last_state_t_ns); - PoseVelBiasState::Ptr data(new PoseVelBiasState(p.getState())); + PoseVelBiasState::Ptr data(new PoseVelBiasState(p.getState())); out_state_queue->push(data); } @@ -692,7 +693,7 @@ void KeypointVioEstimator::marginalize( } for (const int64_t id : states_to_marg_vel_bias) { - const PoseVelBiasStateWithLin& state = frame_states.at(id); + const PoseVelBiasStateWithLin& state = frame_states.at(id); PoseStateWithLin pose(state); frame_poses[id] = pose; diff --git a/src/vi_estimator/keypoint_vio_linearize.cpp b/src/vi_estimator/keypoint_vio_linearize.cpp index d62b036..c0dfca7 100644 --- a/src/vi_estimator/keypoint_vio_linearize.cpp +++ b/src/vi_estimator/keypoint_vio_linearize.cpp @@ -40,8 +40,9 @@ namespace basalt { void KeypointVioEstimator::linearizeAbsIMU( const AbsOrderMap& aom, Eigen::MatrixXd& abs_H, Eigen::VectorXd& abs_b, double& imu_error, double& bg_error, double& ba_error, - const Eigen::aligned_map& states, - const Eigen::aligned_map& imu_meas, + const Eigen::aligned_map>& states, + const Eigen::aligned_map>& + imu_meas, const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) { imu_error = 0; @@ -62,10 +63,10 @@ void KeypointVioEstimator::linearizeAbsIMU( PoseVelBiasStateWithLin start_state = states.at(start_t); PoseVelBiasStateWithLin end_state = states.at(end_t); - IntegratedImuMeasurement::MatNN d_res_d_start, d_res_d_end; - IntegratedImuMeasurement::MatN3 d_res_d_bg, d_res_d_ba; + IntegratedImuMeasurement::MatNN d_res_d_start, d_res_d_end; + IntegratedImuMeasurement::MatN3 d_res_d_bg, d_res_d_ba; - PoseVelState::VecN res = kv.second.residual( + PoseVelState::VecN res = kv.second.residual( start_state.getStateLin(), g, end_state.getStateLin(), start_state.getStateLin().bias_gyro, start_state.getStateLin().bias_accel, &d_res_d_start, &d_res_d_end, @@ -97,7 +98,7 @@ void KeypointVioEstimator::linearizeAbsIMU( d_res_d_end.transpose() * kv.second.get_cov_inv() * res; // bias - IntegratedImuMeasurement::MatN6 d_res_d_bga; + IntegratedImuMeasurement::MatN6 d_res_d_bga; d_res_d_bga.topLeftCorner<9, 3>() = d_res_d_bg; d_res_d_bga.topRightCorner<9, 3>() = d_res_d_ba; @@ -179,8 +180,9 @@ void KeypointVioEstimator::linearizeAbsIMU( void KeypointVioEstimator::computeImuError( const AbsOrderMap& aom, double& imu_error, double& bg_error, double& ba_error, - const Eigen::aligned_map& states, - const Eigen::aligned_map& imu_meas, + const Eigen::aligned_map>& states, + const Eigen::aligned_map>& + imu_meas, const Eigen::Vector3d& gyro_bias_weight, const Eigen::Vector3d& accel_bias_weight, const Eigen::Vector3d& g) { imu_error = 0; @@ -198,7 +200,7 @@ void KeypointVioEstimator::computeImuError( PoseVelBiasStateWithLin start_state = states.at(start_t); PoseVelBiasStateWithLin end_state = states.at(end_t); - const PoseVelState::VecN res = kv.second.residual( + const PoseVelState::VecN res = kv.second.residual( start_state.getState(), g, end_state.getState(), start_state.getState().bias_gyro, start_state.getState().bias_accel); diff --git a/src/vi_estimator/keypoint_vo.cpp b/src/vi_estimator/keypoint_vo.cpp index a21f317..4652ccb 100644 --- a/src/vi_estimator/keypoint_vo.cpp +++ b/src/vi_estimator/keypoint_vo.cpp @@ -91,7 +91,7 @@ void KeypointVoEstimator::initialize(int64_t t_ns, const Sophus::SE3d& T_w_i, T_w_i_init = T_w_i; last_state_t_ns = t_ns; - frame_poses[t_ns] = PoseStateWithLin(t_ns, T_w_i, true); + frame_poses[t_ns] = PoseStateWithLin(t_ns, T_w_i, true); marg_order.abs_order_map[t_ns] = std::make_pair(0, POSE_SIZE); marg_order.total_size = POSE_SIZE; @@ -122,7 +122,7 @@ void KeypointVoEstimator::initialize(const Eigen::Vector3d& bg, // curr_frame->t_ns += calib.cam_time_offset_ns; while (!imu_data_queue.empty()) { - ImuData::Ptr d; + ImuData::Ptr d; imu_data_queue.pop(d); } @@ -132,7 +132,7 @@ void KeypointVoEstimator::initialize(const Eigen::Vector3d& bg, last_state_t_ns = curr_frame->t_ns; frame_poses[last_state_t_ns] = - PoseStateWithLin(last_state_t_ns, T_w_i_init, true); + PoseStateWithLin(last_state_t_ns, T_w_i_init, true); marg_order.abs_order_map[last_state_t_ns] = std::make_pair(0, POSE_SIZE); @@ -172,11 +172,13 @@ void KeypointVoEstimator::addVisionToQueue(const OpticalFlowResult::Ptr& data) { bool KeypointVoEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas, const bool add_pose) { if (add_pose) { - const PoseStateWithLin& curr_state = frame_poses.at(last_state_t_ns); + const PoseStateWithLin& curr_state = + frame_poses.at(last_state_t_ns); last_state_t_ns = opt_flow_meas->t_ns; - PoseStateWithLin next_state(opt_flow_meas->t_ns, curr_state.getPose()); + PoseStateWithLin next_state(opt_flow_meas->t_ns, + curr_state.getPose()); frame_poses[last_state_t_ns] = next_state; } @@ -319,11 +321,11 @@ bool KeypointVoEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas, marginalize(num_points_connected); if (out_state_queue) { - const PoseStateWithLin& p = frame_poses.at(last_state_t_ns); + const PoseStateWithLin& p = frame_poses.at(last_state_t_ns); - PoseVelBiasState::Ptr data( - new PoseVelBiasState(p.getT_ns(), p.getPose(), Eigen::Vector3d::Zero(), - Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero())); + PoseVelBiasState::Ptr data(new PoseVelBiasState( + p.getT_ns(), p.getPose(), Eigen::Vector3d::Zero(), + Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero())); out_state_queue->push(data); } diff --git a/src/vi_estimator/nfr_mapper.cpp b/src/vi_estimator/nfr_mapper.cpp index 5c286e9..80a3512 100644 --- a/src/vi_estimator/nfr_mapper.cpp +++ b/src/vi_estimator/nfr_mapper.cpp @@ -62,7 +62,7 @@ void NfrMapper::addMargData(MargData::Ptr& data) { if (valid) { for (const auto& kv : data->frame_poses) { - PoseStateWithLin p(kv.second.getT_ns(), kv.second.getPose()); + PoseStateWithLin p(kv.second.getT_ns(), kv.second.getPose()); frame_poses[kv.first] = p; } @@ -70,7 +70,8 @@ void NfrMapper::addMargData(MargData::Ptr& data) { for (const auto& kv : data->frame_states) { if (data->kfs_all.count(kv.first) > 0) { auto state = kv.second; - PoseStateWithLin p(state.getState().t_ns, state.getState().T_w_i); + PoseStateWithLin p(state.getState().t_ns, + state.getState().T_w_i); frame_poses[kv.first] = p; } } @@ -420,7 +421,8 @@ void NfrMapper::optimize(int num_iterations) { } } -Eigen::aligned_map& NfrMapper::getFramePoses() { +Eigen::aligned_map>& +NfrMapper::getFramePoses() { return frame_poses; } diff --git a/src/vio.cpp b/src/vio.cpp index e40ba1f..e31de68 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -118,7 +118,8 @@ pangolin::OpenGlRenderState camera; std::unordered_map vis_map; tbb::concurrent_bounded_queue out_vis_queue; -tbb::concurrent_bounded_queue out_state_queue; +tbb::concurrent_bounded_queue::Ptr> + out_state_queue; std::vector vio_t_ns; Eigen::aligned_vector vio_t_w_i; @@ -172,7 +173,7 @@ void feed_images() { void feed_imu() { for (size_t i = 0; i < vio_dataset->get_gyro_data().size(); i++) { - basalt::ImuData::Ptr data(new basalt::ImuData); + basalt::ImuData::Ptr data(new basalt::ImuData); data->t_ns = vio_dataset->get_gyro_data()[i].timestamp_ns; data->accel = vio_dataset->get_accel_data()[i].data; @@ -328,7 +329,7 @@ int main(int argc, char** argv) { })); std::thread t4([&]() { - basalt::PoseVelBiasState::Ptr data; + basalt::PoseVelBiasState::Ptr data; while (true) { out_state_queue.pop(data); diff --git a/src/vio_sim.cpp b/src/vio_sim.cpp index b4c3b34..4ecbcb0 100644 --- a/src/vio_sim.cpp +++ b/src/vio_sim.cpp @@ -116,7 +116,8 @@ basalt::VioEstimatorBase::Ptr vio; // Visualization vars std::unordered_map vis_map; tbb::concurrent_bounded_queue out_vis_queue; -tbb::concurrent_bounded_queue out_state_queue; +tbb::concurrent_bounded_queue::Ptr> + out_state_queue; std::vector images; @@ -208,7 +209,7 @@ int main(int argc, char** argv) { std::thread t0([&]() { for (size_t i = 0; i < gt_imu_t_ns.size(); i++) { - basalt::ImuData::Ptr data(new basalt::ImuData); + basalt::ImuData::Ptr data(new basalt::ImuData); data->t_ns = gt_imu_t_ns[i]; data->accel = noisy_accel[i]; @@ -264,7 +265,7 @@ int main(int argc, char** argv) { }); std::thread t3([&]() { - basalt::PoseVelBiasState::Ptr data; + basalt::PoseVelBiasState::Ptr data; while (true) { out_state_queue.pop(data); diff --git a/test/src/test_nfr.cpp b/test/src/test_nfr.cpp index 726a9c7..5055b90 100644 --- a/test/src/test_nfr.cpp +++ b/test/src/test_nfr.cpp @@ -38,7 +38,7 @@ TEST(PreIntegrationTestSuite, RelPoseTest) { "d_res_d_T_w_i", d_res_d_T_w_i, [&](const Sophus::Vector6d& x) { auto T_w_i_new = T_w_i; - basalt::PoseState::incPose(x, T_w_i_new); + basalt::PoseState::incPose(x, T_w_i_new); return basalt::relPoseError(T_i_j, T_w_i_new, T_w_j); }, @@ -52,7 +52,7 @@ TEST(PreIntegrationTestSuite, RelPoseTest) { "d_res_d_T_w_j", d_res_d_T_w_j, [&](const Sophus::Vector6d& x) { auto T_w_j_new = T_w_j; - basalt::PoseState::incPose(x, T_w_j_new); + basalt::PoseState::incPose(x, T_w_j_new); return basalt::relPoseError(T_i_j, T_w_i, T_w_j_new); }, @@ -75,7 +75,7 @@ TEST(PreIntegrationTestSuite, AbsPositionTest) { "d_res_d_T_w_i", d_res_d_T_w_i, [&](const Sophus::Vector6d& x) { auto T_w_i_new = T_w_i; - basalt::PoseState::incPose(x, T_w_i_new); + basalt::PoseState::incPose(x, T_w_i_new); return basalt::absPositionError(T_w_i_new, pos); }, @@ -101,7 +101,7 @@ TEST(PreIntegrationTestSuite, YawTest) { "d_res_d_T_w_i", d_res_d_T_w_i, [&](const Sophus::Vector6d& x) { auto T_w_i_new = T_w_i; - basalt::PoseState::incPose(x, T_w_i_new); + basalt::PoseState::incPose(x, T_w_i_new); double res = basalt::yawError(T_w_i_new, yaw_dir_body); @@ -128,7 +128,7 @@ TEST(PreIntegrationTestSuite, RollPitchTest) { "d_res_d_T_w_i", d_res_d_T_w_i, [&](const Sophus::Vector6d& x) { auto T_w_i_new = T_w_i; - basalt::PoseState::incPose(x, T_w_i_new); + basalt::PoseState::incPose(x, T_w_i_new); return basalt::rollPitchError(T_w_i_new, R_w_i); }, diff --git a/test/src/test_vio.cpp b/test/src/test_vio.cpp index 898ca5d..e43fdc1 100644 --- a/test/src/test_vio.cpp +++ b/test/src/test_vio.cpp @@ -33,7 +33,7 @@ TEST(VioTestSuite, ImuNullspace2Test) { basalt::Se3Spline<5> gt_spline(int64_t(10e9)); gt_spline.genRandomTrajectory(num_knots); - basalt::PoseVelBiasState state0, state1, state1_gt; + basalt::PoseVelBiasState state0, state1, state1_gt; state0.t_ns = 0; state0.T_w_i = gt_spline.pose(int64_t(0)); @@ -55,7 +55,7 @@ TEST(VioTestSuite, ImuNullspace2Test) { (gt_spline.transAccelWorld(t_ns) - basalt::constants::g); Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns); - basalt::ImuData data; + basalt::ImuData data; data.accel = accel_body + ba; data.gyro = rot_vel_body + bg; @@ -86,9 +86,11 @@ TEST(VioTestSuite, ImuNullspace2Test) { Eigen::Vector3d accel_weight; accel_weight.setConstant(1e6); - Eigen::aligned_map imu_meas_vec; - Eigen::aligned_map frame_states; - Eigen::aligned_map frame_poses; + Eigen::aligned_map> + imu_meas_vec; + Eigen::aligned_map> + frame_states; + Eigen::aligned_map> frame_poses; imu_meas_vec[state0.t_ns] = imu_meas; frame_states[state0.t_ns] = state0; @@ -159,7 +161,7 @@ TEST(VioTestSuite, ImuNullspace3Test) { basalt::Se3Spline<5> gt_spline(int64_t(10e9)); gt_spline.genRandomTrajectory(num_knots); - basalt::PoseVelBiasState state0, state1, state2; + basalt::PoseVelBiasState state0, state1, state2; state0.t_ns = 0; state0.T_w_i = gt_spline.pose(int64_t(0)); @@ -181,7 +183,7 @@ TEST(VioTestSuite, ImuNullspace3Test) { (gt_spline.transAccelWorld(t_ns) - basalt::constants::g); Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns); - basalt::ImuData data; + basalt::ImuData data; data.accel = accel_body + ba; data.gyro = rot_vel_body + bg; @@ -208,7 +210,7 @@ TEST(VioTestSuite, ImuNullspace3Test) { (gt_spline.transAccelWorld(t_ns) - basalt::constants::g); Eigen::Vector3d rot_vel_body = gt_spline.rotVelBody(t_ns); - basalt::ImuData data; + basalt::ImuData data; data.accel = accel_body + ba; data.gyro = rot_vel_body + bg; @@ -247,9 +249,11 @@ TEST(VioTestSuite, ImuNullspace3Test) { Eigen::Vector3d accel_weight; accel_weight.setConstant(1e6); - Eigen::aligned_map imu_meas_vec; - Eigen::aligned_map frame_states; - Eigen::aligned_map frame_poses; + Eigen::aligned_map> + imu_meas_vec; + Eigen::aligned_map> + frame_states; + Eigen::aligned_map> frame_poses; imu_meas_vec[imu_meas1.get_start_t_ns()] = imu_meas1; imu_meas_vec[imu_meas2.get_start_t_ns()] = imu_meas2; @@ -305,7 +309,7 @@ TEST(VioTestSuite, RelPoseTest) { "d_rel_d_h", d_rel_d_h, [&](const Sophus::Vector6d& x) { Sophus::SE3d T_w_h_new = T_w_i_h; - basalt::PoseState::incPose(x, T_w_h_new); + basalt::PoseState::incPose(x, T_w_h_new); Sophus::SE3d T_t_h_sophus_new = basalt::KeypointVioEstimator::computeRelPose(T_w_h_new, T_i_c_h, @@ -323,7 +327,7 @@ TEST(VioTestSuite, RelPoseTest) { "d_rel_d_t", d_rel_d_t, [&](const Sophus::Vector6d& x) { Sophus::SE3d T_w_t_new = T_w_i_t; - basalt::PoseState::incPose(x, T_w_t_new); + basalt::PoseState::incPose(x, T_w_t_new); Sophus::SE3d T_t_h_sophus_new = basalt::KeypointVioEstimator::computeRelPose(T_w_i_h, T_i_c_h, diff --git a/thirdparty/CLI11 b/thirdparty/CLI11 index 32e6c3f..4af78be 160000 --- a/thirdparty/CLI11 +++ b/thirdparty/CLI11 @@ -1 +1 @@ -Subproject commit 32e6c3f34750b3ff6ba07d092ed16d7bb469fc85 +Subproject commit 4af78beef777e313814b4daff70e2da9171a385a diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index 4b62bcb..813ce2d 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit 4b62bcbdb343cc9464ba1de800ddcfd9942a9a6f +Subproject commit 813ce2d7a6854f25561b8b3810b0bf355658a161