Sunday, September 15, 2019

Photogrammetric Intersection

Linear Intersection for a Stereo Pair

There are two images.
Object point vs. image point, in the first image:
$$ \begin{pmatrix} X \\ Y \\ Z \end{pmatrix} = \begin{pmatrix} X^0_a \\ Y^0_a \\ Z^0_a \end{pmatrix} + {\lambda_a}{R_a} \begin{pmatrix} x_a \\ y_a \\ -f_a \end{pmatrix} \tag{1} $$
Object point vs. image point, in the second image:
$$ \begin{pmatrix} X \\ Y \\ Z \end{pmatrix} = \begin{pmatrix} X^0_b \\ Y^0_b \\ Z^0_b \end{pmatrix} + {\lambda_b}{R_b} \begin{pmatrix} x_b \\ y_b \\ -f_b \end{pmatrix} \tag{2} $$
Therefore,
$$ \begin{pmatrix} X^0_b \\ Y^0_b \\ Z^0_b \end{pmatrix} - \begin{pmatrix} X^0_a \\ Y^0_a \\ Z^0_a \end{pmatrix} = {\lambda_a}{R_a} \begin{pmatrix} x_a \\ y_a \\ -f_a \end{pmatrix} - {\lambda_b}{R_b} \begin{pmatrix} x_b \\ y_b \\ -f_b \end{pmatrix} \tag{3} $$ $$ \begin{pmatrix} r_{11_a}x_a + r_{12_a}y_a - r_{13_a}f_a & - r_{11_b}x_b - r_{12_b}y_b + r_{13_b}f_b \\ r_{21_a}x_a + r_{22_a}y_a - r_{23_a}f_a & - r_{21_b}x_b - r_{22_b}y_b + r_{23_b}f_b \\ r_{31_a}x_a + r_{32_a}y_a - r_{33_a}f_a & - r_{31_b}x_b - r_{32_b}y_b + r_{33_b}f_b \end{pmatrix} = \begin{pmatrix} X^0_b - X^0_a \\ Y^0_b - Y^0_a \\ Z^0_b - Z^0_a \end{pmatrix} \tag{4} $$
where, $ \begin{pmatrix} X \ Y \ Z \end{pmatrix}^T $ are object coordinates,
$\begin{pmatrix} X^0_a \ Y^0_a \ Z^0_a \end{pmatrix}^T$, $\begin{pmatrix} X^0_b \ Y^0_b \ Z^0_b \end{pmatrix}^T$, $R_a$, and $R_b$ denote positoions and attitudes of two images. $\begin{pmatrix} x_a \ y_a \ -f_a \end{pmatrix}^T$ and $\begin{pmatrix} x_b \ y_b \ -f_b \end{pmatrix}^T$ are photo coordinates of each image. If two images are obtained by a camera, $f_a = f_b$.
In the equation (4), there are two unknowns ($\lambda_a$ and $\lambda_b$) and three linear equations. Once $\lambda_a$ and $\lambda_b$ are determined, we get two sets of object coordinates from two scale factors and can use mean values of them.

General Case: Linear Intersection for Multiple Images

