added use_calib option

This commit is contained in:
Vladyslav Usenko 2019-07-31 15:39:03 +02:00
parent 1e2fcb4ce9
commit f1fe43a250
3 changed files with 37 additions and 28 deletions

View File

@ -67,7 +67,7 @@ class EurocVioDataset : public VioDataset {
Eigen::vector<Sophus::SE3d> Eigen::vector<Sophus::SE3d>
device_pose_data; // TODO: change to eigen aligned device_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns; int64_t mocap_to_imu_offset_ns = 0;
std::vector<std::unordered_map<int64_t, double>> exposure_times; std::vector<std::unordered_map<int64_t, double>> exposure_times;

View File

@ -68,7 +68,7 @@ class UzhVioDataset : public VioDataset {
Eigen::vector<Sophus::SE3d> Eigen::vector<Sophus::SE3d>
device_pose_data; // TODO: change to eigen aligned device_pose_data; // TODO: change to eigen aligned
int64_t mocap_to_imu_offset_ns; int64_t mocap_to_imu_offset_ns = 0;
std::vector<std::unordered_map<int64_t, double>> exposure_times; std::vector<std::unordered_map<int64_t, double>> exposure_times;

View File

@ -66,17 +66,15 @@ int main(int argc, char **argv) {
std::string output_gyro_path; std::string output_gyro_path;
std::string output_mocap_path; std::string output_mocap_path;
bool show_gui = false; bool show_gui = true;
CLI::App app{"Calibrate time offset"}; CLI::App app{"Calibrate time offset"};
app.add_option("-d,--dataset-path", dataset_path, "Path to dataset") app.add_option("-d,--dataset-path", dataset_path, "Path to dataset")
->required(); ->required();
app.add_option("--calibration", calibration_path, "Path to calibration file") app.add_option("--calibration", calibration_path, "Path to calibration file");
->required();
app.add_option("--mocap-calibration", mocap_calibration_path, app.add_option("--mocap-calibration", mocap_calibration_path,
"Path to mocap calibration file") "Path to mocap calibration file");
->required();
app.add_option("--dataset-type", dataset_type, "Dataset type <euroc, bag>.") app.add_option("--dataset-type", dataset_type, "Dataset type <euroc, bag>.")
->required(); ->required();
@ -101,27 +99,32 @@ int main(int argc, char **argv) {
basalt::VioDatasetPtr vio_dataset; basalt::VioDatasetPtr vio_dataset;
std::ifstream is(calibration_path); const bool use_calib =
!(calibration_path.empty() || mocap_calibration_path.empty());
if (is.good()) { if (use_calib) {
cereal::JSONInputArchive archive(is); std::ifstream is(calibration_path);
archive(calib);
std::cout << "Loaded calibration from: " << calibration_path << std::endl;
} else {
std::cerr << "No calibration found" << std::endl;
std::abort();
}
std::ifstream mocap_is(mocap_calibration_path); if (is.good()) {
cereal::JSONInputArchive archive(is);
archive(calib);
std::cout << "Loaded calibration from: " << calibration_path << std::endl;
} else {
std::cerr << "No calibration found" << std::endl;
std::abort();
}
if (mocap_is.good()) { std::ifstream mocap_is(mocap_calibration_path);
cereal::JSONInputArchive archive(mocap_is);
archive(mocap_calib); if (mocap_is.good()) {
std::cout << "Loaded mocap calibration from: " << mocap_calibration_path cereal::JSONInputArchive archive(mocap_is);
<< std::endl; archive(mocap_calib);
} else { std::cout << "Loaded mocap calibration from: " << mocap_calibration_path
std::cerr << "No mocap calibration found" << std::endl; << std::endl;
std::abort(); } else {
std::cerr << "No mocap calibration found" << std::endl;
std::abort();
}
} }
basalt::DatasetIoInterfacePtr dataset_io = basalt::DatasetIoInterfacePtr dataset_io =
@ -148,7 +151,11 @@ int main(int argc, char **argv) {
gyro_timestamps.push_back(vio_dataset->get_gyro_data()[i].timestamp_ns); gyro_timestamps.push_back(vio_dataset->get_gyro_data()[i].timestamp_ns);
Eigen::Vector3d measurement = vio_dataset->get_gyro_data()[i].data; Eigen::Vector3d measurement = vio_dataset->get_gyro_data()[i].data;
gyro_data.push_back(calib.calib_gyro_bias.getCalibrated(measurement)); if (use_calib) {
gyro_data.push_back(calib.calib_gyro_bias.getCalibrated(measurement));
} else {
gyro_data.push_back(measurement);
}
} }
std::cout << "saturated gyro measurement count: " << saturation_count std::cout << "saturated gyro measurement count: " << saturation_count
<< std::endl; << std::endl;
@ -156,7 +163,8 @@ int main(int argc, char **argv) {
// compute rotational velocity from mocap data // compute rotational velocity from mocap data
{ {
Sophus::SE3d T_mark_i = mocap_calib.T_i_mark.inverse(); Sophus::SE3d T_mark_i;
if (use_calib) T_mark_i = mocap_calib.T_i_mark.inverse();
int saturation_count = 0; int saturation_count = 0;
for (size_t i = 1; i < vio_dataset->get_gt_timestamps().size() - 1; i++) { for (size_t i = 1; i < vio_dataset->get_gt_timestamps().size() - 1; i++) {
@ -373,7 +381,8 @@ int main(int argc, char **argv) {
std::cout << "Saving aligned dataset in " std::cout << "Saving aligned dataset in "
<< dataset_path + "mav0/gt/data.csv" << std::endl; << dataset_path + "mav0/gt/data.csv" << std::endl;
// output corrected mocap data // output corrected mocap data
Sophus::SE3d T_mark_i = mocap_calib.T_i_mark.inverse(); Sophus::SE3d T_mark_i;
if (use_calib) T_mark_i = mocap_calib.T_i_mark.inverse();
fs::create_directory(dataset_path + "mav0/gt/"); fs::create_directory(dataset_path + "mav0/gt/");
std::ofstream gt_out_stream; std::ofstream gt_out_stream;
gt_out_stream.open(dataset_path + "mav0/gt/data.csv"); gt_out_stream.open(dataset_path + "mav0/gt/data.csv");