<?xml version="1.0" encoding="utf-8"?><feed xmlns="http://www.w3.org/2005/Atom" xml:lang="kr"><generator uri="https://jekyllrb.com/" version="3.10.0">Jekyll</generator><link href="/feed.xml" rel="self" type="application/atom+xml" /><link href="/" rel="alternate" type="text/html" hreflang="kr" /><updated>2025-04-16T05:42:39+00:00</updated><id>/feed.xml</id><title type="html">Riibotics</title><subtitle>Write an awesome description for your new site here. You can edit this line in _config.yml. It will appear in your document head meta (for Google search results) and in your feed.xml site description.</subtitle><author><name>JinyongJeong</name></author><entry><title type="html">[CS] Multi-core에서의 Multi Processing</title><link href="/computerscience/2025/04/13/Multi_processing_in_multi_core.html" rel="alternate" type="text/html" title="[CS] Multi-core에서의 Multi Processing" /><published>2025-04-13T00:00:00+00:00</published><updated>2025-04-13T00:00:00+00:00</updated><id>/computerscience/2025/04/13/Multi_processing_in_multi_core</id><content type="html" xml:base="/computerscience/2025/04/13/Multi_processing_in_multi_core.html"><![CDATA[<h1 id="로봇-개발과-multi-processor">로봇 개발과 Multi Processor</h1>

<p>이번 글에서는 Multi Core에서 어떤식으로 Multi Processing이 진행되고, 개발자가 고려해야하는 data syncronization, 의도치 않은 문제가 생길 수 있는 data의 write와 read 사이의 괴리등에 대해서 알아보고자 한다.</p>

<h1 id="coherency">Coherency</h1>

<p>일단 Mutex도 메모리상에 존재하는 값이기 때문에 점유상태인지 아닌지 여부(is Locked)가 memory, 나아가 cache에 저장된다. 근데 Multi-Processor환경에서는 아래 사진과 같이 코어마다 cache가 존재한다.</p>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/cache.png?raw=true" alt="cpu 구조" /></p>

<p>그럼 두 프로세서가 cache에 각각 캐시에 저장된 값을 읽고 같은 clock에 mutex값을 바꿔버리면 원자명령이 의미 없는게 아닐까? 란 생각이 들었다. 여러 cache가 동기화되지 않으면 해당 데이터가 접근할 때 최신이라는 보장인 Coherency가 없기 때문이다. 하지만 찾아보니 데이터에 접근할 때 해당 데이터가 최신임을 보장해주는 Cache-coherency Protocol이 존재했다. 이 프로토콜은 cache를 실시간으로 하드웨어에 의해 동기화하여 동시에 값을 수정하는것을 물리적으로 불가능 하게 만든다. 그럼 어떤방식으로 하드웨어에서 Coherency를 유지해주는지 알아보자.</p>

<h2 id="bus-snooping"><strong>Bus snooping</strong></h2>

<p>말그대로 Bus를 통해 오가는 데이터를 관찰하는 방법이다. Bus를 관찰하고 있다가 특정 주소에 대한 작업이 발생하면 캐시에 맵핑된 주소값에 대한 동작이 감지되면 해당 프로세서의 캐시에 업데이트가 아니더라도 같이 업데이트를 해주는 방식이다. 이 Bus snooping기법에는 두가지 방식이 있다.</p>

<ul>
  <li>
    <p><strong>Write-update</strong> : 프로세서가 공유 캐시 블록에 write 작업을 하면 Bus snooping을 통해 다른 캐시의 모두 공유 캐시 블록의 값을 업데이트한다. 이 경우 Bus 내부에 bottleneck현상이 발생 할 수 있다.</p>

    <p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/BusSnooping_write.png?raw=true" alt="Write-update" /></p>
  </li>
  <li>
    <p><strong>Write-invalidate</strong> : 프로세서가 공유 캐시 블록에 write 작업을 하면 Bus snooping을 통해 다른 캐시의 공유 캐시 블록에 invalid flag를 업데이트 한다.</p>

    <p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/BusSnooping_tag.png?raw=true" alt="Write-invalidate" /></p>
  </li>
</ul>

<p>이를 위한 여러 프로토콜중 MESI 프로토콜에 대해 알아보자.</p>

<h2 id="mesi-protocol"><strong>MESI protocol</strong></h2>

<p>MESI protocol에서 각각의 cache block(참고 논문에선 cache line이라고 함) “modified”, “exclusive”, “shared”, 그리고 “invalid” 총 4가지 state로 구분된다. 물론 더 많은 상태가 있지만 이것만 봐도 된다고 한다.</p>

<ul>
  <li>
    <p><strong>modified</strong> : 해당 core가 해당하는 cache block에 대한 write연산을 가장 최근에 한 경우이다. 이 경우 해당 core에서 가장 최근의 값을 수정했기 때문에 해당하는 cache block의 유일한 최신 사본을 갖고있는 상태가 된다.</p>
  </li>
  <li>
    <p><strong>exclusive</strong> : 특정 cache block이 메모리로부터 처음 복사되어 다른 cache는 아무도 갖고있지 않은 상태이다. 즉 해당 cache block이 해당 cache에만 존재한다는 의미로 core가 값을 수정하면 책임지고 memory에 변경사항을 저장해야 하는 상태이다.</p>
  </li>
  <li>
    <p><strong>shared</strong> : 해당하는 cache block이 모두 최신 상태이고, 다른 cache와 공유된 상태를 의미한다. 이 경우 cache block을 수정하기 위해서는 다른 core와 상의가 필요한 상태이다.</p>
  </li>
  <li>
    <p><strong>invalid</strong> : 해당 cache block이 최신사본이 아닌경우이다. 다른 core의 cache에서 해당 cache block이 수정되는 경우 해당 상태로 진입하고, 해당 cache block이 없는 상태이다.</p>
  </li>
</ul>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/MESISTATE.png?raw=true" alt="MESI state machine" /></p>

<p>MESI protocol에서 각각의 cache block은 위 사진과 같은 state를 가지게 된다. 상태전이 이벤트에 대해 간략히 설명하면 다음과 같다.</p>

<h3 id="processor-requests-event"><strong>Processor Requests Event</strong></h3>

<ul>
  <li>
    <p><strong>PrRd</strong>: 해당 cache block이 core에 의해 read연산이 진행되는 이벤트</p>
  </li>
  <li>
    <p><strong>PrWr</strong>: 해당 cache block이 core에 의해 write연산이 진행되는 이벤트</p>
  </li>
</ul>

<h3 id="bus-side-event"><strong>Bus side Event</strong></h3>

<ul>
  <li>
    <p><strong>BusRd</strong>: 다른 core에서 특정 캐시블럭에 대한 읽기 요청이 발생한 경우</p>
  </li>
  <li>
    <p><strong>BusRdX</strong>: 보유중인 cache block에 대해 다른 core에서 쓰기 요청이 발생한 경우.</p>
  </li>
  <li>
    <p><strong>BusUpgr</strong>: 가지고 있지 않은 cache block에 대해 다른 core에서 쓰기 요청이 발생한 경우.</p>
  </li>
  <li>
    <p><strong>Flush</strong>: 전체 cache block에 대해 다른 core에서 쓰기 요청이 발생한 경우. 해당 변경 내용과 같이 다른 캐시에 전달하는 메세지</p>
  </li>
  <li>
    <p><strong>FlushOpt</strong>: 특정 cache block에 대해 다른 core에서 쓰기 요청이 발생한 경우. 해당 변경 내용과 같이 다른 캐시에 전달하는 메세지</p>
  </li>
</ul>

<p>메모리 계층구조에 의해 프로세스는 캐시에서 모든 정보를 불러와 처리를 한다. 그렇기 때문에 같은 메모리 주소(위에서 의문으로 제기했던 멀티코어에서 하나의 뮤텍스)는 하나의 cache에 대응되고, 위에서 설명한것 과 같이 동일 cache block에 대해서 하드웨어적으로 항상 최신의 값이라는 보장을 해주어 Coherency를 지키게 된다.</p>

<h1 id="consistency"><strong>Consistency</strong></h1>

<p>그렇다면 이러한 일관성 문제는 모두 해결 된걸까? 위에서의 Cache Coherency Protocol은 여러개의 cache 저장소의 일관성을 유지하기 위한 프로토콜이었다. 하지만 메인메모리(RAM)는 하나기 때문에 항상 메모리가 최신이라는 보장인 일관성(Coherency)에 대해 신경 쓸 필요가 없다. 여기서 또 다른 개념 하나가 더 등장하는데, 바로 Consistency이다. 이 단어도 마찬가지로 일관성이라는 뜻을 가지지만 위에서 다룬 의미와는 조금 다르다.</p>

<p>Consistency는 서로 다른 데이터 사이의 순서를 지키는 것이다. 쉽게 말해 Program Order와 Execution Order의 순서가 일치함을 의미한다. Program Order는 작성된 프로그램에서의 실행순서, Execution Order 실제로 프로세서가 메모리에 접근하는 순서를 가르킨다.</p>

<p>하지만 Consistency는 바로 앞에서 설명한 cache-coherency protocol에 의해 깨질 수 있다(물론 pipelining을 위한 최적화에 의해서도 깨질 수 있음). 일단 이번 포스트에서는 cache-coherency protocol에 관련해서 Consistency에 문제가 생기는 원인 만 알아보려고 한다.</p>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/cachestall.png?raw=true" alt="cache write stall example" /></p>

<p>위 사진은 CPU 0(이하 0번 core)이 특정 cache블럭에 대한 write연산을 하기위해 CPU 1((이하 1번 core))의 cache block을 무효화 하는 과정을 나타낸 그림이다. 이와 같이 하나의 코어가 write연산을 하기위해서는 다른 코어에 의해 허락을 받아야 하는 불필요한 지연 시간이 생기게 된다.</p>

<p><br /></p>

<h2 id="store-buffer"><strong>Store buffer</strong></h2>

<p>현대 컴퓨터 구조에서는 이러한 불필요한 지연을 해결하기 위해 아래 그림과 같은 <strong>Storer buffer</strong>를 도입하였다. Store buffer는 현재 아래 그림과 같이 core와 cache사이에 존재하는 버퍼이다. 이 버퍼는 write연산 결과를 잠깐 저장했다가 core대신 Acknowledgement 메세지를 받아 캐시에 적어주는 역할을 한다. 그럼 core는 굳이 Acknowledgement를 기다리지 않고 다른 작업을 하면 된다.</p>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/storebuffer.png?raw=true" alt="store buffer" /></p>

<h3 id="self-consistency-violation"><strong>self-consistency violation</strong></h3>

<p>하지만 store buffer의 도입에 따라 새로운 문제가 발생한다. 첫번째 문제는 <strong>self-consistency violation</strong>이다. 먼저 아래 사진과 같은 상태의 시스템을 생각해보자.</p>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/selfviolation.png?raw=true" alt="self-consistency violation" /></p>

<p>이 상황에서 0번째 코어가 아래와 같은 코드를 수행한다면 정상적으로 b가 2가 되지 않을 수 있다. Cache 0에서 a=1을 업데이트하기 위해서는 1번 core의 허락을 맡아야 하는데, 허락을 맡기위해 a=1결과가 store buffer에 체류하는 동안 두번째 줄을 실행하면 a가 아직 0인 상태로 계산되기 때문이다.</p>

<div class="language-c highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="n">a</span> <span class="o">=</span> <span class="mi">1</span><span class="p">;</span>
<span class="n">b</span> <span class="o">=</span> <span class="n">a</span> <span class="o">+</span> <span class="mi">1</span><span class="p">;</span>
<span class="n">assert</span><span class="p">(</span><span class="n">b</span><span class="o">==</span><span class="mi">2</span><span class="p">);</span>
</code></pre></div></div>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/selfviolationtimeline.png?raw=true" alt="self cosistency violation timeline" /></p>

<p>위 사진은 self-consistency violation이 일어나는 타임라인에 대해 그린 그림이다. 앞 명령어의 stall 기간 동안 뒤의 명령어가 앞의 명령의의 결과를 사용하는 경우에 주로 self-consistency violation이 발생할 수 있다.</p>

<p>물론 위 문제의 해결책은 존재한다. 각각의 core가 자신의 store buffer에서 load 중인 cache block을 조회 할 수 있게 하면 된다. 이를 <strong>Caches With Store Forwarding</strong>이라고 한다. 아래 사진은 Caches With Store Forwarding의 간략한 구조와 적용되었을때 위 문제사항에서 실행 플로우이다.</p>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/store_buffer_timeline.png?raw=true" alt="Timeline Caches With Store Forwarding" /></p>

<h3 id="violation-of-global-memory-ordering"><strong>violation of global memory ordering</strong></h3>

<p>두번째 문제는 <strong>violation of global memory ordering</strong>이다. 아래 사진과 같이 캐시에 데이터가 저장되어있고, 아래의 코드를 각각의 프로세스가 실행하는 상황이라고 가정해보자.</p>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/global_order_violation_example.png?raw=true" alt="violation of global memory ordering" /></p>

<div class="language-c highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="kt">void</span> <span class="nf">foo</span><span class="p">(</span><span class="kt">void</span><span class="p">)</span> <span class="c1">// execute by Core 0</span>
<span class="p">{</span>
    <span class="n">a</span><span class="o">=</span><span class="mi">1</span><span class="p">;</span>
    <span class="n">b</span><span class="o">=</span><span class="mi">1</span><span class="p">;</span>
<span class="p">}</span>
<span class="kt">void</span> <span class="nf">bar</span><span class="p">(</span><span class="kt">void</span><span class="p">)</span> <span class="c1">// execute by Core 1</span>
<span class="p">{</span>
    <span class="k">while</span> <span class="p">(</span><span class="n">b</span> <span class="o">==</span> <span class="mi">0</span><span class="p">)</span> <span class="k">continue</span><span class="p">;</span>
    <span class="n">assert</span><span class="p">(</span><span class="n">a</span> <span class="o">==</span> <span class="mi">1</span><span class="p">);</span>
<span class="p">}</span>
</code></pre></div></div>

<p>이 경우 아래 그림의 타임라인과 같이 실행 순서가 바뀌는 문제가 발생 할 수 있다. 이는 앞서 말했던 <strong>Consistency</strong> 문제로 서로 다른 core사이에서 특정 변수들의 업데이트 순서를 관찰 할 때, 실제 코드의 실행순서와 다르게 관찰 될 수 있게 된다. 이는 core 0에서 <code class="language-plaintext highlighter-rouge">a=1, b=1</code> <strong>명령어를 순차적으로 수행</strong>되었지만, 다른 캐시의 core에 있는  변수 <code class="language-plaintext highlighter-rouge">a</code>의 cache의 경우 원 소유주인 core 1의 허가를 받으며 느리게 수행되어 <strong>결과가 다른 core에 전파되는 순서가 바뀌게 된 것</strong>이다.</p>

<p><img src="https://github.com/reofard/reofard.github.io/blob/master/assets/img/global_order_violation_seq.png?raw=true" alt="violation of global memory ordering timeline" /></p>

<p>물론 <a href="https://www.intel.com/content/www/us/en/developer/articles/technical/intel-sdm.html">Intel 공식 도큐먼트</a>를 보면 위와같은 상황에서 명령어 재배치는 이루어지지 않는다고 한다. 하지만 ARM등의 다른 cpu 들은 여전히 발생할 수 있다고 한다.</p>

<p>이러한 문제를 해결하기 위해서 프로그래머는 <strong>Memory Barriers</strong>라는 기능을 사용할 수 있다. Memory Barrier란 뭔지, 어떤 원리로 동작하는건지는  다음에 정리해보겠다.</p>

<p><strong>본 글을 참조하실 때에는 출처 명시 부탁드립니다.</strong></p>]]></content><author><name>HongnyeomSung</name></author><category term="ComputerScience" /><category term="Computer Science" /><category term="Cache" /><summary type="html"><![CDATA[로봇 개발과 Multi Processor]]></summary></entry><entry><title type="html">[Dev-Culture] 로봇 개발과 테스트 코드</title><link href="/dev-culture/2025/02/19/test_code.html" rel="alternate" type="text/html" title="[Dev-Culture] 로봇 개발과 테스트 코드" /><published>2025-02-19T00:00:00+00:00</published><updated>2025-02-19T00:00:00+00:00</updated><id>/dev-culture/2025/02/19/test_code</id><content type="html" xml:base="/dev-culture/2025/02/19/test_code.html"><![CDATA[<p><img src="https://i.imgur.com/SHHQGIE.png" alt="picture 3" /></p>

<h1 id="로봇-개발과-테스트-코드">로봇 개발과 테스트 코드</h1>

<p>아직도 많은 개발자들에게 ‘테스트 코드를 작성한다’라는 개념은 왠지 모르게 부담스럽거나 추가적인 번거로움으로 느껴질 수 있다. 특히 빠른 개발 속도와 결과물을 강조하는 환경일수록, 테스트 코드 작성이 우선순위에서 밀려나는 경우가 흔하다. 그러나 로봇 소프트웨어처럼 하드웨어와 직접 연동되고 동작 안정성이 중요한 도메인에서는 테스트 코드의 중요성을 결코 간과할 수 없다. 이번 글에서는 테스트 코드가 무엇이며, 왜 필요한지, 그리고 어떻게 작성하고 자동화할 수 있는지 살펴본다.</p>

<h2 id="1-테스트-코드란-무엇인가">1. 테스트 코드란 무엇인가?</h2>

<p>테스트 코드(Test Code)란, 구현한 기능이나 로직이 의도대로 동작하는지 자동으로 검증하기 위한 프로그램 코드를 의미한다. 예를 들어 로봇이 ‘장애물을 피하고 목적지로 이동’하는 기능을 구현했다면, 장애물을 정상적으로 감지하고 회피하는지, 특정 센서 입력값이 비정상일 때도 예외 처리가 적절히 이뤄지는지를 자동으로 검사할 수 있도록 만든 코드가 테스트 코드에 해당한다.</p>

<p>일반적으로 테스트 코드는 규모와 범위에 따라 다양한 단계로 구성된다. 가장 작은 단위(함수, 메서드, 클래스 등)를 대상으로 하는 유닛 테스트(Unit Test)가 있고, 여러 모듈을 연동해 전체 기능을 점검하는 통합 테스트(Integration Test)가 있다. 로봇 소프트웨어에서는 하드웨어 의존성이 크기 때문에, 단순히 유닛 테스트만으로 모든 상황을 검증하기는 어렵다. 따라서 통합 테스트도 함께 고려해 종합적인 시나리오가 의도대로 동작하는지 확인해야 한다. 유닛 테스트와 통합 테스트 이외에도 E2E 테스트, Nightly 테스트와 같은 더 넓은 범위의 테스트들도 있다. 컴파일 옵션에 따라 전체적인 프로세스 시간이 많이 길어지는 경우, 혹은 대용량의 데이터를 기반으로 테스트를 수행해야 하는 경우 특정 시간을 정해서 테스트를 수행하며, 보통 새벽에 많이 수행하기 때문에 Nightly 테스트라고 부르기도 한다.</p>

<h2 id="2-테스트-코드를-왜-작성해야-하는가">2. 테스트 코드를 왜 작성해야 하는가?</h2>

<p>“굳이 테스트 코드를 작성하지 않아도, 일단 코드를 돌려보면 제대로 작동하는지 확인할 수 있지 않을까?”라고 생각할 수 있다. 그러나 수동 테스트에는 분명한 한계가 존재한다. 특히 로봇 소프트웨어는 물리적인 환경에서 동작하기 때문에, 현장에서 모든 기능을 매번 직접 검증하는 데에는 많은 비용과 시간이 든다. 게다가 사람은 반복되는 테스트 과정에서 실수를 하기도 쉽다.</p>

<p>테스트 코드를 작성하면 다음과 같은 이점을 얻을 수 있다.</p>

<ol>
  <li>
    <p><strong>안정성 확보</strong> <br />
코드가 변경되거나 신규 기능이 추가되었을 때, 기존 기능이 문제없이 동작하는지 빠르게 확인할 수 있다. 본인이 작성한 코드도 몇일이 지나면 기억이 나지 않는다. 심지어 여러사람이 함께 협업을 하는 경우라고 하면 이러한 안정성 확보는 무엇보다 중요하다.</p>
  </li>
  <li>
    <p><strong>기술 부채 감소</strong><br />
기능이 늘어날수록 “어디선가 예기치 않은 버그가 생기면 어떡하지?”라는 우려가 커진다. 테스트 코드가 있으면 수정 작업 후에도 빠르게 문제 여부를 파악할 수 있어 기술 부채가 쌓이는 속도를 줄일 수 있다.</p>
  </li>
  <li>
    <p><strong>개발 효율 증가</strong><br />
처음에는 테스트 코드를 작성하는 데 드는 시간이 부담이 될 수 있다. 그러나 장기적으로 보면 문제를 미리 발견·해결하는 과정이 빨라져, 전체 개발 효율이 오히려 높아진다.</p>
  </li>
  <li>
    <p><strong>협업 효율 상승</strong><br />
팀원이 새 코드를 추가하거나 기존 코드를 수정할 때, 해당 로직이 어떻게 동작해야 하는지 테스트 코드를 통해 빠르게 파악할 수 있다. 즉, 테스트 코드는 그 코드의 사용법에 대한 설명서이다.</p>
  </li>
  <li>
    <p><strong>코드의 구조화</strong><br />
테스트 코드를 작성하다 보면 자연스럽게 코드의 구조가 개선된다. 특히 유닛테스트는 하나의 함수에 대한 기능을 테스트하는 테스트이기 때문에 하나의 함수에 너무 많은 기능이 들어가게 되면 정확한 유닛 테스트를 작성하기 어려워 진다. 따라서 유닛 테스트 코드를 작성하다 보면 자연스럽게 하나의 함수는 최소한의 기능만 갖도록 구조화가 된다.</p>
  </li>
  <li>
    <p><strong>코드의 디버깅</strong><br />
테스트 코드를 작성하다 보면 다양한 테스트 케이스를 고민하게 되고, 그 과정에서 다양한 코드의 버그들을 발견하게 된다.</p>
  </li>
</ol>

<h2 id="3-unit-test와-integration-test">3. Unit Test와 Integration Test</h2>

<p>테스트 코드를 구성할 때 자주 등장하는 개념이 바로 ‘유닛 테스트(Unit Test)’와 ‘통합 테스트(Integration Test)’다. 보통 두 테스트는 다음과 같은 차이가 있다.</p>

<ol>
  <li><strong>유닛 테스트(Unit Test)</strong>
    <ul>
      <li>함수나 클래스, 혹은 모듈 수준으로 쪼갠 작은 단위를 테스트한다.</li>
      <li>외부 의존성(네트워크, 하드웨어, 데이터베이스 등)을 가짜 객체(Mock)로 대체해 테스트한다.</li>
      <li>내부 함수끼리의 의존 관계가 있는 경우 각 함수들의 출력을 Mocking 하여 원하는 시나리오를 테스트한다.</li>
      <li>각 단위가 제대로 동작하는지 확인하므로 테스트가 빠르게 끝나고, 문제 발생 지점을 정확히 파악하기 쉽다.</li>
    </ul>
  </li>
  <li><strong>통합 테스트(Integration Test)</strong>
    <ul>
      <li>여러 모듈이 합쳐졌을 때 전체 기능이 정상적으로 작동하는지를 검증한다.</li>
      <li>로봇 소프트웨어라면 실제 센서값 수신부터 제어 모듈까지 연결해 시나리오 테스트를 진행하기도 하고, Mocking 센서 테이터를 기반으로 진행하기도 한다.</li>
      <li>유닛 테스트만으로는 찾지 못하는 모듈 간 연동 문제나 예외 상황을 포착할 수 있다.</li>
    </ul>
  </li>
</ol>

<p>로봇 소프트웨어는 하드웨어 연동이 많아 단순 유닛 테스트만으로는 한계가 있다. 따라서 유닛 테스트로 기본 로직을 탄탄하게 검증하고, 통합 테스트를 통해 실제 운영 환경과 유사한 조건에서 시스템을 점검하는 방식이 권장된다. 추가적으로 Nightly 테스트를 관리하기도 하는데, 큰 데이터를 기반으로 한 테스트, 혹은 컴파일 시간이 매우 오래걸리는 테스트 (Memory Sanitizer, Thread Sanitizer 등)는 PR을 생성할 때마다 수행되는 CI 프로세스에서 수행하게 되면 전체적인 개발 프로세스에 지연을 야기 시킨다. 따라서 이런 테스트는 CI 프로세스와는 별개로 일정 시간에 수행되어 리포트를 생성하는 방식으로 테스트 하기도 한다.</p>

<p><img src="https://i.imgur.com/fLKZalr.png" alt="picture 1" /></p>

<h2 id="4-tddtest-driven-development">4. TDD(Test Driven Development)</h2>

<p>TDD(Test Driven Development)는 기능 구현보다 테스트 코드를 먼저 작성하는 개발 방법론이다. 보통은 기능을 먼저 구현한 뒤 검증용 테스트 코드를 작성하는데, TDD는 이 순서를 뒤집는다. 일반적인 TDD 흐름은 다음과 같다.</p>

<ol>
  <li><strong>테스트 작성</strong>: 아직 구현되지 않은 기능에 대해, 원하는 동작과 인터페이스를 정의하는 테스트 코드를 먼저 작성한다.</li>
  <li><strong>테스트 실행</strong>: 당연히 구현되지 않았으므로 테스트가 실패(Fail)한다.</li>
  <li><strong>기능 구현</strong>: 실패한 테스트를 통과시키기 위한 최소한의 코드만 작성한다.</li>
  <li><strong>리팩터링</strong>: 테스트가 통과했다면, 코드를 리팩터링하여 품질과 가독성을 개선한다.</li>
  <li><strong>테스트 재확인</strong>: 리팩터링 후에도 모든 테스트가 통과하는지 다시 확인한다.</li>
</ol>

<p>이 과정을 반복하면서 기능 요구사항이 더욱 명확해지고, 필요 이상의 코드를 작성하지 않게 된다. 다만 TDD를 적극적으로 적용하려면 팀원들이 테스트 작성에 익숙해져야 하며, 명확한 설계와 테스트 전략이 뒷받침되어야 한다. 로봇 개발 기준으로 데이터의 입력과 출력이 명확한 특정 기능을 갖춘 라이브러리를 작성할때는 이러한 TDD 개발 방식이 적합한 경우가 많다. 반면에 외부 센서 혹은 내부 프로세스들과의 통신이 빈번이 발생하는 인터페이스 프로세스는 개발 초기에 각 함수별로 입력과 출력을 나누기 어려운 경우가 많다. 이런 경우에는 기능을 먼저 개발하고 추후에 테스트 코드를 작성하는 방식으로 하기도 한다.</p>

<p><img src="https://i.imgur.com/DQ4DaPy.png" alt="picture 4" /></p>

<h2 id="5-테스트-코드를-작성하면-개발-속도가-느려질까">5. 테스트 코드를 작성하면 개발 속도가 느려질까?</h2>

<p>테스트 코드를 작성하면 당장 눈앞의 개발 속도가 느려지는 것처럼 느껴질 수 있다. 특히 빠른 릴리스를 요구하는 환경에서는 “기능 구현만으로도 시간이 모자란데, 테스트 코드까지 작성해야 하느냐”는 불만이 나올 수도 있다.</p>

<p>그러나 장기적인 관점에서 보면, 테스트 코드가 없을 때 발생하는 디버깅과 재배포, 문제 파악에 드는 막대한 비용을 고려해야 한다. 버그가 뒤늦게 발견될수록 고치는 데 더 많은 자원과 시간이 투입된다. 반면 테스트 코드가 잘 마련되어 있으면 수정사항을 적용한 뒤 자동 테스트를 돌려 잠재적인 문제를 빠르게 발견할 수 있다. 이는 “빠른 실패(Fail Fast)”를 가능하게 하며, 전체 프로젝트의 안정성과 생산성을 모두 높이는 결과로 이어진다. 새로운 기능을 빠르게 개발하는 것도 중요하지만, 장기적인 관점에서 개발 속도가 조금은 느릴 수 있지만 안정적인 시스템을 갖추고 테스트 코드를 함께 개발하는 것이 올바른 개발 방법이다. 속도보다 중요한 것은 올바른 방향으로 가는 것이다.</p>

<h2 id="6-테스트-자동화">6. 테스트 자동화</h2>

<p><img src="https://i.imgur.com/3002Tk1.png" alt="picture 5" /></p>

<p>테스트 코드의 가치를 극대화하기 위해서는, 작성된 테스트를 자동으로 돌려주는 환경(CI, Continuous Integration)을 구축하는 것이 중요하다. 대표적인 CI 툴로는 GitHub Actions, GitLab CI/CD, Jenkins 등이 있으며, 대체로 Pull Request가 올라오면 빌드와 테스트를 실행하고 결과를 알려준다.</p>

<p>특히 로봇 소프트웨어는 하드웨어 시뮬레이션 환경이 필요할 때가 많다. 이를 위해 Docker를 이용하거나 시뮬레이션 툴(ROS 시뮬레이터 등)과 연동해 자동 테스트를 구성한다. 이렇게 하면 실제 로봇 없이도 비교적 빠르게 코드가 의도대로 동작하는지 검증할 수 있다.</p>

<p>Riibotics의 CI 프로세스에서는 일반적으로 새로운 PR이 생성될 때마다 다음의 항목들을 CI 과정을 통해 자동으로 테스트 한다. (위 그림은 PR 생성 시 모든 테스트가 통과한 모습)</p>

<ol>
  <li><strong>Linter</strong>: 각 언어 (c++, python, shell script, xml) 에 대해 정해진 포멧을 지키고 있는지 확인.</li>
  <li><strong>Code compile 및 테스트</strong> : 정상적으로 컴파일이 수행되는지 확인하고, 테스트 코드가 모두 성공하는지 확인. 로봇 내부에서 동작하는 코드의 경우에는 X86 시스템 뿐만 아니라 ARM Architecture CI 서버에서도 함께 테스트 진행.</li>
  <li><strong>테스트 커버리지 확인</strong> : 유닛 테스트 수행 완료 후 테스트 코드의 커버리지를 확인하고 수정된 파일에 대한 커버리지를 리포트 해준다. 커버리지가 특정 값 (일반적으로 80%)를 넘지 못하는 경우 CI가 실패하여 Merge를 수행할 수 없다.</li>
  <li><strong>File consistency 체크</strong> : 로봇의 경우 여러개의 Repository를 관리하는 경우가 많기 때문에 동일한 역할을 하는 파일들이 각 Repository에 분산되어 있는 경우가 많다. 각 Repository에 있는 파일들이 master file과 동일하게 유지되고 있는지 확인</li>
</ol>

<p>사람은 누구나 실수를 할 수 있기 때문에 최대한의 자동화를 통해 이러한 휴먼 에러를 최소화 해야 한다.</p>

<h2 id="7-테스트-커버리지-체크">7. 테스트 커버리지 체크</h2>

<p>테스트 코드가 있다고 해도, 실제로 얼마나 많은 부분을 테스트하는지는 또 다른 문제다. 이때 도움이 되는 지표가 <strong>테스트 커버리지</strong>다. 테스트 커버리지 분석 툴(gcov, lcov, cobertura 등)을 사용하면 특정 파일 혹은 함수가 테스트에서 얼마나 커버되었는지 확인할 수 있다. 일반적으로 다음과 같은 항목을 살펴본다.</p>

<ol>
  <li><strong>라인 커버리지(Line Coverage)</strong>: 전체 코드 라인 중 테스트를 통해 실행된 라인의 비율</li>
  <li><strong>함수 커버리지(Function Coverage)</strong>: 전체 함수 중 테스트를 통해 실행된 함수의 비율</li>
  <li><strong>분기 커버리지(Branch Coverage)</strong>: 조건문(if/else 등)의 각 분기가 실제 테스트 과정에서 모두 실행되었는지를 측정한 비율</li>
</ol>

<p>테스트 커버리지가 높다고 해서 모든 오류를 자동으로 검출할 수 있는 것은 아니다. 그러나 커버리지를 확인하면 테스트되지 않은 부분을 명확히 파악할 수 있어, 테스트의 사각지대를 줄이는 데 큰 도움이 된다.
테스트 커버리지를 확인하는 주된 목적은 각 유닛 테스트가 함수 내부의 로직을 다양하게 검증했는지를 평가하는 데 있다. 단 하나의 테스트 케이스만 존재하더라도 해당 함수가 실행되었다면 커버리지 리포트에는 포함되므로, 커버리지 수치만으로 테스트 코드의 질적 수준(즉, 얼마나 다양한 케이스를 검증했는지)을 평가하는 것은 어렵다. 따라서 유닛 테스트의 품질은 테스트를 작성하는 개발자의 역량에 크게 의존하며, 이를 위해 개발 문화가 중요한 역할을 한다.
테스트 코드를 작성할 때 중요한 점은 유닛 테스트와 통합 테스트를 분리하는 것이다. 통합 테스트가 커버리지 체크 프로세스에 포함되면 어떤 문제가 발생할까? 통합 테스트는 여러 함수가 연결된 상태에서 실행되므로, 특정 함수를 테스트하는 과정에서 해당 함수가 호출하는 모든 함수들까지 커버리지에 포함된다. 이로 인해 유닛 테스트에서 개별적으로 검증되지 않은 함수가 무엇인지 명확히 파악하기 어려워진다. 따라서 통합 테스트는 커버리지 확인 프로세스에서 제외하는 것이 바람직하다고 생각한다.</p>

<p><img src="https://i.imgur.com/NS5vZiW.png" alt="picture 8" /></p>

<p>Riibotics에서는 테스트 작성 시 유닛 테스트와 통합 테스트를 명확히 분리하며, CI를 구성할 때 유닛 테스트의 실행 결과만 커버리지 리포트에 포함되도록 설정하였다. 이를 통해 생성된 커버리지 리포트는 PR 생성자가 확인할 수 있도록 자동으로 업로드되며, 커버리지가 사전에 설정한 기준에 미달할 경우 CI가 실패하도록 구성되어 있다.
또한, 상세 리포트를 다운로드하여 코드 라인별로 커버리지 미충족 부분을 확인할 수 있도록 하여, 어떤 코드에 대한 유닛 테스트가 부족한지 쉽게 파악할 수 있는 시스템을 구축하였다.</p>

<p><img src="https://i.imgur.com/j3fOuoy.png" alt="picture 9" /></p>

<p><img src="https://i.imgur.com/fqv7wLH.png" alt="picture 12" /></p>

<h2 id="마치며">마치며</h2>

<p>테스트 코드 작성은 코드 품질을 개선하는 도구를 넘어, <strong>지속 가능한 개발 문화를 위한 필수 요소</strong>라고 할 수 있다. 특히 로봇 소프트웨어처럼 하드웨어와 맞물려 안전성과 신뢰도가 중요한 분야에서는 더욱 강조될 수밖에 없다.</p>

<p>처음에는 테스트 코드를 작성하는 데 드는 시간이 부담스러울 수 있다. 그러나 테스트가 존재함으로써 얻게 되는 조기 오류 발견, 협업 효율성 향상, 기술 부채 감소 등은 장기적인 관점에서 절대적인 가치를 지닌다. 또한 코드 리뷰, 스타일 가이드, CI/CD 파이프라인과 결합하면 그 효과가 배가된다.</p>

<p>결국 테스트 코드는 “필요하면 추가로 작성하는 옵션 사항”이 아니라, 프로젝트의 토대를 견고하게 만드는 핵심적인 도구다. 로봇 소프트웨어 개발에서도 테스트 코드 작성 문화를 정착시키고, 이를 자동화와 결합해 장기적으로 안정된 개발 환경을 구축하기를 권장한다. 테스트 코드가 제공하는 안정성과 예측 가능성은, 하드웨어와 깊이 연동되는 로봇 소프트웨어 개발에서 더욱 빛을 발하게 된다.</p>]]></content><author><name>JinyongJeong</name></author><category term="Dev-Culture" /><category term="Dev-Culture" /><summary type="html"><![CDATA[]]></summary></entry><entry><title type="html">[Dev-Culture] 로봇 소프트웨어 개발 문화</title><link href="/dev-culture/2025/02/14/developmen_culture.html" rel="alternate" type="text/html" title="[Dev-Culture] 로봇 소프트웨어 개발 문화" /><published>2025-02-14T00:00:00+00:00</published><updated>2025-02-14T00:00:00+00:00</updated><id>/dev-culture/2025/02/14/developmen_culture</id><content type="html" xml:base="/dev-culture/2025/02/14/developmen_culture.html"><![CDATA[<h1 id="로봇-소프트웨어-개발-문화">로봇 소프트웨어 개발 문화</h1>

<p><img src="https://i.imgur.com/Nne6p1i.png" alt="picture 5" /></p>

<h2 id="로봇-소프트웨어-개발-문화를-어떻게-만들어야-할까">로봇 소프트웨어 개발 문화를 어떻게 만들어야 할까?</h2>

<p>로봇 소프트웨어 개발은 단순한 소프트웨어 개발과는 차이가 있다. 로봇은 물리적인 환경에서 동작하며, 하드웨어와의 긴밀한 연계가 필요하기 때문에 코드의 신뢰성과 유지보수가 특히 중요하다. 하지만 아무리 뛰어난 기술을 보유하고 있어도, 좋은 개발 문화가 정착되지 않으면 장기적으로 효율적인 개발이 어려워지며, 어느순간 부터는 기술 부채가 쌓여 되돌릴 수 없는 상태에 이르게 된다. 또한 개발 처음부터 재대로된 개발 문화와 시스템을 구축하지 않고 중간에 레거시 시스템을 고친다는 것은 매우 머리아픈 일이다. 이번 포스팅에서는 두번째 스타트업에서 개발문화를 구축하면서 생각하고 있는 로봇 소프트웨어 개발 문화의 중요성과 이를 구축하는 방법에 대해 이야기해 보겠다.</p>

<h2 id="개발-문화의-중요성">개발 문화의 중요성</h2>

<p>좋은 개발 문화는 단순히 “잘 짜인 코드”를 만드는 것 이상의 가치를 제공한다. 조직 내 개발 문화가 건강하면 다음과 같은 장점이 있다.</p>

<ul>
  <li><strong>협업이 원활해진다</strong>: 코드 스타일과 리뷰 방식이 일관되면, 팀원들이 더 쉽게 서로의 코드를 이해할 수 있다.</li>
  <li><strong>버그와 기술 부채를 줄일 수 있다</strong>: 체계적인 코드 리뷰와 테스트 문화가 자리 잡으면, 문제를 조기에 발견하고 해결할 수 있다.</li>
  <li><strong>신규 개발자 온보딩이 쉬워진다</strong>: 문서화와 코드 컨벤션이 정립되어 있다면, 새로운 팀원이 빠르게 적응할 수 있다.</li>
  <li><strong>개발 속도가 꾸준히 유지된다</strong>: 단기적인 속도보다 장기적인 생산성을 고려한 개발 문화가 정착되면, 프로젝트의 유지보수가 쉬워지고 확장성도 높아진다.</li>
</ul>

<p>로봇 소프트웨어 개발은 하드웨어와 직접 연결되므로, 잘못된 코드 하나가 물리적인 사고를 초래할 수 있다. 따라서 체계적인 개발 문화가 더욱 필수적이다. 아쉬운 부분은 한국의 많은 로봇 회사들이 이런 개발문화에 대한 중요성을 모르고 있는 경우가 매우 많다. 일반적인 B2C 소프트웨어를 개발하는 풀스택의 경우 컴퓨터공학을 기반으로 개발 문화가 형성되어 이런 개발문화의 중요성, 에자일 개발 등 개발 문화에 대해 신경을 쓰는 경우가 많다. 하지만 로봇 개발 회사의 경우 (특히 한국 로봇회사의 경우) 학교 출신의 로봇 관련 연구를 했었던 석사/박사 출신이 개발 문화를 만드는 경우가 많다 보니 이런 개발 문화에 대한 중요성, 그리고 필요성에 대해 인지하지 못하는 경우가 많은 것 같다. 심지어 형상관리 툴을 사용하지만 모든 개발자가 master 브랜치에서 코드 작업을 하고 바로 master로 commit을 넣는 경우도 심심치 않게 보았다. 이런 개발 문화에서는 코드의 퀄리티가 유지될 수 없을 뿐만 아니라 코드의 버전 관리도 불가능하다. 따라서 무엇보다도 로봇 소프트웨어 개발에서 개발문화는 가장 중요한 요소이다.</p>

<p><img src="https://i.imgur.com/gHAabPY.png" alt="picture 6" /></p>

<h2 id="코드-리뷰의-중요성">코드 리뷰의 중요성</h2>

<p>코드 리뷰는 개발자의 실수를 줄이고 코드 품질을 향상시키는 중요한 과정이다. 단순히 “버그를 잡는 과정”이 아니라, 팀이 공유하는 개발 원칙과 철학을 유지하는 역할도 한다.</p>

<h3 id="코드-리뷰가-필요한-이유">코드 리뷰가 필요한 이유</h3>

<ol>
  <li><strong>버그 예방</strong>: 코드 리뷰를 통해 동료 개발자가 실수를 발견하고 수정할 수 있다.</li>
  <li><strong>지식 공유</strong>: 리뷰 과정에서 코드에 대한 이해도를 높이고, 팀 전체의 개발 역량을 향상시킬 수 있다.</li>
  <li><strong>일관성 유지</strong>: 코드 스타일과 아키텍처가 팀의 기준에 맞게 유지될 수 있다.</li>
  <li><strong>기술 부채 감소</strong>: 불필요한 복잡성이나 잘못된 설계를 초기에 바로잡을 수 있다.</li>
</ol>

<p>로봇 소프트웨어의 경우, 코드 한 줄이 실제 로봇의 동작에 영향을 미칠 수 있기 때문에 코드 리뷰는 더욱 철저히 진행해야 한다. 단순히 “문제가 없어 보인다”는 식의 리뷰가 아니라, 코드가 실제 환경에서 어떻게 동작할지 고려하며 리뷰해야 한다. 또한 코드에 버그가 있는 상태로 Master로 머지가 된다면 그에 대한 책임 또한 리뷰어에게도 있다는 사실을 항상 인지해야 한다. 리뷰 과정은 단순히 다른 동료의 코드를 훑어보는 과정이 아닌 개발의 일 부분임을 잊어서는 안된다.</p>

<h2 id="효율적인-코드-리뷰-프로세스">효율적인 코드 리뷰 프로세스</h2>

<p>효율적인 코드 리뷰 프로세스를 구축하기 위해서는 몇 가지 원칙을 따라야 한다.</p>

<h3 id="1-리뷰를-위한-명확한-기준-설정">1. <strong>리뷰를 위한 명확한 기준 설정</strong></h3>

<p>코드 리뷰를 시작하기 전에 팀 내에서 명확한 기준을 세워야 한다. 예를 들어, 다음과 같은 항목을 정할 수 있다.</p>

<ul>
  <li>코드 스타일 가이드라인 (ex: Google C++ Style Guide)</li>
  <li>PR(Pull Request) 크기 제한 (너무 큰 PR은 리뷰하기 어렵다)</li>
  <li>테스트 코드 포함 여부</li>
</ul>

<p>이러한 기준이 없다면 리뷰어마다 다르게 판단할 가능성이 높아지고, 리뷰 과정이 비효율적으로 변할 수 있다.</p>

<h3 id="2-코드-스타일-가이드-설정">2. <strong>코드 스타일 가이드 설정</strong></h3>

<p>개발자마다 함수명, 변수명, namespace 이름을 만드는 규칙, 포멧이 다르다면 코드는 금방 엉망이 될 것이며, 리뷰를 어렵게 만드는 요소가 된다. 공통으로 사용할 코드 스타일 가이드를 설정하는 것은 개발 문화의 가장 기본적인 요소이다. TIM Robotics의 경우에는 모든 언어가 Google Style Guide 따르고 있다. (<a href="https://google.github.io/styleguide/cppguide.html">링크</a>)
일반적으로 Style Guide에는 코드 작성을 위한 함수명, 변수명, namespace명 등 모든 요소들에 대한 규칙을 포함한다. 예를 들어 Google Style Guide 기준으로 Class의 맴버 변수는 <code class="language-plaintext highlighter-rouge">_</code> 라는 suffix 를 사용하며, const variable인 경우에는 <code class="language-plaintext highlighter-rouge">k</code>의 prefix를 사용한다. 모든 개발자가 이렇게 동일한 규칙을 사용함으로써 코드의 Readability가 높아지고, 리뷰하는데 시간이 감소하게 된다. 
추가적으로 Riibotics는 좌표계의 변환 관계를 표현하는 방법, Time을 포함한 변수 표현 방법 등 다양한 스타일 가이드를 추가적으로 만들어 공유하고 있다. 내부 스타일 가이드는 개발을 하면서 부족한 부분을 발견하면 함께 논의하여 만들어 가는 과정이 매우 중요하며, 이 과정에서는 모든 개발자가 참여해야 한다.</p>

<h3 id="3-test-코드-작성하기">3. <strong>Test 코드 작성하기</strong></h3>

<p>테스트 코드 작성은 하나의 추가 업무가 아닌 당연한 개발의 일부이다. 테스트 코드가 없는 코드는 개발 단계에서 검증 단계를 거치지 않았으며, 추가 수정이 있을때마다 버그가 발생할 수 있는 코드이다. 따라서 개발되는 모든 코드는 테스트 코드가 포함되어야 하며 PR에 항상 함꼐 포함되어 있어야 한다. CI 프로세스에서는 해당 PR에 테스트 코드가 포함되었는지, 커버리지가 일정 이상 인지 확인하여 Merge 가능 여부를 자동으로 판단하도록 시스템을 구성해야 한다.</p>

<h3 id="4-prpull-request-크기-조절하기">4. <strong>PR(Pull Request) 크기 조절하기</strong></h3>

<p><img src="https://i.imgur.com/RaX1eDo.png" alt="picture 3" /></p>

<p>PR은 Pull Request의 약자로 수정된 코드를 다른 사람에게 리뷰받는 프로세스를 의미한다. PR을 작성할 때 리뷰어에게 도움을 주기 위해 어떤 내용에 대한 수정사항인지 자세히 PR에 정리해서 PR을 생성하는 것이 기본 프로세스이다. PR을 작성 시 PR이 너무 크면 리뷰어가 모든 내용을 꼼꼼히 살펴보기가 어렵다. 10줄의 코드를 리뷰 요청하면 10개의 comment가 달리는데, 500줄의 코드 리뷰를 요청하면 한개의 리뷰도 돌아오지 않는다 라는 농담이 있다. 하나의 브랜치에서 여러 작업을 동시에 수행하여 수정사항이 너무 많으면 리뷰어는 수정사항에 대해서 이해하는데 시간이 오래걸려 포기해버리고 LGTM (Looks good to me) 리뷰를 던지는 경우가 발생한다. 
따라서 일반적으로 한 번의 PR에서 너무 많은 기능을 추가하기보다는, 작은 단위로 쪼개어 올리는 것이 좋다. 작은 PR은 리뷰 시간이 단축되고, 변경 사항을 명확히 이해하는 데 도움이 된다.</p>

<h3 id="5-자동화-도구-활용하기">5. <strong>자동화 도구 활용하기</strong></h3>

<p>개발 프로세스에서 자동화 툴을 사용하여 자동화 할 수 있는 부분은 최대한 적용하는 것이 효과적이다.</p>

<ul>
  <li><strong>코드 스타일 검사</strong>: Clang-Format, Prettier, Clang-tidy 등을 활용하여 스타일을 자동으로 맞춘다.</li>
  <li><strong>정적 분석 도구</strong>: SonarQube, cppcheck 등을 이용해 잠재적인 버그를 찾는다.</li>
  <li><strong>CI/CD 파이프라인</strong>: 코드 리뷰 전에 자동으로 빌드, 테스트를 실행해 코드의 안정성을 확인한다.</li>
</ul>

<p>개발자들끼리 특정 스타일 가이드를 따르기로 약속했다고 하더라도 개발과정에서 누구라도 실수를 할 수 있다. 이러한 실수들은 자동화 툴을 사용해서 자동으로 감지되도록 시스템을 구성해야 한다. Riibotics는 clang-format과 clang-tidy를 함께 사용하고 있다. clang-format은 코드 전반적인 format에 대하여 자동으로 감지하고 수정해준다. clang-tidy는 코드 스타일 검사와 정적 분석이 모두 포함되 있으며, 특히 함수, 변수명들이 정의한 스타일과 다르게 작성된 부분을 자동 검출하는데 유용하며, 잠재적인 버그들도 함께 검출할 수 있다. 이러한 툴들은 개발 과정에서 개발자들이 활용하는 것 뿐만 아니라 CI 과정에서 다시 한번 수정 사항에 대하여 테스트를 하고, 문제가 발견했을 때 오류를 발생시켜 수정된 코드가 Master 브랜치로 Merge되는 것을 방지한다. 
이런 자동화 도구를 적극적으로 활용함으로써 리뷰어는 코드 스타일이나 간단한 실수를 지적 하는 대신, 전체적인 코드의 구성, 아키텍처, 그리고 로직에 집중하여 리뷰를 할 수 있다.</p>

<h3 id="6-건설적인-피드백-제공하기">6. <strong>건설적인 피드백 제공하기</strong></h3>

<p>코드 리뷰는 팀원 간의 협업 과정이므로, 피드백을 줄 때는 긍정적인 방식으로 접근하는 것이 중요하다.</p>

<ul>
  <li>“이 부분은 잘 구현하셨네요! 하지만 여기서는 이런 방식이 더 좋을 것 같습니다.”</li>
  <li>“이 로직이 잘 동작하긴 하지만, 성능 최적화를 위해 이런 접근도 고려해볼 수 있을 것 같아요.”</li>
</ul>

<p>단순한 비판이 아니라, 대안을 제시하는 피드백이 더 효과적이다. 때로는 리뷰만으로는 의도한 내용을 정확히 전달하기 어려울 때 코드를 수정하여 제안하는 것도 방법이다.</p>

<h3 id="7-리뷰-시간을-정기적으로-확보하기">7. <strong>리뷰 시간을 정기적으로 확보하기</strong></h3>

<p>코드 리뷰는 “남는 시간에 하는 것”이 아니라, 개발 프로세스의 핵심 단계다. 따라서 리뷰를 위한 시간을 정기적으로 확보하는 것이 중요하다. 예를 들어, 매일 오전 10~11시는 코드 리뷰 시간으로 지정하는 등의 방식이 있다. 이런 방식이 재대로 동작하기 위해서는 PR을 작은 단위로 자주 작성하여 리뷰 요청을 해야 한다.</p>

<h2 id="신규-개발자-온보딩-및-문서화">신규 개발자 온보딩 및 문서화</h2>

<p>새로운 개발자가 팀에 합류했을 때 빠르게 적응할 수 있도록 돕는 것도 좋은 개발 문화의 일부다. 이를 위해 다음과 같은 요소를 준비하는 것이 좋다.</p>

<h3 id="1-개발-환경-설정-가이드-제공">1. <strong>개발 환경 설정 가이드 제공</strong></h3>

<ul>
  <li>개발 환경을 빠르게 구축할 수 있도록, Docker 같은 자동화된 환경 설정 방법을 제공한다.</li>
  <li>“개발 환경 설정 가이드” 문서를 준비해 놓는다.</li>
</ul>

<h3 id="2-코드베이스-문서화">2. <strong>코드베이스 문서화</strong></h3>

<ul>
  <li>프로젝트의 전체 구조를 설명하는 아키텍처 다이어그램을 제공한다.</li>
  <li>주요 모듈과 라이브러리의 역할을 정리한 문서를 만든다.</li>
</ul>

<h3 id="3-온보딩-미션-제공">3. <strong>온보딩 미션 제공</strong></h3>

<ul>
  <li>신규 개발자가 코드베이스를 이해할 수 있도록, 작은 기능을 직접 추가해보는 미션을 제공한다.</li>
  <li>테스트 코드 작성이나 작은 버그 수정부터 시작하도록 유도한다.</li>
</ul>

<h2 id="결론">결론</h2>

<p>좋은 개발 문화를 만드는 것은 하루아침에 이루어지는 일이 아니다. 하지만 코드 리뷰 프로세스를 정립하고, 자동화 도구를 활용하며, 명확한 문서화와 온보딩 시스템을 갖춘다면, 지속 가능한 로봇 소프트웨어 개발 환경을 만들 수 있다.</p>

<p>개발 문화는 단순한 규칙이 아니라, 팀 전체가 함께 만들어가는 과정이다. 한국의 로봇 산업이 더 고도화 되기 위해서는 모든 로봇 회사들이 이러한 개발 문화를 갖춰야 한다고 생각한다.</p>]]></content><author><name>JinyongJeong</name></author><category term="Dev-Culture" /><category term="Dev-Culture" /><summary type="html"><![CDATA[로봇 소프트웨어 개발 문화]]></summary></entry><entry><title type="html">[SLAM] Camera Model 과 Distortion Model (Perspective, Fisheye, Omni)</title><link href="/slam/2024/01/14/camera_model_and_distortion_model.html" rel="alternate" type="text/html" title="[SLAM] Camera Model 과 Distortion Model (Perspective, Fisheye, Omni)" /><published>2024-01-14T00:00:00+00:00</published><updated>2024-01-14T00:00:00+00:00</updated><id>/slam/2024/01/14/camera_model_and_distortion_model</id><content type="html" xml:base="/slam/2024/01/14/camera_model_and_distortion_model.html"><![CDATA[<p>Visual SLAM, SfM 등 카메라를 이용한 연구를 하기 위해서 가장 기본적으로 알아야 할 부분이 바로 카메라 모델이다. 일반적인 카메라는 대부분 핀홀(Pin-hole) 카메라 형태로 모델링하여 사용하고 있으며, 처음 카메라에 대해서 배울 때 대부분의 설명 자료들이 핀홀 모델을 기반으로 설명을 하고 있다. 이러한 핀홀 모델은 렌즈를 통해 들어오는 빛이 굴절되지 않고 바로 이미지 센서로 들어오는 perspective projection을 기반으로 하고 있다. 하지만 실제 연구를 진행하다 보면 넓은 화각이 이점이 되는 어플리케이션 (ex. Visual SLAM)이 있기 때문에 FOV (Field of View)가 170도 이상인 어안렌즈 (Fisheye lens)를 사용하는 경우도 많다. 이러한 화각이 매우 넓은 렌즈의 경우에는 일반적인 모델로 표현하기 어렵다. 따라서 이러한 경우에는 perspective projection이 아닌 equidistance projection 모델을 사용한다.</p>

<p><img src="https://i.imgur.com/0tM1TTC.png" alt="" /></p>

<p>카메라를 통해 이미지를 얻는다는 것은 3차원 공간에 있는 물체에 반사되어 오는 빛이 이미지 센서에 들어오는 신호가 변형되는 것이다. 따라서 카메라를 모델링 한다는 것은 실제 3차원에 있는 점을 카메라를 통해 보았을 때 실제 이미지상에 어떤 위치에 놓이게 되는지 추정하는 것이다. 이러한 카메라 모델이 정확히 정의 되어야 3차원 공간의 정보와 2D 이미지 상의 매칭을 수학적으로 표현 가능하다. VIsual SLAM, Visual odometry, SfM (Structure from Motion)등이 이러한 카메라 모델이 필요한 대표적인 분야라고 할 수 있다.</p>

<h1 id="camera-models">Camera Models</h1>

<p>카메라 모델은 물체에 반사되는 빛이 이미지 센서까지 도달하는 방법에 대한 모델이다. 이러한 과정에서 빛의 왜곡에 의한 영향은 distortion 파라미터로 보정하게 된다. 먼저 다양한 카메라 모델을 살펴보자.</p>

<p>여기서 $f$ 는 빛이 모이는 중심점에서 image plane 까지의 거리인 Focal length, $\theta$ 는 principal axis인 $Z_c$ 와 빛이 들어오는 각도, 그리고 $r$은 이미지의 중심인 principal point와 image point의 거리를 의미한다.</p>

<p><img src="https://i.imgur.com/7xf1biT.png" alt="" /></p>

<ul>
  <li>다양한 카메라 모델들
    <ul>
      <li>Perspective projection</li>
    </ul>

\[r = f tan \theta\]

    <ul>
      <li>Stereographic projection</li>
    </ul>

\[r = 2ftan(\theta/2)\]

    <ul>
      <li>Equidistance projection</li>
    </ul>

\[r=f\theta\]

    <ul>
      <li>Equisolid angle projection</li>
    </ul>

\[r = 2fsin(\theta/2)\]

    <ul>
      <li>Orthogonal projection</li>
    </ul>

\[r = f sin(\theta)\]
  </li>
</ul>

<p>빛이 이미지로 변환되는 과정은 다양한 모델로 표현이 되지만, 주로 사용되는 모델은 perspective projection, Equidistance projection 모델이다. 이 글에서는 이 두 모델, 그리고 추가적으로 omni camera model까지 다뤄본다.</p>

<h2 id="perspective-projection">Perspective Projection</h2>

<p><img src="https://i.imgur.com/McwecHf.png" alt="" /></p>

<p>Perspective projection은 기본적인 pin-hole 카메라 모델이며, Principal axis (노란색 line) 기준으로 빛이 들어오는 각도와 image plane으로 나아가는 빛의 각도가 동일한 모델이다. 즉 위 그림 기준으로 $\alpha = \beta$ 이다. 다양한 카메라 모델을 설명할 때의 식으로 표현하면 다음과 같다.</p>

\[r = f tan \beta\]

<p>여기서 $f$ 는 focal length 로 image plane과 빨간색 선의 거리를 의미하며, $r$ 은 principal axis (노란색 line)에서 부터 image plane상의 빛까지의 거리를 의미한다. 즉 빛이 들어오는 각도와 focal length에 의해 이미지에서의 위치가 결정된다. 이러한 perspective model은 가장 기본적인 카메라 모델로, 대부분의 카메라 모델에서 활용된다.</p>

<h2 id="equidistance-projection-fisheye-projection">Equidistance Projection (Fisheye projection)</h2>

<p><img src="https://i.imgur.com/mdGdFnt.png" alt="" /></p>

<p>최근에는 Visual odometry 분야에서는 빠른 움직임에서 tracking loss를 최소화 하기 위하여 화각이 넓은 Fish-eye 카메라를 많이 활용한다. 이러한 Fish-Eye camera는 렌즈 설계부터 넓은 화각을 목적으로 설계되었기 때문에 일반적인 Perspective model로 모델링하기 어렵다. 이런 Fish-Eye 카메라에서 가장 많이 활용되는 모델이 Equidistance projection 모델이다. Perspective projection 모델은 들어오는 빛과 나가는 빛의 각도가 동일하였다면, Equidistance projection 모델은 들어오는 빛의 각도와  principal axis (노란색 선)으로 부터 떨어진 거리가 선형적인 모델이다.  즉 각도가 거리와 선형적이기 때문에 이름이 Equidistance 인 것 같다. 식으로 표현하면 위의 그림처럼</p>

\[\frac{\alpha_1}{d_1} = \frac{\alpha_2}{d_2}\]

<p>와 같이 표현할 수 있다.</p>

<p>이러한 렌즈 설계는 오른쪽 그림과 같이 넓은 화각의 데이터를 얻을 수 있지만, 상당한 왜곡 (Distortion)을 발생시킴을 알 수 있다.</p>

<h2 id="omni-directional-카메라-모델-catadioptric-camera">Omni Directional 카메라 모델 (Catadioptric Camera)</h2>

<p><img src="https://i.imgur.com/NyJttT9.png" alt="" /></p>

<p>Omni-direction 카메라는 주변, 즉 360도를 전부 바라보는 카메라를 의미한다. 위  그림은 대표적인 omni-directional camera들의 종류를 보여준다. 첫번째 그림은 앞에서 설명한 Fish-eye lens로 180 도 이상의 화각을 갖기도 한다. 두번째는 일반적인 카메라에 거울을 붙여 수평 360도를 바라보는 카메라이다. 세번째는 여러개의 카메라를 사방으로 부착하여 사용하는 카메라이다.  여기서는 두번째 모델인 Catadioptic camera model을 설명한다.</p>

<p><img src="https://i.imgur.com/yVmyVtS.png" alt="" /></p>

<p>위 그림은 Catadioptric camera model을 설명하기 위한 그림이다. 이 모델은 central catadioptric camera를 위한 unified model로 다양한 곡선의 거울과, perspective model을 동시에 고려가 가능한 모델이다.</p>

<p>모델을 이해하기 위한 단계는 크게 4단계로 나눠진다.</p>

<ol>
  <li>
    <p>Scene point인 $P$ 를 unit sphere로 projection 한다.</p>

\[P_s = \frac{P}{||P||} = (x_s, y_s, z_s)\]
  </li>
  <li>
    <p>중심점이 $C_{\epsilon} = (0,0,\epsilon)$ 인 새로운 reference frame으로 $P_s$를 옮긴다. 여기서 $\epsilon$ 은 conic의 foci인 $d$ 와 latus rectum인 $l$ 에 의해서 결정된다.</p>

    <p><img src="https://i.imgur.com/XC18DTM.png" alt="" /></p>

\[P_{\epsilon} = (x_s, y_s, z_s + \epsilon)\]
  </li>
  <li>
    <p>$C_{\epsilon}$ 으로 부터 거리 1떨어진 normalized image plane으로 $P_{\epsilon}$ 을 projection 한다.</p>

\[\tilde{m}=\left(x_{m}, y_{m}, 1\right)=\left(\frac{x_{s}}{z_{s}+\epsilon}, \frac{y_{s}}{z_{s}+\epsilon}, 1\right)=g^{-1}\left(P_{s}\right)\]
  </li>
  <li>
    <p>마지막으로 일반적인 intrinsic matrix $K$ 를 곱해서 image coordinate으로 변환한다.</p>

\[\tilde{p} = K \tilde{m}\]
  </li>
</ol>

<p>위 모델을 다시 간단히 살펴보면, $\epsilon$ 값에 따라 다양한 모델을 표현할 수 있다. 즉 일반적인 perspective model은 곡선이 없는, 즉 $\epsilon = 0$ 인 경우이며,  parabolic 형태의 거울일 경우에는 $\epsilon=1$ 인 경우이다. 더욱 자세한 내용은 아래 링크를 참고바란다.</p>

<p><a href="http://rpg.ifi.uzh.ch/docs/omnidirectional_camera.pdf">link</a></p>

<h1 id="distortion-models">Distortion Models</h1>

<p>위에서는 주로 많이 사용되는 카메라 모델에 대해서 알아보았다. 위의 카메라 모델들은 렌즈의 형태에 의해 빛이 굴절되는 모델을 표현하는 것이며, 이상적인 경우를 의미한다. 하지만 실제로는 다양한 이유로 (렌즈의 왜곡 등) 정확히 해당 모델로 표현이 되지 않는다. 일반적으로 이러한 왜곡은 두가지 distortion 모델로 정의된다.</p>

<h2 id="radial-distortion">Radial Distortion</h2>

<p><img src="https://i.imgur.com/fvzSqPC.png" alt="" /></p>

<p>Radial distortion은 렌즈의 중앙부와 바깥영역의 굴절률이 달라짐에 따라서 발생한다. Distortion이 없는 경우 (No distortion)에는 정면으로 바로보았을 때 모든 직선이 직선으로 보인다. 하지만 Barrel distortion 혹은 Pincushion distortion이 발생하였을 때는 실제 환경의 직선이 휘어져 보이게 된다.  Barrel, 그리고 Pincusion이라는 이름은 실제 Barrel (통)의 형태와 Cusion(쿠션)을 눌렀을때의 형태에서 따온 것이다.</p>

\[x_{\text {distorted }}=x\left(1+k_{1}^{*} r^{2}+k_{2}^{*} r^{4}+k_{3}^{*} r^{6}\right)\]

\[y_{\text {distorted }}=y\left(1+k_{1}^{*} r^{2}+k_{2}^{*} r^{4}+k_{3}^{*} r^{6}\right)\]

<p>Radial distortion은 일반적으로 위와 같이 모델링된다. $x, y$ 는 normalized image coordinate에서의 undistorted된 pixel의 위치이다. 즉 distortion이 발생하지 않았을 때의 pixel 위치를 의미한다. $r$은 중심축에서의 각 픽셀의 거리로 $r^2 = x^2 + y^2$ 이다. 그리고 나머지 $k_1, k_2, k_3$는 radial distortion을 표현하는 parameter이다.</p>

<h2 id="tangential-distortion">Tangential Distortion</h2>

<p><img src="https://i.imgur.com/SdkelYP.png" alt="" /></p>

<p>Tangential distortion은 실제 물리적으로 카메라 렌즈와 이미지 센서의 mis-alignment로 인해 발생하는 distortion이다. 최근에는 기술의 발달로 하드웨어 적인 mis-alignment가 매우 적기 때문에 크게 신경쓰지는 않는 추세이긴 하나 상황에 따라서 필요하기도 하다.</p>

\[x_{\text {distorted }}=x+\left[2^{*} p_{1}^{*} x^{*} y+p_{2}^{*}\left(r^{2}+2^{*} x^{2}\right)\right]\]

\[y_{\text {distorted }}=y+\left[p_{1}^{*}\left(r^{2}+2^{*} y^{2}\right)+2^{*} p_{2}^{*} x^{*} y\right]\]

<p>Tangential distortion은 위와 같이 모델링된다. Radial distortion과 마찬가지로 $x,y$ 는 undistorted pixel 위치이다. Tangential distortion은 $p_1, p_2$ coefficient로 표현된다.</p>

<h1 id="정리">정리</h1>

<p>이 글에서는 가장 기본적으로 많이 사용되는 카메라 모델과 Distortion 모델에 대해서 설명하였다. 대부분의 카메라 calibration 을 위한 tool 들 (Matlab, Kalibr 등)은 대부분 위에서 설명한 camera model들과 distortion model들을 지원한다. Kalibr의 경우 Pinhole camera 모델의 경우 radial distortion과 tangential distortion을 모두 추정하지만, equidistance model의 경우에는 radial distortion만 추정하여 사용한다.</p>

<h1 id="reference">Reference</h1>

<p><a href="https://www.isprs.org/proceedings/XXXVI/5-W8/Paper/PanoWS_Berlin2005_Schwalbe.pdf">Camera Model</a></p>

<p><a href="https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1642666&amp;casa_token=oJyQMwYes2EAAAAA:ND5Nt9U4Nnbc-I1zaC4X334O9nq9LTRaQjVvhhKij0i90ylqfq5MGmq1I8h6Q9MhCQ5GiPxypcQ&amp;tag=1">A generic camera model and calibration method for conventional, wide-angle, and fish-eye lenses</a></p>

<p><a href="https://kr.mathworks.com/help/vision/ug/camera-calibration.html">What Is Camera Calibration?</a></p>

<p><a href="https://github.com/ethz-asl/image_undistort">ethz-asl/image_undistort</a></p>

<p><a href="http://rpg.ifi.uzh.ch/docs/omnidirectional_camera.pdf">omni directional camera</a></p>]]></content><author><name>JinyongJeong</name></author><category term="SLAM" /><category term="SLAM" /><summary type="html"><![CDATA[Visual SLAM, SfM 등 카메라를 이용한 연구를 하기 위해서 가장 기본적으로 알아야 할 부분이 바로 카메라 모델이다. 일반적인 카메라는 대부분 핀홀(Pin-hole) 카메라 형태로 모델링하여 사용하고 있으며, 처음 카메라에 대해서 배울 때 대부분의 설명 자료들이 핀홀 모델을 기반으로 설명을 하고 있다. 이러한 핀홀 모델은 렌즈를 통해 들어오는 빛이 굴절되지 않고 바로 이미지 센서로 들어오는 perspective projection을 기반으로 하고 있다. 하지만 실제 연구를 진행하다 보면 넓은 화각이 이점이 되는 어플리케이션 (ex. Visual SLAM)이 있기 때문에 FOV (Field of View)가 170도 이상인 어안렌즈 (Fisheye lens)를 사용하는 경우도 많다. 이러한 화각이 매우 넓은 렌즈의 경우에는 일반적인 모델로 표현하기 어렵다. 따라서 이러한 경우에는 perspective projection이 아닌 equidistance projection 모델을 사용한다.]]></summary></entry><entry><title type="html">[SLAM] Bundle Adjustment의 Jacobian 계산</title><link href="/slam/2024/01/14/jacobian_of_BA.html" rel="alternate" type="text/html" title="[SLAM] Bundle Adjustment의 Jacobian 계산" /><published>2024-01-14T00:00:00+00:00</published><updated>2024-01-14T00:00:00+00:00</updated><id>/slam/2024/01/14/jacobian_of_BA</id><content type="html" xml:base="/slam/2024/01/14/jacobian_of_BA.html"><![CDATA[<h1 id="jacobian-with-respect-to-lie-algebra">Jacobian with respect to Lie Algebra</h1>

<p>일반적으로 최적화를 수행할 때 가장 쉬운 방법은 함수의 미분을 계산하고, 현재의 값에서 미분값이 작아지는 방향으로 값을 변경해 가면서 최적화를 수행하는 방법이다. 이러한 방법을 gradient descent 방법이라고 하는데, Jacobian은 이러한 함수가 multi-variable 일때의 미분을 의미한다.</p>

<p>즉 Jacobian은 multi-variable 문제에서 내가 최적화 하고 싶은 parameter들에 대한 편미분을 matrix로 표현한 것이다.</p>

<p>Graph-based SLAM 뿐만 아니라 BA(Bundle Adjustment), Visual SLAM쪽을 공부하다 보면 계속 이러한 Jacobian을 만나게 되는데 정확히 어떻게 계산이 되는지 알고 넘어가는게 좋을 것 같아서 수식을 정리해 보았다.</p>

<p>수식이 너무 많아서 손필기로 정리하였으며, Error function은 일반적인 BA에서 사용하는 reprojection error에 대한 Jacobian을 구하는 방법이다.</p>

<p>(혹시 수식적으로 틀린 부분이 있거나, 더 간단하게 풀수있는 방법이 있으면 댓글로 알려주세요)</p>

<p><img src="https://i.imgur.com/CSRioZC.jpg" alt="Bundle Adjustment 2" /></p>

<p><img src="https://i.imgur.com/RA96sfx.jpg" alt="Bundle Adjustment 3" /></p>

<p><img src="https://i.imgur.com/t7IywjE.jpg" alt="Bundle Adjustment 4" /></p>

<p><img src="https://i.imgur.com/M9hgzCv.jpg" alt="Bundle Adjustment 5" /></p>

<p><img src="https://i.imgur.com/8LMs1e0.jpg" alt="Bundle Adjustment 6" /></p>

<p><img src="https://i.imgur.com/FvV324V.jpg" alt="Bundle Adjustment 7" /></p>

<p><img src="https://i.imgur.com/638WFPO.jpg" alt="Bundle Adjustment 8" /></p>

<p><img src="https://i.imgur.com/4n0GHlo.jpg" alt="Bundle Adjustment 9" /></p>

<p>최종 rotation의 Jacobian을 보면 상당히 복잡하다.</p>

<p>이 식을 조금 단순화 시키기 위해서 roation matrix를 단순화 시킨다.</p>

<p>Multiple view geometry 책의 Appendix 6의 A6.9.1에 이런 말이 있다.</p>

<p>Lie algebra t가 작으면, rotation matrix는 다음과 같이 근사 가능하다.</p>

\[R = I + [t]_{\times}\]

<p>따라서 근사화된 R에 대해서 jacobian을 구해보면 다음과 같다.</p>

<p><img src="https://i.imgur.com/2OxBKrR.jpg" alt="Bundle Adjustment 11" /></p>

<p>즉 근사화된 Rotation matrix를 이용해서 Jacobian을 구해보면 입력 measurement의 skew symmetric matrix가 됨을 알 수 있다.</p>

<p>대부분의 tracking 문제에서는 계산하고자 하는 state는 두 frame 사이의 relative pose이고, 이 relative는 크지 않다.</p>

<p>따라서 R을 위와같이 근사화 해서 풀 수 있다.</p>

<h1 id="jacobian-with-respect-to-quaternion">Jacobian with respect to Quaternion</h1>

<p>그렇다면 state가 quaternion으로 rotation을 표현한다면 어떻게 될까?</p>

<p>visual SLAM 코드를 공부하기 좋은 Pro-SLAM의 코드는 quaternion으로 rotation state를 표현하고 있으며, w를 제외한 3개로 state를 표현한다.</p>

<p>w는 3개의 state로 recovery가 가능하다.</p>

<p>Point를 Quaternion으로 회전 시키는 식을 미분하면 다음과 같다.</p>

<p><img src="https://i.imgur.com/uXIapKE.png" alt="quaternion" /></p>

<p>state를 3개만으로 표현하였기 때문에 최종 matrix에서 뒤의 3 column 즉</p>

\[2 (v^TaI+va^T-av^T-w[a]_{\times})\]

<p>만 해당이 된다.</p>

<p>위에서 설명한 것 처럼 대부분의 tracking 문제에서는 두 frame 사이의 relative를 계산하는 것이며, 이 relative는 크지 않다.</p>

<p>따라서 Jacobian은 회전이 0 (identity) 에서 미분한 값으로 단순화 할 수 있다. (찾고자 하는 relative rotation이 작기 때문에)</p>

<p>회전이 0, identity일 때 quaternion의 w 는 1, 그리고 i,j,k term (위 식에서는 v vector)은 0이 된다.</p>

<p>따라서 quaternion의 jacobian은 다음이 된다.</p>

\[-2[a]_{\times}\]

<p>Pro-SLAM 코드에서 rotation state는 quaternion으로 표현되기 때문에 위의 Jacobian을 사용한다.</p>

<p>Rotation의 Jacobian을 계산하는 코드를 보면 일치함을 알 수 있다.</p>

<div class="language-cpp highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">//ds update total error</span>
<span class="n">_total_error</span> <span class="o">+=</span> <span class="n">_errors</span><span class="p">[</span><span class="n">u</span><span class="p">];</span>

<span class="c1">//ds compute the jacobian of the transformation</span>
<span class="n">Matrix3_6</span> <span class="n">jacobian_transform</span><span class="p">;</span>

<span class="c1">//ds translation contribution (will be scaled with omega)</span>
<span class="c1">// proj(M_i)를 p_cam 으로 미분한 부분에서 translation부분</span>
<span class="n">jacobian_transform</span><span class="p">.</span><span class="n">block</span><span class="o">&lt;</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="o">&gt;</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">)</span> <span class="o">=</span> <span class="n">_weights_translation</span><span class="p">[</span><span class="n">u</span><span class="p">]</span><span class="o">*</span><span class="n">Matrix3</span><span class="o">::</span><span class="n">Identity</span><span class="p">();</span>

<span class="c1">//ds rotation contribution - compensate for inverse depth (far points should have an equally strong contribution as close ones)</span>
<span class="c1">// *******************여기가 Rotation의 Jacobian 계산하는 부분, 위에서 설명한대로 넣어주고 있다 **********************</span>
<span class="n">jacobian_transform</span><span class="p">.</span><span class="n">block</span><span class="o">&lt;</span><span class="mi">3</span><span class="p">,</span><span class="mi">3</span><span class="o">&gt;</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="mi">3</span><span class="p">)</span> <span class="o">=</span> <span class="o">-</span><span class="mi">2</span><span class="o">*</span><span class="n">srrg_core</span><span class="o">::</span><span class="n">skew</span><span class="p">(</span><span class="n">sampled_point_in_camera_left</span><span class="p">);</span>

<span class="c1">//ds precompute</span>
<span class="c1">// Intrinsic matrix를 곱해주는 부분</span>
<span class="k">const</span> <span class="n">Matrix3_6</span> <span class="nf">camera_matrix_per_jacobian_transform</span><span class="p">(</span><span class="n">_camera_calibration_matrix</span><span class="o">*</span><span class="n">jacobian_transform</span><span class="p">);</span>

<span class="c1">//ds precompute</span>
<span class="k">const</span> <span class="n">real</span> <span class="n">inverse_sampled_c_left</span>  <span class="o">=</span> <span class="mi">1</span><span class="o">/</span><span class="n">sampled_c_left</span><span class="p">;</span>
<span class="k">const</span> <span class="n">real</span> <span class="n">inverse_sampled_c_right</span> <span class="o">=</span> <span class="mi">1</span><span class="o">/</span><span class="n">sampled_c_right</span><span class="p">;</span>
<span class="k">const</span> <span class="n">real</span> <span class="n">inverse_sampled_c_squared_left</span>  <span class="o">=</span> <span class="n">inverse_sampled_c_left</span><span class="o">*</span><span class="n">inverse_sampled_c_left</span><span class="p">;</span>
<span class="k">const</span> <span class="n">real</span> <span class="n">inverse_sampled_c_squared_right</span> <span class="o">=</span> <span class="n">inverse_sampled_c_right</span><span class="o">*</span><span class="n">inverse_sampled_c_right</span><span class="p">;</span>

<span class="c1">//ds jacobian parts of the homogeneous division: left</span>
<span class="n">Matrix2_3</span> <span class="n">jacobian_left</span><span class="p">;</span>
<span class="c1">// 각 카메라의 coordinate로 이동한 measurement로 만든 matrix (미분 term에서 가장 앞에 곱해지는 matrix)</span>
<span class="n">jacobian_left</span> <span class="o">&lt;&lt;</span> <span class="n">inverse_sampled_c_left</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="n">sampled_abc_in_camera_left</span><span class="p">.</span><span class="n">x</span><span class="p">()</span><span class="o">*</span><span class="n">inverse_sampled_c_squared_left</span><span class="p">,</span>
           <span class="mi">0</span><span class="p">,</span> <span class="n">inverse_sampled_c_left</span><span class="p">,</span> <span class="o">-</span><span class="n">sampled_abc_in_camera_left</span><span class="p">.</span><span class="n">y</span><span class="p">()</span><span class="o">*</span><span class="n">inverse_sampled_c_squared_left</span><span class="p">;</span>

<span class="c1">//ds we compute only the contribution for the horizontal error: right</span>
<span class="n">Matrix2_3</span> <span class="n">jacobian_right</span><span class="p">;</span>
<span class="n">jacobian_right</span> <span class="o">&lt;&lt;</span> <span class="n">inverse_sampled_c_right</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="o">-</span><span class="n">sampled_abc_in_camera_right</span><span class="p">.</span><span class="n">x</span><span class="p">()</span><span class="o">*</span><span class="n">inverse_sampled_c_squared_right</span><span class="p">,</span>
            <span class="mi">0</span><span class="p">,</span> <span class="n">inverse_sampled_c_right</span><span class="p">,</span> <span class="o">-</span><span class="n">sampled_abc_in_camera_right</span><span class="p">.</span><span class="n">y</span><span class="p">()</span><span class="o">*</span><span class="n">inverse_sampled_c_squared_right</span><span class="p">;</span>

<span class="c1">//ds assemble final jacobian</span>
<span class="n">_jacobian</span><span class="p">.</span><span class="n">setZero</span><span class="p">();</span>

<span class="c1">//ds we have to compute the full block</span>
<span class="c1">// 최종 p_cam으로 미분 한 matrix, 여기에선 left, right의 reporjection error를 concat해서 사용</span>
<span class="n">_jacobian</span><span class="p">.</span><span class="n">block</span><span class="o">&lt;</span><span class="mi">2</span><span class="p">,</span><span class="mi">6</span><span class="o">&gt;</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span><span class="mi">0</span><span class="p">)</span> <span class="o">=</span> <span class="n">jacobian_left</span><span class="o">*</span><span class="n">camera_matrix_per_jacobian_transform</span><span class="p">;</span>

