sfm_utils.cpp

Go to the documentation of this file.

//part of the source code of master thesis, source code produced by Jiri Divis
#include "sfm_utils.h"

using std::vector;
using Sophus::SE3; using Sophus::SO3;
using Eigen::Vector3d; using Eigen::VectorXd; using Eigen::Matrix3d; using Eigen::MatrixXd;



//solves for depth multipliers of matches in coordinate frame c1 for given image transform.
/*void computeStructure0( const SE3 & T_c1c2, const ImageMatchVec & matches_c1c2,
        VectorXd & depths_c1){
    int n = matches_c1c2.size();
    assert(n >= 1);
    MatrixXd A = MatrixXd::Zero(3*n, n);
    VectorXd b = MatrixXd::Zero(3*n, 1);
    for(int i = 0 ; i < n; i++){
        Vector3d m;
        m = matches_c1c2[i].second.cross(T_c1c2.rotation_matrix() * matches_c1c2[i].first);
        A.block(i*3, i, 3, 1) = m;
        b.segment(i*3, 3) = -matches_c1c2[i].second.cross(T_c1c2.translation());
    }
    depths_c1 = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
}


//solves for depth multipliers of matches in coordinate frame c1 for given image transform.
//sparse matrix version
void computeStructure0( const SE3 & T_c1c2, const ImageMatchVec & matches_c1c2,
        VectorXd & depths_c1){
    int n = matches_c1c2.size();
    assert(n >= 1);
    MatrixXd A = MatrixXd::Zero(3*n, n);
    VectorXd b = MatrixXd::Zero(3*n, 1);
    for(int i = 0 ; i < n; i++){
        Vector3d m;
        m = matches_c1c2[i].second.cross(T_c1c2.rotation_matrix() * matches_c1c2[i].first);
        A.block(i*3, i, 3, 1) = m;
        b.segment(i*3, 3) = -matches_c1c2[i].second.cross(T_c1c2.translation());
    }
    depths_c1 = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
}
*/
//solves for depth multipliers of matches in coordinate frame c1 for given image transform.
//version where individual features are done separately
void computeStructure0( const SE3 & T_c1c2, const ImageMatchVec & matches_c1c2,
        VectorXd & depths_c1){
    int n = matches_c1c2.size();
    assert(n >= 1);
    depths_c1 = VectorXd::Zero(n);
    for(int i = 0 ; i < n; i++){
        Vector3d A;
        Vector3d b;
        A = matches_c1c2[i].second.cross(T_c1c2.rotation_matrix() * matches_c1c2[i].first);
        b = -matches_c1c2[i].second.cross(T_c1c2.translation());
        Eigen::Matrix<double, 1, 1> tmp = 
                A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
        depths_c1(i) = tmp(0,0);
    }
}


void computeStructure0ForcePositiveDepths( const SE3 & T_c1c2, const ImageMatchVec & matches_c1c2,
        VectorXd & depths_c1){
    computeStructure0(T_c1c2, matches_c1c2, depths_c1);
    int n = matches_c1c2.size();
    for(int i = 0 ; i < n; i++){
        if(depths_c1(i) < 0.0) depths_c1(i) = - depths_c1(i);
    }
}


//solves for depth. Outputs 3d points wrt. to c1 frame.
void computeStructure(
        const SE3 & T_c1c2,
        const ImageMatchVec & matches_c1c2,
        Points3dSTL & triangulated_points_c1){
    VectorXd lambda_1;
    computeStructure0(T_c1c2, matches_c1c2, lambda_1);
    triangulated_points_c1.clear();
    for(unsigned int i=0 ; i < matches_c1c2.size(); i++){
        triangulated_points_c1.push_back(matches_c1c2[i].first * lambda_1(i));
    }
}

//solves for depth. Outputs 3d points wrt. to c1 frame.
void computeStructureForcePositiveDepths(
        const SE3 & T_c1c2,
        const ImageMatchVec & matches_c1c2,
        Points3dSTL & triangulated_points_c1){
    VectorXd lambda_1;
    computeStructure0ForcePositiveDepths(T_c1c2, matches_c1c2, lambda_1);
    triangulated_points_c1.clear();
    for(unsigned int i=0 ; i < matches_c1c2.size(); i++){
        triangulated_points_c1.push_back(matches_c1c2[i].first * lambda_1(i));
    }
}

void computeStructure(
        const Sophus::SE3 & T_c1c2, const ImageMatch& match_c1c2, Vector3d & triangulated_point_c1){
    ImageMatchVec imv;
    imv.push_back(match_c1c2);
    Points3dSTL pv;
    computeStructure(T_c1c2, imv, pv);
    triangulated_point_c1=pv[0];
}

