diff --git a/include/basalt/device/rs_t265.h b/include/basalt/device/rs_t265.h index f317462..49d440b 100644 --- a/include/basalt/device/rs_t265.h +++ b/include/basalt/device/rs_t265.h @@ -103,6 +103,8 @@ class RsT265Device { int skip_frames; int webp_quality; + int frame_counter = 0; + Eigen::deque gyro_data_queue; std::shared_ptr prev_accel_data; @@ -111,6 +113,7 @@ class RsT265Device { rs2::context context; rs2::config config; rs2::pipeline pipe; + rs2::sensor sensor; rs2::pipeline_profile profile; }; diff --git a/scripts/compare_calib.py b/scripts/compare_calib.py new file mode 100755 index 0000000..498e660 --- /dev/null +++ b/scripts/compare_calib.py @@ -0,0 +1,97 @@ +#!/usr/bin/env python3 + + +import argparse +import json +import numpy as np +from scipy.spatial.transform import Rotation + + +def print_abs_rel(info, v_0, v_1): + diff = np.abs(np.linalg.norm(v_0 - v_1)) + out = f'{info}:\t{diff:.5f}' + + if diff < 10e-7: + out += ' (0.0%)' + else: + out += f' ({diff / (np.abs(np.linalg.norm(v_0)) * 100.0):.7f}%)' + + print(out) + + +def main(calib_path_1, calib_path_2): + with open(calib_path_1, 'r') as c_1, open(calib_path_2, 'r') as c_2: + calib0 = json.load(c_1) + calib1 = json.load(c_2) + + for i, (t_imu_cam_0, t_imu_cam_1) in enumerate( + zip(calib0['value0']['T_imu_cam'], calib1['value0']['T_imu_cam'])): + print(f'\nCamera {i} transformation differences') + t_0 = np.array(list(t_imu_cam_0.values())[0:2]) + t_1 = np.array(list(t_imu_cam_1.values())[0:2]) + r_0 = Rotation(list(t_imu_cam_0.values())[3:7]) + r_1 = Rotation(list(t_imu_cam_1.values())[3:7]) + + print_abs_rel(f'Transformation', t_0, t_1) + print_abs_rel(f'Rotation', r_0.as_rotvec(), r_1.as_rotvec()) + + for i, (intrinsics0, intrinsics1) in enumerate( + zip(calib0['value0']['intrinsics'], calib1['value0']['intrinsics'])): + print(f'\nCamera {i} intrinsics differences') + + for ( + k_0, v_0), (_, v_1) in zip( + intrinsics0['intrinsics'].items(), intrinsics1['intrinsics'].items()): + print_abs_rel(f'Difference for {k_0}', v_0, v_1) + + print_abs_rel('\nAccel Bias Difference', + np.array(calib0['value0']['calib_accel_bias'][0:2]), + np.array(calib1['value0']['calib_accel_bias'][0:2])) + + print_abs_rel('Accel Scale Difference', + np.array(calib0['value0']['calib_accel_bias'][3:9]), + np.array(calib1['value0']['calib_accel_bias'][3:9])) + + print_abs_rel('Gyro Bias Difference', + np.array(calib0['value0']['calib_gyro_bias'][0:2]), + np.array(calib1['value0']['calib_gyro_bias'][0:2])) + + print_abs_rel('Gyro Scale Difference', + np.array(calib0['value0']['calib_gyro_bias'][3:12]), + np.array(calib1['value0']['calib_gyro_bias'][3:12])) + + print_abs_rel( + '\nAccel Noise Std Difference', + calib0['value0']['accel_noise_std'], + calib1['value0']['accel_noise_std']) + print_abs_rel( + 'Gyro Noise Std Difference', + calib0['value0']['gyro_noise_std'], + calib1['value0']['gyro_noise_std']) + print_abs_rel( + 'Accel Bias Std Difference', + calib0['value0']['accel_bias_std'], + calib1['value0']['accel_bias_std']) + print_abs_rel( + 'Gyro Bias Std Difference', + calib0['value0']['gyro_bias_std'], + calib1['value0']['gyro_bias_std']) + + print_abs_rel( + '\nCam Time Offset Difference', + calib0['value0']['cam_time_offset_ns'], + calib0['value0']['cam_time_offset_ns']) + + +def create_parser(): + parser = argparse.ArgumentParser() + parser.add_argument('calib_path_1') + parser.add_argument('calib_path_2') + + return parser + + +if __name__ == '__main__': + args = create_parser().parse_args() + + main(args.calib_path_1, args.calib_path_2) diff --git a/src/device/rs_t265.cpp b/src/device/rs_t265.cpp index bb93ea1..5bc5377 100644 --- a/src/device/rs_t265.cpp +++ b/src/device/rs_t265.cpp @@ -70,15 +70,15 @@ RsT265Device::RsT265Device(bool manual_exposure, int skip_frames, } } - if (manual_exposure) { - auto device = context.query_devices()[0]; - std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME) - << " connected" << std::endl; - auto sens = device.query_sensors()[0]; + auto device = context.query_devices()[0]; + std::cout << "Device " << device.get_info(RS2_CAMERA_INFO_NAME) + << " connected" << std::endl; + sensor = device.query_sensors()[0]; + if (manual_exposure) { std::cout << "Enabling manual exposure control" << std::endl; - sens.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); - sens.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000); + sensor.set_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, 0); + sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure_value * 1000); } } @@ -153,6 +153,8 @@ void RsT265Device::start() { } } } else if (auto fs = frame.as()) { + if (frame_counter++ % skip_frames != 0) return; + OpticalFlowInput::Ptr data(new OpticalFlowInput); data->img_data.resize(NUM_CAMS); @@ -214,9 +216,7 @@ void RsT265Device::stop() { bool RsT265Device::setExposure(double exposure) { if (!manual_exposure) return false; - auto device = context.query_devices()[0]; - auto sens = device.query_sensors()[0]; - sens.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure * 1000); + sensor.set_option(rs2_option::RS2_OPTION_EXPOSURE, exposure * 1000); return true; } diff --git a/src/rs_t265_record.cpp b/src/rs_t265_record.cpp index 58fbe04..0f4b588 100644 --- a/src/rs_t265_record.cpp +++ b/src/rs_t265_record.cpp @@ -97,7 +97,7 @@ void image_save_worker() { while (!stop_workers) { if (image_data_queue.try_pop(img)) { - if (recording) + if (recording) { for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) { #if CV_MAJOR_VERSION >= 3 cam_data[cam_id] << img->t_ns << "," << img->t_ns << ".webp" @@ -112,38 +112,39 @@ void image_save_worker() { << std::endl; } - for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) { - basalt::ManagedImage::Ptr image_raw = - img->img_data[cam_id].img; + for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) { + basalt::ManagedImage::Ptr image_raw = + img->img_data[cam_id].img; - if (!image_raw.get()) continue; + if (!image_raw.get()) continue; - cv::Mat image(image_raw->h, image_raw->w, CV_8U); + cv::Mat image(image_raw->h, image_raw->w, CV_8U); - uint8_t *dst = image.ptr(); - const uint16_t *src = image_raw->ptr; + uint8_t *dst = image.ptr(); + const uint16_t *src = image_raw->ptr; - for (size_t i = 0; i < image_raw->size(); i++) { - dst[i] = (src[i] >> 8); - } + for (size_t i = 0; i < image_raw->size(); i++) { + dst[i] = (src[i] >> 8); + } #if CV_MAJOR_VERSION >= 3 - std::string filename = dataset_dir + "mav0/cam" + - std::to_string(cam_id) + "/data/" + - std::to_string(img->t_ns) + ".webp"; + std::string filename = dataset_dir + "mav0/cam" + + std::to_string(cam_id) + "/data/" + + std::to_string(img->t_ns) + ".webp"; - std::vector compression_params = {cv::IMWRITE_WEBP_QUALITY, - webp_quality}; - cv::imwrite(filename, image, compression_params); + std::vector compression_params = {cv::IMWRITE_WEBP_QUALITY, + webp_quality}; + cv::imwrite(filename, image, compression_params); #else - std::string filename = dataset_dir + "mav0/cam" + - std::to_string(cam_id) + "/data/" + - std::to_string(img->t_ns) + ".jpg"; + std::string filename = dataset_dir + "mav0/cam" + + std::to_string(cam_id) + "/data/" + + std::to_string(img->t_ns) + ".jpg"; - std::vector compression_params = {cv::IMWRITE_JPEG_QUALITY, - webp_quality}; - cv::imwrite(filename, image, compression_params); + std::vector compression_params = {cv::IMWRITE_JPEG_QUALITY, + webp_quality}; + cv::imwrite(filename, image, compression_params); #endif + } } } else { std::this_thread::sleep_for(std::chrono::milliseconds(10)); @@ -272,6 +273,7 @@ void stopRecording() { exposure_data[0].close(); exposure_data[1].close(); imu0_data.close(); + pose_data.close(); std::cout << "Stopped recording dataset in " << dataset_dir << std::endl; } @@ -300,6 +302,10 @@ int main(int argc, char *argv[]) { return app.exit(e); } + if (dataset_path[dataset_path.length() - 1] != '/') { + dataset_path += '/'; + } + bool show_gui = true; stop_workers = false; @@ -379,6 +385,17 @@ int main(int argc, char *argv[]) { } }; + iv->OnSelectionCallback = + [&](pangolin::ImageView::OnSelectionEventData o) { + int64_t curr_t_ns = std::chrono::high_resolution_clock::now() + .time_since_epoch() + .count(); + if (std::abs(record_t_ns - curr_t_ns) > int64_t(2e9)) { + toggleRecording(dataset_path); + record_t_ns = curr_t_ns; + } + }; + img_view.push_back(iv); img_view_display.AddDisplay(*iv); } diff --git a/src/rs_t265_vio.cpp b/src/rs_t265_vio.cpp index f555d23..e707443 100644 --- a/src/rs_t265_vio.cpp +++ b/src/rs_t265_vio.cpp @@ -116,6 +116,7 @@ basalt::OpticalFlowBase::Ptr opt_flow_ptr; basalt::VioEstimatorBase::Ptr vio; int main(int argc, char** argv) { + bool terminate = false; bool show_gui = true; bool print_queue = false; std::string cam_calib_path; @@ -239,7 +240,7 @@ int main(int argc, char** argv) { if (print_queue) { t5.reset(new std::thread([&]() { - while (true) { + while (!terminate) { std::cout << "opt_flow_ptr->input_queue " << opt_flow_ptr->input_queue.size() << " opt_flow_ptr->output_queue " @@ -348,9 +349,11 @@ int main(int argc, char** argv) { } t265_device->stop(); + terminate = true; if (t3.get()) t3->join(); t4.join(); + if (t5.get()) t5->join(); return 0; }