<span class="c1">//ds we only have to compute the horizontal block</span>
<span class="n">_jacobian</span><span class="p">.</span><span class="n">block</span><span class="o">&lt;</span><span class="mi">2</span><span class="p">,</span><span class="mi">6</span><span class="o">&gt;</span><span class="p">(</span><span class="mi">2</span><span class="p">,</span><span class="mi">0</span><span class="p">)</span> <span class="o">=</span> <span class="n">jacobian_right</span><span class="o">*</span><span class="n">camera_matrix_per_jacobian_transform</span><span class="p">;</span>

<span class="c1">//ds precompute transposed</span>
<span class="k">const</span> <span class="n">Matrix6_4</span> <span class="nf">jacobian_transposed</span><span class="p">(</span><span class="n">_jacobian</span><span class="p">.</span><span class="n">transpose</span><span class="p">());</span>

<span class="c1">//ds update H and b</span>
<span class="c1">// 최적화 문제를 풀기 위해 계산된 jacobian으로 hessian 계산 (이 부분은 이전 Graph SLAM post 참고)</span>
<span class="n">_H</span> <span class="o">+=</span> <span class="n">jacobian_transposed</span><span class="o">*</span><span class="n">_omega</span><span class="o">*</span><span class="n">_jacobian</span><span class="p">;</span>
<span class="n">_b</span> <span class="o">+=</span> <span class="n">jacobian_transposed</span><span class="o">*</span><span class="n">_omega</span><span class="o">*</span><span class="n">error</span><span class="p">;</span>
</code></pre></div></div>

