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

Merge branch 'demmeln/optical-flow-fix-numeric-crash' into 'master' 

optical flow fix numeric crash

Closes #17

See merge request basalt/basalt!48
This commit is contained in:
Vladyslav Usenko 2021-12-04 10:50:41 +00:00
parent 99321d1af3 64a6ab4262
commit 94db673166
4 changed files with 91 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
include/basalt/optical_flow/frame_to_frame_optical_flow.h
View File

@ -249,8 +249,11 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
PatchT p(old_pyr.lvl(level), old_transform.translation() / scale); PatchT p(old_pyr.lvl(level), old_transform.translation() / scale);
// Perform tracking on current level patch_valid &= p.valid;
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform); if (patch_valid) {
// Perform tracking on current level
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform);
}
transform.translation() *= scale; transform.translation() *= scale;
} }
@ -274,18 +277,24 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
transform.linear().matrix() * PatchT::pattern2; transform.linear().matrix() * PatchT::pattern2;
transformed_pat.colwise() += transform.translation(); transformed_pat.colwise() += transform.translation();
bool valid = dp.residual(img_2, transformed_pat, res); patch_valid &= dp.residual(img_2, transformed_pat, res);
if (valid) { if (patch_valid) {
Vector3 inc = -dp.H_se2_inv_J_se2_T * res; const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2; // avoid NaN in increment (leads to SE2::exp crashing)
patch_valid &= inc.array().isFinite().all();
if (!img_2.InBounds(transform.translation(), filter_margin)) // avoid very large increment
patch_valid = false; patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
} else {
patch_valid = false; if (patch_valid) {
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2;
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
}
} }
} }

43
include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h
View File

