ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

405
include/basalt/optical_flow/frame_to_frame_optical_flow.h Normal file
View File

@@ -0,0 +1,405 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <thread>
#include <sophus/se2.hpp>
#include <tbb/blocked_range.h>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/parallel_for.h>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/optical_flow/patch.h>
#include <basalt/image/image_pyr.h>
#include <basalt/utils/keypoints.h>
namespace basalt {
/// Unlike PatchOpticalFlow, FrameToFrameOpticalFlow always tracks patches
/// against the previous frame, not the initial frame where a track was created.
/// While it might cause more drift of the patch location, it leads to longer
/// tracks in practice.
template <typename Scalar, template <typename> typename Pattern>
class FrameToFrameOpticalFlow : public OpticalFlowBase {
public:
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
typedef Sophus::SE2<Scalar> SE2;
FrameToFrameOpticalFlow(const VioConfig& config,
const basalt::Calibration<double>& calib)
: t_ns(-1), frame_counter(0), last_keypoint_id(0), config(config) {
input_queue.set_capacity(10);
this->calib = calib.cast<Scalar>();
patch_coord = PatchT::pattern2.template cast<float>();
if (calib.intrinsics.size() > 1) {
Eigen::Matrix4d Ed;
Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1];
computeEssential(T_i_j, Ed);
E = Ed.cast<Scalar>();
}
processing_thread.reset(
new std::thread(&FrameToFrameOpticalFlow::processingLoop, this));
}
~FrameToFrameOpticalFlow() { processing_thread->join(); }
void processingLoop() {
OpticalFlowInput::Ptr input_ptr;
while (true) {
input_queue.pop(input_ptr);
if (!input_ptr.get()) {
if (output_queue) output_queue->push(nullptr);
break;
}
processFrame(input_ptr->t_ns, input_ptr);
}
}
void processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) {
for (const auto& v : new_img_vec->img_data) {
if (!v.img.get()) return;
}
if (t_ns < 0) {
t_ns = curr_t_ns;
transforms.reset(new OpticalFlowResult);
transforms->observations.resize(calib.intrinsics.size());
transforms->t_ns = t_ns;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
pyramid->at(i).setFromImage(
*new_img_vec->img_data[i].img,
config.optical_flow_levels);
}
});
transforms->input_images = new_img_vec;
addPoints();
filterPoints();
} else {
t_ns = curr_t_ns;
old_pyramid = pyramid;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
pyramid->at(i).setFromImage(
*new_img_vec->img_data[i].img,
config.optical_flow_levels);
}
});
OpticalFlowResult::Ptr new_transforms;
new_transforms.reset(new OpticalFlowResult);
new_transforms->observations.resize(calib.intrinsics.size());
new_transforms->t_ns = t_ns;
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
trackPoints(old_pyramid->at(i), pyramid->at(i),
transforms->observations[i],
new_transforms->observations[i]);
}
transforms = new_transforms;
transforms->input_images = new_img_vec;
addPoints();
filterPoints();
}
if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) {
output_queue->push(transforms);
}
frame_counter++;
}
void trackPoints(const basalt::ManagedImagePyr<uint16_t>& pyr_1,
const basalt::ManagedImagePyr<uint16_t>& pyr_2,
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_1,
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_2) const {
size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids;
Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
ids.reserve(num_points);
init_vec.reserve(num_points);
for (const auto& kv : transform_map_1) {
ids.push_back(kv.first);
init_vec.push_back(kv.second);
}
tbb::concurrent_unordered_map<KeypointId, Eigen::AffineCompact2f,
std::hash<KeypointId>>
result;
auto compute_func = [&](const tbb::blocked_range<size_t>& range) {
for (size_t r = range.begin(); r != range.end(); ++r) {
const KeypointId id = ids[r];
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
Eigen::AffineCompact2f transform_2 = transform_1;
bool valid = trackPoint(pyr_1, pyr_2, transform_1, transform_2);
if (valid) {
Eigen::AffineCompact2f transform_1_recovered = transform_2;
valid = trackPoint(pyr_2, pyr_1, transform_2, transform_1_recovered);
if (valid) {
Scalar dist2 = (transform_1.translation() -
transform_1_recovered.translation())
.squaredNorm();
if (dist2 < config.optical_flow_max_recovered_dist2) {
result[id] = transform_2;
}
}
}
}
};
tbb::blocked_range<size_t> range(0, num_points);
tbb::parallel_for(range, compute_func);
// compute_func(range);
transform_map_2.clear();
transform_map_2.insert(result.begin(), result.end());
}
inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& old_pyr,
const basalt::ManagedImagePyr<uint16_t>& pyr,
const Eigen::AffineCompact2f& old_transform,
Eigen::AffineCompact2f& transform) const {
bool patch_valid = true;
transform.linear().setIdentity();
for (int level = config.optical_flow_levels; level >= 0 && patch_valid;
level--) {
const Scalar scale = 1 << level;
transform.translation() /= scale;
PatchT p(old_pyr.lvl(level), old_transform.translation() / scale);
patch_valid &= p.valid;
if (patch_valid) {
// Perform tracking on current level
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform);
}
transform.translation() *= scale;
}
transform.linear() = old_transform.linear() * transform.linear();
return patch_valid;
}
inline bool trackPointAtLevel(const Image<const uint16_t>& img_2,
const PatchT& dp,
Eigen::AffineCompact2f& transform) const {
bool patch_valid = true;
for (int iteration = 0;
patch_valid && iteration < config.optical_flow_max_iterations;
iteration++) {
typename PatchT::VectorP res;
typename PatchT::Matrix2P transformed_pat =
transform.linear().matrix() * PatchT::pattern2;
transformed_pat.colwise() += transform.translation();
patch_valid &= dp.residual(img_2, transformed_pat, res);
if (patch_valid) {
const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
// avoid NaN in increment (leads to SE2::exp crashing)
patch_valid &= inc.array().isFinite().all();
// avoid very large increment
patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
if (patch_valid) {
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2;
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
}
}
}
return patch_valid;
}
void addPoints() {
Eigen::aligned_vector<Eigen::Vector2d> pts0;
for (const auto& kv : transforms->observations.at(0)) {
pts0.emplace_back(kv.second.translation().cast<double>());
}
KeypointsData kd;
detectKeypoints(pyramid->at(0).lvl(0), kd,
config.optical_flow_detection_grid_size, 1, pts0);
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses0,
new_poses1;
for (size_t i = 0; i < kd.corners.size(); i++) {
Eigen::AffineCompact2f transform;
transform.setIdentity();
transform.translation() = kd.corners[i].cast<Scalar>();
transforms->observations.at(0)[last_keypoint_id] = transform;
new_poses0[last_keypoint_id] = transform;
last_keypoint_id++;
}
if (calib.intrinsics.size() > 1) {
trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1);
for (const auto& kv : new_poses1) {
transforms->observations.at(1).emplace(kv);
}
}
}
void filterPoints() {
if (calib.intrinsics.size() < 2) return;
std::set<KeypointId> lm_to_remove;
std::vector<KeypointId> kpid;
Eigen::aligned_vector<Eigen::Vector2f> proj0, proj1;
for (const auto& kv : transforms->observations.at(1)) {
auto it = transforms->observations.at(0).find(kv.first);
if (it != transforms->observations.at(0).end()) {
proj0.emplace_back(it->second.translation());
proj1.emplace_back(kv.second.translation());
kpid.emplace_back(kv.first);
}
}
Eigen::aligned_vector<Eigen::Vector4f> p3d0, p3d1;
std::vector<bool> p3d0_success, p3d1_success;
calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success);
calib.intrinsics[1].unproject(proj1, p3d1, p3d1_success);
for (size_t i = 0; i < p3d0_success.size(); i++) {
if (p3d0_success[i] && p3d1_success[i]) {
const double epipolar_error =
std::abs(p3d0[i].transpose() * E * p3d1[i]);
if (epipolar_error > config.optical_flow_epipolar_error) {
lm_to_remove.emplace(kpid[i]);
}
} else {
lm_to_remove.emplace(kpid[i]);
}
}
for (int id : lm_to_remove) {
transforms->observations.at(1).erase(id);
}
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
int64_t t_ns;
size_t frame_counter;
KeypointId last_keypoint_id;
VioConfig config;
basalt::Calibration<Scalar> calib;
OpticalFlowResult::Ptr transforms;
std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
pyramid;
Matrix4 E;
std::shared_ptr<std::thread> processing_thread;
};
} // namespace basalt

