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

Merge branch 'demmeln/fix-rs-t265' into 'master' 

fix timestamp issues with realsense t265

Closes #16

See merge request basalt/basalt!50
This commit is contained in:
Vladyslav Usenko 2021-12-11 14:04:11 +00:00
parent 6df075a1f7 700376d846
commit 412229a2c5
8 changed files with 102 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,7 +79,7 @@ class RsT265Device {
RsT265Device(bool manual_exposure, int skip_frames, int webp_quality,
double exposure_value = 10.0);
~RsT265Device();
void start();
void stop();

2
include/basalt/utils/vio_config.h
View File

@ -78,8 +78,6 @@ struct VioConfig {
double vio_lm_lambda_initial;
double vio_lm_lambda_min;
double vio_lm_lambda_max;
int vio_lm_landmark_damping_variant;
int vio_lm_pose_damping_variant;
bool vio_scale_jacobian;

2
include/basalt/vi_estimator/sqrt_keypoint_vio.h
View File

@ -248,6 +248,8 @@ class SqrtKeypointVioEstimator : public VioEstimatorBase,
VioConfig config;
constexpr static Scalar vee_factor = Scalar(2.0);
constexpr static Scalar initial_vee = Scalar(2.0);
Scalar lambda, min_lambda, max_lambda, lambda_vee;
std::shared_ptr<std::thread> processing_thread;

2
include/basalt/vi_estimator/sqrt_keypoint_vo.h
View File

@ -238,6 +238,8 @@ class SqrtKeypointVoEstimator : public VioEstimatorBase,
VioConfig config;
constexpr static Scalar vee_factor = Scalar(2.0);
constexpr static Scalar initial_vee = Scalar(2.0);
Scalar lambda, min_lambda, max_lambda, lambda_vee;
std::shared_ptr<std::thread> processing_thread;

52
src/device/rs_t265.cpp
View File

@ -84,8 +84,6 @@ RsT265Device::RsT265Device(bool manual_exposure, int skip_frames,
}
}
RsT265Device::~RsT265Device(){};
void RsT265Device::start() {
auto callback = [&](const rs2::frame& frame) {
exportCalibration();
@ -148,20 +146,47 @@ void RsT265Device::start() {
}
}
} else if (auto fs = frame.as<rs2::frameset>()) {
if (frame_counter++ % skip_frames != 0) return;
BASALT_ASSERT(fs.size() == NUM_CAMS);
std::vector<rs2::video_frame> vfs;
for (int i = 0; i < NUM_CAMS; ++i) {
rs2::video_frame vf = fs[i].as<rs2::video_frame>();
if (!vf) {
std::cout << "Weird Frame, skipping" << std::endl;
return;
}
vfs.push_back(vf);
}
// Callback is called for every new image, so in every other call, the
// left frame is updated but the right frame is still from the previous
// timestamp. So we only process framesets where both images are valid and
// have the same timestamp.
for (int i = 1; i < NUM_CAMS; ++i) {
if (vfs[0].get_timestamp() != vfs[i].get_timestamp()) {
return;
}
}
// skip frames if configured
if (frame_counter++ % skip_frames != 0) {
return;
}
OpticalFlowInput::Ptr data(new OpticalFlowInput);
data->img_data.resize(NUM_CAMS);
for (int i = 0; i < NUM_CAMS; i++) {
auto f = fs[i];
if (!f.as<rs2::video_frame>()) {
std::cout << "Weird Frame, skipping" << std::endl;
continue;
}
auto vf = f.as<rs2::video_frame>();
// std::cout << "Reading frame " << frame_counter << std::endl;
data->t_ns = vf.get_timestamp() * 1e6;
for (int i = 0; i < NUM_CAMS; i++) {
const auto& vf = vfs[i];
int64_t t_ns = vf.get_timestamp() * 1e6;
// at this stage both image timestamps are expected to be equal
BASALT_ASSERT(i == 0 || t_ns == data->t_ns);
data->t_ns = t_ns;
data->img_data[i].exposure =
vf.get_frame_metadata(RS2_FRAME_METADATA_ACTUAL_EXPOSURE) * 1e-6;
@ -178,6 +203,11 @@ void RsT265Device::start() {
val = val << 8;
data_out[j] = val;
}
// std::cout << "Timestamp / exposure " << i << ": " <<
// data->t_ns << " / "
// << int(data->img_data[i].exposure * 1e3) << "ms" <<
// std::endl;
}
last_img_data = data;

8
src/utils/vio_config.cpp
View File

@ -75,11 +75,9 @@ VioConfig::VioConfig() {
vio_enforce_realtime = false;
vio_use_lm = false;
vio_lm_lambda_initial = 1e-8;
vio_lm_lambda_min = 1e-32;
vio_lm_lambda_initial = 1e-4;
vio_lm_lambda_min = 1e-6;
vio_lm_lambda_max = 1e2;
vio_lm_landmark_damping_variant = 0;
vio_lm_pose_damping_variant = 0;
vio_scale_jacobian = true;
@ -192,8 +190,6 @@ void serialize(Archive& ar, basalt::VioConfig& config) {
ar(CEREAL_NVP(config.vio_lm_lambda_initial));
ar(CEREAL_NVP(config.vio_lm_lambda_min));
ar(CEREAL_NVP(config.vio_lm_lambda_max));
ar(CEREAL_NVP(config.vio_lm_landmark_damping_variant));
ar(CEREAL_NVP(config.vio_lm_pose_damping_variant));
ar(CEREAL_NVP(config.vio_scale_jacobian));

39
src/vi_estimator/sqrt_keypoint_vio.cpp
View File

@ -226,6 +226,10 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
prev_frame->t_ns, last_state.getState().bias_gyro,
last_state.getState().bias_accel));
BASALT_ASSERT_MSG(prev_frame->t_ns < curr_frame->t_ns,
"duplicate frame timestamps?! zero time delta leads "
"to invalid IMU integration.");
while (data->t_ns <= prev_frame->t_ns) {
data = popFromImuDataQueue();
if (!data) break;
@ -1127,6 +1131,7 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
}
if (config.vio_debug) {
// TODO: num_points debug output missing
std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl;
std::cout << "Iteration " << it << " " << error_total << std::endl;
@ -1197,14 +1202,32 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
stats.add("get_dense_H_b", t.reset()).format("ms");
if (config.vio_lm_pose_damping_variant == 1) {
VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
H.diagonal() += Hdiag_lambda;
}
int iter = 0;
bool inc_valid = false;
constexpr int max_num_iter = 3;
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
while (iter < max_num_iter && !inc_valid) {
VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
MatX H_copy = H;
H_copy.diagonal() += Hdiag_lambda;
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H_copy);
inc = ldlt.solve(b);
stats.add("solve", t.reset()).format("ms");
if (!inc.array().isFinite().all()) {
lambda = lambda_vee * lambda;
lambda_vee *= vee_factor;
} else {
inc_valid = true;
}
iter++;
}
if (!inc_valid) {
std::cerr << "Still invalid inc after " << max_num_iter
<< " iterations." << std::endl;
}
}
// backup state (then apply increment and check cost decrease)
@ -1283,8 +1306,8 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
relative_decrease, step_norminf);
}
// TODO: consider to remove assert. For now we want to test if we even
// run into the l_diff <= 0 case ever in practice
// TODO: consider to remove assert. For now we want to test if we
// even run into the l_diff <= 0 case ever in practice
// BASALT_ASSERT_STREAM(l_diff > 0, "l_diff " << l_diff);
// l_diff <= 0 is a theoretical possibility if the model cost change
@ -1327,7 +1350,6 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
1 - std::pow<Scalar>(2 * relative_decrease - 1, 3));
lambda = std::max(min_lambda, lambda);
constexpr Scalar initial_vee = Scalar(2.0);
lambda_vee = initial_vee;
it++;
@ -1356,7 +1378,6 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
}
lambda = lambda_vee * lambda;
constexpr Scalar vee_factor = Scalar(2.0);
lambda_vee *= vee_factor;
// lambda = std::max(min_lambda, lambda);

