sfm_utils.h

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#ifndef VSLAM_SFM_UTILS_H_INCLUDED
#define VSLAM_SFM_UTILS_H_INCLUDED

#include "utils.h"
#include <Eigen/Dense>
#include "Sophus/se3.h"


void computeStructure(
        const SE3 & T_c1c2,
        const ImageMatchVec & matches_c1c2,
        const IdxPoint3dVec & idx_points3d_c1,
        Points3dSTL & points3d_c1,
        SE3 & scaled_T_c1c2);

void computeStructure(
        const Sophus::SE3 & T_c1c2,
        const ImageMatchVec & matches_c1c2,
        Points3dSTL & triangulated_points_c1);

void computeStructureForcePositiveDepths(
        const Sophus::SE3 & T_c1c2,
        const ImageMatchVec & matches_c1c2,
        Points3dSTL & triangulated_points_c1);

void computeStructure(
        const Sophus::SE3 & T_c1c2, const ImageMatch& match_c1c2, Vector3d & triangulated_point_c1);

void computeStructureForcePositiveDepth(
        const Sophus::SE3 & T_c1c2, const ImageMatch& match_c1c2, Vector3d & triangulated_point_c1);


double computeScale(const Points3dSTL& points1, const Points3dSTL& points2);


void computeScale( const Points3dSTL& points1, const Points3dSTL& points2, 
        double& low, double& mid, double& hi);

bool extractTransformFromE(const ImageMatchVec & image_matches, const Matrix3d & E, SE3 & res_tr);

//XXX does not work
inline Matrix3d computeE(const ImageMatchVec & matches){
    int n = matches.size();//number of correspondences
    MatrixXd A(9, n);

    for (int i=0 ; i < n; i++){
        Vector3d pt1 = matches[i].first;
        Vector3d pt2 = matches[i].second;
        A(0,i) = pt1.x()*pt2.x();
        A(1,i) = pt1.x()*pt2.y();
        A(2,i) = pt1.x()*pt2.z();

        A(3,i) = pt1.y()*pt2.x();
        A(4,i) = pt1.y()*pt2.y();
        A(5,i) = pt1.y()*pt2.z();
        
        A(6,i) = pt1.z()*pt2.x();
        A(7,i) = pt1.z()*pt2.y();
        A(8,i) = pt1.z()*pt2.z();
    }

    //compute LSE to E
    Matrix3d F;
    VectorXd E_s = A.transpose().jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(VectorXd::Zero(n));
    Eigen::SelfAdjointEigenSolver<MatrixXd> eigensolver(A * A.transpose());
    E_s = eigensolver.eigenvectors().col(1);
    F.col(0) = E_s.segment(0,3);
    F.col(1) = E_s.segment(3,3);
    F.col(2) = E_s.segment(6,3);



    //project onto essential space
    Eigen::JacobiSVD<MatrixXd> svd2(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Matrix3d Sigma = Matrix3d::Zero();
    Sigma(0,0) = 1;
    Sigma(1,1) = 1;
    Matrix3d E = svd2.matrixU() * Sigma * svd2.matrixV().transpose();
    return E;
}

inline Vector3d closestPointOnEpipolarPlane(const Vector3d & hom_line, const Vector3d & hom_point){
    // {x | hom_line.transpose() * x == 0} is plane through (0,0,0).
    // normal to that plane is hom_line (http://mathworld.wolfram.com/NormalVector.html)
    double tmp = hom_line.dot(hom_line);
    if(isApprox(tmp, 0.0, 0.0000000001)) return hom_point; //got an in-plane point
    double l_param = -hom_line.dot(hom_point) / tmp;
    return hom_point + l_param * hom_line;
}



#endif