KR101107735B1 - Camera pose decision method - Google Patents

Abstract

으로 표현되며, R은 3×3 회전 행렬이고, t(

)는 이동 벡터(translation vector)이다. 본 발명에 따르면 카메라의 급격한 모션 변화에도 카메라 포즈를 종래보다 상대적으로 적은 에러율로 정확하게 구할 수 있다.The present invention relates to a method of determining a pose of a camera by performing Particle Swarm Optimization (PSO) on a Special Euclidean Group SE (3). The method according to the present invention provides a plurality of particles representing a pose of a camera in a previous frame. And a plurality of landmarks for each of the plurality of particles, extracting a plurality of corner points respectively corresponding to the plurality of landmarks in a current frame, and using the extracted corner points. Determining particle pose of the camera for the current frame by performing Particle Swarm Optimization (PSO) on Special Euclidean Group SE (3) for the three particles, the particles being a homogeneous matrix.

Where R is a 3x3 rotation matrix and t (

) Is a translation vector. According to the present invention, the camera pose can be accurately obtained with a relatively low error rate even in the sudden motion change of the camera.

Description

How to determine camera pose {CAMERA POSE DECISION METHOD}

The present invention relates to a method of determining a camera pose, and more particularly, to a method of determining a pose of a camera by performing Particle Swarm Optimization (PSO) on a Special Euclidean Group SE (3).

Robots, which were introduced to industrial sites for simple assembly or processing in the past, have been able to perform high-performance tasks by adding their own recognition and judgment capabilities with the development of related technologies. Moreover, mobile robots, which can move themselves, have a wide working area in a fixed location or in a pre-structured work environment, such as any workplace or outdoors, and thus have superior capabilities in terms of their role or diversity of work. Can be.

The mobile robot needs to have autonomous route planning, obstacle detection and collision avoidance capability to move or perform tasks even where advance information about the surrounding environment is lacking. Simultaneous localization and map building (SLAM), a technology for estimating the location of robots while generating a map, is being actively researched.

In order for a mobile robot to perform a SLAM, the ability to extract information that can be used for position estimation from the surrounding environment is essential. Various methods using a visual sensor, an ultrasonic sensor, or a contact sensor are applied to the sensor. In particular, a visual sensor using a laser and a camera and a three-dimensional recognition algorithm using the same have a small computational burden and a high brightness change. It can be used anywhere, so it is very effective.

As a mathematical method for solving the SLAM system, filtering techniques such as Extended Kalman Filter (EKF) or Rao-Blackwellised Particle Filter (RBPF) have been used. However, the slam system under the filtering technique is mainly based on the assumption that the robot moves smoothly or slowly. This assumption fits, for example, in a 3-DOF limited slow moving robot on the floor of a plane. However, for 6-DOF robots with dynamic movement, this assumption does not work, and the system will not work properly. The reason is that the existing filtering technique recognizes the position of the robot through the linearization of the observation function based on this smooth motion assumption.

As shown in FIG. 1 (a), the conventional filtering technique is based on the position of the immediately preceding robot and the observed feature information under the assumption of smooth motion that the difference between the movement of the actual robot or the observation characteristic through the sensor is small. We estimate the position through the first order Taylor approximation of the observation function using. However, when an active movement occurs, as shown in FIG. 1 (b), a difference in actual robot movement or difference in observation information becomes large, and when one of the two shows a large difference, a linearization based on the assumption of smooth motion is applied. Large errors can occur, which is a fatal problem for slam systems. For example, when the motion of the camera acquiring the image is suddenly made by the mobile robot, as described above, the pose or position of the actual camera cannot be properly obtained, which is a fatal problem of the slam system.

The problem to be solved by the present invention is to provide a camera pose determination method that can obtain the pose of the camera robustly even in a sudden motion.

으로 표현되며, R은 3×3 회전 행렬이고, t(

)는 이동 벡터(translation vector)이다.Camera pose determination method according to an embodiment of the present invention for solving the technical problem, a plurality of particles (particle) representing the pose of the camera in the previous frame and a plurality of landmarks for each of the plurality of particles Extracting a plurality of corner points respectively corresponding to the plurality of landmarks in a current frame, and using the extracted corner points, Particle Swarm on a Special Euclidean Group SE (3) for the plurality of particles. Performing optimization to determine the pose of the camera with respect to the current frame, wherein the particles comprise a homogeneous matrix.

Where R is a 3x3 rotation matrix and t (

) Is a translation vector.

The extracted corner point may be used in comparing the image patch of the landmark in the initial frame with the image patch centering the extracted corner point in the current frame, and determining the pose of the camera when the image is matched. have.

The camera pose determination step,

(k번째 프레임에서 i번째 파티클에 대응하는 카메라 포즈 행렬)을 구하는 단계,(a) a camera pose matrix corresponding to each of the initialized plurality of particles according to a predefined camera state transition model

(camera pose matrix corresponding to the i th particle in the k th frame),

(b) applying the camera pose matrix to a predefined goodness-of-fit function to obtain each individual best solution and a total best solution for a plurality of particles;

에 대해 다음 수학식 (c) the camera pose matrix

Equation for

- 상기 수학식에서 Vⁱ _R 및 Vⁱ _T는 각각 파티클(Xi)로부터 분할된 이동 파트(Rⁱ) 및 회전 파트(tⁱ)에 대한 속도를 나타내고, c₁, c₂는 각각 독립적인 가중치 상수이며, r₁, r₂는 [0,1]에 관한 균등한 분포로부터 도출되는 랜덤 변수 벡터이고, w는 관성 상수이며, Rⁱ _ib와 R_gb는 개별적 파티클 및 전체 베스트 파티클 각각에 대한 회전 파트이고, tⁱ _ib와 t_gb는 개별적 및 전체 베스트 파티클 각각의 이동 파트이며, 연산자 ←는 오른쪽 항을 좌측 항에 대입하는 연산자이고, 연산자 T는 트랜스포즈 변환 연산자이며, 연산자