$$ \left. \begin {matrix} \begin{pmatrix} X \\ Y \\ Z \end{pmatrix} - {\lambda_1}{R_1} \begin{pmatrix} x_1 \\ y_1 \\ -f_1 \end{pmatrix} = \begin{pmatrix} X^0_1 \\ Y^0_1 \\ Z^0_1 \end{pmatrix} \\ \begin{pmatrix} X \\ Y \\ Z \end{pmatrix} - {\lambda_2}{R_2} \begin{pmatrix} x_2 \\ y_2 \\ -f_2 \end{pmatrix} = \begin{pmatrix} X^0_2 \\ Y^0_2 \\ Z^0_2 \end{pmatrix} \\ \vdots \\ \begin{pmatrix} X \\ Y \\ Z \end{pmatrix} - {\lambda_n}{R_n} \begin{pmatrix} x_n \\ y_n \\ -f_n \end{pmatrix} = \begin{pmatrix} X^0_n \\ Y^0_n \\ Z^0_n \end{pmatrix} \end {matrix} \right ] \tag{5} $$
Threrefore,
$$ \begin{pmatrix} 1 & 0 & 0 & -r_{11_1}x_1 - r_{12_1}y_1 + r_{13_1}f_1 & \cdots \\ 0 & 1 & 0 & -r_{21_1}x_1 - r_{22_1}y_1 + r_{23_1}f_1 & \cdots \\ 1 & 0 & 1 & -r_{31_1}x_1 - r_{32_1}y_1 + r_{33_1}f_1 & \cdots \\ 1 & 0 & 0 & 0 & -r_{11_2}x_2 - r_{12_1}y_2 + r_{13_2}f_2 & \cdots \\ 0 & 1 & 0 & 0 & -r_{21_2}x_2 - r_{22_2}y_2 + r_{23_2}f_2 & \cdots \\ 0 & 0 & 1 & 0 & -r_{31_2}x_2 - r_{32_2}y_2 + r_{33_2}f_2 & \cdots \\ \vdots \\ 1 & 0 & 0 & 0 & 0 & \cdots & -r_{11_n}x_n - r_{12_n}y_n + r_{13_n}f_n \\ 0 & 1 & 0 & 0 & 0 & \cdots & -r_{21_n}x_n - r_{22_n}y_n + r_{23_n}f_n \\ 0 & 0 & 1 & 0 & 0 & \cdots & -r_{31_n}x_n - r_{32_n}y_n + r_{33_n}f_n \end{pmatrix} \begin{pmatrix} X \\ Y \\ Z \\ \lambda_1 \\ \lambda_2 \\ \vdots \\ \lambda_n \end{pmatrix} = \begin{pmatrix} X^0_1 \\ Y^0_1 \\ Z^0_1 \\ X^0_2 \\ Y^0_2 \\ Z^0_2 \\ \lambda_2 \\ \vdots \\ X^0_n \\ Y^0_n \\ Z^0_n \end{pmatrix} \tag{6} $$
In the equation (6), there are $(3+n)$ unknowns ($\begin{pmatrix}X&Y&Z\end{pmatrix}^T$, and $3n$ equations. If there are 2 or more than 2 images, there should be redundancy.

Monday, August 26, 2019

[GoodToKnow] MathJax for Blogger

Aftter adding this line,

in 'head' of the blogger template editor in html mode, you can use MathJax expressions.
For a new line, use this block,


and, for an inline equation, use this line,

Friday, June 7, 2019

[LaTeX] Download from TeX Users Group

Download 'install-tl.zip' from Installing TeX Live over the Internet. After extracting all files, and run the install bat file as administrator. After finishing installation, run 'Texworks editor'.





Wednesday, September 19, 2012

Short memo for angle calculation using atan2


Angle and arc-tangent function (in C/C++)




θ = atan2(x,y)
θ' = -(360 - θ)

#1: Bearing and an arc-tangent function

b = atan2(E, N)
Angle between 'N' axis and a vector.
CW is positive.




#2: Rotation angle about an axis and an arc-tangent function

ω = -atan2(x, y)
Angle between 'Y' axis and a vector.
CCW is positive.



Monday, September 3, 2012

Visual Studio 2010 C++ tips: for loop initializer

In Visual Studio 2010, for a loop's initialization variable is not available out of for loop scope, unlike Visual C++ 6.0. Whenever we compile old codes (generated in VC++ 6.0) using newest version, it annoys us. Then, take easy, and change project option to release your blood pressure.

Go to "Project property pages - C/C++ - Language - Force Conformance in For Loop Scope", and select "No (/Zc:forScope-)".

For more information, "http://msdn.microsoft.com/en-us/library/84wcsx8x.aspx".



Visual Studio 2010 C++ tips: Deprecated functions

strcpy, sscanf, fscanf.... etc. There functions are not available anymore in recent Visual Studio .Net C++. Instead of these function, it is recommend to use new functions such as strcpy_s, sscanf_s, fscanf_s ... etc. If you are not pleased to change your old style functions with new ones, you can avoid this problem easily by using this define statement, "#define _CRT_SECURE_NO_DEPRECATE".

For more information about deprecated functions, refer to "http://msdn.microsoft.com/en-us/library/ms235384(v=vs.80).aspx"

Monday, July 30, 2012

Visual Studio 2010 C++ tips: Instead of istream::eatwhite

istream::eatwhite is not supported anymore by recent visual studio C++.
So far, I've used an alternative function implemented by myself. I realized recently that there is a simpler and easier way to avoiding the effort.

In Visual Studio 2010 and 2008, the use of "ws" is the way to skips white space in the stream.

For example, in order to skip white spaces end of each line of a text file.

fstream imufile;
imufile.open(fname, ios::in);
imufile>>record0>>record1>>ws;

For more information,

http://msdn.microsoft.com/query/dev10.query?appId=Dev10IDEF1&l=KO-KR&k=k(%22ISTREAM%2fSTD%3a%3aWS%22);k(%22STD%3a%3aWS%22);k(WS);k(DevLang-%22C%2B%2B%22);k(TargetOS-WINDOWS)&rd=true

Friday, May 18, 2012

Derivation of direct linear transformation for a line scanner

image

clip_image002
clip_image004: focal length
clip_image006: rotation matrix element
clip_image008: CCD array coordinate (column)
clip_image010: offset (for the three line scanner)
clip_image012
clip_image014: First order function of scan line
clip_image016: Constant (zero order function of scan line)
clip_image018: Scan line number
clip_image020
clip_image022
 
clip_image030
clip_image032
clip_image034
clip_image036
-. Matrix form
clip_image040
















Friday, April 20, 2012

Simple test: Kalman filter and least square filter for a dynamic system

Kalman filter is one of most popular signal filter. As well known, the fundamental of Kalman Filter and Least Square Method is same. Here is a sample for a dynamic system, where filtering is conducted by KF and LS approaches.

double rand_num()
{    
    return 2*((rand()/(double)RAND_MAX) - 0.5);
}

int KF_LS_Test()
{
    double pos[NUMDATA];
    double pos_t[NUMDATA];
   
    srand((unsigned int)time(NULL));

    double noise = 0.09;
   
    for(int t=0; t<NUMDATA; t++)
    {
        double p = (double)cosf(2.0f*(float)t/(float)NUMDATA*(float)acos(-1.0));
       
        pos_t[t] = p;
        pos[t] = p + rand_num()*noise;
    }

    //Initial values
    CSMMatrix<double> x0(1,1,0.0);
    CSMMatrix<double> P0(1,1,0.0);

    CSMMatrix<double> xk0(1,1,0.0);
    CSMMatrix<double> Pk0(1,1,0.0);

    CSMMatrix<double> xk1_(1,1,0.0);
    CSMMatrix<double> Pk1_(1,1,0.0);

    CSMMatrix<double> xk1(1,1,0.0);
    CSMMatrix<double> Pk1(1,1,0.0);

    CSMMatrix<double> Qk(1,1);
    Qk(0,0) = noise*noise*0.01;

    CSMMatrix<double> Rk(1,1);
    Rk(0,0) = noise*noise;

    CSMMatrix<double> K(1,1,1.0);
    CSMMatrix<double> I(1,1,1.0);
    xk1(0,0) = pos_t[0] + rand_num()*noise;
    Pk1(0,0) = 0.0;//1.0e99;

    fstream outfile;
    outfile.open(_T("C:\\Users\\kibang\\Desktop\\KFtestout.txt"), ios::out);

    _tprintf(_T("R = %lf\n"),Rk(0,0));
    _tprintf(_T("Q = %lf\n"),Qk(0,0));
    _tprintf(_T("@time = %d\n"),0);
    _tprintf(_T("x = %lf\n"),xk1(0,0));
    _tprintf(_T("P = %lf\n"),Pk1(0,0));

    int count = 0;
    CSMMatrix<double> Var_x_hat(1,1);
    double s_hat;
    CSMMatrix<double> X, H(1,1,1.0), L(1,1), L2(1,1), W(1,1), N(1,1,0.0), C(1,1,0.0);
    W(0,0) = 1.0/Rk(0,0);

    //Var_x_hat(0,0) = Pk1(0,0);
    Var_x_hat(0,0) = Qk(0,0);

    double VTV = 0;
   
    for(int t=0; t<NUMDATA; t++)
    {
        //
        //LS approach
        //

        count ++;
        L(0,0) = pos[t];
        L2(0,0) = xk1(0,0);

        Var_x_hat = Qk;
        CSMMatrix<double> Ni = ( H.Transpose() % W % H + Var_x_hat.Inverse() );
        //CSMMatrix<double> Ni = ( H.Transpose() % W % H);
        CSMMatrix<double> Ci = (H.Transpose() % W % L + Var_x_hat.Inverse() % L2);

        //N = N + Ni; C = C + Ci;
        N = Ni; C = Ci;

        X = N.Inverse() % C;
        //VTV += (X(0,0) - pos[t])*(X(0,0) - pos[t])*W(0,0);
        VTV = (X(0,0) - pos[t])*(X(0,0) - pos[t])*W(0,0);
        s_hat = sqrt(VTV/count);

        Var_x_hat = N.Inverse()*(s_hat*s_hat);
        //
        //KF approach
        //

        _tprintf(_T("@time = %d\n"),t+1);
        _tprintf(_T("True x = %lf\n"),0.0);
        xk0 = xk1;
        Pk0 = Pk1;

        //Predict
        xk1_ = xk0;
        Pk1_ = Pk0 + Qk;

        _tprintf(_T("Predicted x = %lf\n"),xk1_(0,0));
        _tprintf(_T("Predicted P = %lf\n"),Pk1_(0,0));

        //Kalman gain
        K = Pk1_ % (Pk1_ + Rk).Inverse();

        _tprintf(_T("Measured x = %lf\n"),pos[t]);
        //Update state using measurement
        CSMMatrix<double> Pos(1,1,pos[t]);
        xk1 = xk1_ + K % (Pos - xk1_);

        //Update error covariance
        Pk1 = (I - K)%Pk1_;
       
        _tprintf(_T("Updated x = %lf\n"),xk1(0,0));
        _tprintf(_T("Updated P = %lf\n"),Pk1(0,0));
        _tprintf(_T("Adjusted(LS) P = %lf\n"),X(0,0));

        outfile<<t+1<<"\t"<<pos_t[t]<<"\t"<<xk1_(0,0)<<"\t"<<xk1(0,0)<<"\t"<<X(0,0)<<"\t"<<pos[t]<<endl;
    }

    outfile.close();
    return 0;
}


KF_LS_Test

Thursday, July 28, 2011

Visual Studio 2010 C++ tips: sscanf_s and fopen_s

-. sscanf vs. sscanf_s

구 버전과 비교하여 파일 및 스트링 관련 함수들에 "_s"가 붙은 함수들이 제공되고 있다. 그러한 함수들 중에는 buffer overflow를 방지하기위한 목적 buffer 사이즈를 명시하도록 규정하고 있다.예를들어strcpy와strcpy_s를 비교해보면, 새로제공되는 함수는 target과source사이에 buffer 사이즈를 입력해야 한다.
"_s"가 붙은 신규함수와 붙지않은 구함수 사이에 함수원형에 있어서 차이점이 없는 경우는, #define을 사용하여 손쉽게 구함수를 신함수로 전환할 수 있으나, 주의해야할 함수가 있다. sscanf_s는 원형이 구 함수인 sscanf와 동일한것 처럼 보이고, 단순히 함수이름만 변경하여도 컴파일 에러가 발생하지 않지만, 실행시 에러가 발생하는 경우가 있다. %c 또는 %s의 포맷으로 데이터를 입력받는 변수의 경우, sizeof()를 사용하여 buffer의 사이즈를 명시하지 않는 경우, 에러가 발생하게 됨으로 주의해야 한다.

-. fopen vs fopen_s 과 fsopen

fopen_s 또한 fopen을 대신하도록 권고되고 있는데, 두 함수사이에 차이점이 있다.
outfile = fopen(filename, "w");
fopen(outfile, filename, "w");
위와 같이 파일을 open하는 경우, fopen과 달리 fopen_s를 사용하는 경우, open된 파일을 sharing이 허용되지 않는다.
따라서, sharing이 허용되도록 하기 위해서는
outfile = fsopen(filename, "w", _SH_DENYNO);
와 같이 fsopen함수와 _SH_DENYNO 플래그를 사용하여 파일을 open해야 한다.