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

small changes

This commit is contained in:
Vladyslav Usenko 2019-08-01 12:59:50 +02:00
parent f1fe43a250
commit b9a2931468
2 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
include/basalt/io/dataset_io_uzh.h
View File

@ -175,19 +175,17 @@ class UzhIO : public DatasetIoInterface {
<< " left images and " << data->right_image_path.size() << " left images and " << data->right_image_path.size()
<< std::endl; << std::endl;
{ // {
int64_t t_ns = data->get_image_timestamps()[0]; // int64_t t_ns = data->get_image_timestamps()[0];
std::cout << t_ns << " " << data->left_image_path.at(t_ns) << " " // std::cout << t_ns << " " << data->left_image_path.at(t_ns) << " "
<< data->right_image_path.at(t_ns) << std::endl; // << data->right_image_path.at(t_ns) << std::endl;
} // }
read_imu_data(path + "/imu.txt"); read_imu_data(path + "/imu.txt");
std::cout << "Loaded " << data->get_gyro_data().size() << " imu msgs." std::cout << "Loaded " << data->get_gyro_data().size() << " imu msgs."
<< std::endl; << std::endl;
{}
if (file_exists(path + "/groundtruth.txt")) { if (file_exists(path + "/groundtruth.txt")) {
read_gt_data_pose(path + "/groundtruth.txt"); read_gt_data_pose(path + "/groundtruth.txt");
} }
@ -307,7 +305,10 @@ class UzhIO : public DatasetIoInterface {
ss >> tmp >> timestamp >> pos[0] >> pos[1] >> pos[2] >> q.x() >> q.y() >> ss >> tmp >> timestamp >> pos[0] >> pos[1] >> pos[2] >> q.x() >> q.y() >>
q.z() >> q.w(); q.z() >> q.w();
data->gt_timestamps.emplace_back(timestamp * 1e9); int64_t t_ns = timestamp * 1e9;
t_ns += -99902802;
data->gt_timestamps.emplace_back(t_ns);
data->gt_pose_data.emplace_back(q, pos); data->gt_pose_data.emplace_back(q, pos);
} }
} }

12
src/vi_estimator/keypoint_vio.cpp
View File

@ -125,6 +125,13 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
while (true) { while (true) {
vision_data_queue.pop(curr_frame); vision_data_queue.pop(curr_frame);
if (!curr_frame.get()) {
break;
}
// Correct camera time offset
// curr_frame->t_ns += calib.cam_time_offset_ns;
if (!initialized) { if (!initialized) {
Eigen::Vector3d vel_w_i_init; Eigen::Vector3d vel_w_i_init;
vel_w_i_init.setZero(); vel_w_i_init.setZero();
@ -150,10 +157,6 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
initialized = true; initialized = true;
} }
if (!curr_frame.get()) {
break;
}
if (prev_frame) { if (prev_frame) {
// preintegrate measurements // preintegrate measurements
@ -179,6 +182,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg,
} }
if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) { if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) {
if (!data.get()) break;
int64_t tmp = data->t_ns; int64_t tmp = data->t_ns;
data->t_ns = curr_frame->t_ns; data->t_ns = curr_frame->t_ns;
meas->integrate(*data, accel_cov, gyro_cov); meas->integrate(*data, accel_cov, gyro_cov);