<p>아래의 github 링크는 Pro-SLAM 에 대해서 한글로 주석을 달아놓은 code이다.</p>

<p><a href="https://github.com/JinyongJeong/proSLAM.git">https://github.com/JinyongJeong/proSLAM.git</a></p>]]></content><author><name>JinyongJeong</name></author><category term="SLAM" /><category term="SLAM" /><summary type="html"><![CDATA[Jacobian with respect to Lie Algebra]]></summary></entry><entry><title type="html">[SLAM] OpenCV Camera Model</title><link href="/slam/2024/01/14/opencv_camera_model.html" rel="alternate" type="text/html" title="[SLAM] OpenCV Camera Model" /><published>2024-01-14T00:00:00+00:00</published><updated>2024-01-14T00:00:00+00:00</updated><id>/slam/2024/01/14/opencv_camera_model</id><content type="html" xml:base="/slam/2024/01/14/opencv_camera_model.html"><![CDATA[<p>Opencv에는 다양한 카메라 모델을 다루기 위한 기본 Class들이 정의되어 있다. 그 중에서 Pinhole model과 Fish-eye model이 어떻게 모델되어 있는지 식으로 살펴보려고 한다. 카메라 모델에는 이 두가지 모델 이외에도 다양한 모델이 있으며, 이러한 다양한 카메라 모델에 대해서는 다음의 이전 글을 참고하자.</p>