468
include/basalt/optical_flow/multiscale_frame_to_frame_optical_flow.h Normal file
View File

@@ -0,0 +1,468 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
// Original source for multi-scale implementation:
// https://github.com/DLR-RM/granite (MIT license)
#pragma once
#include <thread>
#include <sophus/se2.hpp>
#include <tbb/blocked_range.h>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/parallel_for.h>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/optical_flow/patch.h>
#include <basalt/image/image_pyr.h>
#include <basalt/utils/keypoints.h>
namespace basalt {
/// MultiscaleFrameToFrameOpticalFlow is the same as FrameToFrameOpticalFlow,
/// but patches can be created at all pyramid levels, not just the lowest
/// pyramid.
template <typename Scalar, template <typename> typename Pattern>
class MultiscaleFrameToFrameOpticalFlow : public OpticalFlowBase {
public:
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
typedef Sophus::SE2<Scalar> SE2;
MultiscaleFrameToFrameOpticalFlow(const VioConfig& config,
const basalt::Calibration<double>& calib)
: t_ns(-1), frame_counter(0), last_keypoint_id(0), config(config) {
input_queue.set_capacity(10);
this->calib = calib.cast<Scalar>();
patch_coord = PatchT::pattern2.template cast<float>();
if (calib.intrinsics.size() > 1) {
Eigen::Matrix4d Ed;
Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1];
computeEssential(T_i_j, Ed);
E = Ed.cast<Scalar>();
}
processing_thread.reset(new std::thread(
&MultiscaleFrameToFrameOpticalFlow::processingLoop, this));
}
~MultiscaleFrameToFrameOpticalFlow() { processing_thread->join(); }
void processingLoop() {
OpticalFlowInput::Ptr input_ptr;
while (true) {
input_queue.pop(input_ptr);
if (!input_ptr.get()) {
if (output_queue) output_queue->push(nullptr);
break;
}
processFrame(input_ptr->t_ns, input_ptr);
}
}
bool processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) {
for (const auto& v : new_img_vec->img_data) {
if (!v.img.get()) {
std::cout << "Image for " << curr_t_ns << " not present!" << std::endl;
return true;
}
}
if (t_ns < 0) {
t_ns = curr_t_ns;
transforms.reset(new OpticalFlowResult);
transforms->observations.resize(calib.intrinsics.size());
transforms->pyramid_levels.resize(calib.intrinsics.size());
transforms->t_ns = t_ns;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
pyramid->at(i).setFromImage(
*new_img_vec->img_data[i].img,
config.optical_flow_levels);
}
});
transforms->input_images = new_img_vec;
addPoints();
filterPoints();
} else {
t_ns = curr_t_ns;
old_pyramid = pyramid;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
pyramid->resize(calib.intrinsics.size());
tbb::parallel_for(tbb::blocked_range<size_t>(0, calib.intrinsics.size()),
[&](const tbb::blocked_range<size_t>& r) {
for (size_t i = r.begin(); i != r.end(); ++i) {
pyramid->at(i).setFromImage(
*new_img_vec->img_data[i].img,
config.optical_flow_levels);
}
});
OpticalFlowResult::Ptr new_transforms;
new_transforms.reset(new OpticalFlowResult);
new_transforms->observations.resize(calib.intrinsics.size());
new_transforms->pyramid_levels.resize(calib.intrinsics.size());
new_transforms->t_ns = t_ns;
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
trackPoints(old_pyramid->at(i), pyramid->at(i),
transforms->observations[i], transforms->pyramid_levels[i],
new_transforms->observations[i],
new_transforms->pyramid_levels[i]);
}
// std::cout << t_ns << ": Could track "
// << new_transforms->observations.at(0).size() << " points."
// << std::endl;
transforms = new_transforms;
transforms->input_images = new_img_vec;
addPoints();
filterPoints();
}
if (frame_counter % config.optical_flow_skip_frames == 0) {
try {
output_queue->push(transforms);
} catch (const tbb::user_abort&) {
return false;
};
}
frame_counter++;
return true;
}
void trackPoints(
const basalt::ManagedImagePyr<uint16_t>& pyr_1,
const basalt::ManagedImagePyr<uint16_t>& pyr_2,
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_1,
const std::map<KeypointId, size_t>& pyramid_levels_1,
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>& transform_map_2,
std::map<KeypointId, size_t>& pyramid_levels_2) const {
size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids;
Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
std::vector<size_t> pyramid_level;
ids.reserve(num_points);
init_vec.reserve(num_points);
pyramid_level.reserve(num_points);
for (const auto& kv : transform_map_1) {
ids.push_back(kv.first);
init_vec.push_back(kv.second);
pyramid_level.push_back(pyramid_levels_1.at(kv.first));
}
tbb::concurrent_unordered_map<KeypointId, Eigen::AffineCompact2f,
std::hash<KeypointId>>
result_transforms;
tbb::concurrent_unordered_map<KeypointId, size_t, std::hash<KeypointId>>
result_pyramid_level;
auto compute_func = [&](const tbb::blocked_range<size_t>& range) {
for (size_t r = range.begin(); r != range.end(); ++r) {
const KeypointId id = ids[r];
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
Eigen::AffineCompact2f transform_2 = transform_1;
bool valid = trackPoint(pyr_1, pyr_2, transform_1, pyramid_level[r],
transform_2);
if (valid) {
Eigen::AffineCompact2f transform_1_recovered = transform_2;
valid = trackPoint(pyr_2, pyr_1, transform_2, pyramid_level[r],
transform_1_recovered);
if (valid) {
const Scalar scale = 1 << pyramid_level[r];
Scalar dist2 = (transform_1.translation() / scale -
transform_1_recovered.translation() / scale)
.squaredNorm();
if (dist2 < config.optical_flow_max_recovered_dist2) {
result_transforms[id] = transform_2;
result_pyramid_level[id] = pyramid_level[r];
}
}
}
}
};
tbb::blocked_range<size_t> range(0, num_points);
tbb::parallel_for(range, compute_func);
// compute_func(range);
transform_map_2.clear();
transform_map_2.insert(result_transforms.begin(), result_transforms.end());
pyramid_levels_2.clear();
pyramid_levels_2.insert(result_pyramid_level.begin(),
result_pyramid_level.end());
}
inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& old_pyr,
const basalt::ManagedImagePyr<uint16_t>& pyr,
const Eigen::AffineCompact2f& old_transform,
const size_t pyramid_level,
Eigen::AffineCompact2f& transform) const {
bool patch_valid = true;
transform.linear().setIdentity();
for (ssize_t level = config.optical_flow_levels;
level >= static_cast<ssize_t>(pyramid_level); level--) {
const Scalar scale = 1 << level;
Eigen::AffineCompact2f transform_tmp = transform;
transform_tmp.translation() /= scale;
PatchT p(old_pyr.lvl(level), old_transform.translation() / scale);
patch_valid &= p.valid;
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) {
return false;
}
transform_tmp.translation() *= scale;
if (patch_valid) {
transform = transform_tmp;
}
}
transform.linear() = old_transform.linear() * transform.linear();
return patch_valid;
}
inline bool trackPointAtLevel(const Image<const uint16_t>& img_2,
const PatchT& dp,
Eigen::AffineCompact2f& transform) const {
bool patch_valid = true;
for (int iteration = 0;
patch_valid && iteration < config.optical_flow_max_iterations;
iteration++) {
typename PatchT::VectorP res;
typename PatchT::Matrix2P transformed_pat =
transform.linear().matrix() * PatchT::pattern2;
transformed_pat.colwise() += transform.translation();
patch_valid &= dp.residual(img_2, transformed_pat, res);
if (patch_valid) {
const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
// avoid NaN in increment (leads to SE2::exp crashing)
patch_valid &= inc.array().isFinite().all();
// avoid very large increment
patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
if (patch_valid) {
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2;
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
}
}
}
return patch_valid;
}
void addPoints() {
KeypointsData kd;
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses_main,
new_poses_stereo;
std::map<KeypointId, size_t> new_pyramid_levels_main,
new_pyramid_levels_stereo;
for (ssize_t level = 0;
level < static_cast<ssize_t>(config.optical_flow_levels) - 1;
level++) {
Eigen::aligned_vector<Eigen::Vector2d> pts;
for (const auto& kv : transforms->observations.at(0)) {
const ssize_t point_level =
transforms->pyramid_levels.at(0).at(kv.first);
// do not create points were already points at similar levels are
if (point_level <= level + 1 && point_level >= level - 1) {
// if (point_level == level) {
const Scalar scale = 1 << point_level;
pts.emplace_back(
(kv.second.translation() / scale).template cast<double>());
}
}
detectKeypoints(pyramid->at(0).lvl(level), kd,
config.optical_flow_detection_grid_size, 1, pts);
const Scalar scale = 1 << level;
for (size_t i = 0; i < kd.corners.size(); i++) {
Eigen::AffineCompact2f transform;
transform.setIdentity();
transform.translation() =
kd.corners[i].cast<Scalar>() * scale; // TODO cast float?
transforms->observations.at(0)[last_keypoint_id] = transform;
transforms->pyramid_levels.at(0)[last_keypoint_id] = level;
new_poses_main[last_keypoint_id] = transform;
new_pyramid_levels_main[last_keypoint_id] = level;
last_keypoint_id++;
}
trackPoints(pyramid->at(0), pyramid->at(1), new_poses_main,
new_pyramid_levels_main, new_poses_stereo,
new_pyramid_levels_stereo);
for (const auto& kv : new_poses_stereo) {
transforms->observations.at(1).emplace(kv);
transforms->pyramid_levels.at(1)[kv.first] =
new_pyramid_levels_stereo.at(kv.first);
}
}
}
void filterPoints() {
std::set<KeypointId> lm_to_remove;
std::vector<KeypointId> kpid;
Eigen::aligned_vector<Eigen::Vector2f> proj0, proj1;
for (const auto& kv : transforms->observations.at(1)) {
auto it = transforms->observations.at(0).find(kv.first);
if (it != transforms->observations.at(0).end()) {
proj0.emplace_back(it->second.translation());
proj1.emplace_back(kv.second.translation());
kpid.emplace_back(kv.first);
}
}
Eigen::aligned_vector<Eigen::Vector4f> p3d_main, p3d_stereo;
std::vector<bool> p3d_main_success, p3d_stereo_success;
calib.intrinsics[0].unproject(proj0, p3d_main, p3d_main_success);
calib.intrinsics[1].unproject(proj1, p3d_stereo, p3d_stereo_success);
for (size_t i = 0; i < p3d_main_success.size(); i++) {
if (p3d_main_success[i] && p3d_stereo_success[i]) {
const double epipolar_error =
std::abs(p3d_main[i].transpose() * E * p3d_stereo[i]);
const Scalar scale = 1 << transforms->pyramid_levels.at(0).at(kpid[i]);
if (epipolar_error > config.optical_flow_epipolar_error * scale) {
lm_to_remove.emplace(kpid[i]);
}
} else {
lm_to_remove.emplace(kpid[i]);
}
}
for (int id : lm_to_remove) {
transforms->observations.at(1).erase(id);
}
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
int64_t t_ns;
size_t frame_counter;
KeypointId last_keypoint_id;
VioConfig config;
basalt::Calibration<Scalar> calib;
OpticalFlowResult::Ptr transforms;
std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
pyramid;
// map from stereo pair -> essential matrix
Matrix4 E;
std::shared_ptr<std::thread> processing_thread;
};
} // namespace basalt

