안드로이드 센서를 이용한 방향계산 및 벡터표시

스마트폰을 사용하다보면 스마트폰의 센서를 사용한다.

그중에서 방향 계산을 하는 일이 있을 것이다.

일반적으로 아래 그림처럼 Azimuth, Pitch, Roll을 이용하여 방향을 계산한다.

여기서 Azimuth는 항상 Y+의 방향과 북쪽 방향의 각도 차이를 나타낸다.

지도 앱처럼 북쪽이 Y+인 경우는 큰 문제가 없다. 하지만 카메라가 가르키는 방향을 북으로 잡으려면 어떻게 할까

그에 따라서 방향각을 다시 계산해야하는 경우가 있다.

예제는 책(안드로이드 프로그래밍 / 한빛미디어 / 김상형 지음)의 내용을 응용하여 만들었다.

SensorEventListener mSeonsorListneer = new SensorEventListener() {
    float[] mR = new float[9];
    float[] mI = new float[9];
    float[] mV = new float[9];
    @SuppressLint("SetTextI18n")
    @Override
    public void onSensorChanged(SensorEvent event) {
        switch (event.sensor.getType()) {
            case Sensor.TYPE_ACCELEROMETER:
                mGravity = event.values.clone();
                break;
            case Sensor.TYPE_MAGNETIC_FIELD:
                mGeometric = event.values.clone();
                break;
        }

        if(mOrientCount++ % FREQ != 0) return;

        if(mGravity != null && mGeometric != null) {
            SensorManager.getRotationMatrix(mR, mI, mGravity, mGeometric);
            float inclination = SensorManager.getInclination(mI);
            SensorManager.getOrientation(mR, mV);

            mTxtOrient.setText(
                    ("회수 = " + mOrientCount / FREQ + "회\n")
                    + "\nGra : " + dumpValues(mGravity)
                    + "\nMag : " + dumpValues(mGeometric)
                    + "\n\nR : \n" + dumpMatrix(mR)
                    + "\nI : \n" + dumpMatrix(mI)
                    + "\ninclination : " + inclination
                    + "\n\nRot : \n" + dumpMatrix(mV)
                    + "\n\nTop : "
                    + "\nx : " + String.format("%.3f", Math.cos(mV[0])*Math.cos(mV[1]))
                    + "\ny : " + String.format("%.3f", Math.sin(mV[0])*Math.cos(mV[1]))
                    + "\nz : " + String.format("%.3f", -Math.cos(mV[1]-Math.PI/2))

                    + "\n\nLeft : "
                    + "\nx : " + String.format("%.3f", -Math.cos(mV[0])*Math.sin(mV[1])*Math.sin(mV[2]) + Math.sin(mV[0])*Math.cos(mV[2]))
                    + "\ny : " + String.format("%.3f", -Math.sin(mV[0])*Math.sin(mV[1])*Math.sin(mV[2]) - Math.cos(mV[0])*Math.cos(mV[2]))
                    + "\nz : " + String.format("%.3f", Math.cos(mV[1])*Math.sin(mV[2]))

                    + "\n\nBack : "
                    + "\nx : " + String.format("%.3f", -Math.cos(mV[0])*Math.sin(mV[1])*Math.cos(mV[2]) + Math.sin(mV[0])*Math.sin(mV[2]))
                    + "\ny : " + String.format("%.3f", -Math.sin(mV[0])*Math.sin(mV[1])*Math.cos(mV[2]) - Math.cos(mV[0])*Math.sin(mV[2]))
                    + "\nz : " + String.format("%.3f", Math.cos(mV[1])*Math.sin(mV[2]-Math.PI/2))
            );
        }
    }

다른 부분은 레이아웃 설정 관련 설정이므로 생략한다. 센서리스너만 보도록 한다.

센서의 사용법은 다른 블로그에도 많으니 생략한다.

mV는 센서의 azimuth, pitch, roll값을 저장한다. 이 값을 이용하여 아래는 벡터를 이용하였다.

수식은 아래 링크를 참고하면 된다.

http://stackoverflow.com/questions/1568568/how-to-convert-euler-angles-to-directional-vector

Top은 Y+방향의 벡터

Left는 X-방향의 벡터

Back은 Z-방향의 벡터이다.

실질적으로 만들고 싶었던 기능은 카메라가 어느 방향을 가르키는지 알려주는 코드를 만들고 싶었다.

그럼 어떻게 만들었는지 보도록 하자.

수학적으로 회전행렬 * 벡터를 사용하였다.

회전행렬은 다음과 같다.

| cos(yaw)cos(pitch) -cos(yaw)sin(pitch)sin(roll)-sin(yaw)cos(roll) -cos(yaw)sin(pitch)cos(roll)+sin(yaw)sin(roll)|
| sin(yaw)cos(pitch) -sin(yaw)sin(pitch)sin(roll)+cos(yaw)cos(roll) -sin(yaw)sin(pitch)cos(roll)-cos(yaw)sin(roll)|
| sin(pitch)          cos(pitch)sin(roll)                            cos(pitch)sin(roll)|

여기에 [1, 0, 0]을 곱하면 Top(Y+)방향의 벡터

여기에 [0, 1, 0]을 곱하면 Left(X-)방향의 벡터

여기에 [0, 0, 1]을 곱하면 Back(Z-)방향의 벡터

 

                    + "\n\nTop : "
                    + "\nx : " + String.format("%.3f", Math.cos(mV[0])*Math.cos(mV[1]))
                    + "\ny : " + String.format("%.3f", Math.sin(mV[0])*Math.cos(mV[1]))
                    + "\nz : " + String.format("%.3f", -Math.cos(mV[1]-Math.PI/2))

우선, Top(Y+)방향을 보자

여기서 주의할 점은 pitch의 범위이다. 센서매너저에서 출력된는 범위는 0 ~ 2*PI이다. 하지만 실질적으로 필요한 z의 값은 -1 ~ 1이다.

따라서 z의 값을 -PI ~ PI로 변경하기 위해서 코사인 함수내에 PI/2를 빼주었다. 

                    + "\n\nBack : "
                    + "\nx : " + String.format("%.3f", -Math.cos(mV[0])*Math.sin(mV[1])*Math.cos(mV[2]) + Math.sin(mV[0])*Math.sin(mV[2]))
                    + "\ny : " + String.format("%.3f", -Math.sin(mV[0])*Math.sin(mV[1])*Math.cos(mV[2]) - Math.cos(mV[0])*Math.sin(mV[2]))
                    + "\nz : " + String.format("%.3f", Math.cos(mV[1])*Math.sin(mV[2]-Math.PI/2))

이와 마찬가지로 Back(Z-)의 벡터는 roll값에 PI/2의 값을 빼주었다. 

이렇게 하지 않는다면, z값이 1이 나와야할 경우에 0이 나오거나 항상 양수값만 나오는 경우가 발생한다.

코드를 대충 짰기 때문에 x, y, z가 순방향이 아닐것이다. 사용할거라면 직접 고치기 바란다.

GitHub : https://github.com/pchan1401-ICIL/GetOrientation.git