<h1 id="pinhole-camera-model">Pinhole Camera Model</h1>

<p><img src="https://i.imgur.com/8AZgr1L.png" alt="" /></p>

<p>Pin-hole 카메라 모델은 일반적으로 많이 사용되는 카메라 모델이며, 물체로부터 오는 lay를 focal length 가 1인 normalized image plane으로 projection하고, image coordinate으로 변환시키는 모델이다. 이때 일반적으로 distortion은 radial distortion과 tangential distortion을 고려한다</p>

<p>먼저 distortion을 고려하지 않는 경우 다음과 같다.</p>

\[\begin{array}{l}
{\left[\begin{array}{l}
x \\
y \\
z
\end{array}\right]=R\left[\begin{array}{l}
X \\
Y \\
Z
\end{array}\right]+t} 
\end{array}\]

<p>Distortion을 고려하지 않은 모델은 매우 단순하다. $P=(X,Y,Z)$ 는 global coordinate에서의 물체의 위치이다. 이때 global coordinate에서 카메라의 rotation과 translation 이 각각 $R$, $t$ 라면 첫번째 수식을 통해 P는 Camera coordinate에서의 좌표인 $(x,y,z)$ 로 변환된다.</p>

\[\begin{array}{l}
x^{\prime}=x / z \\
y^{\prime}=y / z 
\end{array}\]

