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

new triangulation method

This commit is contained in:
Vladyslav Usenko 2019-07-23 15:21:39 +02:00
parent 843942bf44
commit 2f18e1d333
4 changed files with 51 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
include/basalt/vi_estimator/ba_base.h
View File

@ -148,6 +148,7 @@ class BundleAdjustmentBase {
Eigen::Matrix<double, 2, 4> Jp;
bool valid = cam.project(p_t_3d, res, &Jp);
valid &= res.array().isFinite().all();
if (!valid) {
// std::cerr << " Invalid projection! kpt_pos.dir "
@ -197,6 +198,7 @@ class BundleAdjustmentBase {
Eigen::Matrix<double, 2, 4> Jp;
bool valid = cam.project(p_h_3d, res, &Jp);
valid &= res.array().isFinite().all();
if (!valid) {
// std::cerr << " Invalid projection! kpt_pos.dir "
@ -246,9 +248,46 @@ class BundleAdjustmentBase {
Eigen::MatrixXd& marg_H,
Eigen::VectorXd& marg_b);
static Eigen::Vector4d triangulate(const Eigen::Vector3d& p0_3d,
const Eigen::Vector3d& p1_3d,
const Sophus::SE3d& T_0_1);
/// Triangulates the point and returns homogenous representation. First 3
/// components - unit-length direction vector. Last component inverse
/// distance.
template <class Derived>
static Eigen::Matrix<typename Derived::Scalar, 4, 1> triangulate(
const Eigen::MatrixBase<Derived>& p0_3d,
const Eigen::MatrixBase<Derived>& p1_3d,
const Sophus::SE3<typename Derived::Scalar>& T_0_1) {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
using Scalar = typename Derived::Scalar;
using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
using Vec4 = Eigen::Matrix<Scalar, 4, 1>;
using Mat33 = Eigen::Matrix<Scalar, 3, 3>;
Vec3 t12 = T_0_1.translation();
Mat33 R12 = T_0_1.rotationMatrix();
Eigen::Matrix<Scalar, 3, 4> P1 = Eigen::Matrix<Scalar, 3, 4>::Zero();
P1.template block<3, 3>(0, 0) = Eigen::Matrix<Scalar, 3, 3>::Identity();
Eigen::Matrix<Scalar, 3, 4> P2 = Eigen::Matrix<Scalar, 3, 4>::Zero();
P2.template block<3, 3>(0, 0) = R12.transpose();
P2.template block<3, 1>(0, 3) = -R12.transpose() * t12;
Vec3 f1 = p0_3d;
Vec3 f2 = p1_3d;
Eigen::Matrix<Scalar, 4, 4> A(4, 4);
A.row(0) = f1[0] * P1.row(2) - f1[2] * P1.row(0);
A.row(1) = f1[1] * P1.row(2) - f1[2] * P1.row(1);
A.row(2) = f2[0] * P2.row(2) - f2[2] * P2.row(0);
A.row(3) = f2[1] * P2.row(2) - f2[2] * P2.row(1);
Eigen::JacobiSVD<Eigen::Matrix<Scalar, 4, 4>> mySVD(A, Eigen::ComputeFullV);
Vec4 worldPoint = mySVD.matrixV().col(3);
worldPoint /= worldPoint.template head<3>().norm();
// Enforse same direction of bearing vector and initial point
if (p0_3d.dot(worldPoint.template head<3>()) < 0) worldPoint *= -1;
return worldPoint;
}
template <class AccumT>
static void linearizeAbs(const Eigen::MatrixXd& rel_H,

23
src/vi_estimator/ba_base.cpp
View File

@ -431,29 +431,6 @@ void BundleAdjustmentBase::get_current_points(
}
}
Eigen::Vector4d BundleAdjustmentBase::triangulate(const Eigen::Vector3d& p0_3d,
const Eigen::Vector3d& p1_3d,
const Sophus::SE3d& T_0_1) {
Eigen::Vector3d p1_3d_unrotated = T_0_1.so3() * p1_3d;
Eigen::Vector2d b;
b[0] = T_0_1.translation().dot(p0_3d);
b[1] = T_0_1.translation().dot(p1_3d_unrotated);
Eigen::Matrix2d A;
A(0, 0) = p0_3d.dot(p0_3d);
A(1, 0) = p0_3d.dot(p1_3d_unrotated);
A(0, 1) = -A(1, 0);
A(1, 1) = -p1_3d_unrotated.dot(p1_3d_unrotated);
Eigen::Vector2d lambda = A.inverse() * b;
Eigen::Vector3d xm = lambda[0] * p0_3d;
Eigen::Vector3d xn = T_0_1.translation() + lambda[1] * p1_3d_unrotated;
Eigen::Vector4d p0_triangulated;
p0_triangulated.head<3>() = (xm + xn) / 2;
p0_triangulated[3] = 0;
return p0_triangulated;
}
void BundleAdjustmentBase::marginalizeHelper(Eigen::MatrixXd& abs_H,
Eigen::VectorXd& abs_b,
const std::set<int>& idx_to_keep,

14
src/vi_estimator/keypoint_vio.cpp
View File

@ -335,18 +335,16 @@ bool KeypointVioEstimator::measure(const OpticalFlowResult::Ptr& opt_flow_meas,
Eigen::Vector4d p0_triangulated =
triangulate(p0_3d.head<3>(), p1_3d.head<3>(), T_0_1);
if (p0_triangulated[2] > 0) {
if (p0_triangulated.array().isFinite().all() &&
p0_triangulated[3] > 0 && p0_triangulated[3] < 3.0) {
KeypointPosition kpt_pos;
kpt_pos.kf_id = tcidl;
kpt_pos.dir = StereographicParam<double>::project(p0_triangulated);
kpt_pos.id = 1.0 / p0_triangulated.norm();
kpt_pos.id = p0_triangulated[3];
kpts[lm_id] = kpt_pos;
if (kpt_pos.id > 0 && kpt_pos.id < 10) {
kpts[lm_id] = kpt_pos;
num_points_added++;
valid_kp = true;
}
num_points_added++;
valid_kp = true;
}
}

5
src/vi_estimator/nfr_mapper.cpp
View File

@ -632,12 +632,13 @@ void NfrMapper::setup_opt() {
Eigen::Vector4d pos_3d = triangulate(
pos_3d_h.head<3>(), pos_3d_o.head<3>(), T_w_h.inverse() * T_w_o);
if (pos_3d[2] < 0.5 || pos_3d.norm() < 0.5) continue;
if (!pos_3d.array().isFinite().all() || pos_3d[3] <= 0 || pos_3d[3] > 2.0)
continue;
KeypointPosition pos;
pos.kf_id = tcid_h;
pos.dir = StereographicParam<double>::project(pos_3d);
pos.id = 1.0 / pos_3d.norm();
pos.id = pos_3d[3];
kpts[kv.first] = pos;