void computeStructureForcePositiveDepth(
        const Sophus::SE3 & T_c1c2, const ImageMatch& match_c1c2, Vector3d & triangulated_point_c1){
    ImageMatchVec imv;
    imv.push_back(match_c1c2);
    Points3dSTL pv;
    computeStructureForcePositiveDepths(T_c1c2, imv, pv);
    triangulated_point_c1=pv[0];
}

typedef ALIGNED<pair<int, Vector3d> >::vector IdxPoint3dVec;


//solves for depth with T_c1c2 given up to a positive scale.
void computeStructure(
        const SE3 & T_c1c2,
        const ImageMatchVec & matches_c1c2,
        const IdxPoint3dVec & idx_points3d_c1,
        Points3dSTL & points3d_c1,
        SE3 & scaled_T_c1c2){
    points3d_c1.clear();
    computeStructure(T_c1c2, matches_c1c2, points3d_c1);
    IdxPoint3dVec tmp=switchCont<vector>(getByIdx(
            indexBy( makeSeq(0, (int)matches_c1c2.size() - 1, 1), points3d_c1), 
            transformF<int>(idx_points3d_c1, getFirst)));
    double scale = computeScale(
            transformF<Vector3d>(idx_points3d_c1, getSecond), 
            transformF<Vector3d>(tmp, getSecond));
    scaled_T_c1c2 = SE3(T_c1c2.rotation_matrix(), T_c1c2.translation() * scale);
    for(unsigned int i=0 ; i < points3d_c1.size(); i++){
        points3d_c1[i] *= scale;
    }
}

void computeScale( const Points3dSTL& points1, const Points3dSTL& points2, 
        double& low, double& mid, double& hi){
    assert(points1.size() >= 3);
    assert(points1.size() == points2.size());
    vector<double> ratios;
    for(unsigned int i = 0; i < points1.size(); i++){
        ratios.push_back(points1[i].norm() / (points2[i]).norm());
    }
    std::sort(ratios.begin(), ratios.end());
    //for(int i=0; i<ratios.size(); i++) cout << ratios[i] << endl;
    int idx = ratios.size() / 2;
    low = ratios[idx-1];
    mid = ratios[idx];
    hi = ratios[idx+1];
    ROS_INFO_STREAM_NAMED("SFM:", "scale ratios around median (low, mid, hi):" << low << " " << mid << " " << hi);
}

double computeScale( const Points3dSTL& points1, const Points3dSTL& points2){
    double low, mid, hi;
    computeScale(points1, points2, low, mid, hi);
    return mid;
}

//extracts possible transforms from E up to a scale.
ALIGNED<SE3>::vector extractRtFromE(const Matrix3d & E){
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Matrix3d U, V;
    U=svd.matrixU();
    V=svd.matrixV();

    //XXX a am not sure if i can do that...
    if(U.determinant() < 0) U = (-U).eval(); 
    if(V.determinant() < 0) V = (-V).eval();
    assert(isApprox(U.determinant(), 1.0, 0.0001));
    assert(isApprox(V.determinant(), 1.0, 0.0001));
    
    
    Matrix3d W = Matrix3d::Zero();
    W(0,1) = -1; W(1,0) = 1; W(2,2) = 1;
    Matrix3d Z = Matrix3d::Zero();
    Z(0,1) = 1; Z(1,0) = -1;

    Matrix3d R1 = U * W * V.transpose();
    Matrix3d R2 = U * W.transpose() * V.transpose();

    Matrix3d Sigma = Matrix3d::Zero(); Sigma(0,0) = 1; Sigma(1,1) = 1;
    Vector3d t = SO3::vee(U * Z * U.transpose());

    ALIGNED<SE3>::vector transforms;
    transforms.push_back(SE3(R1,t));
    transforms.push_back(SE3(R2,t));
    return transforms;
}



//extracts possible transforms from E and image matches used to compute E up to a positive scale.
bool extractTransformFromE( const ImageMatchVec & image_matches, const Matrix3d & E, SE3 & res_tr){
    using std::tr1::signbit;
    ALIGNED<SE3>::vector transforms = extractRtFromE(E);
    BOOST_FOREACH(SE3 transform, transforms){ 
        bool assigned = true;
        VectorXd lambda_1;
        computeStructure0(transform, image_matches, lambda_1);
        if(lambda_1(0) < 0){
            res_tr = SE3(transform.rotation_matrix(), -transform.translation());
            lambda_1 = (-lambda_1).eval();
        } else{
            res_tr = transform;
        }
        for(Points3dSTL::size_type i=0; i < image_matches.size(); i++){
            if (lambda_1(i) < 0){
                assigned=false;
                break;
            }
            Vector3d a, b;
            a=res_tr * (image_matches[i].first * lambda_1(i));
            b=image_matches[i].second;

            for(int j=0; j<3; j++){
                if(signbit(a(j)) != signbit(b(j))){
                    i=image_matches.size();
                    assigned=false;
                    break;
                }
            }
        }
        if(assigned) return true;
    }
    return false;
}