91
include/basalt/optical_flow/optical_flow.h Normal file
View File

@@ -0,0 +1,91 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <memory>
#include <Eigen/Geometry>
#include <basalt/utils/vio_config.h>
#include <basalt/io/dataset_io.h>
#include <basalt/calibration/calibration.hpp>
#include <basalt/camera/stereographic_param.hpp>
#include <basalt/utils/sophus_utils.hpp>
#include <tbb/concurrent_queue.h>
#include <opencv2/highgui/highgui.hpp>
namespace basalt {
using KeypointId = size_t;
struct OpticalFlowInput {
using Ptr = std::shared_ptr<OpticalFlowInput>;
int64_t t_ns;
std::vector<ImageData> img_data;
std::vector<cv::Mat> img_cv_data;
};
struct OpticalFlowResult {
using Ptr = std::shared_ptr<OpticalFlowResult>;
int64_t t_ns;
std::vector<Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>>
observations;
std::vector<std::map<KeypointId, size_t>> pyramid_levels;
OpticalFlowInput::Ptr input_images;
};
class OpticalFlowBase {
public:
using Ptr = std::shared_ptr<OpticalFlowBase>;
tbb::concurrent_bounded_queue<OpticalFlowInput::Ptr> input_queue;
tbb::concurrent_bounded_queue<OpticalFlowResult::Ptr>* output_queue = nullptr;
Eigen::MatrixXf patch_coord;
};
class OpticalFlowFactory {
public:
static OpticalFlowBase::Ptr getOpticalFlow(const VioConfig& config,
const Calibration<double>& cam);
};
} // namespace basalt

