104 lines
14 KiB
XML
104 lines
14 KiB
XML
<?xml version='1.0' encoding='UTF-8' standalone='no'?>
|
|
<doxygen xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="compound.xsd" version="1.8.11">
|
|
<compounddef id="JacobianFactorQ_8h" kind="file" language="C++">
|
|
<compoundname>JacobianFactorQ.h</compoundname>
|
|
<includes local="no">gtsam/linear/RegularJacobianFactor.h</includes>
|
|
<incdepgraph>
|
|
<node id="10">
|
|
<label>gtsam/linear/RegularJacobianFactor.h</label>
|
|
</node>
|
|
<node id="9">
|
|
<label>tests/doc-test-files/JacobianFactorQ.h</label>
|
|
<link refid="JacobianFactorQ.h"/>
|
|
<childnode refid="10" relation="include">
|
|
</childnode>
|
|
</node>
|
|
</incdepgraph>
|
|
<innerclass refid="classgtsam_1_1JacobianFactorQ" prot="public">gtsam::JacobianFactorQ</innerclass>
|
|
<innerclass refid="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4" prot="public">gtsam::traits< JacobianFactorQ< D, ZDim > ></innerclass>
|
|
<innernamespace refid="namespacegtsam">gtsam</innernamespace>
|
|
<briefdescription>
|
|
</briefdescription>
|
|
<detaileddescription>
|
|
</detaileddescription>
|
|
<programlisting>
|
|
<codeline lineno="1"><highlight class="comment">/*<sp/>----------------------------------------------------------------------------</highlight></codeline>
|
|
<codeline lineno="2"><highlight class="comment"></highlight></codeline>
|
|
<codeline lineno="3"><highlight class="comment"><sp/>*<sp/>GTSAM<sp/>Copyright<sp/>2010,<sp/>Georgia<sp/>Tech<sp/>Research<sp/>Corporation,</highlight></codeline>
|
|
<codeline lineno="4"><highlight class="comment"><sp/>*<sp/>Atlanta,<sp/>Georgia<sp/>30332-0415</highlight></codeline>
|
|
<codeline lineno="5"><highlight class="comment"><sp/>*<sp/>All<sp/>Rights<sp/>Reserved</highlight></codeline>
|
|
<codeline lineno="6"><highlight class="comment"><sp/>*<sp/>Authors:<sp/>Frank<sp/>Dellaert,<sp/>et<sp/>al.<sp/>(see<sp/>THANKS<sp/>for<sp/>the<sp/>full<sp/>author<sp/>list)</highlight></codeline>
|
|
<codeline lineno="7"><highlight class="comment"></highlight></codeline>
|
|
<codeline lineno="8"><highlight class="comment"><sp/>*<sp/>See<sp/>LICENSE<sp/>for<sp/>the<sp/>license<sp/>information</highlight></codeline>
|
|
<codeline lineno="9"><highlight class="comment"></highlight></codeline>
|
|
<codeline lineno="10"><highlight class="comment"><sp/>*<sp/>--------------------------------------------------------------------------<sp/>*/</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="11"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="12"><highlight class="normal"></highlight><highlight class="comment">/*</highlight></codeline>
|
|
<codeline lineno="13"><highlight class="comment"><sp/>*<sp/>@file<sp/><sp/>JacobianFactorQ.h</highlight></codeline>
|
|
<codeline lineno="14"><highlight class="comment"><sp/>*<sp/>@date<sp/><sp/>Oct<sp/>27,<sp/>2013</highlight></codeline>
|
|
<codeline lineno="15"><highlight class="comment"><sp/>*<sp/>@uthor<sp/>Frank<sp/>Dellaert</highlight></codeline>
|
|
<codeline lineno="16"><highlight class="comment"><sp/>*/</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="17"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="18"><highlight class="normal"></highlight><highlight class="preprocessor">#pragma<sp/>once</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="19"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="20"><highlight class="normal"></highlight><highlight class="preprocessor">#include<sp/><gtsam/linear/RegularJacobianFactor.h></highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="21"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="22"><highlight class="normal"></highlight><highlight class="keyword">namespace<sp/></highlight><highlight class="normal"><ref refid="namespacegtsam" kindref="compound">gtsam</ref><sp/>{</highlight></codeline>
|
|
<codeline lineno="26"><highlight class="keyword">template</highlight><highlight class="normal"><</highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>D,<sp/></highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>ZDim></highlight></codeline>
|
|
<codeline lineno="27" refid="classgtsam_1_1JacobianFactorQ" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">class<sp/></highlight><highlight class="normal"><ref refid="classgtsam_1_1JacobianFactorQ" kindref="compound">JacobianFactorQ</ref>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/>RegularJacobianFactor<D><sp/>{</highlight></codeline>
|
|
<codeline lineno="28"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="29"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>RegularJacobianFactor<D><sp/>Base;</highlight></codeline>
|
|
<codeline lineno="30"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>Eigen::Matrix<double,<sp/>ZDim,<sp/>D><sp/>MatrixZD;</highlight></codeline>
|
|
<codeline lineno="31"><highlight class="normal"><sp/><sp/></highlight><highlight class="keyword">typedef</highlight><highlight class="normal"><sp/>std::pair<Key,<sp/>Matrix><sp/>KeyMatrix;</highlight></codeline>
|
|
<codeline lineno="32"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="33"><highlight class="normal"></highlight><highlight class="keyword">public</highlight><highlight class="normal">:</highlight></codeline>
|
|
<codeline lineno="34"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="36" refid="classgtsam_1_1JacobianFactorQ_1a4dd3fbba46a6dff75bcf902931a3b769" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1JacobianFactorQ_1a4dd3fbba46a6dff75bcf902931a3b769" kindref="member">JacobianFactorQ</ref>()<sp/>{</highlight></codeline>
|
|
<codeline lineno="37"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
|
|
<codeline lineno="38"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="40" refid="classgtsam_1_1JacobianFactorQ_1abfb38e58b5b2d1293cf374b7eca2b2d4" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1JacobianFactorQ_1abfb38e58b5b2d1293cf374b7eca2b2d4" kindref="member">JacobianFactorQ</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>KeyVector&<sp/>keys,<sp/></highlight><highlight class="comment">//</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="41"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedDiagonal&<sp/>model<sp/>=<sp/>SharedDiagonal())<sp/>:</highlight></codeline>
|
|
<codeline lineno="42"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>Base()<sp/>{</highlight></codeline>
|
|
<codeline lineno="43"><highlight class="normal"><sp/><sp/><sp/><sp/>Matrix<sp/>zeroMatrix<sp/>=<sp/>Matrix::Zero(0,<sp/>D);</highlight></codeline>
|
|
<codeline lineno="44"><highlight class="normal"><sp/><sp/><sp/><sp/>Vector<sp/>zeroVector<sp/>=<sp/>Vector::Zero(0);</highlight></codeline>
|
|
<codeline lineno="45"><highlight class="normal"><sp/><sp/><sp/><sp/>std::vector<KeyMatrix><sp/>QF;</highlight></codeline>
|
|
<codeline lineno="46"><highlight class="normal"><sp/><sp/><sp/><sp/>QF.reserve(keys.size());</highlight></codeline>
|
|
<codeline lineno="47"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal">(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Key&<sp/>key:<sp/>keys)</highlight></codeline>
|
|
<codeline lineno="48"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>QF.push_back(KeyMatrix(key,<sp/>zeroMatrix));</highlight></codeline>
|
|
<codeline lineno="49"><highlight class="normal"><sp/><sp/><sp/><sp/>JacobianFactor::fillTerms(QF,<sp/>zeroVector,<sp/>model);</highlight></codeline>
|
|
<codeline lineno="50"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
|
|
<codeline lineno="51"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="53" refid="classgtsam_1_1JacobianFactorQ_1a549fef1272025c45472f2e4294bd319e" refkind="member"><highlight class="normal"><sp/><sp/><ref refid="classgtsam_1_1JacobianFactorQ_1a549fef1272025c45472f2e4294bd319e" kindref="member">JacobianFactorQ</ref>(</highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>KeyVector&<sp/>keys,</highlight></codeline>
|
|
<codeline lineno="54"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>std::vector<MatrixZD,<sp/>Eigen::aligned_allocator<MatrixZD><sp/>>&<sp/>FBlocks,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Matrix&<sp/>E,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Matrix3&<sp/>P,</highlight></codeline>
|
|
<codeline lineno="55"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>Vector&<sp/>b,<sp/></highlight><highlight class="keyword">const</highlight><highlight class="normal"><sp/>SharedDiagonal&<sp/>model<sp/>=<sp/>SharedDiagonal())<sp/>:</highlight></codeline>
|
|
<codeline lineno="56"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>Base()<sp/>{</highlight></codeline>
|
|
<codeline lineno="57"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>j<sp/>=<sp/>0,<sp/>m2<sp/>=<sp/>E.rows(),<sp/>m<sp/>=<sp/>m2<sp/>/<sp/>ZDim;</highlight></codeline>
|
|
<codeline lineno="58"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Calculate<sp/>projector<sp/>Q</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="59"><highlight class="normal"><sp/><sp/><sp/><sp/>Matrix<sp/>Q<sp/>=<sp/>Matrix::Identity(m2,m2)<sp/>-<sp/>E<sp/>*<sp/>P<sp/>*<sp/>E.transpose();</highlight></codeline>
|
|
<codeline lineno="60"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Calculate<sp/>pre-computed<sp/>Jacobian<sp/>matrices</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="61"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>TODO:<sp/>can<sp/>we<sp/>do<sp/>better<sp/>?</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="62"><highlight class="normal"><sp/><sp/><sp/><sp/>std::vector<KeyMatrix><sp/>QF;</highlight></codeline>
|
|
<codeline lineno="63"><highlight class="normal"><sp/><sp/><sp/><sp/>QF.reserve(m);</highlight></codeline>
|
|
<codeline lineno="64"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Below,<sp/>we<sp/>compute<sp/>each<sp/>mZDim*D<sp/>block<sp/>A_j<sp/>=<sp/>Q_j<sp/>*<sp/>F_j<sp/>=<sp/>(mZDim*ZDim)<sp/>*<sp/>(Zdim*D)</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="65"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="keywordflow">for</highlight><highlight class="normal"><sp/>(</highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>k<sp/>=<sp/>0;<sp/>k<sp/><<sp/>FBlocks.size();<sp/>++k)<sp/>{</highlight></codeline>
|
|
<codeline lineno="66"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>Key<sp/>key<sp/>=<sp/>keys[k];</highlight></codeline>
|
|
<codeline lineno="67"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/>QF.push_back(</highlight></codeline>
|
|
<codeline lineno="68"><highlight class="normal"><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/><sp/>KeyMatrix(key,<sp/>-<sp/>Q.block(0,<sp/>ZDim<sp/>*<sp/>j++,<sp/>m2,<sp/>ZDim)<sp/>*<sp/>FBlocks[k]));</highlight></codeline>
|
|
<codeline lineno="69"><highlight class="normal"><sp/><sp/><sp/><sp/>}</highlight></codeline>
|
|
<codeline lineno="70"><highlight class="normal"><sp/><sp/><sp/><sp/></highlight><highlight class="comment">//<sp/>Which<sp/>is<sp/>then<sp/>passed<sp/>to<sp/>the<sp/>normal<sp/>JacobianFactor<sp/>constructor</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="71"><highlight class="normal"><sp/><sp/><sp/><sp/>JacobianFactor::fillTerms(QF,<sp/>-<sp/>Q<sp/>*<sp/>b,<sp/>model);</highlight></codeline>
|
|
<codeline lineno="72"><highlight class="normal"><sp/><sp/>}</highlight></codeline>
|
|
<codeline lineno="73"><highlight class="normal">};</highlight></codeline>
|
|
<codeline lineno="74"><highlight class="normal"></highlight><highlight class="comment">//<sp/>end<sp/>class<sp/>JacobianFactorQ</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="75"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="76"><highlight class="normal"></highlight><highlight class="comment">//<sp/>traits</highlight><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="77" refid="structgtsam_1_1traits_3_01JacobianFactorQ_3_01D_00_01ZDim_01_4_01_4" refkind="compound"><highlight class="normal"></highlight><highlight class="keyword">template</highlight><highlight class="normal"><</highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>D,<sp/></highlight><highlight class="keywordtype">size_t</highlight><highlight class="normal"><sp/>ZDim><sp/></highlight><highlight class="keyword">struct<sp/></highlight><highlight class="normal">traits<<ref refid="classgtsam_1_1JacobianFactorQ" kindref="compound">JacobianFactorQ</ref><D,<sp/>ZDim><sp/>><sp/>:<sp/></highlight><highlight class="keyword">public</highlight><highlight class="normal"><sp/>Testable<</highlight></codeline>
|
|
<codeline lineno="78"><highlight class="normal"><sp/><sp/><sp/><sp/>JacobianFactorQ<D,<sp/>ZDim><sp/>><sp/>{</highlight></codeline>
|
|
<codeline lineno="79"><highlight class="normal">};</highlight></codeline>
|
|
<codeline lineno="80"><highlight class="normal"></highlight></codeline>
|
|
<codeline lineno="81"><highlight class="normal">}</highlight></codeline>
|
|
</programlisting>
|
|
<location file="tests/doc-test-files/JacobianFactorQ.h"/>
|
|
</compounddef>
|
|
</doxygen>
|