<p><img src="https://i.imgur.com/ASjtXKz.png" alt="" /></p>

<p>그 다음은 Camera coordinate으로 변환된 좌표를 normalized image plane으로 변환합니다. normalized image plane은 원점으로 부터 거리가 1인 평면을 의미하며, 이때의 x, y값에 f를 곱함으로써 초첨거리가 f인 평면으로 변환할 수 있다. 따라서 첫번째로 camera coordinate으로 변환된 좌표를 각각 $z$ 로 나눠줌으로써 normalized image plane으로 변환한다.</p>

\[\begin{array}{l}
u=f_{x} * x^{\prime}+c_{x} \\
v=f_{y} * y^{\prime}+c_{y}
\end{array}\]

<p>그리고 마지막으로는 $x’, y’$ 각각에 focal length ($f$)를 곱하고 principal point ($c$) 를 더해줌으로써 image coordinate으로 변환시킨다. focal length를 곱해주는것은 위에서 설명한 것 처럼 normalized image plane에서 초점거리가 f인 image plane으로 변환하는 과정이다. 그리고  camera coordinate의 경우 이미지의 중심이 $(0,0)$ 이지만 이미지의 경우 일반적으로 좌측상단이 $(0,0)$ 이다. 따라서 이러한 좌표의 변환을 해주는 것이 principal point를 더해주는 것이다. 이 계산은 일반적으로 3X3의 Camera Intrinsic matrix ($K$)를 곱해주는 과정으로 표현하기도 한다.</p>

<p>이러한 과정을 통해 3차원 포인트를 카메라로 바라보았을 때 해당하는 물체가 어느 픽셀에 도달할지를 계산할 수 있게 되었다. 하지만 실제 카메라 모델은 이렇게 이상적이지 않다. 렌즈의 물리적인 이유, 센서와 렌즈의 misalignment와 같은 이유로 발생하는 distortion을 보정해 주어야 한다. 일반적인 카메라 모델의 경우 보통 Radial distortion과 Tangential distortion 모델을 활용한다. 두가지 distortion model에 대해서 익숙하지 않다면, 위 링크의 글을 살펴보길 바란다.</p>

<p>일반적인 Radial distortion은 다음과 같은 식으로 표현된다.</p>

\[x_{\text {distorted }}=x\left(1+k_{1}^{*} r^{2}+k_{2}^{*} r^{4}+k_{3}^{*} r^{6}\right)\]

\[y_{\text {distorted }}=y\left(1+k_{1}^{*} r^{2}+k_{2}^{*} r^{4}+k_{3}^{*} r^{6}\right)\]

<p>그리고 Tangential distortion은 다음과 같이 표현된다.</p>

\[x_{\text {distorted }}=x+\left[2^{*} p_{1}^{*} x^{*} y+p_{2}^{*}\left(r^{2}+2^{*} x^{2}\right)\right]\]

\[y_{\text {distorted }}=y+\left[p_{1}^{*}\left(r^{2}+2^{*} y^{2}\right)+2^{*} p_{2}^{*} x^{*} y\right]\]

<p>각 Distortion 식에서 $x,y$는 이상적인 (undistorted) pixel이며, $k_1, k_2, k_3$는 radial distortion parameter, 그리고 $p_1, p_2$는 tangential distortion parameter이다. 이러한 Distortion model을 적용한 모델은 다음과 같다.</p>

