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

T265 record manual exposure control, compare calib script

This commit is contained in:
Michael Loipfuehrer 2019-06-19 13:24:25 +00:00 committed by Vladyslav Usenko
parent 077af5937f
commit 36880bd9ef
5 changed files with 154 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
include/basalt/device/rs_t265.h
View File

@ -103,6 +103,8 @@ class RsT265Device {
int skip_frames;
int webp_quality;
int frame_counter = 0;
Eigen::deque<RsIMUData> gyro_data_queue;
std::shared_ptr<RsIMUData> 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;
};

97
scripts/compare_calib.py Executable file
View File

@ -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)

20
src/device/rs_t265.cpp
View File

@ -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<rs2::frameset>()) {
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;
}

63
src/rs_t265_record.cpp
View File

@ -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<uint16_t>::Ptr image_raw =
img->img_data[cam_id].img;
for (size_t cam_id = 0; cam_id < NUM_CAMS; ++cam_id) {
basalt::ManagedImage<uint16_t>::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<int> compression_params = {cv::IMWRITE_WEBP_QUALITY,
webp_quality};
cv::imwrite(filename, image, compression_params);
std::vector<int> 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<int> compression_params = {cv::IMWRITE_JPEG_QUALITY,
webp_quality};
cv::imwrite(filename, image, compression_params);
std::vector<int> 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);
}

5
src/rs_t265_vio.cpp
View File

@ -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;
}