lidar (laser scanner) georeferencingf (sensor model)

Multi-byte vs. Unicode

Multi-byte	Unicode
char	TCHAR
strcat_s()	_tcscaft_s()
strcpy_s()	_tcscpy_s()
strncpy_s()	_tcsncpy_s()
strien()	_tcsien()
sprinif_s()	_stprintf_s()

How to get paths defined in the system environment

char path[_MAX_PATH];

const char* varName = "[system environment variable name]";
size_t len;
getenv_s(&len, path, 80, varName);

Print out a (decimal) number as a hexadecimal number

int decNum = 123456;
char hexNum[16];

// decimal number to hexadecimal number
sprintf_s(hexNum, 16, "%X", decNum);
printf("%d -> %s \n", decNum, hexNum);

// hexadecimal number to decimal number
sprintf_s(hexNum, 16, "FF");
decNum = (int)strtol(hexNum, nullptr, 16);
printf(" %s -> %d \n", hexNum, decNum);

Derivation of projective transformation based on shift, rotation and scale parameters

Reduced normal matrix for solving fast photogrammetric bundle adjustment

Reduced normatrix for photogrammetric bundle adjustment based on the collinearity equations

Note: There are $m_1$ photos and $m_2$ object points, and the number of EOPs is 6. If 6*$m_1$ is less than 3*$m_2$, solve unknowns related with EOPs. If not, solve object points coordinates, first. This post shows only the first case, 3*$m_2$ > 6*$m_1$.

[Fig. Normal matrix sample]

$$ \begin{pmatrix} N_1 & N_{12} \\ N_{12}^T & N_2 \end{pmatrix}\begin{pmatrix} X_1 \\ X_2 \end{pmatrix} = \begin{pmatrix} C_1 \\ C_2 \end{pmatrix} $$ $$ N_1 X_1 + N_{12} X_2 = C_1$$ $$ N_{12}^T X_1 + N_2 X_2 = C_2$$ In case $6m_1<3m_2$, let's slove $X_1$ first.
$$ X_2 = N_2^{-1} C_2 - N_2^{-1} N_{12}^T X_1$$, therefore
$$ (N_1 - N_{12} N_2^{-1} N_{12}^T) X_1 = C_1 - N_{12} N_2^{-1} C_2$$ $$ X_1 = (N_1 - N_{12} N_2^{-1} N_{12}^T)^{-1}(C_1 - N_{12} N_2^{-1} C_2)$$ And
$$ X_2 = N_2^{-1}(C_2 - N_{12}^T X_1) $$ For $X_2$, we can avoid solving the inverse matrix of $3m \times 3m$ .
For $i_{th}$ point,
$$ X_{2_i} = N_{2_{i}}^{-1}(C_{2_i} - N_{12_i}^T X_1) $$

Brief memo for the units in least squares

-. A-priori reference variable has no unit (unitless). It is typically 1.0 or can be a different value for scaling.
-. Weight is determined by a-prior reference variable and a-prior variance of each observation. Therefore, the unit of weight is the same as the square of the observation unit.
-. A-posterior reference variance has no unit.
-. The variances of estimated unknown parameters after adjustment are the same as the square of the unknown parameters unit.

How to calculate areas of triangles and polygons

3 vertices and cross product

If 3 vertices' coordinates are given, the size of the cross product of the two vectors is same to two times of the triangle area.
삼각형의 3꼭지점 좌표를 알고 있다면, 인접한 두 벡터의 외적의 크기는 삼각형 면적의 두배와 같다.

$$ A = 0.5|(p_2-p_1)\times(p_3-p_1)| \tag{1}$$

If $\vec{N}$ is the result of the cross product of the two vectors, $\vec{p_1p_2}$ and $\vec{p_1p_3}$, the size of $\vec{N}$ is same to the parallelogram including $\vec{p_1p_2}$ and $\vec{p_1p_3}$.
만약 $\vec{N}$이 두 벡터 $\vec{p_1p_2}$ and $\vec{p_1p_3}$의 외적이면, $\vec{N}$의 크기는 두 벡터 두 벡터 $\vec{p_1p_2}$ and $\vec{p_1p_3}$을 포함하는 평행사변형의 면적과 같다.

Using the cross product and vertices' coordinates, we can get the area of a polygon by spliting a polygon into sub-triangles.
벡터외적과 꼭지점들의 좌표를 이용하면, 다각형을 내부의 작은 삼각형으로 분할하여 그 면적을 계산할 수 있다.

$$ A = \sum_{i=2}^{n-1} 0.5|(p_i-p_1)\times(p_{i+1}-p_1)| \tag{2}$$