\[\begin{array}{l}{\left[\begin{array}{l}x \\y \\z\end{array}\right]=R\left[\begin{array}{l}X \\Y \\z\end{array}\right]+t} \\ x'=x / z \\y^{\prime}=y / z \end{array}\]

<p>Normalized image plane으로 변환하는 과정은 동일하다.</p>

\[\begin{array}{l}x^{\prime \prime}=x^{\prime} \frac{1+k_{1} r^{2}+k_{2 }r^4+k_{3} r^{6}}{1+k_{4} r^{2}+k_{5} r^{4}+k_{6} r^{6}}+2 p_{1} x^{\prime} y^{\prime}+p_{2}\left(r^{2}+2 x^{\prime 2}\right) \\y^{\prime \prime}=y^{\prime} \frac{1+k_{1} r^{2}+k_{2} r^4+ k_{3} r^{6}}{1+k_{4} r^{2}+k_{5} r^{4}+k_{6} r^{6}}+p_{1}\left(r^{2}+2 y^{\prime 2}\right)+2 p_{2} x^{\prime} y^{\prime} \\\text { where } r^{2}=x^{\prime 2}+y^{\prime 2} \end{array}\]

<p>Normalized image plane으로 변환된 값 $x’, y’$ 은 radial, tangential distortion 모델에 의해서 distortion이 발생한다. 위 식에서 우항의 첫번째 항은 radial distortion, 그리고 두번째 세번째 항은 tangential distortion이다. 여기서 $r$은 normalized image plane에서 각 점의 principal axis로 부터의 거리를 의미한다.</p>

\[\begin{array}{l}u=f_{x} * x^{\prime \prime}+c_{x} \\v=f_{y} * y^{\prime \prime}+c_{y}\end{array}\]

<p>Distortion model을 적용한 이후의 normalized image plane 의 pixel ($x’’, y’’$)은 Camera intrinsic matrix를 곱해줌으로써 image plane으로 변환된다. 위와 같이 distortion이 포함된 카메라 모델을 통해 3차원 좌표상에 있는 물체가 실제 핀홀 카메라 모델에서 어느 위치의 픽셀에 나타날지 계산할 수 있게 되었다. 반대로 이미지 plane에 있는 픽셀이 실제 3차원 좌표에서 어디에 위치하는지 계산할 수 있다. 하지만 이런 경우에 위 모델을 통해 알 수 것은 normalized image plane에서의 좌표인 $x’,y’$ 이기 때문에 실제 3차원 좌표로 복원되기 위해서는 카메라로 부터 떨어진 거리인 depth ($z$)을 알고 있어야 한다.</p>

<h1 id="fish-eye-camera-model">Fish-eye Camera Model</h1>

<p><img src="https://i.imgur.com/mEqeYL3.png" alt="" /></p>

<p>Fish-eye camera는 물고기의 눈처럼 렌즈가 볼록하여 넓은 화각을 갖는 카메라 모델을 의미한다. 일반적인 핀홀 카메라와는 다르게 Object point의 빛이 렌즈를 통과하여 직진으로 image plane에 도달하는 것이 아닌 렌즈의 왜곡에 의해 빛이 꺽여서 image plane에 도달하게 된다. Fish-eye camera를 모델링하기 위해 가장 많이 활용되는 모델은 Equidistant model로 빛의 입시각 $\theta$와 image plane에서 principal point와의 거리인 $r$ 가 비례적인 관계를 갖는 모델이다. 즉,</p>

\[\frac{r_1}{\theta_1} = \frac{r_2}{\theta_2}\]

<p>이다. OpenCV의 fish-eye 카메라 모델도 이와 같은 equidistant model을 이용하고 있다. 보통 equidistant model의 경우에는 radial distortion 만을 고려한다. 지금부터 3차원 point인 $X$가 실제 fish-eye 모델을 통해 image plane으로 변환되는 과정을 살펴보자.</p>

\[X_c = RX + T\]

\[x = X_{c1}, y = X_{c2}, z= X_{c3}\]

<p>핀홀카메라와 동일하게 3차원 공간의 점 \(X\)는 카메라의 rotation, translation인 $R, T$를 이용하여 camera coordinate인 \(X_c\)로 변환된다.</p>

\[a = \frac{x}{z}, b=\frac{y}{z}\]

<p>Camera coordinate으로 변환된 점은 depth인 $z$로 normalize를 함으로써 normalized image plane으로 변환된다.</p>

\[r^2 = a^2 + b^2\]

\[\theta=atan(r)\]

<p>이때 중요한것은 여기서의 $r$은 일반적은 핀홀 카메라의 normalized image plane에서 중심점으로 부터 해당 픽셀까지의 euclidean distance를 의미한다. $\theta$는 빛의 입사각을 의미한다.</p>

\[\theta_d = \theta(1+k_1 \theta^2 + k_2 \theta^4 + k_3 \theta^6 + k_4 \theta^8)\]

<p>위 식은 radial distortion을 적용하는 식을 나타낸다. 일반적인 핀홀 카메라와 가장 큰 차이점의 있는 부분 중에 하나이다. 핀홀 카메라의 경우에는 pixel의 x, y좌표인 $x’, y’$에 distortion 모델이 적용되었으나, equidistant model에서는 입사각인 $\theta$에 distortion 모델이 적용된다. 이때 일반적으로 4개의 radial distortion parameter가 사용된다.</p>

\[x' =\frac{\theta_d}{r} * a\]

\[y' = \frac{\theta_d}{r}*b\]

<p>위 식은 equidistant model의 특징인 $\frac{r_1}{\theta_1} = \frac{r_2}{\theta_2}$의 특징을 이용하여 pixel의 값을 scaling 하는 식이다. normalized image plane에서의 pixel 값 ($a, b$)는 $r$과 $\theta_d$의 비율로 scaling된다. 위에서는 distortion model이 적용되었기 때문에 $\theta_d$가 사용되었다. 만약 distortion model을 적용하지 않고 이상적인 모델이라면 $\theta_d$ 대신 $\theta$를 사용하면 된다.</p>

\[u= f_x(x' + ay') + c_x\]

\[v=f_yy'+c_y\]

<p>마지막으로 normalized image plane에서 scaling된 pixel값에 Intrinsic matrix를 곱해 줌으로써 image coordinate으로 변환하게 된다. 이러한 계산을 통해 실제 3차원에 있는 물체가 fish-eye 카메라 모델에서 어느 pixel에 위치할지 계산할 수 있다. 보통 이러한 Fish-eye 카메라의 이미지는 많은 경우 undistortion과정 (실제 직선은 직선으로 보이도록 변환) 을 통해 이미지를 변환하여 사용하게 된다. Undistortion 과정은 위의 과정과 반대로 실제 fish-eye model로 변환된 픽셀값 $u, v$ 로부터 normalized image plane의 점인 $a, b$를 계산하는 과정으로 생각할 수 있다. 이때 depth 값은 모르기 때문에 $z$를 복원할 수는 없다.</p>

<h1 id="reference">Reference</h1>

<p><a href="https://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html">Camera Calibration and 3D Reconstruction - OpenCV 2.4.13.7 documentation</a></p>

<p><a href="https://docs.opencv.org/3.4/db/d58/group_calib3d_fisheye.html">OpenCV: Fisheye camera model</a></p>]]></content><author><name>JinyongJeong</name></author><category term="SLAM" /><category term="SLAM" /><summary type="html"><![CDATA[Opencv에는 다양한 카메라 모델을 다루기 위한 기본 Class들이 정의되어 있다. 그 중에서 Pinhole model과 Fish-eye model이 어떻게 모델되어 있는지 식으로 살펴보려고 한다. 카메라 모델에는 이 두가지 모델 이외에도 다양한 모델이 있으며, 이러한 다양한 카메라 모델에 대해서는 다음의 이전 글을 참고하자.]]></summary></entry><entry><title type="html">[SLAM] Graph Based SLAM</title><link href="/slam/2024/01/13/graph_based_slam.html" rel="alternate" type="text/html" title="[SLAM] Graph Based SLAM" /><published>2024-01-13T00:00:00+00:00</published><updated>2024-01-13T00:00:00+00:00</updated><id>/slam/2024/01/13/graph_based_slam</id><content type="html" xml:base="/slam/2024/01/13/graph_based_slam.html"><![CDATA[<p><strong>본 글은 University Freiburg의 <a href="http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/">Robot Mapping</a> 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었다.</strong></p>

<p>이 글에서는 Least square를 이용하여 실제로 어떻게 SLAM에 적용이 되는지 알아볼 것이다. 아직 least squre에 대해서 익숙하지 않다면 이전 글을 참고하기 바란다. 이번 글에서의 표기법은 이전 글의 표기법을 그대로 사용한다.</p>

<h1 id="graph-based-slam">Graph-based SLAM</h1>

<p>Graph-based SLAM은 로봇의 위치와 움직임을 graph처럼 node와 edge로 표현함을 의미한다.</p>

<ul>
  <li>
    <p>Node: Graph의 node는 로봇의 pose를 의미한다.</p>
  </li>
  <li>
    <p>Edge: 두 node사이의 edge는 로봇의 위치 사이의 odometry정보이며 constraint라고 한다.</p>
  </li>
</ul>

<p><img src="https://i.imgur.com/jpmvmGd.png" alt="" /></p>

<p>로봇이 움직이며 odometry 정보를 이용하여 node와 constraint를 생성한다.</p>

<p><img src="https://i.imgur.com/k2UIxHY.png" alt="" /></p>

<p>로봇이 위 그림과 같이 이전에 방문했던 지역을 다시 방문할 경우 주변 환경에 대한 정보를 이용하여 같은 위치임을 인식하고 연속적이지 않은(non-successive) node사이에 constraint를 추가하고, graph를 최적화 함으로써 measurement에 최적화된 로봇의 위치를 계산할 수 있다.</p>

<p>아래의 youtube 동영상은 pose graph SLAM을 이용한 SLAM의 예이다.</p>

<iframe width="640" height="480" src="https://www.youtube.com/embed/JLjFxDMovnc" frameborder="0" allowfullscreen=""> </iframe>

<p>동영상에서 차량이 움직이는 경로의 실선은 constraint이며, 실선 중간에 생성되는 네모는 graph의 node이다. 위 application은 도로 상의 marking정보를 이용하여 장소를 인식하고 non-successive node사이의 constraint를 추가함으로써 odometry의 error를 보정한다. 동영상에서 이러한 constraint를 추가함으로써 계속적으로 graph가 최적화 되는 모습을 볼 수 있다.</p>

<h1 id="overall-slam-system">Overall SLAM System</h1>

<p>Graph SLAM은 아래 그림과 같이 크게 Front-end와 Back-end로 나눌 수 있다.</p>

<p><img src="https://i.imgur.com/SU6Cmsp.png" alt="" /></p>

<p>위의 동영상을 예로 들면, Front-end는 camera로 부터 얻어지는 이미지로 부터 데이터 처리를 통해 sub-map을 만들고, 생성된 sub-map을 통해서 다시 방문한 장소를 인식함으로써 각 node사이의 constraint를 생성하는 과정을 말한다. 이렇게 생성된 constraint들과 node들의 정보를 이용하여 최적화된 node의 위치를 계산하는 과정은 graph SLAM의 back-end에서 수행한다. 최근 많이 사용되는 graph SLAM의 back-end로는 <a href="http://ieeexplore.ieee.org/abstract/document/4682731/">iSAM</a>과 <a href="http://ieeexplore.ieee.org/abstract/document/5979949/">g2o</a>가 있다.</p>

<h1 id="graph">Graph</h1>

<p>이 글에서 로봇이 이동하는 환경은 2-dimension 으로 가정하며, 로봇의 위치는 다음과 같이 3 DOF로 표현한다.</p>

\[\mathbf{x}_i = (x_i, y_i, \theta_i)\]

<p>Graph는 이러한 로봇의 위치를 표현하는 $\mathbf{x}_i$로 구성된 vector이다.</p>

\[\mathbf{x} = \mathbf{x}_{1:n}\]

<p>Graph를 구성하는 노드 $\mathbf{x}<em>i$와 $\mathbf{x}</em>{i+1}$ 사이에는 odometry 센서로부터 얻어진 measurement가 constraint/edge로 생성된다.</p>

<p><a href="https://i.imgur.com/VnNVFYB.png"></a></p>

<p>만약 로봇이 같은 장소를 다시 방문했을 때 두 로봇의 위치에서 얻은 measurement를 이용하여 두 노드 사이의 상대적인 위치(relative pose)를 계산할 수 있다. 아래 그림은 다른 위치에서 획득한 같은 물체의 센서 데이터를 보여준다.</p>

<p><img src="https://i.imgur.com/00linOy.png" alt="" /></p>

<p>두 센서데이터의 매칭을 통해서 두 node사이의 상대위치를 계산할 수 있고, 이 상대위치가 두 node사이의 constraint가 된다. 이때 직접적으로 node사이의 관계를 구한 것이 아닌 센서 measurement로부터 계산하였기 때문에 virtual measurement라고 부른다.</p>

<p><img src="https://i.imgur.com/szChOpw.png" alt="" /></p>

<h1 id="pose-graph">Pose Graph</h1>

<p>아래 그림은 Pose graph SLAM에서 measurement에 대한 error를 그림으로 표현하여 보여준다.</p>

<p><img src="https://i.imgur.com/ILYSEpx.png" alt="" /></p>

<p>$\mathbf{x}<em>i$와 $\mathbf{x}_j$는 현재 graph에서 두 node의 위치이다. $&lt;\mathbf{z}</em>{ij},\Omega_{ij}&gt;$는 $\mathbf{x}<em>i$의 위치에서 센서를 이용하여 측정한 $\mathbf{x}_j$의 위치이다. 센서로 측정한 node의 위치와 현재 graph상의 위치의 차이를 $\mathbf{e}</em>{ij}$로 표현한다. Graph optimization은 이러한 error를 최소화 시키는 graph를 계산하는 것이다. 여기서 $\Omega_{ij}$가 센서 measurement의 information matrix임을 기억하자.</p>

<p>Error function ($\mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j)$)을 homogeneous coordinate로 표현하면 다음과 같이 표현할 수 있다.</p>

\[\mathbf{e}_{ij}(\mathbf{x}_i, \mathbf{x}_j) = \text{t2v}(\mathbf{Z}_{ij}^{-1}(\mathbf{X}_i^{-1}\mathbf{X}_j))\]

<p>homogeneous coordinate은 로봇의 translation과 rotation을 하나의 matrix로 표현하는 방법이며, 이러한 표현방법에 대해서는 <a href="http://jinyongjeong.github.io/2016/06/07/se3_so3_transformation/">이글</a>을 참고하라.</p>

<p>위 식에서 $\mathbf{Z}_{ij}$는 i에서 바라본 j의 measurement이며, $\mathbf{X}_i^{-1}\mathbf{X}_j$는 현재 graph에서 i를 기준으로 j의 위치를 의미한다. t2v함수는 homogeneous coordinate를 vector form으로 바꾸는 transform 함수이다.</p>

<p>Graph optimization식을 전체 state를 표현하는 $\mathbf{x}$를 이용하여 표현하면 다음과 같다.</p>

\[\begin{aligned}
x^* &amp;=\text{argmin}_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}^T(\mathbf{x}_i,\mathbf{x}_j) \mathbf{\Omega}_{ij} \mathbf{e}_{ij}(\mathbf{x}_i,\mathbf{x}_j)\\
&amp;=\text{argmin}_{\mathbf{x}} \sum_k \mathbf{e}_k^T(\mathbf{x}) \mathbf{\Omega}_k \mathbf{e}_k(\mathbf{x})
\end{aligned}\]

<p>여기서 state vector $\mathbf{x}$는 graph의 node가 각각의 block을 구성하는 vector이다.</p>

\[\mathbf{x}^T = \begin{pmatrix}\mathbf{x}_1^T &amp; \mathbf{x}_2^T &amp; \cdots &amp; \mathbf{x}_n^T
\end{pmatrix}\]

<p>error function을 선형화하면 Jacobian으로 표현할 수 있다.</p>

\[\begin{aligned}
\mathbf{e}_{ij}(\mathbf{x}+\triangle\mathbf{x}) &amp;\approx
\mathbf{e}_{ij}(\mathbf{x}) + \mathbf{J}_{ij}(\mathbf{x})\triangle \mathbf{x}
\end{aligned}\]

<p>선형화된 error function $\mathbf{e}_{ij}$는 state vector의 $\mathbf{x}_i$와 $\mathbf{x}_j$에만 관련이 있으므로 Jacobian은 sparse matrix가 된다.</p>

\[\frac{\partial \mathbf{e}_{ij}(\mathbf{x})}{\partial \mathbf{x}} = \begin{pmatrix}
0 &amp; \cdots &amp; \frac{\partial \mathbf{e}_{ij}(\mathbf{x}_i)}{\partial \mathbf{x}_i} &amp; \cdots &amp;&amp; \frac{\partial \mathbf{e}_{ij}(\mathbf{x}_j)}{\partial \mathbf{x}_j} &amp; \cdots &amp; 0
\end{pmatrix}\]

\[\mathbf{J}_{ij} = \begin{pmatrix} 0 &amp; \cdots &amp; \mathbf{A}_{ij} &amp; \cdots &amp; \mathbf{B}_{ij} &amp; \cdots &amp; 0)
\end{pmatrix}\]

<p><img src="https://i.imgur.com/95PwOAP.png" alt="" /></p>

<p>이전 글에서 optimization 과정을 통해서 최적화된 $\mathbf{x}^*$를 계산하였다. 최적화 식을 만족하는 state를 계산하기 위해서는 $\mathbf{b}^T$와 $\mathbf{H}$를 계산해야 한다.</p>

\[\begin{aligned}
\mathbf{b}^T &amp;= \sum_{ij} \mathbf{e}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{ij}\\
\mathbf{H} &amp;= \sum_{ij} \mathbf{J}_{ij}^T  \mathbf{\Omega}_{ij} \mathbf{J}_{ij}
\end{aligned}\]

<p>Jacobian matrix $\mathbf{J}_{ij}$가 sparse matrix이기 때문에 information matrix인 $\mathbf{H}$도 sparse matrix가 된다. Jacobian matrix의 sparse함이 각 파라미터에 어떻게 영향을 미치는지 그림으로 보자.</p>

<p><img src="https://i.imgur.com/U8OxZNC.png" alt="" /></p>

<p><img src="https://i.imgur.com/gPspIOf.png" alt="" /></p>

<p>위 그림에서 볼 수 있듯이 Jacobian이 sparse하기 때문에 information matrix인 $\mathbf{H}$도 sparse해 짐을 알 수 있다. 식으로 표현하면 다음과 같다.</p>

\[\begin{aligned}
\mathbf{b}_{ij}^T &amp;=  \mathbf{e}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{i}\\
&amp;=  \mathbf{e}_{ij}^T \mathbf{\Omega}_{ij} \begin{pmatrix} 0 &amp; \cdots &amp; \mathbf{A}_{ij} &amp; \cdots &amp; \mathbf{B}_{ij} &amp; \cdots &amp; 0
\end{pmatrix}\\
&amp;=   \begin{pmatrix} 0 &amp; \cdots &amp; \mathbf{e}_{ij}^T \mathbf{\Omega}_{ij}\mathbf{A}_{ij} &amp; \cdots &amp; \mathbf{e}_{ij}^T \mathbf{\Omega}_{ij}\mathbf{B}_{ij} &amp; \cdots &amp; 0
\end{pmatrix}\\
\end{aligned}\]

\[\mathbf{b} = \sum_{ij} \mathbf{b}_{ij}^T\]

\[\begin{aligned}
\mathbf{H}_{ij} &amp;= \mathbf{J}_{ij}^T  \mathbf{\Omega}_{ij} \mathbf{J}_{ij}\\
&amp;= \begin{pmatrix} 0 &amp; \cdots &amp; \mathbf{A}_{ij} &amp; \cdots &amp; \mathbf{B}_{ij} &amp; \cdots &amp; 0
\end{pmatrix}^T \mathbf{\Omega}_{ij} \begin{pmatrix} 0 &amp; \cdots &amp; \mathbf{A}_{ij} &amp; \cdots &amp; \mathbf{B}_{ij} &amp; \cdots &amp; 0
\end{pmatrix}\\
&amp;=
\begin{pmatrix}
0 &amp; \cdots &amp; \cdots &amp; \cdots &amp; \cdots &amp;\cdots &amp; 0\\
0 &amp; \cdots &amp; \mathbf{A}_{ij}^T \mathbf{\Omega}_{ij}\mathbf{A}_{ij} &amp; \cdots &amp;  \mathbf{A}_{ij}^T \mathbf{\Omega}_{ij}\mathbf{B}_{ij} &amp; \cdots &amp; 0\\
0 &amp; \cdots &amp; \cdots &amp; \cdots &amp; \cdots &amp;\cdots &amp; 0\\
0 &amp; \cdots &amp;  \mathbf{B}_{ij}^T \mathbf{\Omega}_{ij}\mathbf{A}_{ij} &amp; \cdots &amp; \mathbf{B}_{ij}^T \mathbf{\Omega}_{ij}\mathbf{B}_{ij} &amp; \cdots &amp; 0\\
 0 &amp; \cdots &amp; \cdots &amp; \cdots &amp; \cdots &amp;\cdots &amp; 0
\end{pmatrix}
\end{aligned}\]

\[\mathbf{H} = \sum_{ij} \mathbf{H}_{ij}\]

<p>위에서 설명한 과정을 통해서 최적의 $\mathbf{x}$를 계산하는 과정을 정리하면 다음과 같다.</p>

<p><img src="https://i.imgur.com/lk85aEJ.png" alt="" /></p>

<p>위에서 설명한 과정을 통해 모든 measurement에 대한 information matrix($\mathbf{H}$)와 $\mathbf{b}$를 계산하고, 두 값을 이용하여 state의 변화량인 $\triangle \mathbf{x}$를 계산한다($\triangle \mathbf{x} = -\mathbf{H}^{-1}\mathbf{b}$). $\mathbf{H}$는 크기가 큰 matrix이므로 inverse과정이 높은 계산량을 요구하므로, 이를 쉽게 계산하기 위해서 Cholesky factorization 방법을 사용한다. 이제 계산된 변화량을 이용하여 state를 업데이트하고($\mathbf{x} = \mathbf{x}+\triangle \mathbf{x}$) state가 수렴할 때 까지 반복하여 최적화된 state를 계산한다.</p>

<h1 id="example-of-pose-graph">Example of Pose Graph</h1>

<p>이해를 돕기위해 graph의 optimization과정을 통한 state update과정의 예를 통해 설명할 것이다.</p>

<p><img src="https://i.imgur.com/XNvLNTI.png" alt="" /></p>

<p>위 그림과 같이 현재 2개의 state가 있으며, 센서를 통해 측정한 두 node 사이의 odometry정보는 1이다. 처음에는 모든 state의 값을 모르기 때문에 모두 0으로 설정한다.</p>

\[\begin{aligned}
\mathbf{x} &amp;= \begin{pmatrix} 0 &amp; 0 \end{pmatrix}^T\\
\mathbf{z}_{12} &amp;= 1\\
\mathbf{\Omega} &amp;= 2\\
\mathbf{e}_{12} &amp;= \mathbf{z}_{12} -(x_2 - x_1) = 1-(0-0) = 1\\
\mathbf{J}_{12} &amp;= \begin{pmatrix}1 -1\end{pmatrix}^T\\
\mathbf{b}_{12} &amp;= \mathbf{e}_{12}^T \mathbf{\Omega}_{12} \mathbf{J}_{12} = \begin{pmatrix} 2 &amp; -2 \end{pmatrix}\\
\mathbf{H}_{12} &amp;= \mathbf{J}_{12}^T \mathbf{\Omega} \mathbf{J}_{12} = \begin{pmatrix} 2 &amp; -2 \\ -2 &amp; 2\end{pmatrix}\\
\triangle \mathbf{x} &amp;= -\mathbf{H}_{12}^{-1} b_{12}
\end{aligned}\]

<p>센서 measurement의 information은 2로 설정하였다. state update를 위해 \(\mathbf{b}, \mathbf{H}\)를 계산하였다. 이때 \(\triangle\mathbf{x}\)를 계산하기 위해서는 \(\mathbf{H}\)의 inverse를 계산해야 하지만 현재 \(det(\mathbf{H})\)는 0이므로 계산할 수 없다. 이것은 모든 node의 uncertainty가 같아 graph가 floating상태이기 때문이다. 이를 해결하기 위해서 첫번째 node를 고정(fix)시켜 줘야한다. 이를 위해 첫번째 node의 uncertainty를 낮춰준다(즉, information을 더해서 크게 해준다).</p>

\[\begin{aligned}
\mathbf{H} &amp;= \begin{pmatrix} 2 &amp; -2 \\ -2 &amp; 2 \end{pmatrix} + \begin{pmatrix} 1 &amp; 0 \\ 0 &amp; 0 \end{pmatrix}\\
\triangle \mathbf{x} &amp;= -\mathbf{H}^{-1} b_{12}\\
\triangle \mathbf{x} &amp;= \begin{pmatrix} 0 &amp; 1 \end{pmatrix}^T
\end{aligned}\]

<p>따라서 error를 최소화 하는 state의 변화량은 $\begin{pmatrix} 0 &amp; 1 \end{pmatrix}^T$가 되며 state를 업데이트하면 최종적으로 $\mathbf{x} = \mathbf{x} + \triangle \mathbf{x} = \begin{pmatrix} 0 &amp; 1 \end{pmatrix}^T$가 된다.</p>

<h1 id="hierarchical-approach-to-least-square-slam">Hierarchical Approach to Least Square SLAM</h1>

<p>Hierarchical Approach는 Graph SLAM의 속도를 향상시키기 위한 한가지 방법이다.</p>

<p><img src="https://i.imgur.com/yFPTQZZ.png" alt="" /></p>

<p>맨 왼쪽 그림은 원래의 pose graph를 보여주고 있다. Hierarchical Approach는 이름에서 알 수 있듯이 여러개의 계층구조로 구성되어 있으며 아래의 layer일수록 많은 node를, 위로 갈수록 적은 수의 node를 갖는 graph로 구성되어 있다. Optimization 문제는 node의 갯수가 많을 수록 계산시간이 오래 걸리기 때문에 이러한 계층구조를 이용하여, 적은 수의 node의 graph에서(sampling된 node로 이루어진 graph) 최적의 node위치를 계산 후, 계산된 node를 기준으로 주변 node의 위치를 다시 계산하는 방법을 이용한다. Hierarchical Approach에 대해서는 자세히 언급하지 않겠다. 이 부분에 대해서 자세히 알고 싶으면 <a href="http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/">Robot Mapping</a> 강의 16강을 참고하기 바란다.</p>

<p><strong>본 글을 참조하실 때에는 출처 명시 부탁드립니다.</strong></p>]]></content><author><name>JinyongJeong</name></author><category term="SLAM" /><category term="SLAM" /><summary type="html"><![CDATA[본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었다.]]></summary></entry><entry><title type="html">[SLAM] Graph Based SLAM with Landmark</title><link href="/slam/2024/01/13/graph_based_slam_landmark.html" rel="alternate" type="text/html" title="[SLAM] Graph Based SLAM with Landmark" /><published>2024-01-13T00:00:00+00:00</published><updated>2024-01-13T00:00:00+00:00</updated><id>/slam/2024/01/13/graph_based_slam_landmark</id><content type="html" xml:base="/slam/2024/01/13/graph_based_slam_landmark.html"><![CDATA[<p><strong>본 글은 University Freiburg의 <a href="http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/">Robot Mapping</a> 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었다.</strong></p>

<p>graph-based SLAM 포스트에서는 landmark가 없는 환경에서 로봇의 위치 간의 관계 만을 이용하여 graph를 최적화 시키는 방법에 대해서 설명하였다. 이번 글에서는 landmark가 있는 환경에서 이 landmark들을 이용하여 로봇 간의 위치 정보를 알 수 있을 때 graph-based SLAM이 어떻게 달라지는지 설명할 것이다.</p>

<h1 id="graph-based-slam-with-landmark">Graph-based SLAM with Landmark</h1>

<p>아래 그림은 Global 좌표계에서 로봇의 위치와 landmark의 위치를 보여주는 그림이다.</p>

<p><img src="https://i.imgur.com/SwMVzwi.png" alt="" /></p>

<p>Landmark가 존재하는 실제 환경은 아래와 같다. 아래 그림과 같은 공원의 경우 공원에 있는 나무들이 landmark가 되고 나무들의 위치정보를 이용하여 SLAM을 적용한다.</p>

<p><img src="https://i.imgur.com/5jUj86m.jpg" alt="" /></p>

<p>로봇의 위치와 landmark와의 관계를 도식적으로 그려보면 다음과 같다.</p>

<p><img src="https://i.imgur.com/g95XjR8.png" alt="" /></p>

<p>이제 graph에서 node는 로봇의 위치뿐만 아니라 landmark의 위치도 포함하게 된다. 이때 landmark는 방향없이 x,y좌표로만 구성된다.</p>

<ul>
  <li>Node
    <ul>
      <li>로봇의 위치</li>
      <li>landmark의 위치</li>
    </ul>
  </li>
  <li>Edge
    <ul>
      <li>로봇의 Odometry measurement</li>
      <li>로봇의 Landmark observation</li>
    </ul>
  </li>
</ul>

<p>위의 정보들로 구성된 graph의 error를 최소화 하는 landmark와 로봇의 위치를 계산함으로써 로봇의 위치를 구할 수 있다. Landmark가 없는 환경에서 “virtual measurement”라는 것을 언급했었다. “virtual measurement”는 non-successive한 로봇의 위치에서 얻어진 센서 데이터를 이용하여 계산된 두 로봇간의 상대 위치(relative pose)를 의미한다. 이렇게 계산된 “virtual measurement”는 non-successive한 노드 사이의 constraint가 된다. Landmark가 있는 환경에서는 위 그림과 같이 non-succesive한 로봇간의 direct한 edge는 발생하지 않으며, landmark를 통해서만 연결된다.</p>

<h2 id="rank-of-information-matrix">Rank of Information Matrix</h2>

<p>Landmark를 통한 graph SLAM의 경우 센서 특성에 따른 information matrix의 rank를 살펴보아야 한다. 여기서는 <em>bearing only observation</em> 센서모델로 로봇의 위치에서 landmark까지의 각도만을 측정할 수 있는 센서의 경우를 생각한다.</p>

<p>Bearing observation의 경우 observation function은 다음과 같다.</p>

<p><img src="https://i.imgur.com/RT8AyZw.png" alt="" /></p>

<p>이 observation function을 이용한 error term은 다음과 같다.</p>

<p><img src="https://i.imgur.com/aaPO0Zy.png" alt="" /></p>

<p>위 error term으로 구성되는 information matrix의 rank는 어떻게 될까?</p>

\[\mathbf{H}_{ij} = \mathbf{J}_{ij}^T \mathbf{\Omega}_{ij} \mathbf{J}_{ij}\]

<p>위 식에서 $\mathbf{J}<em>{ij}$는 error term을 편미분한 행렬인데, bearing only 센서의 경우 error term의 dimension은 1이므로 Jacobian의 rank는 1이다. 따라서 $\mathbf{H}</em>{ij}$의 rank도 1이 된다.</p>

<p><img src="https://i.imgur.com/KiVL5Zc.png" alt="" /></p>

<p>rank가 1이라는 것은 무엇을 의미할까? rank가 1이라는 것은 위 그림과 같이 어떠한 landmark가 있을 때 로봇은 x-y plane에 어디든 존재할 수 있으며 단지 그 x-y좌표에 해당하는 heading 각도만 한정된다는 의미이다. 따라서 bearing only sensor의 경우 로봇의 위치를 정확히 알기 위해서는 3개 이상의 observation이 필요하다. 이러한 시스템을 “under-determined” 시스템이라고 부른다.</p>

<p>따라서 Information matrix의 rank는 로봇의 위치 중(x,y,heading) 몇가지의 정보를 한정할 수 있는지를 의미하며, 로봇의 위치에 대한 unique solusion을 계산하기 위해서는 full lank가 되어야 한다.</p>

<h2 id="under-determined-system">Under-determined System</h2>

<p>이러한 under-determined system, 즉 information matrix의 rank가 full lank가 아닌 경우 이러한 상황을 해결하기 위한 방법이 “Levenberge Marquardt” method 이다. 즉 “damping factor”를 더함으로써 full rank의 matrix를 만들어 unique한 해를 찾는다. 원래의 식은 아래와 같다.</p>

\[\mathbf{H} \triangle \mathbf{x} = -\mathbf{b}\]

<p>damping factor가 추가된 식은 아래와 같다.</p>

\[(\mathbf{H} + \lambda \mathbf{I})\triangle \mathbf{x} = -\mathbf{b}\]

<p>damping factor는 error의 증감에 따라 크기를 변화시킨다. 전체적인 알고리즘은 다음과 같다.</p>

<p><img src="https://i.imgur.com/DXCNCh9.png" alt="" /></p>

<h2 id="정리">정리</h2>

<p>이 글에서는 Landmark가 있는 환경에서의 Graph-based SLAM에 대해서 알아보았다. Landmark가 없는 환경에서는 센서 데이터를 이용하여 non-successive node간의 edge정보를 계산하였다. 이러한 edge는 node간의 직접적인 연결이다. 반면, landmark가 있는 환경에서는 각 node에서 바라보는 landmark의 위치가 observation 정보가 되며, non-successive한 node간의 연결은 landmark를 통해서 이루어진다. 이때 센서의 종류에 따라 information matrix의 rank가 결정되고 under-determined system이 되므로 이를 풀기위한 기법이 필요하게 된다(LM method).</p>

<p><strong>본 글을 참조하실 때에는 출처 명시 부탁드립니다.</strong></p>]]></content><author><name>JinyongJeong</name></author><category term="SLAM" /><category term="SLAM" /><summary type="html"><![CDATA[본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었다.]]></summary></entry><entry><title type="html">[SLAM] Robust Graph SLAM</title><link href="/slam/2024/01/13/robust_graph_slam.html" rel="alternate" type="text/html" title="[SLAM] Robust Graph SLAM" /><published>2024-01-13T00:00:00+00:00</published><updated>2024-01-13T00:00:00+00:00</updated><id>/slam/2024/01/13/robust_graph_slam</id><content type="html" xml:base="/slam/2024/01/13/robust_graph_slam.html"><![CDATA[<p><strong>본 글은 University Freiburg의 <a href="http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/">Robot Mapping</a> 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었다.</strong></p>

<p>이번 글에서는 이전에 설명했던 Graph based SLAM이 outlier(잘못된 정보)에 Robust하게 만드는 방법에 대해서 설명한다.</p>

<p>Graph-based SLAM은 least square방법을 사용하여 로봇과 landmark의 위치를 최적화 시킨다. 즉, 현재의 graph상의 로봇의 위치에서 얻어질 것으로 예상되는 측정값과 실제 측정값과의 차이를 최소화 시키는 방향으로 graph가 최적화 된다.</p>

<p>만약 그래프가 최적화 되는 과정에서 아래 그림과 같이 잘못된 연결(edge)이 생성되면 어떻게 될까?</p>

<p><img src="https://i.imgur.com/AEgKATh.png" alt="" /></p>

<p>위 그림과 같이 전혀 다른 두 위치를 같은 위치로 인식하여 edge를 발생시켰을 경우 아래와 같이 그래프 전체에 왜곡이 발생하게 된다.</p>

<p><img src="https://i.imgur.com/rCFUHBM.png" alt="" /></p>

<p>그리고 이렇게 잘못 발생된 edge가 많아질 수록 왜곡은 심해지게 된다. 이러한 문제는 다음과 같은 상황에서 주로 발생한다.</p>

<ol>
  <li>특징이 없는 환경(아무런 특징이 없는 복도 등)</li>
  <li>같은 빌딩내의 비슷한 환경의 방</li>
  <li>GPS의 multi-path(GPS 신호가 다른 건물에 반사된 후 수신되어 오차가 발생하는 현상)</li>
</ol>

<p>따라서 실제 graph-based SLAM을 수행할 때 위의 상황 뿐만아니라 다양한 상황에서 왜곡이 발생하게 된다. 이러한 잘못된 정보를 outlier라고 하며, 이러한 outlier에 덜 영향을 받는 최적화 방법이 필요하다. 이번 글에서는 최적화 과정을 Robust하게 만드는 방법에 대해서 설명하도록 한다.</p>

<h1 id="robust-m-estimator">Robust M-estimator</h1>

<p>이전 글인 Least Square관련 글에서는 noise를 Gaussina 분포로 가정하고, least square방법으로 최적해를 구하는 방법에 대해서 설명하였다. 또한 least square와 Gaussian 분포의 관계에 대해서도 언급하였었다. Least square방법은 M-estimator의 한 종류이며, Gaussian noise를 가정한 model이다. M-estimator는 noise의 형태를 Gaussian으로 가정하지 않는다. M-estimator의 PDF(Probability density function)은 아래와 같이 정의된다.</p>

\[p(e)= exp(-\rho(e))\]

<p>그렇다면 least square에서 최적해를 계산하는 것과 같이 negative log likelihood form으로 표기하면 PDF의 최대값을 찾는 문제는 log likelihood의 최소값을 찾는 문제가 된다.</p>

\[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_i \rho(e_i(\mathbf{x}))\]

<p>즉 위의 log likelihood식에서 $\rho(e)$가 $\rho(e) = e^2$ 처럼 제곱의 형태인 것이 Least square문제가 되는 것이다. 그렇다면 $\rho$ fucntion에는 어떤 종류들이 있는지 살펴보도록 하자.</p>

<ul>
  <li>
    <p>Gaussian: $\rho(e) = e^2$</p>
  </li>
  <li>
    <p>L1 norm: $\rho(e) = \mid e \mid$</p>
  </li>
  <li>
    <p>Huber M-estimator: $\rho(e) = \begin{cases} \frac{e^2}{2} &amp; if \mid e \mid &lt; c \ c(\mid e \mid -\frac{c}{2}) &amp; otherwise \end{cases}$</p>
  </li>
  <li>
    <p>Others: Tukey, Cauchy …</p>
  </li>
</ul>

<p><img src="https://i.imgur.com/oduSDck.png" alt="" /></p>

<p>위 그림은 각 함수의 형태를 보여준다. 최적화를 하기 위한 cost function의 형태에 변화를 줌으로써 outlier에 강인한 특성을 주기 위한 노력들이다. 이렇게 cost function의 형태를 바꿈으로써 outlier에 Robust한 특성을 어떻게 갖게 되는 것인지 살펴보자. 최적화는 error들의 합을 최소화 하는 방향으로 입력을 변화시키며 해를 찾아가는 방법이다. 이러한 과정에서 error가 매우 큰(Outlier라고 생각 할 수 있는) term들이 최적화 과정에서 큰 weight로 작용하게 된다(영향을 크게 준다). 따라서 위의 함수들은 ourlier라고 생각 할 수 있는, 즉 error가 매우 큰 term에 대해서 weight를 다소 줄이는 방향으로 cost function의 형태를 구성한 것이다. 다양한 형태의 function중에서 가장 많이 사용되는 형태는 Huber M-estimator이다.</p>

<p>앞으로 설명할 Max-mixture와 dynamic covariance scaling 방법도 m-estimator와 비슷한 개념을 이용하여 최적화의 robust함을 높이려는 방법이다.</p>

<h1 id="max-mixture">Max-mixture</h1>

<p>Max-mixture는 M-estimator와 마찬가지로 graph-based optimization을 outlier에 robust하게 만들기 위한 방법중에 하나이다. Max-mixture는 이름에서 알 수 있듯이 여러개의 Gaussian 분포를 합하는 과정에서 수학적인 이점을 얻기 위해서 각 Gaussian 분포의 최대값을 이용한다. 왜 이러한 방법을 사용하는지 아래 수식을 보도록 하자.</p>

<p>세상에 존재하는 다앙햔 분포를 1개의 Gaussian만을 이용하여 표현하기엔 부족한 경우가 많이 발생한다. 따라서 다양한 분포를 표현하기 위해서 여러개의 Gaussian을 더하는 형태로 분포를 표현할 수 있다.</p>

\[p(\mathbf{z}\mid\mathbf{x}) = \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\]

<p>위의 분포를 최적화 시키기 위한 cost function을 얻기 위해 negative log likelihood을 취하면 다음과 같은 형태가 된다.</p>

\[-log p(\mathbf{z}\mid\mathbf{x}) = -log \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\]

<p>1개의 Gaussian분포일 경우에는 sum($\sum$)이 없기 떄문에 log와 exponential이 계산되어 우리가 앞에서 계산한 cost function의 형태로 계산되어 최적화 과정을 진행할 수 있게 된다. 하지만 Gaussian mixture 모델의 경우 Sum의 형태이기 때문에 log가 sum의 안으로 들어갈 수 없다. 따라서 mixture 모델을 최적화 시키기 위해서 최대값을 이용하는 max-mixture approximation 방법을 이용한다.</p>

\[\begin{aligned}
p(\mathbf{z}\mid\mathbf{x}) &amp;= \sum_k w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\\
&amp;\approx max_k \ \ w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k
\end{aligned}\]

<p>즉 위의 근사화과정을 그림으로 표현하면 아래와 같다.</p>

<p><img src="https://i.imgur.com/jNof4Bt.png" alt="" /></p>

<p>즉 두개의 분포의 값을 더하는 것 대신 최대값만을 추출하여 분포를 표현하는 것이다. 이는 두 분포의 평균값이 어느정도 거리로 떨어져 있을 경우에는 오차가 작지만 두 분포의 평균값이 매우 가까운 경우에는 큰 오차를 발생시킨다. 근사화된 식의 negative log likelihood는 다음과 같다.</p>

\[\begin{aligned}
-log p(\mathbf{z}\mid\mathbf{x}) &amp;= -log \ \ max_k \ \ w_k \eta_k exp(-\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k\\
&amp;= min_k (\frac{1}{2}\mathbf{e}_{ij}^T \Omega_{ij}\mathbf{e}_{ij})_k - log(w_k \eta_k)
\end{aligned}\]

<p>즉 max-mixture는 Gaussian 분포의 max값을 취하는 형태로 근사화 함으로써 우리가 풀 수 있는 최적화의 Cost function을 구할 수 있게 되었다.</p>

<p><img src="https://i.imgur.com/U47m0jD.png" alt="" /></p>

<p>위 그림에서 빨간색 그래프는 같은 mean값을 갖는(다른 variance) 두 gaussian의 합을 max-mixture방법을 통해 얻은 분포의 cost-function을 보여준다. 앞에서 설명한 M-estimator의 cost function들과 유사함을 알 수 있다. 따라서 여러개의 Gaussian 분포를 더함으로써 graph optimization과정을 Robust하게 만들 수 있다.</p>

<h1 id="dynamic-covariance-scalingdcs">Dynamic Covariance Scaling(DCS)</h1>

<p>Graph의 최적화를 Robust하게 만드는 다른 방법은 DCS(Dynamic Covariance Scaling)방법이다. DCS는 각 Error값에 해당하는 information matrix의 크기를 조절함으로써 robust하게 만든다. 원래 graph optimization식은 다음과 같다.</p>

\[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}(\mathbf{x})^T \Omega_{ij}\mathbf{e}_{ij}(\mathbf{x})\]

<p>DCS는 cost function의 information matrix의 크기를 조절하는 scaling factor($s_{ij}^2$)를 추가한다.</p>

\[\mathbf{x}^* = argmin_{\mathbf{x}} \sum_{ij} \mathbf{e}_{ij}(\mathbf{x})^T (s_{ij}^2 \Omega_{ij})\mathbf{e}_{ij}(\mathbf{x})\]

<p>scaling factor인 $s_{ij}^2$는 다음과 같이 정의된다.</p>

\[s_{ij} = min(1, \frac{2\Phi}{\Phi+\chi_{ij}^2})\]

<p>즉 두개의 파라미터 $\Phi$와 $\chi$에 의해 scaling factor가 조절되며, 이 두 파라미터에 의해서 cost function이 변하게 된다.</p>

<p><img src="https://i.imgur.com/VoWGHnA.png" alt="" /></p>

<p>위 그림은 DCS의 원래 cost function(검정색)과 scaling factor(파란색)그리고 최종 scaling이 된 cost function(빨간색)을 보여준다. Error가 적은 영역에서 scaling factor는 1이지만 error가 큰 영역으로 갈 수록 scaling factor가 작아지며, error가 큰 영역의 함수 출력값을 줄여준다. 이는 m-estimator가 error가 큰 영역의 값을 줄이는 것과 비슷한 효과로 볼 수 있다. 원래 DCS가 이러한 개념을 처음 사용한 것은 아니다. DCS가 나오기 전인 2012년 IROS에 error가 큰 term의 영향을 없애는 “Switchable Constraint for Robust Pose Graph SLAM”이 발표되었다. 이는 획기적으로 graph 최적화를 robust하게 만들었으나 과정이 매우 복잡한 단점이 있었다. 그 이후에 “Switchable Constraint” 논문을 위와같이 close form으로 정리하여 ICRA 2013년도에 발표한 논문이 “Robust Map Optimization using Dynamic Covariance Scaling”이다.</p>

<p>이 글에서 DCS에 대해서 너무 깊이 논하지는 않을 것이다. 조금 더 자세히 내용이 필요한 경우 위 논문을 찾아보기 바란다.</p>

<h1 id="정리">정리</h1>

<p>이번 글에서는 graph-based SLAM을 outlier에 robust하게 만드는 방법에 대해서 살펴보았다. 이러한 목적으로 여러가지 방법이 사용되고 있으나 결과적으로는 최적화의 cost function에서 error가 큰 영역의 출력값을 줄여주는 방법이라는 점에서는 유사함을 알 수 있다.</p>

<p><strong>본 글을 참조하실 때에는 출처 명시 부탁드립니다.</strong></p>]]></content><author><name>JinyongJeong</name></author><category term="SLAM" /><category term="SLAM" /><summary type="html"><![CDATA[본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었다.]]></summary></entry><entry><title type="html">[SLAM] 최소자승법 (Least Square)</title><link href="/slam/2024/01/12/least_square.html" rel="alternate" type="text/html" title="[SLAM] 최소자승법 (Least Square)" /><published>2024-01-12T00:00:00+00:00</published><updated>2024-01-12T00:00:00+00:00</updated><id>/slam/2024/01/12/least_square</id><content type="html" xml:base="/slam/2024/01/12/least_square.html"><![CDATA[<p><strong>본 글은 University Freiburg의 <a href="http://ais.informatik.uni-freiburg.de/teaching/ws13/mapping/">Robot Mapping</a> 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었다.</strong></p>

<p>SLAM은 로봇의 위치를 추정함과 동시에 주변 환경에 대한 맵도 같이 생성하는 기술이다. SLAM의 방법은 크게 3가지로 나눌 수 있다.</p>

<ul>
  <li>Kalman Filter</li>
  <li>Particle Filter</li>
  <li>Graph-based</li>
</ul>

<p>이번 글에서 부터는 Graph-based SLAM에 대해서 설명할 것이며, 이 글에서는 Graph-based SLAM의 접근 방법인 Least square에 대해서 설명한다.</p>

<h1 id="least-square">Least Square</h1>

<ul>
  <li>
    <p>Least square는 “overdetermined system”의 해를 구하기 위한 방법이다. overdetermined system이란 미지수의 갯수보다 식의 수가 더 많기 때문에, 모든 식을 만족하는 해가 존재하지 않는 시스템을 말한다.</p>
  </li>
  <li>
    <p>Least square는 에러의 제곱합(sum of the squared error)을 최소화 하는 방법으로 해를 구한다.</p>
  </li>
</ul>

<h1 id="problem-of-least-square">Problem of Least Square</h1>

<ul>
  <li>
    <p>$\mathbf{x}$는 state vector</p>
  </li>
  <li>
    <p>$\mathbf{z}_i$는 실제 measurement(실제 센서 측정값)</p>
  </li>
  <li>
    <p>$\hat{\mathbf{z}}_i=f_i(x)$는 현재 state vector $\mathbf{x}$에서 예상되는 measurement값</p>
  </li>
  <li>
    <p>목표: 센서로부터 실제 얻어진 measurement($\mathbf{z}_i$)에 가장 적합한 $\mathbf{x}$ 추정</p>
  </li>
</ul>

<p><img src="https://i.imgur.com/ODdhtKd.png" alt="" /></p>

<p>위 그림은 least square를 그림으로 표현하였다. Least square는 센서로부터 측정된 measurement에 가장 적합한 로봇의 state를 계산하는 방법이다. Sensor observation model처럼 로봇의 상태를 알고 있을 때 예상되는 measurement를 예상할 수 있고($\mathbf{\hat{z}}_i$) 실제 measurement와 예상되는 measurement와의 차이를 최소화 함으로써 최적화된 로봇의 state를 계산할 수 있다.</p>

<h1 id="error-function">Error Function</h1>

<p>Error($\mathbf{e}_i$)는 실제 measurement와 예상되는 measurement와의 차이를 나타내며, measurement와 같은 dimension을 갖는 vector이다.</p>

\[\mathbf{e}_i(\mathbf{x}) = \mathbf{z}_i - f_i(\mathbf{x})\]

<p>위 식에서 measurement의 noise는 평균이 0이며, Gaussian 분포를 따른다고 가정한다.</p>

<p>Information matrix($\Omega_i$)는 measurement의 covariance matrix의 inverse이며, measurement($\mathbf{z}_i$)와 dimension의 크기가 같은 matrix이다.</p>

<p>squared error($e_i(\mathbf{x})$)는 아래와 같으며, vector가 아닌 scalar값을 갖는다.</p>

\[e_i(\mathbf{x}) = \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x})\]