226
include/basalt/optical_flow/patch.h Normal file
View File

@@ -0,0 +1,226 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <Eigen/Dense>
#include <sophus/se2.hpp>
#include <basalt/image/image.h>
#include <basalt/optical_flow/patterns.h>
namespace basalt {
template <typename Scalar, typename Pattern>
struct OpticalFlowPatch {
static constexpr int PATTERN_SIZE = Pattern::PATTERN_SIZE;
typedef Eigen::Matrix<int, 2, 1> Vector2i;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 1, 2> Vector2T;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 1> VectorP;
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 2> MatrixP2;
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 3> MatrixP3;
typedef Eigen::Matrix<Scalar, 3, PATTERN_SIZE> Matrix3P;
typedef Eigen::Matrix<Scalar, PATTERN_SIZE, 4> MatrixP4;
typedef Eigen::Matrix<int, 2, PATTERN_SIZE> Matrix2Pi;
static const Matrix2P pattern2;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
OpticalFlowPatch() = default;
OpticalFlowPatch(const Image<const uint16_t> &img, const Vector2 &pos) {
setFromImage(img, pos);
}
template <typename ImgT>
static void setData(const ImgT &img, const Vector2 &pos, Scalar &mean,
VectorP &data, const Sophus::SE2<Scalar> *se2 = nullptr) {
int num_valid_points = 0;
Scalar sum = 0;
for (int i = 0; i < PATTERN_SIZE; i++) {
Vector2 p;
if (se2) {
p = pos + (*se2) * pattern2.col(i);
} else {
p = pos + pattern2.col(i);
};
if (img.InBounds(p, 2)) {
Scalar val = img.template interp<Scalar>(p);
data[i] = val;
sum += val;
num_valid_points++;
} else {
data[i] = -1;
}
}
mean = sum / num_valid_points;
data /= mean;
}
template <typename ImgT>
static void setDataJacSe2(const ImgT &img, const Vector2 &pos, Scalar &mean,
VectorP &data, MatrixP3 &J_se2) {
int num_valid_points = 0;
Scalar sum = 0;
Vector3 grad_sum_se2(0, 0, 0);
Eigen::Matrix<Scalar, 2, 3> Jw_se2;
Jw_se2.template topLeftCorner<2, 2>().setIdentity();
for (int i = 0; i < PATTERN_SIZE; i++) {
Vector2 p = pos + pattern2.col(i);
// Fill jacobians with respect to SE2 warp
Jw_se2(0, 2) = -pattern2(1, i);
Jw_se2(1, 2) = pattern2(0, i);
if (img.InBounds(p, 2)) {
Vector3 valGrad = img.template interpGrad<Scalar>(p);
data[i] = valGrad[0];
sum += valGrad[0];
J_se2.row(i) = valGrad.template tail<2>().transpose() * Jw_se2;
grad_sum_se2 += J_se2.row(i);
num_valid_points++;
} else {
data[i] = -1;
}
}
mean = sum / num_valid_points;
const Scalar mean_inv = num_valid_points / sum;
for (int i = 0; i < PATTERN_SIZE; i++) {
if (data[i] >= 0) {
J_se2.row(i) -= grad_sum_se2.transpose() * data[i] / sum;
data[i] *= mean_inv;
} else {
J_se2.row(i).setZero();
}
}
J_se2 *= mean_inv;
}
void setFromImage(const Image<const uint16_t> &img, const Vector2 &pos) {
this->pos = pos;
MatrixP3 J_se2;
setDataJacSe2(img, pos, mean, data, J_se2);
Matrix3 H_se2 = J_se2.transpose() * J_se2;
Matrix3 H_se2_inv;
H_se2_inv.setIdentity();
H_se2.ldlt().solveInPlace(H_se2_inv);
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,
const Matrix2P &transformed_pattern,
VectorP &residual) const {
Scalar sum = 0;
int num_valid_points = 0;
for (int i = 0; i < PATTERN_SIZE; i++) {
if (img.InBounds(transformed_pattern.col(i), 2)) {
residual[i] = img.interp<Scalar>(transformed_pattern.col(i));
sum += residual[i];
num_valid_points++;
} else {
residual[i] = -1;
}
}
// all-black patch cannot be normalized
if (sum < std::numeric_limits<Scalar>::epsilon()) {
residual.setZero();
return false;
}
int num_residuals = 0;
for (int i = 0; i < PATTERN_SIZE; i++) {
if (residual[i] >= 0 && data[i] >= 0) {
const Scalar val = residual[i];
residual[i] = num_valid_points * val / sum - data[i];
num_residuals++;
} else {
residual[i] = 0;
}
}
return num_residuals > PATTERN_SIZE / 2;
}
Vector2 pos = Vector2::Zero();
VectorP data = VectorP::Zero(); // negative if the point is not valid
// MatrixP3 J_se2; // total jacobian with respect to se2 warp
// Matrix3 H_se2_inv;
Matrix3P H_se2_inv_J_se2_T = Matrix3P::Zero();
Scalar mean = 0;
bool valid = false;
};
template <typename Scalar, typename Pattern>
const typename OpticalFlowPatch<Scalar, Pattern>::Matrix2P
OpticalFlowPatch<Scalar, Pattern>::pattern2 = Pattern::pattern2;
} // namespace basalt