는 각 열벡터의 요소별 위치에 대한 요소별 곱(component wise multiplication)을 수행하는 연산자임 -

- In the equation V ⁱ _R and V ⁱ _T denotes a speed for the moving part (R ⁱ⁾ and the rotational part (t ⁱ⁾ divided from each particle _{(Xi), c 1, c} 2 are each independently of the weight constant , R ₁ , r ₂ are random variable vectors derived from an even distribution with respect to [0,1], w is an inertia constant, and R ⁱ _ib and R _gb are rotating parts for each individual particle and the best best particle respectively. T ⁱ _ib and t _gb are the moving parts of each of the individual and full best particles, operator ← is the operator that assigns the right term to the left term, operator T is the transpose transform operator, and

Is an operator that performs component wise multiplication of the element position of each column vector-

Obtaining a new camera pose matrix using

(d) updating each individual best solution and the overall best solution for a plurality of particles by applying the predefined fit function to the camera pose matrix obtained in step (c);

(e) repeating steps (c) and (d) by a predetermined condition; and

and (f) determining the pose of the camera with respect to the current frame by using each individual optimal solution and the total optimal solution for the plurality of particles after the completion of the step (e).

The average of each individual best solution and the overall best solution can be determined as the pose of the camera for the current frame.

The predetermined condition is

The number of repetitions of steps (c) and (d) has reached a predetermined number, or the difference between the goodness of fit of the particle having the best solution and the goodness of fit of the particle having the worst solution may be smaller than a predetermined criterion. .

The predefined camera state transition model is represented by the following equation

Obtained by the state transition density p (X _k │X _k-1 )

는 공분산

(

)를 갖는 se(3) 상의 위너 과정(Wiener process) 노이즈이고,

은 파티클의 유포를 결정하는 상태 동역학 텀(state dynamics term)이다.here

Covariance

(

Wiener process noise on se (3) with

Is the state dynamics term that determines the distribution of particles.

The camera pose determination step,

)을 추출하고, 추출된 퀀텀 파티클(

)에 대해서 상기 미리 정의된 적합도 함수를 이용하여 적합도 값을 구하고 구해진 적합도 값이 현재의 전체 최적해(P _gb)의 적합도 값보다 큰 경우 전체 최적해(P _gb)를 해당 퀀텀 파티클(

)로 변경하는 단계를 더 포함할 수 있다.Quantum particles (within a certain radius based on the current optimal particle)

) And extracted quantum particles (

) If the value obtained is fit to obtain a fit value using the fitness function is greater than the predefined value for the goodness of fit of the total optimal solution (P _gb) with respect to the overall optimal solution (P _gb) quantum particles (

It may further comprise the step of changing to).

The method may further include updating the plurality of landmarks by using EKF after the camera pose determination step of the current frame.

The method may further include deleting a particle that does not satisfy a predetermined criterion among the plurality of particles after the camera pose determination step of the current frame, and replacing the particle having a predetermined criterion or more among the plurality of particles.

A computer-readable medium according to another embodiment of the present invention records a program for causing a computer to execute any one of the camera pose determination methods described above.

According to the present invention, the camera pose can be accurately obtained with a relatively small error rate even in the sudden motion change of the camera.

1 is a diagram in which a conventional filtering technique is provided to explain an error occurrence of a slam system in a sudden motion change.
2 shows PSOs on manifolds.
3 is a block diagram provided to explain an apparatus for determining a camera pose according to an exemplary embodiment of the present invention.
4 is a flowchart provided to explain a camera pose determination method according to an exemplary embodiment of the present invention.
5 is a view provided for comparing the error rate when using the camera pose determination method according to the present invention and another conventional method.
6 is a view provided to compare the error rate between the PSO-SLAM according to the present invention and the conventional OIF-SLAM when there is a sudden motion change.

First, before describing an embodiment according to the present invention, some important issues addressed by the present invention will be described.

One of the most important issues in applying the PSO algorithm to the visual SLAM problem is that the state of the camera is not a vector but an element of the SE (3). This reflects the geometrical characteristics of the camera motion and is a technique mainly used in the existing robot field. Therefore, it is required to reconstruct a conventional vector space PSO algorithm to take into account the shape of the SE 3. First, we briefly examine PSO and SE (3), and then propose a geometric PSO algorithm on SE (3).

1. Overview of Particle Swarm Optimization in Vector Space

)의 상호 작용을 고려하는 것이다. N-차원 벡터로 표현되는 각각의 파티클(

)은 두 가지 값을 사용하여 최적 상태를 찾아내기 위해 솔루션 공간을 탐사한다.: 두 가지 값은 각각 ⅰ) 각 파티클

와 전체 베스트(global best)파티클

(

) 사이의 상대적인 위치, 그리고 ⅱ) 각 파티클

와 그 개별적 베스트(individual best)파티클

(

) 사이의 상대적인 위치이다. 두 개의 상대 위치 벡터인

와

는 파티클이 실제적인 최적의 상태 가까이로 수렴할 수 있는 더 나은 최적의 상태를 찾기 위한 방향을 제공한다.The idea of the PSO algorithm is that

) Is to consider the interaction. Each particle represented by an N-dimensional vector

) Explores the solution space to find the optimal state using two values: two values respectively)

And global best particles

(

Relative position between a) and ii) each particle

And its individual best particles

