ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Switched to templated version of IMU types

This commit is contained in:
Vladyslav Usenko 2021-04-17 18:41:42 +02:00
parent c370866bbf
commit c7ceee72c0
23 changed files with 145 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/device/rs_t265.h
View File

@ -92,7 +92,7 @@ class RsT265Device {
OpticalFlowInput::Ptr last_img_data;
tbb::concurrent_bounded_queue<OpticalFlowInput::Ptr>* image_data_queue =
nullptr;
tbb::concurrent_bounded_queue<ImuData::Ptr>* imu_data_queue = nullptr;
tbb::concurrent_bounded_queue<ImuData<double>::Ptr>* imu_data_queue = nullptr;
tbb::concurrent_bounded_queue<RsPoseData>* pose_data_queue = nullptr;
private:

46
include/basalt/utils/imu_types.h
View File

@ -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 <class Scalar>
struct PoseStateWithLin;
template <class Scalar_>
struct PoseVelBiasStateWithLin {
using VecN = PoseVelBiasState::VecN;
using Scalar = Scalar_;
using VecN = typename PoseVelBiasState<Scalar>::VecN;
PoseVelBiasStateWithLin() {
linearized = false;
@ -75,7 +80,7 @@ struct PoseVelBiasStateWithLin {
state_current = state_linearized;
}
PoseVelBiasStateWithLin(const PoseVelBiasState& other)
PoseVelBiasStateWithLin(const PoseVelBiasState<Scalar>& 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<Scalar>& getState() const {
if (!linearized) {
return state_linearized;
} else {
@ -110,7 +115,7 @@ struct PoseVelBiasStateWithLin {
}
}
inline const PoseVelBiasState& getStateLin() const {
inline const PoseVelBiasState<Scalar>& 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<Scalar>;
inline void backup() {
backup_delta = delta;
@ -136,10 +141,10 @@ struct PoseVelBiasStateWithLin {
private:
bool linearized;
VecN delta;
PoseVelBiasState state_linearized, state_current;
PoseVelBiasState<Scalar> state_linearized, state_current;
VecN backup_delta;
PoseVelBiasState backup_state_linearized, backup_state_current;
PoseVelBiasState<Scalar> backup_state_linearized, backup_state_current;
friend class cereal::access;
@ -159,8 +164,11 @@ struct PoseVelBiasStateWithLin {
}
};
template <typename Scalar_>
struct PoseStateWithLin {
using VecN = PoseState::VecN;
using Scalar = Scalar_;
using VecN = typename PoseState<Scalar>::VecN;
using SE3 = typename PoseState<Scalar>::SE3;
PoseStateWithLin() {
linearized = false;
@ -174,13 +182,13 @@ struct PoseStateWithLin {
T_w_i_current = T_w_i;
}
PoseStateWithLin(const PoseVelBiasStateWithLin& other)
PoseStateWithLin(const PoseVelBiasStateWithLin<Scalar>& 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<Scalar>::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<Scalar>::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<Scalar>::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<Scalar> pose_linearized;
SE3 T_w_i_current;
VecN backup_delta;
PoseState backup_pose_linearized;
Sophus::SE3d backup_T_w_i_current;
PoseState<Scalar> 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<int64_t, PoseVelBiasStateWithLin> frame_states;
Eigen::aligned_map<int64_t, PoseStateWithLin> frame_poses;
Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>> frame_states;
Eigen::aligned_map<int64_t, PoseStateWithLin<double>> frame_poses;
std::set<int64_t> kfs_all;
std::set<int64_t> kfs_to_marg;
bool use_imu;

11
include/basalt/vi_estimator/ba_base.h
View File

@ -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<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::aligned_map<int64_t, PoseStateWithLin>& frame_poses);
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
frame_states,
const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>& 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<double> 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<int64_t, PoseVelBiasStateWithLin> frame_states;
Eigen::aligned_map<int64_t, PoseStateWithLin> frame_poses;
Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>> frame_states;
Eigen::aligned_map<int64_t, PoseStateWithLin<double>> frame_poses;
// Point management
LandmarkDatabase lmdb;

24
include/basalt/vi_estimator/keypoint_vio.h
View File

@ -75,25 +75,29 @@ class KeypointVioEstimator : public VioEstimatorBase,
virtual ~KeypointVioEstimator() { processing_thread->join(); }
void addIMUToQueue(const ImuData::Ptr& data);
void addIMUToQueue(const ImuData<double>::Ptr& data);
void addVisionToQueue(const OpticalFlowResult::Ptr& data);
bool measure(const OpticalFlowResult::Ptr& data,
const IntegratedImuMeasurement::Ptr& meas);
const IntegratedImuMeasurement<double>::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<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement>& imu_meas,
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<double>>&
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<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement>& imu_meas,
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<double>>&
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<double>& get_state() const {
return frame_states.at(last_state_t_ns).getState();
}
PoseVelBiasState get_state(int64_t t_ns) const {
PoseVelBiasState state;
PoseVelBiasState<double> get_state(int64_t t_ns) const {
PoseVelBiasState<double> state;
auto it = frame_states.find(t_ns);
@ -187,7 +191,7 @@ class KeypointVioEstimator : public VioEstimatorBase,
std::set<int64_t> kf_ids;
int64_t last_state_t_ns;
Eigen::aligned_map<int64_t, IntegratedImuMeasurement> imu_meas;
Eigen::aligned_map<int64_t, IntegratedImuMeasurement<double>> imu_meas;
const Eigen::Vector3d g;

8
include/basalt/vi_estimator/keypoint_vo.h
View File

@ -74,7 +74,7 @@ class KeypointVoEstimator : public VioEstimatorBase,
virtual ~KeypointVoEstimator() { processing_thread->join(); }
void addIMUToQueue(const ImuData::Ptr& data);
void addIMUToQueue(const ImuData<double>::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<double>& get_state() const {
return frame_states.at(last_state_t_ns).getState();
}
PoseVelBiasState get_state(int64_t t_ns) const {
PoseVelBiasState state;
PoseVelBiasState<double> get_state(int64_t t_ns) const {
PoseVelBiasState<double> state;
auto it = frame_states.find(t_ns);

7
include/basalt/vi_estimator/nfr_mapper.h
View File

@ -69,7 +69,8 @@ class NfrMapper : public BundleAdjustmentBase {
MapperLinearizeAbsReduce(
AbsOrderMap& aom,
const Eigen::aligned_map<int64_t, PoseStateWithLin>* frame_poses)
const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>*
frame_poses)
: BundleAdjustmentBase::LinearizeAbsReduce<AccumT>(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<int64_t, PoseStateWithLin>* frame_poses;
const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>* frame_poses;
};
NfrMapper(const basalt::Calibration<double>& calib, const VioConfig& config);
@ -167,7 +168,7 @@ class NfrMapper : public BundleAdjustmentBase {
void optimize(int num_iterations = 10);
Eigen::aligned_map<int64_t, PoseStateWithLin>& getFramePoses();
Eigen::aligned_map<int64_t, PoseStateWithLin<double>>& getFramePoses();
void computeRelPose(double& rel_error);

6
include/basalt/vi_estimator/vio_estimator.h
View File

@ -77,10 +77,10 @@ class VioEstimatorBase {
std::atomic<bool> finished;
tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr> vision_data_queue;
tbb::concurrent_bounded_queue<ImuData::Ptr> imu_data_queue;
tbb::concurrent_bounded_queue<ImuData<double>::Ptr> imu_data_queue;
tbb::concurrent_bounded_queue<PoseVelBiasState::Ptr>* out_state_queue =
nullptr;
tbb::concurrent_bounded_queue<PoseVelBiasState<double>::Ptr>*
out_state_queue = nullptr;
tbb::concurrent_bounded_queue<MargData::Ptr>* out_marg_queue = nullptr;
tbb::concurrent_bounded_queue<VioVisualizationData::Ptr>* out_vis_queue =
nullptr;

4
src/device/rs_t265.cpp
View File

@ -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<double>::Ptr data;
data.reset(new basalt::ImuData<double>);
data->t_ns = gyro_data.timestamp * 1e6;
data->accel = accel_interpolated;
data->gyro = gyro_data.data;

4
src/mapper_sim.cpp
View File

@ -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<double> 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<double> p(kv.first, pose);
kv.second = p;
}

7
src/mapper_sim_naive.cpp
View File

@ -107,7 +107,8 @@ basalt::KeypointVioEstimator::Ptr vio;
// Visualization vars
std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState<double>::Ptr>
out_state_queue;
std::vector<pangolin::TypedImage> 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<double>::Ptr data(new basalt::ImuData<double>);
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<double>::Ptr data;
while (true) {
out_state_queue.pop(data);

4
src/rs_t265_record.cpp
View File

@ -72,7 +72,7 @@ pangolin::Var<float> exposure("ui.exposure", 5.0, 1, 20);
tbb::concurrent_bounded_queue<basalt::OpticalFlowInput::Ptr> image_data_queue,
image_data_queue2;
tbb::concurrent_bounded_queue<basalt::ImuData::Ptr> imu_data_queue;
tbb::concurrent_bounded_queue<basalt::ImuData<double>::Ptr> imu_data_queue;
tbb::concurrent_bounded_queue<basalt::RsPoseData> pose_data_queue;
std::atomic<bool> stop_workers;
@ -164,7 +164,7 @@ void image_save_worker() {
}
void imu_save_worker() {
basalt::ImuData::Ptr data;
basalt::ImuData<double>::Ptr data;
while (!stop_workers) {
if (imu_data_queue.try_pop(data)) {

5
src/rs_t265_vio.cpp
View File

@ -97,7 +97,8 @@ pangolin::Var<bool> follow("ui.follow", true, false, true);
basalt::VioVisualizationData::Ptr curr_vis_data;
tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState<double>::Ptr>
out_state_queue;
std::vector<int64_t> vio_t_ns;
Eigen::aligned_vector<Eigen::Vector3d> vio_t_w_i;
@ -203,7 +204,7 @@ int main(int argc, char** argv) {
}));
std::thread t4([&]() {
basalt::PoseVelBiasState::Ptr data;
basalt::PoseVelBiasState<double>::Ptr data;
while (true) {
out_state_queue.pop(data);

5
src/vi_estimator/ba_base.cpp
View File

@ -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<int64_t, PoseVelBiasStateWithLin>& frame_states,
const Eigen::aligned_map<int64_t, PoseStateWithLin>& frame_poses) {
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>&
frame_states,
const Eigen::aligned_map<int64_t, PoseStateWithLin<double>>& frame_poses) {
BASALT_ASSERT(size_t(H.cols()) == order.total_size);
size_t marg_size = order.total_size;

21
src/vi_estimator/keypoint_vio.cpp
View File

@ -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<double>(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<double>::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<double>::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<double>(
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<double>(
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<double>::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<double>::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<double>::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<double>& state = frame_states.at(id);
PoseStateWithLin pose(state);
frame_poses[id] = pose;

20
src/vi_estimator/keypoint_vio_linearize.cpp
View File

@ -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<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement>& imu_meas,
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>& states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<double>>&
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<double>::MatNN d_res_d_start, d_res_d_end;
IntegratedImuMeasurement<double>::MatN3 d_res_d_bg, d_res_d_ba;
PoseVelState::VecN res = kv.second.residual(
PoseVelState<double>::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<double>::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<int64_t, PoseVelBiasStateWithLin>& states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement>& imu_meas,
const Eigen::aligned_map<int64_t, PoseVelBiasStateWithLin<double>>& states,
const Eigen::aligned_map<int64_t, IntegratedImuMeasurement<double>>&
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<double>::VecN res = kv.second.residual(
start_state.getState(), g, end_state.getState(),
start_state.getState().bias_gyro, start_state.getState().bias_accel);

20
src/vi_estimator/keypoint_vo.cpp
View File

@ -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<double>(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<double>::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<double>(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<double>& 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<double> 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<double>& 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<double>::Ptr data(new PoseVelBiasState<double>(
p.getT_ns(), p.getPose(), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()));
out_state_queue->push(data);
}

8
src/vi_estimator/nfr_mapper.cpp
View File

@ -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<double> 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<double> 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<int64_t, PoseStateWithLin>& NfrMapper::getFramePoses() {
Eigen::aligned_map<int64_t, PoseStateWithLin<double>>&
NfrMapper::getFramePoses() {
return frame_poses;
}

7
src/vio.cpp
View File

@ -118,7 +118,8 @@ pangolin::OpenGlRenderState camera;
std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState<double>::Ptr>
out_state_queue;
std::vector<int64_t> vio_t_ns;
Eigen::aligned_vector<Eigen::Vector3d> 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<double>::Ptr data(new basalt::ImuData<double>);
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<double>::Ptr data;
while (true) {
out_state_queue.pop(data);

7
src/vio_sim.cpp
View File

@ -116,7 +116,8 @@ basalt::VioEstimatorBase::Ptr vio;
// Visualization vars
std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
tbb::concurrent_bounded_queue<basalt::VioVisualizationData::Ptr> out_vis_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState::Ptr> out_state_queue;
tbb::concurrent_bounded_queue<basalt::PoseVelBiasState<double>::Ptr>
out_state_queue;
std::vector<pangolin::TypedImage> 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<double>::Ptr data(new basalt::ImuData<double>);
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<double>::Ptr data;
while (true) {
out_state_queue.pop(data);

10
test/src/test_nfr.cpp
View File

@ -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<double>::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<double>::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<double>::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<double>::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<double>::incPose(x, T_w_i_new);
return basalt::rollPitchError(T_w_i_new, R_w_i);
},

30
test/src/test_vio.cpp
View File

@ -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<double> 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<double> 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<int64_t, basalt::IntegratedImuMeasurement> imu_meas_vec;
Eigen::aligned_map<int64_t, basalt::PoseVelBiasStateWithLin> frame_states;
Eigen::aligned_map<int64_t, basalt::PoseStateWithLin> frame_poses;
Eigen::aligned_map<int64_t, basalt::IntegratedImuMeasurement<double>>
imu_meas_vec;
Eigen::aligned_map<int64_t, basalt::PoseVelBiasStateWithLin<double>>
frame_states;
Eigen::aligned_map<int64_t, basalt::PoseStateWithLin<double>> 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<double> 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<double> 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<double> 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<int64_t, basalt::IntegratedImuMeasurement> imu_meas_vec;
Eigen::aligned_map<int64_t, basalt::PoseVelBiasStateWithLin> frame_states;
Eigen::aligned_map<int64_t, basalt::PoseStateWithLin> frame_poses;
Eigen::aligned_map<int64_t, basalt::IntegratedImuMeasurement<double>>
imu_meas_vec;
Eigen::aligned_map<int64_t, basalt::PoseVelBiasStateWithLin<double>>
frame_states;
Eigen::aligned_map<int64_t, basalt::PoseStateWithLin<double>> 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<double>::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<double>::incPose(x, T_w_t_new);
Sophus::SE3d T_t_h_sophus_new =
basalt::KeypointVioEstimator::computeRelPose(T_w_i_h, T_i_c_h,

2
thirdparty/CLI11 vendored

@ -1 +1 @@
Subproject commit 32e6c3f34750b3ff6ba07d092ed16d7bb469fc85
Subproject commit 4af78beef777e313814b4daff70e2da9171a385a

2
thirdparty/basalt-headers vendored

@ -1 +1 @@
Subproject commit 4b62bcbdb343cc9464ba1de800ddcfd9942a9a6f
Subproject commit 813ce2d7a6854f25561b8b3810b0bf355658a161