<h1 id="find-minimum-of-error-function">Find Minimum of Error Function</h1>

<p>로봇의 최적화된 state를 계산하는 문제는, 주어진 모든 센서 measurement를 이용한 error function의 합이 최소화 되는 state를 찾는 문제이다. 식으로 표현하면 다음과 같다.</p>

\[\begin{aligned}
x* &amp;= \text{argmin}_{\mathbf{x}} F(\mathbf{x})\\
&amp;=\text{argmin}_{\mathbf{x}} \sum_i e_i(\mathbf{x})\\
&amp;=\text{argmin}_{\mathbf{x}} \sum_i \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x})
\end{aligned}\]

<p>위 식에서 각 element들에 대해서는 error function에서 설명했다. 이 식을 보고 넘어갈 때 각 항들의 dimension이 어떻게 되는지 확인하고 넘어가도록 하자. 만약 센서 measurement의 dimension이 n일 때 $\mathbf{e}_i$는 $n\times1$, $\mathbf{\Omega}_i$는 $n\times n$의 dimension을 갖는다.</p>

<p>위와 같은 optimization 문제는 복잡하며, closed form(미지수의 해가 수식으로 정리되는 형태)이 아니기 때문에 수학적인 접근방법으로 해를 구한다.</p>

<p>Optimization 방법으로 최적의 로봇 state($\mathbf{x}^*$)를 구하는 순서는 다음과 같다.</p>