410
include/basalt/optical_flow/patch_optical_flow.h Normal file
View File

@@ -0,0 +1,410 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <thread>
#include <sophus/se2.hpp>
#include <tbb/blocked_range.h>
#include <tbb/concurrent_unordered_map.h>
#include <tbb/parallel_for.h>
#include <basalt/optical_flow/optical_flow.h>
#include <basalt/optical_flow/patch.h>
#include <basalt/image/image_pyr.h>
#include <basalt/utils/keypoints.h>
namespace basalt {
// TODO: patches are currently never erased, so over time memory consumption
// increases
// TODO: some changes from FrameToFrameOpticalFlow could be back-ported
// (adjustments to Scalar=float, tbb parallelization, ...).
/// PatchOpticalFlow keeps reference patches from the frame where the point was
/// initially created. Should result in more consistent tracks (less drift over
/// time) than frame-to-frame tracking, but it results in shorter tracks in
/// practice.
template <typename Scalar, template <typename> typename Pattern>
class PatchOpticalFlow : public OpticalFlowBase {
public:
typedef OpticalFlowPatch<Scalar, Pattern<Scalar>> PatchT;
typedef Eigen::Matrix<Scalar, 2, 1> Vector2;
typedef Eigen::Matrix<Scalar, 2, 2> Matrix2;
typedef Eigen::Matrix<Scalar, 3, 1> Vector3;
typedef Eigen::Matrix<Scalar, 3, 3> Matrix3;
typedef Eigen::Matrix<Scalar, 4, 1> Vector4;
typedef Eigen::Matrix<Scalar, 4, 4> Matrix4;
typedef Sophus::SE2<Scalar> SE2;
PatchOpticalFlow(const VioConfig& config,
const basalt::Calibration<double>& calib)
: t_ns(-1),
frame_counter(0),
last_keypoint_id(0),
config(config),
calib(calib) {
patches.reserve(3000);
input_queue.set_capacity(10);
patch_coord = PatchT::pattern2.template cast<float>();
if (calib.intrinsics.size() > 1) {
Sophus::SE3d T_i_j = calib.T_i_c[0].inverse() * calib.T_i_c[1];
computeEssential(T_i_j, E);
}
processing_thread.reset(
new std::thread(&PatchOpticalFlow::processingLoop, this));
}
~PatchOpticalFlow() { processing_thread->join(); }
void processingLoop() {
OpticalFlowInput::Ptr input_ptr;
while (true) {
input_queue.pop(input_ptr);
if (!input_ptr.get()) {
output_queue->push(nullptr);
break;
}
processFrame(input_ptr->t_ns, input_ptr);
}
}
void processFrame(int64_t curr_t_ns, OpticalFlowInput::Ptr& new_img_vec) {
for (const auto& v : new_img_vec->img_data) {
if (!v.img.get()) return;
}
if (t_ns < 0) {
t_ns = curr_t_ns;
transforms.reset(new OpticalFlowResult);
transforms->observations.resize(calib.intrinsics.size());
transforms->t_ns = t_ns;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
pyramid->resize(calib.intrinsics.size());
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
pyramid->at(i).setFromImage(*new_img_vec->img_data[i].img,
config.optical_flow_levels);
}
transforms->input_images = new_img_vec;
addPoints();
filterPoints();
} else {
t_ns = curr_t_ns;
old_pyramid = pyramid;
pyramid.reset(new std::vector<basalt::ManagedImagePyr<uint16_t>>);
pyramid->resize(calib.intrinsics.size());
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
pyramid->at(i).setFromImage(*new_img_vec->img_data[i].img,
config.optical_flow_levels);
}
OpticalFlowResult::Ptr new_transforms;
new_transforms.reset(new OpticalFlowResult);
new_transforms->observations.resize(new_img_vec->img_data.size());
new_transforms->t_ns = t_ns;
for (size_t i = 0; i < calib.intrinsics.size(); i++) {
trackPoints(old_pyramid->at(i), pyramid->at(i),
transforms->observations[i],
new_transforms->observations[i]);
}
transforms = new_transforms;
transforms->input_images = new_img_vec;
addPoints();
filterPoints();
}
if (output_queue && frame_counter % config.optical_flow_skip_frames == 0) {
output_queue->push(transforms);
}
frame_counter++;
}
void trackPoints(const basalt::ManagedImagePyr<uint16_t>& pyr_1,
const basalt::ManagedImagePyr<uint16_t>& pyr_2,
const Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_1,
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f>&
transform_map_2) const {
size_t num_points = transform_map_1.size();
std::vector<KeypointId> ids;
Eigen::aligned_vector<Eigen::AffineCompact2f> init_vec;
ids.reserve(num_points);
init_vec.reserve(num_points);
for (const auto& kv : transform_map_1) {
ids.push_back(kv.first);
init_vec.push_back(kv.second);
}
tbb::concurrent_unordered_map<KeypointId, Eigen::AffineCompact2f,
std::hash<KeypointId>>
result;
auto compute_func = [&](const tbb::blocked_range<size_t>& range) {
for (size_t r = range.begin(); r != range.end(); ++r) {
const KeypointId id = ids[r];
const Eigen::AffineCompact2f& transform_1 = init_vec[r];
Eigen::AffineCompact2f transform_2 = transform_1;
const Eigen::aligned_vector<PatchT>& patch_vec = patches.at(id);
bool valid = trackPoint(pyr_2, patch_vec, transform_2);
if (valid) {
Eigen::AffineCompact2f transform_1_recovered = transform_2;
valid = trackPoint(pyr_1, patch_vec, transform_1_recovered);
if (valid) {
Scalar dist2 = (transform_1.translation() -
transform_1_recovered.translation())
.squaredNorm();
if (dist2 < config.optical_flow_max_recovered_dist2) {
result[id] = transform_2;
}
}
}
}
};
tbb::blocked_range<size_t> range(0, num_points);
tbb::parallel_for(range, compute_func);
// compute_func(range);
transform_map_2.clear();
transform_map_2.insert(result.begin(), result.end());
}
inline bool trackPoint(const basalt::ManagedImagePyr<uint16_t>& pyr,
const Eigen::aligned_vector<PatchT>& patch_vec,
Eigen::AffineCompact2f& transform) const {
bool patch_valid = true;
for (int level = config.optical_flow_levels; level >= 0 && patch_valid;
level--) {
const Scalar scale = 1 << level;
transform.translation() /= scale;
// TODO: maybe we should better check patch validity when creating points
const auto& p = patch_vec[level];
patch_valid &= p.valid;
if (patch_valid) {
// Perform tracking on current level
patch_valid &= trackPointAtLevel(pyr.lvl(level), p, transform);
}
transform.translation() *= scale;
}
return patch_valid;
}
inline bool trackPointAtLevel(const Image<const uint16_t>& img_2,
const PatchT& dp,
Eigen::AffineCompact2f& transform) const {
bool patch_valid = true;
for (int iteration = 0;
patch_valid && iteration < config.optical_flow_max_iterations;
iteration++) {
typename PatchT::VectorP res;
typename PatchT::Matrix2P transformed_pat =
transform.linear().matrix() * PatchT::pattern2;
transformed_pat.colwise() += transform.translation();
patch_valid &= dp.residual(img_2, transformed_pat, res);
if (patch_valid) {
const Vector3 inc = -dp.H_se2_inv_J_se2_T * res;
// avoid NaN in increment (leads to SE2::exp crashing)
patch_valid &= inc.array().isFinite().all();
// avoid very large increment
patch_valid &= inc.template lpNorm<Eigen::Infinity>() < 1e6;
if (patch_valid) {
transform *= SE2::exp(inc).matrix();
const int filter_margin = 2;
patch_valid &= img_2.InBounds(transform.translation(), filter_margin);
}
}
}
return patch_valid;
}
void addPoints() {
Eigen::aligned_vector<Eigen::Vector2d> pts0;
for (const auto& kv : transforms->observations.at(0)) {
pts0.emplace_back(kv.second.translation().cast<double>());
}
KeypointsData kd;
detectKeypoints(pyramid->at(0).lvl(0), kd,
config.optical_flow_detection_grid_size, 1, pts0);
Eigen::aligned_map<KeypointId, Eigen::AffineCompact2f> new_poses0,
new_poses1;
for (size_t i = 0; i < kd.corners.size(); i++) {
Eigen::aligned_vector<PatchT>& p = patches[last_keypoint_id];
Vector2 pos = kd.corners[i].cast<Scalar>();
for (int l = 0; l <= config.optical_flow_levels; l++) {
Scalar scale = 1 << l;
Vector2 pos_scaled = pos / scale;
p.emplace_back(pyramid->at(0).lvl(l), pos_scaled);
}
Eigen::AffineCompact2f transform;
transform.setIdentity();
transform.translation() = kd.corners[i].cast<Scalar>();
transforms->observations.at(0)[last_keypoint_id] = transform;
new_poses0[last_keypoint_id] = transform;
last_keypoint_id++;
}
if (calib.intrinsics.size() > 1) {
trackPoints(pyramid->at(0), pyramid->at(1), new_poses0, new_poses1);
for (const auto& kv : new_poses1) {
transforms->observations.at(1).emplace(kv);
}
}
}
void filterPoints() {
if (calib.intrinsics.size() < 2) return;
std::set<KeypointId> lm_to_remove;
std::vector<KeypointId> kpid;
Eigen::aligned_vector<Eigen::Vector2d> proj0, proj1;
for (const auto& kv : transforms->observations.at(1)) {
auto it = transforms->observations.at(0).find(kv.first);
if (it != transforms->observations.at(0).end()) {
proj0.emplace_back(it->second.translation().cast<double>());
proj1.emplace_back(kv.second.translation().cast<double>());
kpid.emplace_back(kv.first);
}
}
Eigen::aligned_vector<Eigen::Vector4d> p3d0, p3d1;
std::vector<bool> p3d0_success, p3d1_success;
calib.intrinsics[0].unproject(proj0, p3d0, p3d0_success);
calib.intrinsics[1].unproject(proj1, p3d1, p3d1_success);
for (size_t i = 0; i < p3d0_success.size(); i++) {
if (p3d0_success[i] && p3d1_success[i]) {
const double epipolar_error =
std::abs(p3d0[i].transpose() * E * p3d1[i]);
if (epipolar_error > config.optical_flow_epipolar_error) {
lm_to_remove.emplace(kpid[i]);
}
} else {
lm_to_remove.emplace(kpid[i]);
}
}
for (int id : lm_to_remove) {
transforms->observations.at(1).erase(id);
}
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
int64_t t_ns;
size_t frame_counter;
KeypointId last_keypoint_id;
VioConfig config;
basalt::Calibration<double> calib;
Eigen::aligned_unordered_map<KeypointId, Eigen::aligned_vector<PatchT>>
patches;
OpticalFlowResult::Ptr transforms;
std::shared_ptr<std::vector<basalt::ManagedImagePyr<uint16_t>>> old_pyramid,
pyramid;
Eigen::Matrix4d E;
std::shared_ptr<std::thread> processing_thread;
};
} // namespace basalt

