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

removed device data

Browse Source
This commit is contained in:
Vladyslav Usenko 2019-08-04 16:36:46 +02:00
parent fc0683f65f
commit 1d0fc86003
5 changed files with 4 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/io/dataset_io.h
View File

@ -127,8 +127,6 @@ class VioDataset {
virtual const Eigen::vector<GyroData> &get_gyro_data() const = 0;
virtual const std::vector<int64_t> &get_gt_timestamps() const = 0;
virtual const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const = 0;
virtual const std::vector<int64_t> &get_device_pose_timestamps() const = 0;
virtual const Eigen::vector<Sophus::SE3d> &get_device_pose_data() const = 0;
virtual int64_t get_mocap_to_imu_offset_ns() const = 0;
virtual std::vector<ImageData> get_image_data(int64_t t_ns) = 0;

41
include/basalt/io/dataset_io_euroc.h
View File

@ -63,10 +63,6 @@ class EurocVioDataset : public VioDataset {
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> gt_pose_data; // TODO: change to eigen aligned
std::vector<int64_t> device_pose_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d>
device_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns = 0;
std::vector<std::unordered_map<int64_t, double>> exposure_times;
@ -86,12 +82,7 @@ class EurocVioDataset : public VioDataset {
const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const {
return gt_pose_data;
}
const std::vector<int64_t> &get_device_pose_timestamps() const {
return device_pose_timestamps;
}
const Eigen::vector<Sophus::SE3d> &get_device_pose_data() const {
return device_pose_data;
}
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
std::vector<ImageData> get_image_data(int64_t t_ns) {
@ -180,10 +171,6 @@ class EurocIO : public DatasetIoInterface {
read_gt_data_pose(path + "/mav0/mocap0/");
}
if (file_exists(path + "/mav0/realsense0/data.csv")) {
read_device_data_pose(path + "/mav0/realsense0/");
}
data->exposure_times.resize(data->num_cams);
if (file_exists(path + "/mav0/cam0/exposure.csv")) {
std::cout << "Loading exposure times for cam0" << std::endl;
@ -316,32 +303,8 @@ class EurocIO : public DatasetIoInterface {
}
}
void read_device_data_pose(const std::string &path) {
data->device_pose_timestamps.clear();
data->device_pose_data.clear();
std::ifstream f(path + "data.csv");
std::string line;
while (std::getline(f, line)) {
if (line[0] == '#') continue;
std::stringstream ss(line);
char tmp;
uint64_t timestamp;
Eigen::Quaterniond q;
Eigen::Vector3d pos;
ss >> timestamp >> tmp >> pos[0] >> tmp >> pos[1] >> tmp >> pos[2] >>
tmp >> q.w() >> tmp >> q.x() >> tmp >> q.y() >> tmp >> q.z();
data->device_pose_timestamps.emplace_back(timestamp);
data->device_pose_data.emplace_back(q, pos);
}
}
std::shared_ptr<EurocVioDataset> data;
};
}; // namespace basalt
} // namespace basalt

11
include/basalt/io/dataset_io_rosbag.h
View File

@ -77,10 +77,6 @@ class RosbagVioDataset : public VioDataset {
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> gt_pose_data; // TODO: change to eigen aligned
std::vector<int64_t> device_pose_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d>
device_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns;
public:
@ -98,12 +94,7 @@ class RosbagVioDataset : public VioDataset {
const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const {
return gt_pose_data;
}
const std::vector<int64_t> &get_device_pose_timestamps() const {
return device_pose_timestamps;
}
const Eigen::vector<Sophus::SE3d> &get_device_pose_data() const {
return device_pose_data;
}
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
std::vector<ImageData> get_image_data(int64_t t_ns) {

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

@ -64,10 +64,6 @@ class UzhVioDataset : public VioDataset {
std::vector<int64_t> gt_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d> gt_pose_data; // TODO: change to eigen aligned
std::vector<int64_t> device_pose_timestamps; // ordered gt timestamps
Eigen::vector<Sophus::SE3d>
device_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns = 0;
std::vector<std::unordered_map<int64_t, double>> exposure_times;
@ -87,12 +83,7 @@ class UzhVioDataset : public VioDataset {
const Eigen::vector<Sophus::SE3d> &get_gt_pose_data() const {
return gt_pose_data;
}
const std::vector<int64_t> &get_device_pose_timestamps() const {
return device_pose_timestamps;
}
const Eigen::vector<Sophus::SE3d> &get_device_pose_data() const {
return device_pose_data;
}
int64_t get_mocap_to_imu_offset_ns() const { return mocap_to_imu_offset_ns; }
std::vector<ImageData> get_image_data(int64_t t_ns) {

18
src/vio.cpp
View File

@ -101,7 +101,6 @@ pangolin::Var<bool> continue_btn("ui.continue", false, false, true);
pangolin::Var<bool> continue_fast("ui.continue_fast", true, false, true);
Button align_svd_btn("ui.align_svd", &alignButton);
Button align_device_svd_btn("ui.align_device_svd", &alignDeviceButton);
pangolin::Var<bool> follow("ui.follow", true, false, true);
@ -117,9 +116,6 @@ Eigen::vector<Eigen::Vector3d> vio_t_w_i;
std::vector<int64_t> gt_t_ns;
Eigen::vector<Eigen::Vector3d> gt_t_w_i;
std::vector<int64_t> device_pose_t_ns;
Eigen::vector<Eigen::Vector3d> device_pose_t_w_i;
std::string marg_data_path;
size_t last_frame_processed = 0;
@ -246,13 +242,6 @@ int main(int argc, char** argv) {
gt_t_ns.push_back(vio_dataset->get_gt_timestamps()[i]);
gt_t_w_i.push_back(vio_dataset->get_gt_pose_data()[i].translation());
}
for (size_t i = 0; i < vio_dataset->get_device_pose_data().size(); i++) {
device_pose_t_ns.push_back(vio_dataset->get_device_pose_timestamps()[i]);
device_pose_t_w_i.push_back(
vio_dataset->get_device_pose_data()[i].translation());
}
std::cout << "Len " << device_pose_t_ns.size() << std::endl;
}
const int64_t start_t_ns = vio_dataset->get_image_timestamps().front();
@ -577,10 +566,6 @@ void draw_scene() {
glColor3ubv(gt_color);
if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i);
u_int8_t device_color[3]{150, 150, 0};
glColor3ubv(device_color);
if (show_device_gt) pangolin::glDrawLineStrip(device_pose_t_w_i);
size_t frame_id = show_frame;
int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
auto it = vis_map.find(t_ns);
@ -689,6 +674,3 @@ void draw_plots() {
}
void alignButton() { basalt::alignSVD(vio_t_ns, vio_t_w_i, gt_t_ns, gt_t_w_i); }
void alignDeviceButton() {
basalt::alignSVD(device_pose_t_ns, device_pose_t_w_i, gt_t_ns, gt_t_w_i);
}