@ -122,7 +122,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
transforms->pyramid_levels.resize(calib.intrinsics.size()); transforms->pyramid_levels.resize(calib.intrinsics.size());
transforms->t_ns = t_ns; transforms->t_ns = t_ns;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<u_int16_t>>); pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
pyramid->resize(calib.intrinsics.size()); pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()), tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
@ -143,7 +143,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
old_pyramid = pyramid; old_pyramid = pyramid;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<u_int16_t>>); pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
pyramid->resize(calib.intrinsics.size()); pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()), tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& r) { [&](const tbb::blocked_range<size_t>& r) {
@ -191,8 +191,8 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
} }
void trackPoints( void trackPoints(
const basalt::ManagedImagePyr<u_int16_t>& pyr_1, const basalt::ManagedImagePyr<uint16_t>& pyr_1,
const basalt::ManagedImagePyr<u_int16_t>& pyr_2, const basalt::ManagedImagePyr<uint16_t>& pyr_2,
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_1, transform_map_1,
const std::map<KeypointId, size_t>& pyramid_levels_1, const std::map<KeypointId, size_t>& pyramid_levels_1,
@ -282,8 +282,11 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
PatchT p(old_pyr.lvl(level), old_transform.translation() / scale); PatchT p(old_pyr.lvl(level), old_transform.translation() / scale);
// Perform tracking on current level patch_valid &= p.valid;
patch_valid = trackPointAtLevel(pyr.lvl(level), p, transform_tmp); if (patch_valid) {
// Perform tracking on current level
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform_tmp);
}
if (level == static_cast<ssize_t>(pyramid_level) + 1 && !patch_valid) { if (level == static_cast<ssize_t>(pyramid_level) + 1 && !patch_valid) {
return false; return false;
@ -301,7 +304,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
return patch_valid; return patch_valid;
} }
inline bool trackPointAtLevel(const Image<const u_int16_t>& img_2, inline bool trackPointAtLevel(const Image<const uint16_t>& img_2,
const PatchT& dp, const PatchT& dp,
Eigen::AffineCompact2f& transform) const { Eigen::AffineCompact2f& transform) const {
bool patch_valid = true; bool patch_valid = true;
@ -315,18 +318,24 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
transform.linear().matrix() * PatchT::pattern2; transform.linear().matrix() * PatchT::pattern2;
transformed_pat.colwise() += transform.translation(); transformed_pat.colwise() += transform.translation();
bool valid = dp.residual(img_2, transformed_pat, res); patch_valid &= dp.residual(img_2, transformed_pat, res);
if (valid) { if (patch_valid) {
Vector3 inc = -dp.H_se2_inv_J_se2_T * res; const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2; // avoid NaN in increment (leads to SE2::exp crashing)
patch_valid &= inc.array().isFinite().all();
if (!img_2.InBounds(transform.translation(), filter_margin)) // avoid very large increment
patch_valid = false; patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
} else {
patch_valid = false; if (patch_valid) {
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2;
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
}
} }
} }
@ -444,7 +453,7 @@ class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
basalt::Calibration<Scalar> calib; basalt::Calibration<Scalar> calib;
OpticalFlowResult::Ptr transforms; OpticalFlowResult::Ptr transforms;
std::shared_ptr<std::vector<basalt::ManagedImagePyr<u_int16_t>>> old_pyramid, std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
pyramid; pyramid;
// map from stereo pair -> essential matrix // map from stereo pair -> essential matrix

28
include/basalt/optical_flow/patch.h
View File

@ -65,7 +65,7 @@ struct OpticalFlowPatch {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
OpticalFlowPatch() { mean = 0; } OpticalFlowPatch() = default;
OpticalFlowPatch(const Image<const uint16_t> &img, const Vector2 &pos) { OpticalFlowPatch(const Image<const uint16_t> &img, const Vector2 &pos) {
setFromImage(img, pos); setFromImage(img, pos);
@ -127,6 +127,16 @@ struct OpticalFlowPatch {
H_se2.ldlt().solveInPlace(H_se2_inv); H_se2.ldlt().solveInPlace(H_se2_inv);
H_se2_inv_J_se2_T = H_se2_inv * J_se2.transpose(); H_se2_inv_J_se2_T = H_se2_inv * J_se2.transpose();
// NOTE: while it's very unlikely we get a source patch with all black
// pixels, since points are usually selected at corners, it doesn't cost
// much to be safe here.
// all-black patch cannot be normalized; will result in mean of "zero" and
// H_se2_inv_J_se2_T will contain "NaN" and data will contain "inf"
valid = mean > std::numeric_limits<Scalar>::epsilon() &&
H_se2_inv_J_se2_T.array().isFinite().all() &&
data.array().isFinite().all();
} }
inline bool residual(const Image<const uint16_t> &img, inline bool residual(const Image<const uint16_t> &img,
@ -146,6 +156,12 @@ struct OpticalFlowPatch {
} }
} }
// all-black patch cannot be normalized
if (sum < std::numeric_limits<Scalar>::epsilon()) {
residual.setZero();
return false;
}
int num_residuals = 0; int num_residuals = 0;
for (int i = 0; i < PATTERN_SIZE; i++) { for (int i = 0; i < PATTERN_SIZE; i++) {
@ -162,14 +178,16 @@ struct OpticalFlowPatch {
return num_residuals > PATTERN_SIZE / 2; return num_residuals > PATTERN_SIZE / 2;
} }
Vector2 pos; Vector2 pos = Vector2::Zero();
VectorP data; // negative if the point is not valid VectorP data = VectorP::Zero(); // negative if the point is not valid
// MatrixP3 J_se2; // total jacobian with respect to se2 warp // MatrixP3 J_se2; // total jacobian with respect to se2 warp
// Matrix3 H_se2_inv; // Matrix3 H_se2_inv;
Matrix3P H_se2_inv_J_se2_T; Matrix3P H_se2_inv_J_se2_T = Matrix3P::Zero();
Scalar mean; Scalar mean = 0;
bool valid = false;
}; };
template <typename Scalar, typename Pattern> template <typename Scalar, typename Pattern>

34
include/basalt/optical_flow/patch_optical_flow.h
View File

@ -236,9 +236,13 @@ class PatchOpticalFlow : public OpticalFlowBase {
transform.translation() /= scale; transform.translation() /= scale;
// Perform tracking on current level // TODO: maybe we should better check patch validity when creating points
patch_valid &= const auto& p = patch_vec[level];
trackPointAtLevel(pyr.lvl(level), patch_vec[level], transform); patch_valid &= p.valid;
if (patch_valid) {
// Perform tracking on current level
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform);
}
transform.translation() *= scale; transform.translation() *= scale;
} }
@ -260,18 +264,24 @@ class PatchOpticalFlow : public OpticalFlowBase {
transform.linear().matrix() * PatchT::pattern2; transform.linear().matrix() * PatchT::pattern2;
transformed_pat.colwise() += transform.translation(); transformed_pat.colwise() += transform.translation();
bool valid = dp.residual(img_2, transformed_pat, res); patch_valid &= dp.residual(img_2, transformed_pat, res);
if (valid) { if (patch_valid) {
Vector3 inc = -dp.H_se2_inv_J_se2_T * res; const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2; // avoid NaN in increment (leads to SE2::exp crashing)
patch_valid &= inc.array().isFinite().all();
if (!img_2.InBounds(transform.translation(), filter_margin)) // avoid very large increment
patch_valid = false; patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
} else {
patch_valid = false; if (patch_valid) {
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2;
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
}
} }
} }