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,6 +99,10 @@ int main(int argc, char **argv) {
basalt::VioDatasetPtr vio_dataset; basalt::VioDatasetPtr vio_dataset;
const bool use_calib =
!(calibration_path.empty() || mocap_calibration_path.empty());
if (use_calib) {
std::ifstream is(calibration_path); std::ifstream is(calibration_path);
if (is.good()) { if (is.good()) {
@ -123,6 +125,7 @@ int main(int argc, char **argv) {
std::cerr << "No mocap calibration found" << std::endl; std::cerr << "No mocap calibration found" << std::endl;
std::abort(); std::abort();
} }
}
basalt::DatasetIoInterfacePtr dataset_io = basalt::DatasetIoInterfacePtr dataset_io =
basalt::DatasetIoFactory::getDatasetIo(dataset_type); basalt::DatasetIoFactory::getDatasetIo(dataset_type);
@ -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;
if (use_calib) {
gyro_data.push_back(calib.calib_gyro_bias.getCalibrated(measurement)); 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");