171
include/basalt/optical_flow/patterns.h Normal file
View File

@@ -0,0 +1,171 @@
/**
BSD 3-Clause License
This file is part of the Basalt project.
https://gitlab.com/VladyslavUsenko/basalt.git
Copyright (c) 2019, Vladyslav Usenko and Nikolaus Demmel.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <Eigen/Dense>
namespace basalt {
template <class Scalar>
struct Pattern24 {
// 00 01
//
// 02 03 04 05
//
// 06 07 08 09 10 11
//
// 12 13 14 15 16 17
//
// 18 19 20 21
//
// 22 23
//
// -----> x
// |
// |
// y
static constexpr Scalar pattern_raw[][2] = {
{-1, 5}, {1, 5},
{-3, 3}, {-1, 3}, {1, 3}, {3, 3},
{-5, 1}, {-3, 1}, {-1, 1}, {1, 1}, {3, 1}, {5, 1},
{-5, -1}, {-3, -1}, {-1, -1}, {1, -1}, {3, -1}, {5, -1},
{-3, -3}, {-1, -3}, {1, -3}, {3, -3},
{-1, -5}, {1, -5}
};
static constexpr int PATTERN_SIZE =
sizeof(pattern_raw) / (2 * sizeof(Scalar));
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
static const Matrix2P pattern2;
};
template <class Scalar>
const typename Pattern24<Scalar>::Matrix2P Pattern24<Scalar>::pattern2 =
Eigen::Map<Pattern24<Scalar>::Matrix2P>((Scalar *)
Pattern24<Scalar>::pattern_raw);
template <class Scalar>
struct Pattern52 {
// 00 01 02 03
//
// 04 05 06 07 08 09
//
// 10 11 12 13 14 15 16 17
//
// 18 19 20 21 22 23 24 25
//
// 26 27 28 29 30 31 32 33
//
// 34 35 36 37 38 39 40 41
//
// 42 43 44 45 46 47
//
// 48 49 50 51
//
// -----> x
// |
// |
// y
static constexpr Scalar pattern_raw[][2] = {
{-3, 7}, {-1, 7}, {1, 7}, {3, 7},
{-5, 5}, {-3, 5}, {-1, 5}, {1, 5}, {3, 5}, {5, 5},
{-7, 3}, {-5, 3}, {-3, 3}, {-1, 3}, {1, 3}, {3, 3},
{5, 3}, {7, 3},
{-7, 1}, {-5, 1}, {-3, 1}, {-1, 1}, {1, 1}, {3, 1},
{5, 1}, {7, 1},
{-7, -1}, {-5, -1}, {-3, -1}, {-1, -1}, {1, -1}, {3, -1},
{5, -1}, {7, -1},
{-7, -3}, {-5, -3}, {-3, -3}, {-1, -3}, {1, -3}, {3, -3},
{5, -3}, {7, -3},
{-5, -5}, {-3, -5}, {-1, -5}, {1, -5}, {3, -5}, {5, -5},
{-3, -7}, {-1, -7}, {1, -7}, {3, -7}
};
static constexpr int PATTERN_SIZE =
sizeof(pattern_raw) / (2 * sizeof(Scalar));
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
static const Matrix2P pattern2;
};
template <class Scalar>
const typename Pattern52<Scalar>::Matrix2P Pattern52<Scalar>::pattern2 =
Eigen::Map<Pattern52<Scalar>::Matrix2P>((Scalar *)
Pattern52<Scalar>::pattern_raw);
// Same as Pattern52 but twice smaller
template <class Scalar>
struct Pattern51 {
static constexpr int PATTERN_SIZE = Pattern52<Scalar>::PATTERN_SIZE;
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
static const Matrix2P pattern2;
};
template <class Scalar>
const typename Pattern51<Scalar>::Matrix2P Pattern51<Scalar>::pattern2 =
0.5 * Pattern52<Scalar>::pattern2;
// Same as Pattern52 but 0.75 smaller
template <class Scalar>
struct Pattern50 {
static constexpr int PATTERN_SIZE = Pattern52<Scalar>::PATTERN_SIZE;
typedef Eigen::Matrix<Scalar, 2, PATTERN_SIZE> Matrix2P;
static const Matrix2P pattern2;
};
template <class Scalar>
const typename Pattern50<Scalar>::Matrix2P Pattern50<Scalar>::pattern2 =
0.75 * Pattern52<Scalar>::pattern2;
} // namespace basalt