Subversion Repositories seema-scanner

Rev

Rev 31 | Rev 34 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 31 Rev 33
Line 28... Line 28...
28 
 
 28 
 
29 
    // Data with mean adjusted translations
 29 
    // Data with mean adjusted translations
30 
    cv::Mat X_bar(3, 4*N, CV_32F);
 30 
    cv::Mat X_bar(3, 4*N, CV_32F);
31 
    cv::Mat X_mark_bar(3, 4*N, CV_32F);
 31 
    cv::Mat X_mark_bar(3, 4*N, CV_32F);
32 
    for(int i=0; i<N; i++){
 32 
    for(int i=0; i<N; i++){
33 
        // ASSIGNEMENTS?!
 - 
 
34 
        X_bar.colRange(i*4,i*4+3) = cv::Mat(R[i]);
 33 
        cv::Mat(R[i]).copyTo(X_bar.colRange(i*4,i*4+3));
35 
        X_bar.col(i*4+3) = cv::Mat(t[i] - t_mean);
 34 
        cv::Mat(t[i] - t_mean).copyTo(X_bar.col(i*4+3));
36 
        X_mark_bar.colRange(i*4,i*4+3) = cv::Mat(R_mark[i]);
 35 
        cv::Mat(R_mark[i]).copyTo(X_mark_bar.colRange(i*4,i*4+3));
37 
        X_mark_bar.col(i*4+3) = cv::Mat(t_mark[i] - t_mark_mean);
 36 
        cv::Mat(t_mark[i] - t_mark_mean).copyTo(X_mark_bar.col(i*4+3));
38 
    }
 37 
    }
39 
 
 - 
 
- 
 
 38 
    //std::cout << X_bar << std::endl;
40 
    // SVD
 39 
    // SVD
41 
    cv::Matx33f W, U, VT;
 40 
    cv::Mat W, U, VT;
42 
    cv::SVDecomp(X_bar*X_mark_bar.t(), W, U, VT);
 41 
    cv::SVDecomp(X_bar*X_mark_bar.t(), W, U, VT);
43 
 
 42 
 
44 
    cv::Matx33f D = cv::Matx33f::eye();
 43 
    cv::Matx33f D = cv::Matx33f::eye();
45 
    if(cv::determinant(VT*U) < 0)
 44 
    if(cv::determinant(VT*U) < 0)
46 
        D(3,3) = -1;
 45 
        D(3,3) = -1;
47 
 
 46 
 
48 
    // Best rotation
 47 
    // Best rotation
49 
    Omega = VT*D*U;
 48 
    Omega = cv::Matx33f(cv::Mat(VT.t()))*D*cv::Matx33f(cv::Mat(U.t()));
50 
 
 49 
 
51 
    // Best translation
 50 
    // Best translation
52 
    tau = t_mark_mean - Omega*t_mean;
 51 
    tau = t_mark_mean - Omega*t_mean;
53 
 
 52 
 
54 
}
 53 
}