<ol>
  <li>현재의 state를 기준으로 error term($\mathbf{e(x)}$)의 선형화</li>
  <li>선형화된 error term으로 계산되는 squared error function($e_i(\mathbf{x})$)의 1차 미분계산</li>
  <li>squared error function은 quadratic form이므로 1차 미분값이 0이 되는 점이 최소값이므로 미분값이 0인 solution을 계산</li>
  <li>계산된 solution을 이용하여 state를 update</li>
  <li>위의 과정을 수렴할 때까지 반복</li>
</ol>

<p>위의 과정을 통해 iterative하게 optimal한 state를 계산할 수 있다. 각 단계의 과정을 자세히 살펴보도록 하자.</p>

<h2 id="1-error-term의-선형화">1. Error Term의 선형화</h2>

<p>vector의 형태를 갖는 error term은 현재 measurement와 예상되는 measurement의 차이이다.</p>

\[\mathbf{e}_i(\mathbf{x}) = \mathbf{z}_i - f_i(\mathbf{x})\]

<p>이때 $f_i(\mathbf{x})$는 observation model로 많은 경우에 비선형형태를 갖는다. observation model에 대해 익숙하지 않다면 Motion Model과 Observation Model에 대한 이전 글을 참고하기 바란다.  따라서 식을 풀기 위해서는 현재 state기준으로 선형화를 수행한다. 선형화 방법으로는 EKF와 마찬가지로 Tayor expansion방법을 사용한다.</p>

\[\begin{aligned}
\mathbf{e}_i(\mathbf{x}+\triangle\mathbf{x}) &amp;\approx \mathbf{e}_i(\mathbf{x}) + \mathbf{J}_i(\mathbf{x})((\mathbf{x}+\triangle \mathbf{x})-\mathbf{x})\\
&amp;= \mathbf{e}_i(\mathbf{x}) + \mathbf{J}_i(\mathbf{x})\triangle \mathbf{x}
\end{aligned}\]

<p>위 식에서 $\mathbf{x}$는 initialize의 기준이 되는 현재 state를 의미하며 $\mathbf{x} = (\mathbf{x}_1, \mathbf{x}_2, \cdots, \mathbf{x}_n)$이다. 이때 Jacobian $\mathbf{J}_i$는 다음과 같다.</p>

\[\mathbf{J}_i(x) = \begin{pmatrix} \frac{\partial f_1(x)}{\partial x_1} &amp; \frac{\partial f_1(x)}{\partial x_2} &amp; \cdots &amp; \frac{\partial f_1(x)}{\partial x_n} \\
\frac{\partial f_2(x)}{\partial x_1} &amp; \frac{\partial f_2(x)}{\partial x_2} &amp; \cdots &amp; \frac{\partial f_2(x)}{\partial x_n} \\
\cdots &amp; \cdots &amp; \cdots &amp; \cdots \\
\frac{\partial f_m(x)}{\partial x_1} &amp; \frac{\partial f_m(x)}{\partial x_2} &amp; \cdots &amp; \frac{\partial f_m(x)}{\partial x_n}
\end{pmatrix}\]

<h2 id="2-squared-error-계산">2. Squared Error 계산</h2>

<p>이제 위에서 선형화한 error term을 이용하여 squared error를 계산해보자.</p>

\[\begin{aligned}
e_i(\mathbf{x}+\triangle\mathbf{x}) &amp; =
\mathbf{e}_i^T(\mathbf{x}+\triangle\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x}+\triangle\mathbf{x})\\
&amp;\approx (\mathbf{e}_i(\mathbf{x}) + \mathbf{J}_i(\mathbf{x})\triangle \mathbf{x})^T \mathbf{\Omega}_i (\mathbf{e}_i(\mathbf{x}) + \mathbf{J}_i(\mathbf{x})\triangle \mathbf{x})\\
&amp;= \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{e}_i + \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{J}_i \triangle \mathbf{x} +  \triangle \mathbf{x}^T \mathbf{J}_i^T  \mathbf{\Omega}_i \mathbf{e}_i + \triangle \mathbf{x}^T \mathbf{J}_i^T  \mathbf{\Omega}_i \mathbf{J}_i \triangle \mathbf{x}\\
&amp;= \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{e}_i + 2 \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{J}_i \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{J}_i^T  \mathbf{\Omega}_i \mathbf{J}_i \triangle \mathbf{x}\\
&amp;= c_i + 2 \mathbf{b}_i^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H}_i \triangle \mathbf{x}
\end{aligned}\]

<p>계산된 squared error는 위와 같으며, 마지막 식에서 $c_i, \mathbf{b}_i, \mathbf{H}_i$는 다음과 같다.</p>

\[\begin{aligned}
c_i &amp;= \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{e}_i\\
\mathbf{b}_i^T &amp;= \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{J}_i\\
\mathbf{H}_i &amp;= \mathbf{J}_i^T  \mathbf{\Omega}_i \mathbf{J}_i
\end{aligned}\]

<p>$\mathbf{H}_i$는 선형화 후의 information matrix이다.</p>

<p>위에서 계산된 $e_i(\mathbf{x}+\triangle\mathbf{x})$는 1개의 measurement에 대한 error function이므로 모든 measurement에 대한 error function은 다음과 같다.</p>

\[\begin{aligned}
F(\mathbf{x}+\triangle \mathbf{x}) &amp;\approx \sum_i (c_i + 2 \mathbf{b}_i^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H}_i)\triangle \mathbf{x}\\
&amp;= \sum_i c_i + 2 (\sum_i \mathbf{b}_i^T) \triangle \mathbf{x} + \triangle \mathbf{x}^T (\sum_i \mathbf{H}_i) \triangle \mathbf{x}\\
&amp;= c + 2 \mathbf{b}^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H} \triangle \mathbf{x}
\end{aligned}\]

\[\begin{aligned}
\mathbf{b}^T &amp;= \sum_i \mathbf{e}_i^T \mathbf{\Omega}_i \mathbf{J}_i\\
\mathbf{H} &amp;= \sum_i \mathbf{J}_i^T  \mathbf{\Omega}_i \mathbf{J}_i
\end{aligned}\]

<p>그렇다면 이제 squared error function의 마지막 식을 살펴보자.</p>

\[\begin{aligned}
F(\mathbf{x}+\triangle \mathbf{x}) &amp;\approx
c + 2 \mathbf{b}^T \triangle \mathbf{x} + \triangle \mathbf{x}^T \mathbf{H} \triangle \mathbf{x}
\end{aligned}\]

<p>위의 식은 $\triangle \mathbf{x}$에 대해서 quadratic한 형태를 띄고 있다. quadratic form은 일반 2차방정식의 형태로 생각할 수 있다. 즉, 함수의 극대 혹은 극소값은 함수의 미분이 0이 되는 지점을 계산함으로써 계산할 수 있다.</p>

<p>그럼 이제 위의 식의 1차 미분을 계산한다. 일반적으로 quadratic form의 미분은 다음과 같다(matrix cookbook, section 2.2.4참고).</p>

\[\begin{aligned}
f(x) &amp;= \mathbf{x}^T\mathbf{H}\mathbf{x} + \mathbf{b}^T\mathbf{x}\\
\frac{\partial f}{\partial x} &amp;= (\mathbf{H} + \mathbf{H}^T)\mathbf{x} +\mathbf{b}
\end{aligned}\]

<p>따라서 함수의 1차 미분은 다음과 같다.</p>

\[\frac{\partial F(\mathbf{x}+\triangle \mathbf{x})}{\partial \triangle \mathbf{x}} = 2 \mathbf{b} + 2 \mathbf{H}\triangle \mathbf{x}\]

<h2 id="3-global-error-function의-최소값-계산">3. Global Error Function의 최소값 계산</h2>

<p>2번단계에서 global error function의 1차 미분값을 계산하였다. global error function은 quadratic form이므로 미분값이 0인 지점이 함수의 극대 혹은 극소값이다. 우리가 계산하고 있는 least square의 목적은 error를 최소로 만드는 state를 계산하는 것이므로 계속해서 error function의 최소값을 찾아 나간다. 최소값을 찾기 위하여 미분값을 0으로 만드는 $\mathbf{x}$를 계산한다.</p>

\[\frac{\partial F(\mathbf{x}+\triangle \mathbf{x})}{\partial \triangle \mathbf{x}} = 2 \mathbf{b} + 2 \mathbf{H}\triangle \mathbf{x} = 0\]

<p>따라서 optimal한 state는 다음과 같다.</p>

\[\triangle \mathbf{x}^* = -\mathbf{H}^{-1}\mathbf{b}\]

<p>이때 $\mathbf{H}$는 크기가 큰 matrix이므로 inverse를 계산 시 많은 계산이 소요된다. 이러한 문제를 해결하기 위해서 Cholesky factorization, 혹은 QR decomposition과 같은 방법을 사용한다. Cholesky factorization은 matrix $\mathbf{H}$를 하 삼각행렬(lower triangular matrix)의 곱으로 표현하는 방법이다($\mathbf{H}= \mathbf{L}\mathbf{L}^T$). Triangular matrix는 일반 matrix에 비해 inverse의 계산이 간단하므로 이렇게 분해함으로써 inverse를 더 빠르게 계산할 수 있다.</p>

\[\begin{aligned}
\triangle \mathbf{x}^* &amp;= -\mathbf{H}^{-1}\mathbf{b}\\
&amp;= -\mathbf{L}^{-T}\mathbf{L}^{-1}\mathbf{b}
\end{aligned}\]

<h2 id="4-state-update">4. State Update</h2>

<p>3번 과정에서 global error function을 최소로 만드는 state인 $\triangle \mathbf{x}$를 계산하였다. 이제 이 결과를 이용하여 state를 update한다.</p>

\[\mathbf{x} = \mathbf{x} + \triangle \mathbf{x}\]

<p>업데이트 완료되면 업데이트된 state를 기준으로 다시 선형화를 하여 위 과정을 반복한다. 여러번 반복함으로써 계속 최적화된 state로 업데이트하고, state가 수렴하면 계산을 중지하고 optimization과정을 종료한다.</p>

<p>위와 같이 quadratic form을 계산함으로써 optimization 문제를 해결하는 방법을 <em>Gauss-Newtom optimization</em> 이라고 한다.</p>

<h1 id="least-square와-gaussian의-관계">Least Square와 Gaussian의 관계</h1>

<p>위에서 least square의 계산과정은 모두 설명하였다. Least square문제는 아래의 식을 품으로써 최적의 state를 계산하였다.</p>

\[\begin{aligned}
x*
&amp;=\text{argmin}_{\mathbf{x}} \sum_i \mathbf{e}_i^T(\mathbf{x}) \mathbf{\Omega}_i \mathbf{e}_i(\mathbf{x})
\end{aligned}\]

<p>하지만 왜 이런 형태의 식을 만들어서 풀게 되었을까? 이것은 Gaussian 분포와 관계가 있다. Bayes filter의 posterior의 식은 다음과 같다.</p>

\[p(x_{0:t} \mid z_{1:t}, u_{1:t}) = \eta p(x_0) \prod_t [p(x_t \mid x_{t-1}, u_t)p(z_t \mid x_t)]\]

<p>여기서 prior와, motion model 그리고 observation model은 모두 gaussian 분포를 따른다. 따라서 log likelihood를 계산하면 다음과 같다.</p>

\[log p(x_{0:t} \mid z_{1:t}, u_{1:t}) = \text{const.} +  log p(x_0) +  \sum_t [log p(x_t \mid x_{t-1}, u_t)+ log p(z_t \mid x_t)]\]

<p>여기서 모든 확률은 Gaussian 분포를 따르는데, Gaussian 분포의 log는 다음과 같다.</p>

\[log \mathcal{N}(x,\mu,\Sigma) = \text{const.} -\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu)\]

<p>계산된 Gaussian의 log형태가 매우 익숙하다. 위의 형태는 우리가 최적화에 사용하였던 함수와 같은 형태임을 알수 있다. 따라서 posterior를 error function으로 표현하면 다음과 같다.</p>

\[\begin{aligned}
log p(x_{0:t} \mid z_{1:t}, u_{1:t}) &amp;= \text{const.} +  log p(x_0) +  \sum_t [log p(x_t \mid x_{t-1}, u_t)+ log p(z_t \mid x_t)]\\
&amp;= \text{const.} - \frac{1}{2} e_p(\mathbf{x}) -\frac{1}{2} \sum_t [e_{u_t}(\mathbf{x})+e_{z_t}(\mathbf{x})]
\end{aligned}\]

<p>따라서, posterior의 최대값을 찾는 문제는 error function의 최소값을 찾는 문제가 된다.</p>

\[\begin{aligned}
argmax \ \ log p(x_{0:t} \mid z_{1:t}, u_{1:t})
&amp;= argmin \ \ e_p(\mathbf{x})+ \sum_t [e_{u_t}(\mathbf{x})+e_{z_t}(\mathbf{x})]
\end{aligned}\]

<p>이러한 Gaussian과의 관계에 의해서 위에서와 같이 error function이 정의됨을 기억하자.</p>

<p><strong>본 글을 참조하실 때에는 출처 명시 부탁드립니다.</strong></p>]]></content><author><name>JinyongJeong</name></author><category term="SLAM" /><category term="SLAM" /><summary type="html"><![CDATA[본 글은 University Freiburg의 Robot Mapping 강의를 바탕으로 이해하기 쉽도록 정리하려는 목적으로 작성되었다.]]></summary></entry></feed>