We should know the limitation that any vector defiend by two vertices cannot intersect with any side of the polygon.
두 개의 꼭지점으로 정의되는 어떤 벡터도 다각형의 어떤 변과도 만나면 안된다는 한계점을 알고 있어야 한다.

This page will be update for how to calculate a complex polygon's area.

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.

[GoodToKnow] MathJax for Blogger

Aftter adding this line,
<script type="text/x-mathjax-config"> MathJax.Hub.Config({ tex2jax: { inlineMath: [ ['$','$'], ["\\(","\\)"] ], processEscapes: true } }); </script> <script src='https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.2/MathJax.js?config=TeX-MML-AM_CHTML'></script>
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,
$ ... $

[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'.

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.

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"

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

Derivation of direct linear transformation for a line scanner

: focal length
: rotation matrix element
: CCD array coordinate (column)
: offset (for the three line scanner)

: First order function of scan line
: Constant (zero order function of scan line)
: Scan line number


 




-. Matrix form

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;
}

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해야 한다.

Plane fitting

 

Plane equation:

(1) lx + my + nz =  d

where d is a distance from an origin to the plane,

{l,m,n} is direction consines, and

sqrt(l*l + m*m + n*n)=1.

(2) ax + by + cz = 1

where d presented in (1) can be calculated by 1/sqrt(a*a + b*b + c*c).

(3) Ax + By + Cz = d^2

where (A, B, C) denotes a vector from an origin to the plane, which can be obtained by A = l*d, B=m*d, and C=n*d.

 

For a point cloud, P0, P1, P2, ~ Pn,

Its centroid C, (cx, cy, cz), is on an expected least squre fitted plane.

Therefore, dot product of [l, m, n] and (Pi-C) is zero.

l*xi + m*yi + n*zi = 0

whrer (xi,yi,zi) = Pi

To avoid [l, m, n] are estimated as zero,

a constraint of direction consines is adopted,

l^2 + m^2 + n^2 = 1.0

STL – vector 초간단 설명

vector는 container자료구조중의 하나로서 STL중에서 가장 자주 사용하는 템플릿중 하나이기도 하다. vector의 자료구조는 일반적인 배열과 유사하다. 배열과 상이한 점은 배열의 크기는 일단 정해진 이후에 고정이지만, vector는 동적으로 변할 수 있다는 점이다. 배열의 특징을 더 자세히 살펴보면 데이터가 배열내에 입력된 이후에 특정한 위치에 새로운 데이터의 삽입이 용이하지 않으며, 새로운 데이터의 삽입으로 인해 배열의 크기를 넘어서는 경우 처리가 까다롭다. 특정위치의 데이터를 삭제한 경우에도 빈 공간으로 유지하지 않고, 연속적인 데이터 구조를 유지하기 위해 삭제된 공간을 그 이후의 데이터를 이동하여 채울경우 이 또한 부차적인 처리를 해야하는 불편함이 있다. 반면, 배열은 가장 단순한 형태의 자료구조중 하나이기 때문에 구현이 쉽고, 각 데이터에 대한 random access가 용이하다. vector를 배열의 단점을 어느정도 극복한 형태 자료구조로 여길 수 있다. 즉, 데이터는 순차적으로 저장하지만, 데이터의 개수가 가변적이어도 이에 대응할 수 있다. 그러나, STL의 list와 달리 데이터의 삽입 및 삭제가 용이하지는 않다. 다음의 표는 배열, vector, list에 대한 특징을 간단히 정리한 것이다.

	Array	STL의 vector	STL의 list
가변적 크기	X	O	O
임의 위치의 데이터 삭제/삽입	X	X	O
Random access	O	O	X

<vector 사용법>
#include <vector>//header file
using namespace std;//name space선언
vector<myType> myvector;//vector instance선언
myvector.push_back(newdata1);//vector에 데이터 추가 (뒤에서 추가)
myvector.push_back(newdata2);
int numElements = myvector.size();//저장된 데이터의 갯수
for(int i=0; i<numElements; i++) //일반 배열과 동일한 방식의 데이터 접근 (iterator사용도 가능하다.)
{

myvector.at(i) = myvector.a[i]*2;//at은 특정 위치에 있는 데이터의 reference를 반환한다.

cout<<myvector[i];

}
myType val = (myType)targetvalue;
for(vector<myType>::iterator i=myvector.begin(); i != myvector.end(); )
{

if(*i == val)
{
i = myvector.erase(i);//erase는 iterator i가르키는 데이터를 삭제한 후, 그 다음 데이터를 가르키는 iterator를 반환한다. 따라서, iterator를 증가시키는 프로세스를 필요로 하지 않는다. (주의 요망).
}
else
{
i++;
}

}
myvector.clear();//vector 데이터 모두 삭제