(

Relative position between Two relative position vectors

Wow

Provides a direction for finding a better optimal state where the particles can converge near the actual optimal state.

)를 계산한다. The basic operation of the PSO is to calculate the velocity and movement of the particles. First, using Equation 1, the two relative position vectors are summed so that each particle moves to a velocity (where

Calculate

는 관성(inertia)에 대응하고,

,

는 두 개의 상대 위치 벡터를 위한 가중치 계수이다.

과

(

)는 [0,1]에 관한 균등한 분포로부터 도출되는 랜덤 변수 벡터이다. 이러한 모든 가산(addition) 및 차산(subtraction)은 유클리드 벡터 공간에서 정의된다. 구해진

를 사용하여

가 수학식 2에 의해 업데이트된다.here,

Corresponds to inertia,

,

Is a weighting coefficient for two relative position vectors.

and

(

) Is a random variable vector derived from an even distribution of [0,1]. All these additions and subtractions are defined in Euclidean vector space. Saved

use with

Is updated by equation (2).

를 갖는다면, 현재의 전체 최적 값

와 파티클의 개별적 베스트 값

가 업데이트되며, 이를 나타내면 수학식 3과 같다.After calculating velocity and particle movement, the particles in the updated state have a better fit

If we have, then the current total optimal value

Individual Best Values for Particles and Particles

Is updated, and this is represented by Equation 3.

By repeating the process of Equations 1 to 3 by a certain number of times, the particles explore the area where the optimum state should exist and gradually converge to a specific state. This convergence feature allows for accurate optimization, but may lead to particles with local optima. This often happens with dynamic optimization problems like SLAM. Therefore anti-converging particles, called quantum particles, are used to prevent convergence to this local optimization.

(

)이 다음 수학식 4의 규칙에 따라 임의적으로 생성된다.At the end of a single PSO step, quantum particles

(

) Is arbitrarily generated according to the following rule.

는

가 중심이고

가 반경인 파티클 클라우드(cloud)를 나타낸다. 모든 퀀텀 파티클들은 중립(neutral) 파티클의 경우와 유사하게 적합 함수(fitness function)에 의해 평가된다. 또한 현재의

보다 더 나은 적합치를 갖는 퀀텀 파티클이 존재한다면

는 교체된다. 퀀텀 파티클들의 넓은 범위는 지역적 최소화(local minima)를 모면함으로써 더 나은 기본 위치를 찾을 수 있는 기회를 제공하게 된다. 퀀텀 파티클들 중 하나가 새로운 최적 상태

를 찾아낸다면, 다음 PSO 반복 수행에서 다른 중립 파티클들은 새로운

에 가깝도록 이동한다. 그러고 나서 중립 파티클들은 새로운 영역에서 최적화를 진행해가고, 결국 전체 최적 상태에 도달하게 된다.
here,

Is

Is the center

Represents a particle cloud of which radius is. All quantum particles are evaluated by a fitness function, similar to the case of neutral particles. Also present

If there is a quantum particle with better fit

Is replaced. The wide range of quantum particles offers the opportunity to find a better base position by avoiding local minima. One of the quantum particles has a new optimal state

If we find, then in the next PSO iteration, other neutral particles

Move closer to The neutral particles then optimize in a new area, eventually reaching their full optimum.

2. Special Euclid Group SE (3)

에 의해 표현될 수 있다. 여기서 R은 3×3의 회전 행렬이고, t(

)는 이동 벡터(translation vector)이다. 강체 변환 행렬과 회전 행렬 R은 행렬 리 그룹(Lie group), 스페셜 유클리드(special Euclidean) 그룹 SE(3), 그리고 스페셜 직교(special orthogonal) 그룹 SO(3)과 각각 관련이 있다. 리 대수(Lie algebra)는 항등 요소(identity element)에서의 SO(3)와 SE(3)의 접선 벡터 공간(tangent vector space)이고, 각각 so(3)와 se(3)로 나타내진다. 리 그룹과 리 대수는 지수 맵(exponential map)을 통하여 관련될 수 있다. 즉 exp: so(3)→SO(3)이고, exp: se(3)→SE(3)이다. 행렬 리 그룹에 있어서 지수 맵은 통상의 행렬 지수(matrix exponential)와 대응된다. 로그 맵(log map)은 지수 맵의 역으로 정의된다. SO(3)와 SE(3)에 대한 지수 맵 및 로그 맵의 명시적인 식(explicit formulas)은 기존의 연구에서 주어져 있다.
In the system of the present invention, camera motion is represented by a rigid body transformation of camera coordinates with respect to the initial camera pose. Rigid body transformation is homogeneous matrix

Can be represented by Where R is a rotation matrix of 3 × 3, and t (

) Is a translation vector. The rigid transformation matrix and the rotation matrix R are associated with the matrix Li group, the special Euclidean group SE (3), and the special orthogonal group SO (3), respectively. Lie algebra is the tangent vector space of SO (3) and SE (3) in the identity element, represented by so (3) and se (3), respectively. Lee groups and lee logarithms can be related through an exponential map. That is, exp: so (3) → SO (3) and exp: se (3) → SE (3). In a matrix group, an exponential map corresponds to a normal matrix exponential. The log map is defined as the inverse of the exponential map. Explicit formulas of exponential maps and log maps for SO (3) and SE (3) are given in previous studies.

3. PSO on SE (3)

로 고려될 수 있고, 그 속도

는

의 접선 벡터 공간 상의 속도로 정의될 수 있다. 리만 로그 및 지수 맵은 리만 다양체 상의 측지선(geodesics)로부터 도출되므로, 리만 다양체 상의 요소들 간의 차이를 측지선 특성을 보존하는 리만 로그 맵을 통하여 접선 벡터 공간 상의 차이로 나타낼 수 있다. 그러므로,

와 개별적 베스트 파티클

및 전체 베스트 파티클

사이의 차이는

에서의 리만 로그 맵을 사용하여 접선 벡터 공간 상의 속도로서 나타낼 수 있다. 즉

와

는

에서의 접선 벡터 공간 상의 차이와 대응된다. 그러고 나서 속도 업데이트는 se(3) 상에서 정의된 V_i,

와 개별적 베스트 파티클

및 전체 베스트 파티클

사이의 차이를 이루는 3개의 벡터를 이용하여 수행한다. 파티클 업데이트는 Xⁱ에서의 접선 벡터 공간 상의 결과 속도 벡터 Vi 의 exp 맵을 기존의

에 곱하여 수행한다. 도 2는 다양체 상의 PSO를 도시한 도면이다. 요구되는 작업은 접선 벡터 공간에서 이루어진다.Hereinafter, in consideration of the shape of the SE (3), it redefines various concepts in the basic PSO such as particles, speed, and particle update. First consider the case of a typical Rimannian manifold. Particles in the base PSO point to manifolds

Can be considered as its speed

Is

It can be defined as the velocity on the tangential vector space of. Since the Lehmann log and exponential maps are derived from geodesics on the Lehman manifold, the differences between elements on the Lehman manifold can be represented as the differences in the tangential vector space through the Lehman log map that preserves the geodesic characteristics. therefore,

And individual best particles

And full best particles

The difference between

The Rieman log map in can be expressed as the velocity on the tangential vector space. In other words

Wow

Is

Corresponds to the difference in the tangential vector space at. The velocity update then defines V _i , defined on se (3)

And individual best particles

And full best particles

This is done using three vectors that make up the difference between them. Particle update replaces the exp map of the resulting velocity vector Vi in the tangential vector space at X ⁱ .

Multiply by 2 shows PSOs on manifolds. The required work is done in tangential vector space.

상의 개별적인 측지선의 집합에 의해 주어지기 때문에, SE(3)에 대한 리만 지수 및 로그 맵을 이끌어내는 것은 어렵다. 다행히도, SO(3)에 대한 리만 지수 및 로그 맵은 행렬 지수 및 로그인 지수 및 로그의 좌측 및 우측 이동(translation)에 의해 주어진다. 따라서 파티클

를

(∈SO(3))와

(

)로 분할함으로써 근사적으로 SE(3) 상의 PSO를 수행할 수 있게 된다.However, it is not straightforward that PSOs on a typical Lehmann manifold are applied directly to the SE (3). The minimum geodesic on SE (3) is SO (3) and

Given by the set of individual geodesic phases of the phases, it is difficult to derive the Lehman index and log map for SE (3). Fortunately, the Lehman index and log map for SO 3 are given by the matrix index and login index and the left and right translation of the log. So particles

To

(∈SO (3)) and

(

By dividing by) it is possible to perform PSO on SE (3) approximately.

및 파티클 업데이트 식은 수학식 5와 같이 주어진다.Speed for R ⁱ

And the particle update equation is given by Equation 5.

와

는 개별적 및 전체 베스트 파티클 각각에 대한 회전 파트이다. 거리는

의 곱에 의해 계산되고, 접선 속도 공간상의 대응되는 속도는 로그에 의해 주어진다. 그러고 나서 접선 벡터 공간 상의 결과 벡터의 지수는

에 의해 곱해진다. 수학식 5에서

은 so(3) 상의 균등한 랜덤 벡터를 나타내고,

는 so(3) 상의 기본 요소에 대하여 열벡터(column vector)에서 나타내지는 so(3) 요소 사이의 요소 와이즈 곱(component wise multiplication)을 나타낸다.

에 관해서는 수학식 6과 같이 통상의 PSO 알고리즘에 의해 나타내질 수 있다.here

Wow

Is the rotation part for each of the individual and full best particles. Distance

Is calculated by the product of and the corresponding velocity in the tangential velocity space is given by the logarithm. Then the exponent of the resulting vector in the tangential vector space is

Multiplied by In equation (5)

Denotes an equal random vector on so (3),

Denotes a component wise multiplication between the elements of so (3) represented in the column vector for the element on so (3).

As can be expressed by the conventional PSO algorithm as shown in Equation (6).

와

는 개별적 및 전체 베스트 프티클 각각의 이동 파트이다.

주변의 퀀텀 파티클들은

에 의해 생성되며, u는 so(3) 상의 평균이 0(zero mean)인 균등한 랜덤 벡터이다. 유사하게,

에 관한 퀀텀 파티클들은 평균

를 갖는 균등한 분포로부터 생성된다.
here

Wow

Is the moving part of each of the individual and full best articles.

The surrounding quantum particles

Is a uniform random vector with a mean of zero on so (3). Similarly,

Quantum particles about are average

Is generated from an even distribution with

4. The proposed framework

The system according to the invention uses only images from a handheld camera as sensor input. Both monocular and binocular cameras with known unique camera parameters can be considered. The proposed method is based on RBPF filtering, but instead of approximate optimal importance function, importance sampling is performed through PSO iteration. Zhang et al., Who applied PSO to a visual tracking algorithm, noted that PSO iterations can be considered as multi-layered importance sampling. Similar to this approach, we apply PSO to SLAM systems. The entire process of the proposed algorithm is summarized in Table 1. It will be described in detail below.

4.1. Dynamic model

(∈SE(3))에 의해 카메라 상태를 도출하기 위해 파티클은 수학식 7에 의해 유포된다.The state equation representing the camera motion can be derived from the stochastic differential equation on SE (3). particle

In order to derive the camera state by (SE (3)), the particles are distributed by equation (7).

는 공분산

(

)를 갖는 se(3) 상의 위너 과정(Wiener process) 노이즈이고,

은 파티클의 유포를 결정하는 상태 동역학 텀(state dynamics term)이다. SLAM 시스템에서는 어떤 오도메트리(odometry) 정보도 이용할 수 없으므로, 일정 속도 가정과 같은 부드러운 모션 모델이 일반적으로 사용된다. 여기서는 상태 동역학 텀으로써 SE(3) 상에서 AR 과정(auto-regressive process)을 사용한다. 이러한 AR에 기초한 상태 동역학 모델은 부드러운 카메라 모션을 가정한다. 그러고 나서 AR 상태 동역학은 수학식 8에 의해 업데이트된다.here

Covariance

(

Wiener process noise on se (3) with

Is the state dynamics term that determines the distribution of particles. Since no odometry information is available in the SLAM system, smooth motion models such as constant velocity assumptions are commonly used. Here we use an auto-regressive process on SE (3) as the state dynamics term. This AR based state dynamics model assumes smooth camera motion. The AR state dynamics is then updated by equation (8).

Where a is the first AR process parameter. In the case of a binocular (stereo) camera, the camera state is based on the left camera.

4.2. Observation Model

, 그리고 역 거리(inverse distance)

로 구성되는 6차원 벡터로 표현된다. 그러므로 랜드마크는 공분산

(

을 갖는

로 나타내질 수 있다.Landmarks are considered to be three-dimensional points in Euclidean space with respect to initial camera coordinates. In particular, since it is possible to initialize landmarks that handle infinite three-dimensional points and use a single image, the landmarks are represented by Inverse Depth Point (IDP) parameterization. IDP L is the initial three-dimensional point t ^, azimuth

, And inverse distance

It is represented by a six-dimensional vector consisting of. Therefore, landmarks are covariance

(

Having

It can be represented as.

로부터의 측정치

는 랜드마크의 카메라 투영 상의 2차원 이미지 좌표이고, 수학식 9로 표현된다.and then

Measurements from

Is the two-dimensional image coordinates on the camera projection of the landmark and is represented by equation (9).

는 공분산 Q(

)를 갖는 가우시안 측정 노이즈이다. 그러고 나서 유사도 분포는 수학식 10과 같이 주어진다.Where h is a function that moves L to a three-dimensional landmark location in homogeneous coordinates, g is a perspective camera projection function with internal parameters,

Is the covariance Q (

Gaussian measurement noise with Then, the similarity distribution is given by Equation 10.

는

벡터가 된다.If you have N landmarks on the screen

Is

It becomes a vector.

Using a stereo camera, it is possible to use depth information calculated by stereo triangulation. However, this depth information is sensitive to measurement noise, especially when the landmark is far away from the camera. For this reason, stereo pair points are used directly as measurements without a triangle network. The measurement equation for the case of a binocular camera is expressed as Equation (11).

(∈SE(3))는 좌측 카메라로부터 우측 카메라로의 좌표 변환을 나타내고, n-k는 공분산

(

)를 갖는 가우시안 측정 노이즈이다.

(SE (3)) represents the coordinate transformation from the left camera to the right camera, and nk is the covariance

(

Gaussian measurement noise with

4.3. Feature and Data Association

The corner points are extracted by the FAST-10 corner detector. A 31 × 31 patch centered on the extracted corner point was cut out and stored when the landmark was initialized. In the matching step, the normalized cross correlation (NCC) of 21 × 21 transformed patches is calculated. Landmark patches are converted by taking into account rotation and scale changes in accordance with the evaluated camera motion. If the size of the patch changes significantly compared to the initial size, reregister the new patch.

One important feature of SLAM based on PF is that it is robust to the outliers of the data combination because each particle has its own data association. However, since data combinations require matching of extracted feature points to the landmark map, the naive implementation of per-particles to the visual SLAM can be a huge waste of time. Therefore, more advanced data combining methods have been proposed for effective visual SLAM. Here we follow a careful one-shot data combination that avoids the simplest data combination per particle.

In the case of a binocular camera, stereo matching is further performed on the updated two-dimensional image points of the landmark relative to the reference camera (left camera). A simple template matching method based on SAD is applied and uses modified images to reduce the detection range for horizontal lines. After the initial match, the LR check step is followed to remove outliers of the stereo match.

4.4. Multilayer importance sampling with PSO on SE (3)

는 층 중요도 샘플링(layered importance sampling)의 초기 파트로 사용되었다.

로부터의 초기 샘플링은 수학식 7에 의해 이루어진다. 초기 샘플링 후에, 초기 샘플

를 갖는 PSO 반복 수행이 이루어진다.The particle filtering process based on the state and measurement equations defined above is described. In the algorithm of the present invention, state transition density

Was used as an early part of layered importance sampling.

The initial sampling from is made by equation (7). After initial sampling, initial sample

PSO iterations are performed.

에 따라서

및

를 설정한다.In the PSO iteration step, the goodness-of-fit function defined by Equation 12 first

according to

And

Set.

는 올바르게 매치된 랜드마크의 수이다.

를 최대화함으로써, 측정 오차를 최소화하는 최적의

를 찾을 수 있다.here

Is the number of landmarks matched correctly.

By maximizing the

Can be found.

,

, 그리고

에 대한 적절한 값을 설정하는 법과 반복 수행 회수는 PSO에 있어서 결정적인 요인이다. 전체 최적 값을 찾아내는 능력은

,

, 그리고

에 달려있다. 우선

과

는

와

에 대한 상대적 인력(relative attraction)을 각각 제어한다.

=

로 설정하는 방법이 널리 이용되고 있다. 수렴 속도는 주로

에 의해 영향을 받는다.

가 작으면 탐사 능력이 떨어지고 파티클들이 종종 지역적 최적화에 이르게 되는 반면, 알고리즘의 빠른 수렴이 보장된다. 반대로

가 너무 크면 심각하게 수렴 속도가 떨어지게 된다.

=0.5,

=

=

로 하는 것이 실험에 있어서 가장 훌륭한 성능을 보여주었다.

,

, And

How to set an appropriate value for and the number of iterations is a decisive factor for PSO. The ability to find the overall optimal value

,

, And

Depends on first

and

Is

Wow

Control relative attraction to each.

=

The method of setting is widely used. Convergence rate is mainly

Affected by

Smaller values mean less exploration capability and particles often lead to local optimization, while ensuring fast convergence of the algorithm. Contrary

Too large will seriously slow down the convergence rate.

= 0.5,

=

=

Showed the best performance in the experiment.

Finding the full optimization is also related to the number of PSO iterations. The greater the number of iterations, the higher the probability of reaching full optimization, but the longer it takes to converge. The number of PSO iterations is automatically determined according to equation (13).

는 한 반복 수행 내에서 가장 나쁜 파티클을 의미한다. 반복 수행의 최대 회수는 30회로 제한된다. 이는 적합치(fitness value)의 향상이 30회의 반복 수행 이후에는 미미하기 때문이다.here

Means the worst particle in one iteration. The maximum number of iterations is limited to 30 times. This is because the improvement in fitness value is minor after 30 iterations.

주변으로 반경 rcloud을 갖는 퀀텀 파티클을 생성한다. 여기서는 가 상태 공분산(state covariance)와 동일하도록 설정했다. PSO를 통한 다층 중요도 샘플링이 마쳐지면

는 수학식 8을 통해 업데이트된다.
20 percent of neutral particles after particle update

Create a quantum particle with a radius rcloud around it. Here Is set equal to state covariance. When you have finished multi-level importance sampling with PSO

Is updated through equation (8).

4.5. Landmark update

와 측정치

에 기초하여 EKF에 의해 업데이트된다.When observing a new feature, the landmarks are initialized. In the case of a binocular camera, only one reference camera (left camera in the system of the present invention) is used to initialize the landmark. Landmarks are sampled after importance sampling

And measurements

Is updated by EKF based on.

The particle weight after the landmark update is calculated by Equation 14.

는 PSO 반복 수행 전에 초기 상태를 나타낸다. 마지막으로, 최적화 상태 평가(optimal state estimation)

가 파티클의 기하학적 샘플 평균에 의해 계산된다.
here

Indicates the initial state before performing the PSO iteration. Finally, optimal state estimation

Is calculated by the geometric sample mean of the particles.

DETAILED DESCRIPTION Hereinafter, exemplary embodiments of the present invention will be described in detail with reference to the accompanying drawings so that those skilled in the art may easily implement the present invention.

3 is a block diagram provided to explain an apparatus for determining a camera pose according to an exemplary embodiment of the present invention.

Referring to FIG. 3, first, the camera pose determination apparatus 100 receives an image captured by a camera 200 mounted on a mobile robot (not shown) in frame units to determine a camera pose in a corresponding frame. Do this. Here, the camera pose means the pose of the camera and the position of the camera. The camera pose determination apparatus 100 may be mounted on the mobile robot or implemented to communicate with the mobile robot in a wired or wireless manner.

으로 표현되며, R은 3×3 회전 행렬이고, t(

)는 이동 벡터(translation vector)이다. 이하에서는 위에서 파티클을 표현한 동차 행렬(X)을 카메라 포즈 행렬이라고 부른다.In this embodiment, the pose of the camera can be represented by a rigid body transformation of the camera coordinate system with respect to the initial camera pose, so that the particles representing the pose of the camera are a homogeneous matrix.

Where R is a 3x3 rotation matrix and t (

) Is a translation vector. Hereinafter, the homogeneous matrix X representing the particles above is called a camera pose matrix.

Next, a camera pose determination method according to an exemplary embodiment of the present invention will be described with reference to FIG. 4.

4 is a flowchart provided to explain a camera pose determination method according to an exemplary embodiment of the present invention.

Referring to FIG. 4, first, the camera pose determination apparatus 100 performs a parameter initialization operation for performing an algorithm for obtaining a pose of the camera 200 (S410).

In operation S410, the number N of particles may be set, and the number of repetitions M of particle swarm optimization (PSO) may be set.

The camera pose matrix X and the state dynamics term a ₀ may be initialized as shown in Equation 15 by the set number N of particles. That is, set the particle to its initial state.

The information on the plurality of landmarks for each of the plurality of particles is initialized in the initial (previous) frame. Information about an inverse depth point (IDP) L obtained for a plurality of landmarks is stored for each of the plurality of particles. The landmark depth of field L is It can be represented by a 6-D vector. Meanwhile, landmark image patches are stored for each of the plurality of particles. The landmark image patch means a cropped to a predetermined size, for example, 31 × 31 pixels, around a corner point at which the landmark is projected in the previous frame.

When the initialization is completed by the step S410, the camera pose determination apparatus 100 extracts a plurality of corner points respectively corresponding to the plurality of landmarks from the current frame acquired by the camera 200 (S420).

In more detail, the camera pose determination apparatus 100 may extract a corner point from the current frame using the FAST-10 corner extractor. The camera pose determination apparatus 100 matches an image patch for a landmark in an initial (previous) frame with an image patch having a center of a corner point extracted from the current frame. To this end, the camera pose determination apparatus 100 performs matching with the extracted corner point by modifying the landmark image patch by rotation and scale change. Only corner points matching the landmark image patch are used to determine the pose of the camera.

Next, the camera pose determination apparatus 100 determines the pose of the camera with respect to the current frame by performing Particle Swarm Optimization (PSO) on the Special Euclidean Group SE 3 for a plurality of particles by using the extracted corner points. S430).

Determining the pose of the camera for the current frame by performing PSO on the SE 3 will be described in more detail.

)을 각각 랜덤하게 추출한다. 이에 의해 파티클이 초기 상태에서 새로운 상태로 이동한 것을 의미한다.According to the predefined camera state transition model from the plurality of initialized particles, the camera pose matrix for the new plurality of particles (

) Are randomly extracted. This means that the particles move from the initial state to the new state.

(∈SE(3))의 전파를 나타내는 수학식 7을 이용하여 구해지는 상태 천이 밀도 함수

가 이용될 수 있다.In this case, the camera state transition model may be variously defined by the designer. For example, when the pose of the camera is represented as a particle, the particle

State transition density function obtained using equation (7) representing propagation of (#SE (3))

Can be used.

Next, when the camera pose matrix ( X _k ) for a plurality of particles is extracted, the newly extracted camera pose matrix ( X _k ) is applied to a predefined fitness function to determine each individual optimal solution ( P ⁱ _ib) for the plurality of particles. ) And the overall optimal solution ( P _gb ). Here, the individual optimal solution ( P ⁱ _ib ) means the optimal solution obtained so far for the i-th particle, and the total optimal solution ( P _gb ) means the optimal solution obtained so far for all the particles. The fitness function may be used as Equation 12 described above.

과 개별 최적해(P ⁱ _ib)와 전체 최적해(P _gb)를 이용하여 Special Euclidean Group SE(3) 상에서 PSO(Particle Swarm Optimization)를 미리 정해진 수(M)만큼 반복 수행한다.The next camera pose matrix

Particle Swarm Optimization (PSO) is repeated on the Special Euclidean Group SE (3) by a predetermined number (M) using, and the individual optimal solution ( P ⁱ _ib ) and the global optimal solution ( P _gb ).

에 대해 상기한 수학식 5와 수학식 6을 이용하여 앞에서 설명한 것과 같이 R ⁱ를 위한 속도 벡터 V _R ⁱ 와 t ⁱ를 위한 V _t ⁱ를 se(3) 상에서 구한 후 다시 SE(3) 상으로 변환하는 과정을 통해 각각의 파티클에 대해서 새로운 카메라 포즈 행렬

을 구할 수 있다. 즉 이에 의해 파티클이 새로운 위치로 이동시킨 것을 의미한다.First camera pose matrix

For R ⁱ as described above using Equations 5 and 6 above, V _t ⁱ for the velocity vectors V _R ⁱ and t ⁱ New camera pose matrix for each particle by obtaining it on se (3) and converting it back to SE (3).

Can be obtained. This means that the particles are moved to a new position by this.

에 대해 위에서 수학식 12로 나타낸 적합도 함수를 적용하여 개별 최적해(P ⁱ _ib)와 전체 최적해(P _gb)를 업데이트 한다. 해당 파티클에 대해 수학식 12를 통해 구해진 적합도 값이 해당 파티클의 개별 최적해(P ⁱ _ib)의 적합도 값 보다 클 경우 샘플 행렬

을 해당 파티클의 개별 최적해(P ⁱ _ib)로 업데이트한다. 그리고 해당 파티클에 대해 수학식 12를 통해 구해진 적합도 값이 전체 최적해(P _gb)의 적합도 값보다 클 경우 새롭게 구해진 샘플 행렬

을 전체 최적해(P _gb)로 업데이트한다.Then the newly obtained camera pose matrix

The individual optimal solution ( P ⁱ _ib ) and the global optimal solution ( P _gb ) are updated by applying the fitness function represented by Equation 12 above. A sample matrix when the goodness-of-fit value obtained through Equation 12 for the particle is greater than the goodness-of-fit value of the individual optimal solution ( P ⁱ _ib ) of that particle.

Update to the individual optimal solution ( P ⁱ _ib ) of the particle. If the goodness-of-fit value obtained through Equation 12 for the particle is larger than the goodness-of-fit value of the overall optimal solution ( P _gb ), the newly obtained sample matrix

Update to the full optimal solution ( P _gb ).

)을 상기한 수학식 4에 의해 추출한다.Next, quantum particles (

) Is extracted by the above equation (4).

)에 대해서 수학식 12를 통해 적합도 값을 구하고 구해진 적합도 값이 현재의 전체 최적해(P _gb)의 적합도 값보다 큰 경우 전체 최적해(P _gb)를 해당 퀀텀 파티클(

)로 변경한다.Extracted Quantum Particles (

), And obtain a goodness-of-fit value using Equation 12, and if the obtained goodness-of-fit value is greater than the goodness-of-fit value of the current overall best solution ( P _gb ), then the overall best solution ( P _gb ) is obtained from the corresponding quantum particle (

To).

As described above, if the PSO is repeated as many as the predetermined number (M) or if Equation 13 is satisfied, the PSO is terminated for the current frame.

Next, the camera pose for the current frame is determined using each individual optimal solution and the total optimal solution for the plurality of particles up to that point. The average of each individual best solution and the overall best solution can be determined by the camera's pose for the current frame. Of course, depending on the embodiment, the entire optimal solution may be obtained as the pose of the camera of the current frame.

4, after obtaining the pose of the camera of the current frame by performing the step (S430), the following post-processing operation may be performed to obtain the pose of the camera of the next frame (S440).

와 측정치

에 기초하여 EKF에 의해 업데이트된다. 그리고 파티클의 가중치는 상기한 수학식 14에 의해 계산된다. 다음으로 복수 개의 파티클에 대한 재샘플링을 수행한다. 복수 개의 파티클 중에서 미리 정해진 기준을 만족하지 못하는 파티클을 삭제하고, 대신에 복수 개의 파티클 중 미리 정해진 기준 이상의 파티클로 대체할 수 있다. 그리고 필요한 경우 현재 프레임에서 구해지는 새로운 코너 포인트에 대한 랜드마크 초기화를 수행한다.First importance sampling and then landmarks are sampled camera poses

And measurements

Is updated by EKF based on. And the weight of the particle is calculated by the above equation (14). Next, resampling is performed on a plurality of particles. Particles that do not satisfy a predetermined criterion may be deleted from the plurality of particles, and instead, particles that are greater than or equal to a predetermined criterion among the plurality of particles may be replaced. If necessary, landmark initialization is performed for a new corner point obtained in the current frame.

After completing the above operation, steps S420 to S430 are performed on the next frame to obtain a camera pose of the next frame.

5 is a view provided for comparing the error rate when using the camera pose determination method according to the present invention and another conventional method. 5 (a) shows an error of a camera position, FIG. 5 (b) shows an error of a camera direction, and FIG. 5 (c) shows an error of a landmark position.

Referring to FIG. 5, it can be seen that PSO-SLAM according to the present invention has a much lower error rate than PF-SLAM and is almost similar to OIF-SLAM. Table 2 below shows the number of particles used in each method in the experimental example of FIG.

 Number of particles PF-SLAM 500 (A1), 1000 (A2), 1500 (A3) PSO-SLAM 70 (B1), 100 (B2), 200 (B3), 300 (B4), 400 (B5) OIF-SLAM 100 (C1), 200 (C2), 400 (C3), 600 (C4), 800 (C5)

6 is a view provided to compare the error rate between the PSO-SLAM according to the present invention and the conventional OIF-SLAM when there is a sudden motion change.

Referring to Figure 6 shows that the PSO-SLAM according to the present invention is more robust than the rapid motion change than the conventional OIF-SLAM.

Embodiments of the present invention include a computer-readable medium having program instructions for performing various computer-implemented operations. This medium records a program for executing the camera pose determination method described above. The medium may include program instructions, data files, data structures, etc., alone or in combination. Examples of such media include magnetic media such as hard disks, floppy disks and magnetic tape, optical recording media such as CDs and DVDs, floppy disks and program commands such as magnetic-optical media, ROM, RAM, flash memory, and the like. Hardware devices configured to store and perform such operations. Alternatively, the medium may be a transmission medium such as an optical or metal wire, a waveguide, or the like including a carrier wave for transmitting a signal specifying a program command, a data structure, and the like. Examples of program instructions include not only machine code generated by a compiler, but also high-level language code that can be executed by a computer using an interpreter or the like.

While the present invention has been particularly shown and described with reference to exemplary embodiments thereof, it is to be understood that the invention is not limited to the disclosed exemplary embodiments, Of the right.

Claims (10)

Initializing a plurality of particles representing a pose of a camera in a previous frame and a plurality of landmarks for each of the plurality of particles,
Extracting a plurality of corner points respectively corresponding to the plurality of landmarks in a current frame, and
Determining a pose of a camera with respect to a current frame by performing Particle Swarm Optimization (PSO) on the Special Euclidean Group SE (3) for the plurality of particles using the extracted corner points.
The particle is a homogeneous matrix

Where R is a 3x3 rotation matrix and t (

) Is a translation vector. The method of claim 1,
The extracted corner point is,
And determining the pose of the camera when the image patch of the landmark and the corner point extracted from the current frame are matched with each other by comparing the image patch of the landmark in the initial frame. 3. The method of claim 2,
The camera pose determination step,
(a) a camera pose matrix corresponding to each of the initialized plurality of particles according to a predefined camera state transition model

(camera pose matrix corresponding to the i th particle in the k th frame),
(b) applying the camera pose matrix to a predefined goodness-of-fit function to obtain each individual best solution and a total best solution for a plurality of particles;
(c) the camera pose matrix

Equation for

- In the equation V ⁱ _R and V ⁱ _T denotes a speed for the moving part (R ⁱ⁾ and the rotational part (t ⁱ⁾ divided from each particle _{(Xi), c 1, c} 2 are each independently of the weight constant , R ₁ , r ₂ are random variable vectors derived from an even distribution with respect to [0,1], w is an inertia constant, and R ⁱ _ib and R _gb are rotating parts for each individual particle and the best best particle respectively. T ⁱ _ib and t _gb are the moving parts of each of the individual and full best particles, operator ← is the operator that assigns the right term to the left term, operator T is the transpose transform operator, and

Is an operator that performs component wise multiplication of the element position of each column vector-
Obtaining a new camera pose matrix using
(d) updating each individual best solution and the overall best solution for a plurality of particles by applying the predefined fit function to the camera pose matrix obtained in step (c);
(e) repeating steps (c) and (d) by a predetermined condition; and
and (f) determining the pose of the camera for the current frame using the respective optimal and total optimal solutions for the plurality of particles after completion of step (e). In claim 3,
And determining the average of each individual best solution and the overall best solution as the camera pose for the current frame. In claim 3,
The predetermined condition is
Camera pose when the number of iterations of steps (c) and (d) reaches a predetermined number or when the difference between the goodness-of-fit value of the particle having the best solution and the particle having the worst solution is smaller than the predetermined criterion How to decide. delete In claim 3,
The camera pose determination step,
Quantum particles (within a certain radius based on the current optimal particle)

) And extracted quantum particles (

) If the value obtained is fit to obtain a fit value using the fitness function is greater than the predefined value for the goodness of fit of the total optimal solution (P _gb) with respect to the overall optimal solution (P _gb) quantum particles (

Camera pose determination method. In claim 3,
And updating the plurality of landmarks using EKF after the camera pose determination step of the current frame. In claim 3,
And deleting a particle not satisfying a predetermined criterion from among the plurality of particles after the camera pose determining step of the current frame, and replacing the particle having a predetermined criterion or more among the plurality of particles. 10. A computer readable medium having recorded thereon a program for executing the method of any one of claims 1 to 5.

Images