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

Merge branch 'fix_so3_patch' into 'master' 

Update patch jacobian computation

See merge request basalt/basalt!53
This commit is contained in:
Vladyslav Usenko 2022-01-22 08:00:13 +00:00
parent 92d754e075 3973aebe93
commit 6d55fdab9f
3 changed files with 148 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -35,6 +35,7 @@ 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>
@ -71,23 +72,57 @@ struct OpticalFlowPatch {
setFromImage(img, pos);
}
void setFromImage(const Image<const uint16_t> &img, const Vector2 &pos) {
this->pos = 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;
Vector2 grad_sum(0, 0);
MatrixP2 grad;
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.interpGrad<Scalar>(p);
Vector3 valGrad = img.template interpGrad<Scalar>(p);
data[i] = valGrad[0];
sum += valGrad[0];
grad.row(i) = valGrad.template tail<2>();
grad_sum += valGrad.template tail<2>();
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;
@ -96,30 +131,25 @@ struct OpticalFlowPatch {
mean = sum / num_valid_points;
Scalar mean_inv = num_valid_points / sum;
Eigen::Matrix<Scalar, 2, 3> Jw_se2;
Jw_se2.template topLeftCorner<2, 2>().setIdentity();
MatrixP3 J_se2;
const Scalar mean_inv = num_valid_points / sum;
for (int i = 0; i < PATTERN_SIZE; i++) {
if (data[i] >= 0) {
const Scalar data_i = data[i];
const Vector2 grad_i = grad.row(i);
grad.row(i) =
num_valid_points * (grad_i * sum - grad_sum * data_i) / (sum * sum);
J_se2.row(i) -= grad_sum_se2.transpose() * data[i] / sum;
data[i] *= mean_inv;
} else {
grad.row(i).setZero();
J_se2.row(i).setZero();
}
// Fill jacobians with respect to SE2 warp
Jw_se2(0, 2) = -pattern2(1, i);
Jw_se2(1, 2) = pattern2(0, i);
J_se2.row(i) = grad.row(i) * Jw_se2;
}
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;
@ -143,7 +173,6 @@ struct OpticalFlowPatch {
const Matrix2P &transformed_pattern,
VectorP &residual) const {
Scalar sum = 0;
Vector2 grad_sum(0, 0);
int num_valid_points = 0;
for (int i = 0; i < PATTERN_SIZE; i++) {

4
test/CMakeLists.txt
View File

@ -20,6 +20,9 @@ target_link_libraries(test_qr gtest gtest_main basalt)
add_executable(test_linearization src/test_linearization.cpp)
target_link_libraries(test_linearization gtest gtest_main basalt)
add_executable(test_patch src/test_patch.cpp)
target_link_libraries(test_patch gtest gtest_main basalt)
enable_testing()
include(GoogleTest)
@ -33,3 +36,4 @@ gtest_add_tests(TARGET test_vio AUTO)
gtest_add_tests(TARGET test_nfr AUTO)
gtest_add_tests(TARGET test_qr AUTO)
gtest_add_tests(TARGET test_linearization AUTO)
gtest_add_tests(TARGET test_patch AUTO)

89
test/src/test_patch.cpp Normal file
View File

@ -0,0 +1,89 @@
#include <basalt/optical_flow/patch.h>
#include <sophus/se2.hpp>
#include <iostream>
#include "gtest/gtest.h"
#include "test_utils.h"
struct SmoothFunction {
template <typename Scalar>
Scalar interp(const Eigen::Matrix<Scalar, 2, 1>& p) const {
return sin(p[0] / 100.0 + p[1] / 20.0);
}
template <typename Scalar>
Eigen::Matrix<Scalar, 3, 1> interpGrad(
const Eigen::Matrix<Scalar, 2, 1>& p) const {
return Eigen::Matrix<Scalar, 3, 1>(sin(p[0] / 100.0 + p[1] / 20.0),
cos(p[0] / 100.0 + p[1] / 20.0) / 100.0,
cos(p[0] / 100.0 + p[1] / 20.0) / 20.0);
}
template <typename Derived>
BASALT_HOST_DEVICE inline bool InBounds(
const Eigen::MatrixBase<Derived>& p,
const typename Derived::Scalar border) const {
return true;
}
};
TEST(Patch, ImageInterpolateGrad) {
Eigen::Vector2i offset(231, 123);
SmoothFunction img;
Eigen::Vector2d pd = offset.cast<double>() + Eigen::Vector2d(0.4, 0.34345);
Eigen::Vector3d val_grad = img.interpGrad<double>(pd);
Eigen::Matrix<double, 1, 2> J_x = val_grad.tail<2>();
test_jacobian(
"d_res_d_x", J_x,
[&](const Eigen::Vector2d& x) {
return Eigen::Matrix<double, 1, 1>(img.interp<double>(pd + x));
},
Eigen::Vector2d::Zero(), 1);
}
TEST(Patch, PatchSe2Jac) {
Eigen::Vector2i offset(231, 123);
SmoothFunction img_view;
Eigen::Vector2d pd = offset.cast<double>() + Eigen::Vector2d(0.4, 0.34345);
using PatternT = basalt::Pattern52<double>;
using PatchT = basalt::OpticalFlowPatch<double, PatternT>;
double mean, mean2;
PatchT::VectorP data, data2;
PatchT::MatrixP3 J_se2;
basalt::OpticalFlowPatch<double, basalt::Pattern52<double>>::setDataJacSe2(
img_view, pd, mean, data, J_se2);
basalt::OpticalFlowPatch<double, basalt::Pattern52<double>>::setData(
img_view, pd, mean2, data2);
EXPECT_NEAR(mean, mean2, 1e-8);
EXPECT_TRUE(data.isApprox(data2));
test_jacobian(
"d_res_d_se2", J_se2,
[&](const Eigen::Vector3d& x) {
Sophus::SE2d transform = Sophus::SE2d::exp(x);
double mean3;
PatchT::VectorP data3;
basalt::OpticalFlowPatch<double, basalt::Pattern52<double>>::setData(
img_view, pd, mean3, data3, &transform);
return data3;
},
Eigen::Vector3d::Zero());
}