31
src/vi_estimator/sqrt_keypoint_vo.cpp
View File

@ -1029,6 +1029,7 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
stats.add("performQR", t.reset()).format("ms");
if (config.vio_debug) {
// TODO: num_points debug output missing
std::cout << "[LINEARIZE] Error: " << error_total << " num points "
<< std::endl;
std::cout << "Iteration " << it << " " << error_total << std::endl;
@ -1098,14 +1099,32 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
stats.add("get_dense_H_b", t.reset()).format("ms");
if (config.vio_lm_pose_damping_variant == 1) {
VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
H.diagonal() += Hdiag_lambda;
}
int iter = 0;
bool inc_valid = false;
constexpr int max_num_iter = 3;
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H);
while (iter < max_num_iter && !inc_valid) {
VecX Hdiag_lambda = (H.diagonal() * lambda).cwiseMax(min_lambda);
MatX H_copy = H;
H_copy.diagonal() += Hdiag_lambda;
Eigen::LDLT<Eigen::Ref<MatX>> ldlt(H_copy);
inc = ldlt.solve(b);
stats.add("solve", t.reset()).format("ms");
if (!inc.array().isFinite().all()) {
lambda = lambda_vee * lambda;
lambda_vee *= vee_factor;
} else {
inc_valid = true;
}
iter++;
}
if (!inc_valid) {
std::cerr << "Still invalid inc after " << max_num_iter
<< " iterations." << std::endl;
}
}
// backup state (then apply increment and check cost decrease)
@ -1217,7 +1236,6 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
1 - std::pow<Scalar>(2 * relative_decrease - 1, 3));
lambda = std::max(min_lambda, lambda);
constexpr Scalar initial_vee = Scalar(2.0);
lambda_vee = initial_vee;
it++;
@ -1245,7 +1263,6 @@ void SqrtKeypointVoEstimator<Scalar_>::optimize() {
}
lambda = lambda_vee * lambda;
constexpr Scalar vee_factor = Scalar(2.0);
lambda_vee *= vee_factor;
// lambda = std::max(min_lambda, lambda);