├── .gitignore
├── .idea
    ├── codeStyles
    │   └── Project.xml
    ├── misc.xml
    ├── modules.xml
    ├── sfm_learn.iml
    ├── vcs.xml
    └── workspace.xml
├── BA.cpp
├── BA.h
├── CMakeLists.txt
├── CVVisual
├── Camera.cpp
├── Camera.h
├── Frame.cpp
├── Frame.h
├── Map.cpp
├── Map.h
├── MapPoint.cpp
├── MapPoint.h
├── README.md
├── SFM.cpp
├── SFM.h
├── cmake_modules
    ├── FindCSparse.cmake
    └── FindG2O.cmake
├── common_include.h
├── datasets
    ├── fountain_dense_cameras
    │   ├── 0000.png.camera
    │   ├── 0001.png.camera
    │   ├── 0002.png.camera
    │   ├── 0003.png.camera
    │   ├── 0004.png.camera
    │   ├── 0005.png.camera
    │   ├── 0006.png.camera
    │   ├── 0007.png.camera
    │   ├── 0008.png.camera
    │   ├── 0009.png.camera
    │   └── 0010.png.camera
    └── fountain_dense_images
    │   ├── 0000.png
    │   ├── 0001.png
    │   ├── 0002.png
    │   ├── 0003.png
    │   ├── 0004.png
    │   ├── 0005.png
    │   ├── 0006.png
    │   ├── 0007.png
    │   ├── 0008.png
    │   ├── 0009.png
    │   └── 0010.png
├── images
    ├── 2D2D.png
    ├── 3D2D.png
    ├── afterBA.png
    └── beforeBA.png
└── main.cpp


/.gitignore:
--------------------------------------------------------------------------------
 1 | CMakeCache.txt
 2 | CMakeFiles
 3 | CMakeScripts
 4 | Makefile
 5 | cmake_install.cmake
 6 | install_manifest.txt
 7 | 
 8 | #build文件
 9 | cmake-build-*/
10 | bin/


--------------------------------------------------------------------------------
/.idea/codeStyles/Project.xml:
--------------------------------------------------------------------------------
 1 | <component name="ProjectCodeStyleConfiguration">
 2 |   <code_scheme name="Project" version="173">
 3 |     <Objective-C-extensions>
 4 |       <file>
 5 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Import" />
 6 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Macro" />
 7 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Typedef" />
 8 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Enum" />
 9 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Constant" />
10 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Global" />
11 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Struct" />
12 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="FunctionPredecl" />
13 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Function" />
14 |       </file>
15 |       <class>
16 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Property" />
17 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="Synthesize" />
18 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="InitMethod" />
19 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="StaticMethod" />
20 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="InstanceMethod" />
21 |         <option name="com.jetbrains.cidr.lang.util.OCDeclarationKind" value="DeallocMethod" />
22 |       </class>
23 |       <extensions>
24 |         <pair source="cpp" header="h" fileNamingConvention="NONE" />
25 |         <pair source="c" header="h" fileNamingConvention="NONE" />
26 |       </extensions>
27 |     </Objective-C-extensions>
28 |   </code_scheme>
29 | </component>


--------------------------------------------------------------------------------
/.idea/misc.xml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <project version="4">
3 |   <component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
4 |   <component name="JavaScriptSettings">
5 |     <option name="languageLevel" value="ES6" />
6 |   </component>
7 | </project>


--------------------------------------------------------------------------------
/.idea/modules.xml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <project version="4">
3 |   <component name="ProjectModuleManager">
4 |     <modules>
5 |       <module fileurl="file://$PROJECT_DIR$/.idea/sfm_learn.iml" filepath="$PROJECT_DIR$/.idea/sfm_learn.iml" />
6 |     </modules>
7 |   </component>
8 | </project>


--------------------------------------------------------------------------------
/.idea/sfm_learn.iml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <module classpath="CMake" type="CPP_MODULE" version="4" />


--------------------------------------------------------------------------------
/.idea/vcs.xml:
--------------------------------------------------------------------------------
1 | <?xml version="1.0" encoding="UTF-8"?>
2 | <project version="4">
3 |   <component name="VcsDirectoryMappings">
4 |     <mapping directory="$PROJECT_DIR$" vcs="Git" />
5 |   </component>
6 | </project>


--------------------------------------------------------------------------------
/.idea/workspace.xml:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" encoding="UTF-8"?>
  2 | <project version="4">
  3 |   <component name="CMakeRunConfigurationManager" shouldGenerate="true" shouldDeleteObsolete="true" buildAllGenerated="true">
  4 |     <generated>
  5 |       <config projectName="sfm_learn" targetName="sfm_learn" />
  6 |     </generated>
  7 |   </component>
  8 |   <component name="CMakeSettings" AUTO_RELOAD="true">
  9 |     <configurations>
 10 |       <configuration PROFILE_NAME="Debug" CONFIG_NAME="Debug" GENERATION_OPTIONS="-DCLOUDVIEWER_DEBUG_MODE=OFF -DCVV_DEBUG_MODE=OFF" BUILD_OPTIONS="-j3" />
 11 |       <configuration PROFILE_NAME="DebugWithCvv" CONFIG_NAME="Debug" GENERATION_OPTIONS="-DCVV_DEBUG_MODE=ON -DCLOUDVIEWER_DEBUG_MODE=OFF" BUILD_OPTIONS="-j3" />
 12 |       <configuration PROFILE_NAME="DebugWithCloudViewer" CONFIG_NAME="Debug" GENERATION_OPTIONS="-DCLOUDVIEWER_DEBUG_MODE=ON -DCVV_DEBUG_MODE=OFF" BUILD_OPTIONS="-j3" />
 13 |       <configuration PROFILE_NAME="Release" CONFIG_NAME="Release" GENERATION_OPTIONS="-DDEBUG_MODE=OFF -DCLOUDVIEWER_DEBUG_MODE=OFF -DCVV_DEBUG_MODE=OFF" BUILD_OPTIONS="-j3" />
 14 |     </configurations>
 15 |   </component>
 16 |   <component name="ChangeListManager">
 17 |     <list default="true" id="ad4cc4b4-99f2-48db-935f-f97f9f0e4516" name="Default" comment="">
 18 |       <change beforePath="$PROJECT_DIR$/BA.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/BA.cpp" afterDir="false" />
 19 |       <change beforePath="$PROJECT_DIR$/BA.h" beforeDir="false" afterPath="$PROJECT_DIR$/BA.h" afterDir="false" />
 20 |     </list>
 21 |     <ignored path="$PROJECT_DIR$/cmake-build-debug/" />
 22 |     <ignored path="$PROJECT_DIR$/cmake-build-release/" />
 23 |     <ignored path="$PROJECT_DIR$/cmake-build-debugwithoutcvv/" />
 24 |     <ignored path="$PROJECT_DIR$/cmake-build-debugwithoutcloudviewer/" />
 25 |     <ignored path="$PROJECT_DIR$/cmake-build-debugwithcloudviewer/" />
 26 |     <ignored path="$PROJECT_DIR$/cmake-build-debugwithcvv/" />
 27 |     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
 28 |     <option name="TRACKING_ENABLED" value="true" />
 29 |     <option name="SHOW_DIALOG" value="false" />
 30 |     <option name="HIGHLIGHT_CONFLICTS" value="true" />
 31 |     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
 32 |     <option name="LAST_RESOLUTION" value="IGNORE" />
 33 |   </component>
 34 |   <component name="ExecutionTargetManager" SELECTED_TARGET="CMakeBuildProfile:DebugWithCloudViewer" />
 35 |   <component name="FileEditorManager">
 36 |     <splitter split-orientation="horizontal" split-proportion="0.5">
 37 |       <split-first>
 38 |         <leaf SIDE_TABS_SIZE_LIMIT_KEY="300">
 39 |           <file leaf-file-name="SFM.cpp" pinned="false" current-in-tab="false">
 40 |             <entry file="file://$PROJECT_DIR$/SFM.cpp">
 41 |               <provider selected="true" editor-type-id="text-editor">
 42 |                 <state relative-caret-position="309">
 43 |                   <caret line="254" column="52" selection-start-line="254" selection-start-column="52" selection-end-line="254" selection-end-column="52" />
 44 |                   <folding>
 45 |                     <element signature="e#46#62#0" expanded="true" />
 46 |                     <element signature="e#804#881#0" expanded="true" />
 47 |                   </folding>
 48 |                 </state>
 49 |               </provider>
 50 |             </entry>
 51 |           </file>
 52 |           <file leaf-file-name="SFM.h" pinned="false" current-in-tab="false">
 53 |             <entry file="file://$PROJECT_DIR$/SFM.h">
 54 |               <provider selected="true" editor-type-id="text-editor">
 55 |                 <state relative-caret-position="260">
 56 |                   <caret line="60" column="15" selection-start-line="60" selection-start-column="15" selection-end-line="60" selection-end-column="15" />
 57 |                   <folding>
 58 |                     <element signature="e#95#122#0" expanded="true" />
 59 |                   </folding>
 60 |                 </state>
 61 |               </provider>
 62 |             </entry>
 63 |           </file>
 64 |           <file leaf-file-name="BA.cpp" pinned="false" current-in-tab="true">
 65 |             <entry file="file://$PROJECT_DIR$/BA.cpp">
 66 |               <provider selected="true" editor-type-id="text-editor">
 67 |                 <state relative-caret-position="120">
 68 |                   <caret line="8" selection-start-line="8" selection-end-line="8" />
 69 |                   <folding>
 70 |                     <element signature="e#87#1909#0" />
 71 |                     <element signature="e#1068#1297#0" />
 72 |                     <element signature="e#1372#1592#0" />
 73 |                     <element signature="e#1669#1892#0" />
 74 |                     <element signature="e#1919#4506#0" />
 75 |                     <element signature="e#4516#5052#0" />
 76 |                     <element signature="e#5062#5223#0" />
 77 |                     <element signature="e#4909#5345#0" />
 78 |                     <element signature="e#5323#5494#0" />
 79 |                   </folding>
 80 |                 </state>
 81 |               </provider>
 82 |             </entry>
 83 |           </file>
 84 |         </leaf>
 85 |       </split-first>
 86 |       <split-second>
 87 |         <leaf SIDE_TABS_SIZE_LIMIT_KEY="300">
 88 |           <file leaf-file-name="Map.cpp" pinned="false" current-in-tab="false">
 89 |             <entry file="file://$PROJECT_DIR$/Map.cpp">
 90 |               <provider selected="true" editor-type-id="text-editor">
 91 |                 <state relative-caret-position="160">
 92 |                   <caret line="22" column="14" selection-start-line="22" selection-start-column="14" selection-end-line="22" selection-end-column="14" />
 93 |                 </state>
 94 |               </provider>
 95 |             </entry>
 96 |           </file>
 97 |           <file leaf-file-name="Map.h" pinned="false" current-in-tab="false">
 98 |             <entry file="file://$PROJECT_DIR$/Map.h">
 99 |               <provider selected="true" editor-type-id="text-editor">
100 |                 <state relative-caret-position="330">
101 |                   <caret line="22" column="18" selection-start-line="22" selection-start-column="18" selection-end-line="22" selection-end-column="18" />
102 |                   <folding>
103 |                     <element signature="e#95#116#0" expanded="true" />
104 |                   </folding>
105 |                 </state>
106 |               </provider>
107 |             </entry>
108 |           </file>
109 |           <file leaf-file-name="Frame.h" pinned="false" current-in-tab="false">
110 |             <entry file="file://$PROJECT_DIR$/Frame.h">
111 |               <provider selected="true" editor-type-id="text-editor">
112 |                 <state relative-caret-position="230">
113 |                   <caret line="55" column="61" selection-start-line="55" selection-start-column="61" selection-end-line="55" selection-end-column="61" />
114 |                   <folding>
115 |                     <element signature="e#99#126#0" expanded="true" />
116 |                     <element signature="e#1322#1334#0" expanded="true" />
117 |                     <element signature="e#1648#1660#0" expanded="true" />
118 |                   </folding>
119 |                 </state>
120 |               </provider>
121 |             </entry>
122 |           </file>
123 |           <file leaf-file-name="MapPoint.h" pinned="false" current-in-tab="false">
124 |             <entry file="file://$PROJECT_DIR$/MapPoint.h">
125 |               <provider selected="true" editor-type-id="text-editor">
126 |                 <state relative-caret-position="172">
127 |                   <caret line="45" column="30" selection-start-line="45" selection-start-column="30" selection-end-line="45" selection-end-column="30" />
128 |                   <folding>
129 |                     <element signature="e#105#132#0" expanded="true" />
130 |                     <element signature="e#833#845#0" expanded="true" />
131 |                     <element signature="e#989#1001#0" expanded="true" />
132 |                   </folding>
133 |                 </state>
134 |               </provider>
135 |             </entry>
136 |           </file>
137 |           <file leaf-file-name="Camera.h" pinned="false" current-in-tab="false">
138 |             <entry file="file://$PROJECT_DIR$/Camera.h">
139 |               <provider selected="true" editor-type-id="text-editor">
140 |                 <state relative-caret-position="135">
141 |                   <caret line="22" column="28" selection-start-line="22" selection-start-column="28" selection-end-line="22" selection-end-column="28" />
142 |                   <folding>
143 |                     <element signature="e#428#440#0" expanded="true" />
144 |                   </folding>
145 |                 </state>
146 |               </provider>
147 |             </entry>
148 |           </file>
149 |           <file leaf-file-name="BA.h" pinned="false" current-in-tab="true">
150 |             <entry file="file://$PROJECT_DIR$/BA.h">
151 |               <provider selected="true" editor-type-id="text-editor">
152 |                 <state relative-caret-position="-13">
153 |                   <caret line="64" column="41" lean-forward="true" selection-start-line="64" selection-start-column="41" selection-end-line="64" selection-end-column="41" />
154 |                   <folding>
155 |                     <element signature="e#93#117#0" expanded="true" />
156 |                     <element signature="e#333#1569#1" />
157 |                     <element signature="e#509#1559#0" />
158 |                   </folding>
159 |                 </state>
160 |               </provider>
161 |             </entry>
162 |           </file>
163 |         </leaf>
164 |       </split-second>
165 |     </splitter>
166 |   </component>
167 |   <component name="FindInProjectRecents">
168 |     <findStrings>
169 |       <find>SE3</find>
170 |       <find>keypoints1</find>
171 |       <find>descriptors1</find>
172 |       <find>matchPoints2</find>
173 |       <find>image1aaa</find>
174 |       <find>camera</find>
175 |       <find>keypoints</find>
176 |       <find>descriptors2</find>
177 |       <find>goodMatches</find>
178 |       <find>CVVISUAL_DEBUGMODE</find>
179 |       <find>keyFrame2-&gt;matchPoints</find>
180 |       <find>RotatePoint</find>
181 |     </findStrings>
182 |     <replaceStrings>
183 |       <replace>Sophus::SE3</replace>
184 |       <replace>keyFrame1-&gt;keypoints</replace>
185 |       <replace>keyFrame1-&gt;keyPoints</replace>
186 |       <replace>keyFrame2-&gt;descriptors</replace>
187 |       <replace>keyFrame2-&gt;matchPoints</replace>
188 |       <replace>keyFrame1-&gt;image</replace>
189 |       <replace>keyframe2-&gt;keypoints</replace>
190 |       <replace>keyframe2-&gt;keyPoints</replace>
191 |       <replace>keyframe2-&gt;descriptors</replace>
192 |       <replace>matches</replace>
193 |       <replace>matchPoints2</replace>
194 |     </replaceStrings>
195 |   </component>
196 |   <component name="Git.Settings">
197 |     <option name="RECENT_GIT_ROOT_PATH" value="$PROJECT_DIR$" />
198 |   </component>
199 |   <component name="IdeDocumentHistory">
200 |     <option name="CHANGED_PATHS">
201 |       <list>
202 |         <option value="$PROJECT_DIR$/datasets/common_include.h" />
203 |         <option value="$PROJECT_DIR$/common_include.h" />
204 |         <option value="$PROJECT_DIR$/MapPoint.cpp" />
205 |         <option value="$PROJECT_DIR$/.gitignore" />
206 |         <option value="$PROJECT_DIR$/Camera.cpp" />
207 |         <option value="$PROJECT_DIR$/Frame.cpp" />
208 |         <option value="$PROJECT_DIR$/main.cpp" />
209 |         <option value="$PROJECT_DIR$/Map.cpp" />
210 |         <option value="$PROJECT_DIR$/CMakeLists.txt" />
211 |         <option value="$PROJECT_DIR$/Map.h" />
212 |         <option value="$PROJECT_DIR$/MapPoint.h" />
213 |         <option value="$PROJECT_DIR$/Frame.h" />
214 |         <option value="$PROJECT_DIR$/Camera.h" />
215 |         <option value="$PROJECT_DIR$/SFM.h" />
216 |         <option value="$PROJECT_DIR$/SFM.cpp" />
217 |         <option value="$PROJECT_DIR$/BA.h" />
218 |         <option value="$PROJECT_DIR$/BA.cpp" />
219 |       </list>
220 |     </option>
221 |   </component>
222 |   <component name="JsBuildToolGruntFileManager" detection-done="true" sorting="DEFINITION_ORDER" />
223 |   <component name="JsBuildToolPackageJson" detection-done="true" sorting="DEFINITION_ORDER" />
224 |   <component name="JsGulpfileManager">
225 |     <detection-done>true</detection-done>
226 |     <sorting>DEFINITION_ORDER</sorting>
227 |   </component>
228 |   <component name="NodePackageJsonFileManager">
229 |     <packageJsonPaths />
230 |   </component>
231 |   <component name="OCFindUsagesOptions" text="false" ivars="false" properties="true" derivedClasses="false" />
232 |   <component name="ProjectFrameBounds" extendedState="4">
233 |     <option name="x" value="65" />
234 |     <option name="y" value="24" />
235 |     <option name="width" value="1883" />
236 |     <option name="height" value="907" />
237 |   </component>
238 |   <component name="ProjectView">
239 |     <navigator proportions="" version="1">
240 |       <foldersAlwaysOnTop value="true" />
241 |     </navigator>
242 |     <panes>
243 |       <pane id="ProjectPane">
244 |         <subPane>
245 |           <expand>
246 |             <path>
247 |               <item name="sfm_learn" type="b2602c69:ProjectViewProjectNode" />
248 |               <item name="sfm_learn" type="462c0819:PsiDirectoryNode" />
249 |             </path>
250 |             <path>
251 |               <item name="sfm_learn" type="b2602c69:ProjectViewProjectNode" />
252 |               <item name="sfm_learn" type="462c0819:PsiDirectoryNode" />
253 |               <item name="datasets" type="462c0819:PsiDirectoryNode" />
254 |             </path>
255 |             <path>
256 |               <item name="sfm_learn" type="b2602c69:ProjectViewProjectNode" />
257 |               <item name="External Libraries" type="cb654da1:ExternalLibrariesNode" />
258 |             </path>
259 |             <path>
260 |               <item name="sfm_learn" type="b2602c69:ProjectViewProjectNode" />
261 |               <item name="External Libraries" type="cb654da1:ExternalLibrariesNode" />
262 |               <item name="Header Search Paths" type="70bed36:NamedLibraryElementNode" />
263 |             </path>
264 |             <path>
265 |               <item name="sfm_learn" type="b2602c69:ProjectViewProjectNode" />
266 |               <item name="External Libraries" type="cb654da1:ExternalLibrariesNode" />
267 |               <item name="Header Search Paths" type="70bed36:NamedLibraryElementNode" />
268 |               <item name="Sophus" type="462c0819:PsiDirectoryNode" />
269 |             </path>
270 |             <path>
271 |               <item name="sfm_learn" type="b2602c69:ProjectViewProjectNode" />
272 |               <item name="External Libraries" type="cb654da1:ExternalLibrariesNode" />
273 |               <item name="Header Search Paths" type="70bed36:NamedLibraryElementNode" />
274 |               <item name="Sophus" type="462c0819:PsiDirectoryNode" />
275 |               <item name="sophus" type="462c0819:PsiDirectoryNode" />
276 |             </path>
277 |           </expand>
278 |           <select />
279 |         </subPane>
280 |       </pane>
281 |       <pane id="Scope" />
282 |     </panes>
283 |   </component>
284 |   <component name="PropertiesComponent">
285 |     <property name="WebServerToolWindowFactoryState" value="false" />
286 |     <property name="last_opened_file_path" value="$PROJECT_DIR$" />
287 |     <property name="nodejs_interpreter_path.stuck_in_default_project" value="undefined stuck path" />
288 |     <property name="nodejs_npm_path_reset_for_default_project" value="true" />
289 |     <property name="settings.editor.selected.configurable" value="CMakeSettings" />
290 |   </component>
291 |   <component name="RecentsManager">
292 |     <key name="MoveFile.RECENT_KEYS">
293 |       <recent name="$PROJECT_DIR$" />
294 |     </key>
295 |   </component>
296 |   <component name="RunDashboard">
297 |     <option name="ruleStates">
298 |       <list>
299 |         <RuleState>
300 |           <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
301 |         </RuleState>
302 |         <RuleState>
303 |           <option name="name" value="StatusDashboardGroupingRule" />
304 |         </RuleState>
305 |       </list>
306 |     </option>
307 |   </component>
308 |   <component name="RunManager" selected="Application.sfm_learn">
309 |     <configuration name="Build All" type="CMakeRunConfiguration" factoryName="Application" PASS_PARENT_ENVS_2="true" CONFIG_NAME="DebugWithCloudViewer" RUN_TARGET_PROJECT_NAME="sfm_learn" RUN_TARGET_NAME="sfm_learn" EXPLICIT_BUILD_TARGET_NAME="all" />
310 |     <configuration name="sfm_learn" type="CMakeRunConfiguration" factoryName="Application" WORKING_DIR="file://../" PASS_PARENT_ENVS_2="true" PROJECT_NAME="sfm_learn" TARGET_NAME="sfm_learn" CONFIG_NAME="DebugWithCloudViewer" RUN_TARGET_PROJECT_NAME="sfm_learn" RUN_TARGET_NAME="sfm_learn" />
311 |     <list>
312 |       <item itemvalue="Application.Build All" />
313 |       <item itemvalue="Application.sfm_learn" />
314 |     </list>
315 |   </component>
316 |   <component name="SvnConfiguration">
317 |     <configuration />
318 |   </component>
319 |   <component name="TaskManager">
320 |     <task active="true" id="Default" summary="Default task">
321 |       <changelist id="ad4cc4b4-99f2-48db-935f-f97f9f0e4516" name="Default" comment="" />
322 |       <created>1528875516274</created>
323 |       <option name="number" value="Default" />
324 |       <option name="presentableId" value="Default" />
325 |       <updated>1528875516274</updated>
326 |       <workItem from="1528875517801" duration="848000" />
327 |       <workItem from="1528876381767" duration="267000" />
328 |       <workItem from="1528876726508" duration="6664000" />
329 |       <workItem from="1528885071257" duration="5554000" />
330 |       <workItem from="1528892068474" duration="8714000" />
331 |       <workItem from="1528944308378" duration="11934000" />
332 |       <workItem from="1529027913821" duration="101000" />
333 |       <workItem from="1529028045754" duration="5650000" />
334 |       <workItem from="1529033890911" duration="2390000" />
335 |       <workItem from="1529043679305" duration="415000" />
336 |       <workItem from="1529044591828" duration="1297000" />
337 |       <workItem from="1529050079336" duration="1686000" />
338 |       <workItem from="1529055312625" duration="19422000" />
339 |       <workItem from="1529108364878" duration="1031000" />
340 |       <workItem from="1529111447264" duration="6152000" />
341 |       <workItem from="1529119113399" duration="34520000" />
342 |       <workItem from="1529198732396" duration="2240000" />
343 |       <workItem from="1529202107040" duration="15052000" />
344 |       <workItem from="1529497759238" duration="10662000" />
345 |       <workItem from="1529821181442" duration="21812000" />
346 |     </task>
347 |     <servers />
348 |   </component>
349 |   <component name="TimeTrackingManager">
350 |     <option name="totallyTimeSpent" value="167573000" />
351 |   </component>
352 |   <component name="ToolWindowManager">
353 |     <frame x="65" y="24" width="1883" height="907" extended-state="4" />
354 |     <editor active="true" />
355 |     <layout>
356 |       <window_info content_ui="combo" id="Project" order="0" visible="true" weight="0.105682425" />
357 |       <window_info anchor="bottom" id="TODO" order="6" />
358 |       <window_info active="true" anchor="bottom" id="Messages" order="7" visible="true" weight="0.39899623" />
359 |       <window_info anchor="bottom" id="CMake" order="11" weight="0.32912987" />
360 |       <window_info anchor="bottom" id="Event Log" order="9" side_tool="true" />
361 |       <window_info anchor="bottom" id="Find" order="1" weight="0.32371393" />
362 |       <window_info anchor="bottom" id="Run" order="2" weight="0.32873276" />
363 |       <window_info anchor="bottom" id="Version Control" order="10" weight="0.32912987" />
364 |       <window_info id="Structure" order="1" side_tool="true" weight="0.25" />
365 |       <window_info anchor="bottom" id="Terminal" order="8" weight="0.32912987" />
366 |       <window_info anchor="bottom" id="Debug" order="3" weight="0.39800996" />
367 |       <window_info id="Favorites" order="2" side_tool="true" />
368 |       <window_info anchor="right" content_ui="combo" id="Hierarchy" order="2" weight="0.25" />
369 |       <window_info anchor="bottom" id="Inspection" order="5" weight="0.4" />
370 |       <window_info anchor="right" id="Commander" internal_type="SLIDING" order="0" type="SLIDING" weight="0.4" />
371 |       <window_info anchor="right" id="Ant Build" order="1" weight="0.25" />
372 |       <window_info anchor="bottom" id="Message" order="0" />
373 |       <window_info anchor="bottom" id="Cvs" order="4" weight="0.25" />
374 |     </layout>
375 |   </component>
376 |   <component name="TypeScriptGeneratedFilesManager">
377 |     <option name="version" value="1" />
378 |   </component>
379 |   <component name="Vcs.Log.Tabs.Properties">
380 |     <option name="TAB_STATES">
381 |       <map>
382 |         <entry key="MAIN">
383 |           <value>
384 |             <State>
385 |               <option name="RECENTLY_FILTERED_USER_GROUPS">
386 |                 <collection />
387 |               </option>
388 |               <option name="RECENTLY_FILTERED_BRANCH_GROUPS">
389 |                 <collection />
390 |               </option>
391 |               <option name="COLUMN_ORDER">
392 |                 <list>
393 |                   <option value="0" />
394 |                   <option value="1" />
395 |                   <option value="2" />
396 |                   <option value="3" />
397 |                 </list>
398 |               </option>
399 |             </State>
400 |           </value>
401 |         </entry>
402 |       </map>
403 |     </option>
404 |   </component>
405 |   <component name="VcsContentAnnotationSettings">
406 |     <option name="myLimit" value="2678400000" />
407 |   </component>
408 |   <component name="XDebuggerManager">
409 |     <breakpoint-manager>
410 |       <option name="time" value="122" />
411 |     </breakpoint-manager>
412 |   </component>
413 |   <component name="debuggerHistoryManager">
414 |     <expressions id="evaluateExpression">
415 |       <expression>
416 |         <expression-string>keyFrame1-&gt;frame-&gt;getTcw34MatCV().type()</expression-string>
417 |         <language-id>ObjectiveC</language-id>
418 |         <evaluation-mode>EXPRESSION</evaluation-mode>
419 |       </expression>
420 |       <expression>
421 |         <expression-string>this-&gt;keyFrame1-&gt;keyPoints.size()</expression-string>
422 |         <language-id>ObjectiveC</language-id>
423 |         <evaluation-mode>EXPRESSION</evaluation-mode>
424 |       </expression>
425 |       <expression>
426 |         <expression-string>this-&gt;-&gt;descriptors.rows</expression-string>
427 |         <language-id>ObjectiveC</language-id>
428 |         <evaluation-mode>EXPRESSION</evaluation-mode>
429 |       </expression>
430 |       <expression>
431 |         <expression-string>this-&gt;keyFrame1-&gt;descriptors.rows</expression-string>
432 |         <language-id>ObjectiveC</language-id>
433 |         <evaluation-mode>EXPRESSION</evaluation-mode>
434 |       </expression>
435 |       <expression>
436 |         <expression-string>this-&gt;keyFrame1-&gt;descriptors.cols</expression-string>
437 |         <language-id>ObjectiveC</language-id>
438 |         <evaluation-mode>EXPRESSION</evaluation-mode>
439 |       </expression>
440 |       <expression>
441 |         <expression-string>this-&gt;keyFrame1-&gt;descriptors.size()</expression-string>
442 |         <language-id>ObjectiveC</language-id>
443 |         <evaluation-mode>EXPRESSION</evaluation-mode>
444 |       </expression>
445 |       <expression>
446 |         <expression-string>this-&gt;keyFrame1.get()-&gt;keyPoints.size()</expression-string>
447 |         <language-id>ObjectiveC</language-id>
448 |         <evaluation-mode>EXPRESSION</evaluation-mode>
449 |       </expression>
450 |       <expression>
451 |         <expression-string>matchPointsNorm2.size()</expression-string>
452 |         <language-id>ObjectiveC</language-id>
453 |         <evaluation-mode>EXPRESSION</evaluation-mode>
454 |       </expression>
455 |       <expression>
456 |         <expression-string>matchPointsNorm1.size()</expression-string>
457 |         <language-id>ObjectiveC</language-id>
458 |         <evaluation-mode>EXPRESSION</evaluation-mode>
459 |       </expression>
460 |     </expressions>
461 |   </component>
462 |   <component name="editorHistoryManager">
463 |     <entry file="file:///usr/local/include/pcl-1.8/pcl/impl/point_types.hpp">
464 |       <provider selected="true" editor-type-id="text-editor">
465 |         <state relative-caret-position="9300">
466 |           <caret line="620" column="11" selection-start-line="620" selection-start-column="11" selection-end-line="620" selection-end-column="11" />
467 |         </state>
468 |       </provider>
469 |     </entry>
470 |     <entry file="file:///usr/include/c++/5/bits/stl_vector.h">
471 |       <provider selected="true" editor-type-id="text-editor">
472 |         <state relative-caret-position="148">
473 |           <caret line="317" selection-start-line="317" selection-end-line="317" />
474 |         </state>
475 |       </provider>
476 |     </entry>
477 |     <entry file="file:///usr/local/include/opencv2/cvv/debug_mode.hpp">
478 |       <provider selected="true" editor-type-id="text-editor">
479 |         <state relative-caret-position="346">
480 |           <caret line="31" column="19" selection-start-line="31" selection-start-column="19" selection-end-line="31" selection-end-column="19" />
481 |         </state>
482 |       </provider>
483 |     </entry>
484 |     <entry file="file:///usr/local/include/pcl-1.8/pcl/visualization/cloud_viewer.h">
485 |       <provider selected="true" editor-type-id="text-editor">
486 |         <state relative-caret-position="-103">
487 |           <caret line="61" column="8" selection-start-line="61" selection-start-column="8" selection-end-line="61" selection-end-column="8" />
488 |         </state>
489 |       </provider>
490 |     </entry>
491 |     <entry file="file:///usr/local/include/opencv2/cvv/show_image.hpp">
492 |       <provider selected="true" editor-type-id="text-editor">
493 |         <state relative-caret-position="99">
494 |           <caret line="23" column="25" selection-start-line="23" selection-start-column="16" selection-end-line="23" selection-end-column="25" />
495 |         </state>
496 |       </provider>
497 |     </entry>
498 |     <entry file="file:///usr/local/include/opencv2/cvv/dmatch.hpp">
499 |       <provider selected="true" editor-type-id="text-editor">
500 |         <state relative-caret-position="230">
501 |           <caret line="54" column="20" selection-start-line="54" selection-start-column="20" selection-end-line="54" selection-end-column="20" />
502 |         </state>
503 |       </provider>
504 |     </entry>
505 |     <entry file="file:///usr/include/eigen3/Eigen/src/Geometry/Transform.h">
506 |       <provider selected="true" editor-type-id="text-editor">
507 |         <state relative-caret-position="148">
508 |           <caret line="689" column="34" selection-start-line="689" selection-start-column="34" selection-end-line="689" selection-end-column="34" />
509 |         </state>
510 |       </provider>
511 |     </entry>
512 |     <entry file="file:///usr/local/include/opencv2/core/affine.hpp">
513 |       <provider selected="true" editor-type-id="text-editor">
514 |         <state relative-caret-position="-2001">
515 |           <caret line="125" column="10" selection-start-line="125" selection-start-column="10" selection-end-line="125" selection-end-column="10" />
516 |         </state>
517 |       </provider>
518 |     </entry>
519 |     <entry file="file:///usr/local/include/pcl-1.8/pcl/visualization/pcl_visualizer.h">
520 |       <provider selected="true" editor-type-id="text-editor">
521 |         <state relative-caret-position="375">
522 |           <caret line="366" column="8" selection-start-line="366" selection-start-column="8" selection-end-line="366" selection-end-column="8" />
523 |         </state>
524 |       </provider>
525 |     </entry>
526 |     <entry file="file://$PROJECT_DIR$/../../git/opencv/modules/core/src/matrix_wrap.cpp">
527 |       <provider selected="true" editor-type-id="text-editor">
528 |         <state relative-caret-position="140">
529 |           <caret line="1231" selection-start-line="1231" selection-end-line="1231" />
530 |         </state>
531 |       </provider>
532 |     </entry>
533 |     <entry file="file://$PROJECT_DIR$/../../git/opencv/modules/core/src/convert.cpp">
534 |       <provider selected="true" editor-type-id="text-editor">
535 |         <state relative-caret-position="130">
536 |           <caret line="1322" selection-start-line="1322" selection-end-line="1322" />
537 |         </state>
538 |       </provider>
539 |     </entry>
540 |     <entry file="file:///usr/include/c++/5/bits/shared_ptr.h">
541 |       <provider selected="true" editor-type-id="text-editor">
542 |         <state relative-caret-position="139">
543 |           <caret line="301" column="6" selection-start-line="301" selection-start-column="6" selection-end-line="301" selection-end-column="6" />
544 |         </state>
545 |       </provider>
546 |     </entry>
547 |     <entry file="file:///usr/local/include/opencv2/cvv/call_meta_data.hpp">
548 |       <provider selected="true" editor-type-id="text-editor">
549 |         <state relative-caret-position="75">
550 |           <caret line="7" selection-start-line="7" selection-end-line="7" />
551 |         </state>
552 |       </provider>
553 |     </entry>
554 |     <entry file="file://$PROJECT_DIR$/main.cpp">
555 |       <provider selected="true" editor-type-id="text-editor">
556 |         <state relative-caret-position="419">
557 |           <caret line="45" column="9" selection-start-line="45" selection-start-column="9" selection-end-line="45" selection-end-column="9" />
558 |           <folding>
559 |             <element signature="e#0#19#0" expanded="true" />
560 |           </folding>
561 |         </state>
562 |       </provider>
563 |     </entry>
564 |     <entry file="file://$PROJECT_DIR$/../../git/opencv/modules/core/src/matrix.cpp">
565 |       <provider selected="true" editor-type-id="text-editor">
566 |         <state relative-caret-position="159">
567 |           <caret line="399" selection-start-line="399" selection-end-line="399" />
568 |         </state>
569 |       </provider>
570 |     </entry>
571 |     <entry file="file:///usr/local/include/opencv2/core/mat.inl.hpp">
572 |       <provider selected="true" editor-type-id="text-editor">
573 |         <state relative-caret-position="320">
574 |           <caret line="835" selection-start-line="835" selection-end-line="835" />
575 |         </state>
576 |       </provider>
577 |     </entry>
578 |     <entry file="file://$PROJECT_DIR$/../../git/opencv/modules/core/src/system.cpp">
579 |       <provider selected="true" editor-type-id="text-editor">
580 |         <state relative-caret-position="13695">
581 |           <caret line="913" selection-start-line="913" selection-end-line="913" />
582 |         </state>
583 |       </provider>
584 |     </entry>
585 |     <entry file="file:///usr/local/include/opencv2/features2d.hpp">
586 |       <provider selected="true" editor-type-id="text-editor">
587 |         <state relative-caret-position="309">
588 |           <caret line="895" column="68" selection-start-line="895" selection-start-column="64" selection-end-line="895" selection-end-column="68" />
589 |         </state>
590 |       </provider>
591 |     </entry>
592 |     <entry file="file:///usr/include/c++/5/bits/unordered_map.h">
593 |       <provider selected="true" editor-type-id="text-editor">
594 |         <state relative-caret-position="150">
595 |           <caret line="613" column="6" selection-start-line="613" selection-start-column="6" selection-end-line="613" selection-end-column="6" />
596 |         </state>
597 |       </provider>
598 |     </entry>
599 |     <entry file="file://$PROJECT_DIR$/MapPoint.cpp">
600 |       <provider selected="true" editor-type-id="text-editor">
601 |         <state relative-caret-position="105">
602 |           <caret line="7" lean-forward="true" selection-start-line="7" selection-end-line="7" />
603 |         </state>
604 |       </provider>
605 |     </entry>
606 |     <entry file="file://$PROJECT_DIR$/../../git/opencv/modules/core/include/opencv2/core/base.hpp">
607 |       <provider selected="true" editor-type-id="text-editor">
608 |         <state relative-caret-position="142">
609 |           <caret line="565" selection-start-line="565" selection-end-line="565" />
610 |         </state>
611 |       </provider>
612 |     </entry>
613 |     <entry file="file:///usr/include/c++/5/bits/shared_ptr_base.h">
614 |       <provider selected="true" editor-type-id="text-editor">
615 |         <state relative-caret-position="139">
616 |           <caret line="1054" selection-start-line="1054" selection-end-line="1054" />
617 |         </state>
618 |       </provider>
619 |     </entry>
620 |     <entry file="file:///usr/local/include/opencv2/core/hal/interface.h">
621 |       <provider selected="true" editor-type-id="text-editor">
622 |         <state relative-caret-position="150">
623 |           <caret line="75" column="8" selection-start-line="75" selection-start-column="8" selection-end-line="75" selection-end-column="8" />
624 |         </state>
625 |       </provider>
626 |     </entry>
627 |     <entry file="file://$PROJECT_DIR$/Camera.cpp">
628 |       <provider selected="true" editor-type-id="text-editor">
629 |         <state relative-caret-position="456">
630 |           <caret line="36" column="45" selection-start-line="36" selection-start-column="45" selection-end-line="36" selection-end-column="45" />
631 |         </state>
632 |       </provider>
633 |     </entry>
634 |     <entry file="file:///usr/local/include/opencv2/core/mat.hpp">
635 |       <provider selected="true" editor-type-id="text-editor">
636 |         <state relative-caret-position="324">
637 |           <caret line="1211" column="33" selection-start-line="1211" selection-start-column="33" selection-end-line="1211" selection-end-column="44" />
638 |         </state>
639 |       </provider>
640 |     </entry>
641 |     <entry file="file://$PROJECT_DIR$/../../slambook/3rdparty/Sophus/sophus/so3.h">
642 |       <provider selected="true" editor-type-id="text-editor">
643 |         <state relative-caret-position="75">
644 |           <caret line="110" column="22" selection-start-line="110" selection-start-column="22" selection-end-line="110" selection-end-column="22" />
645 |         </state>
646 |       </provider>
647 |     </entry>
648 |     <entry file="file:///usr/include/eigen3/Eigen/src/Geometry/Quaternion.h">
649 |       <provider selected="true" editor-type-id="text-editor">
650 |         <state relative-caret-position="161">
651 |           <caret line="226" column="6" selection-start-line="226" selection-start-column="6" selection-end-line="226" selection-end-column="6" />
652 |         </state>
653 |       </provider>
654 |     </entry>
655 |     <entry file="file:///usr/local/include/opencv2/core/eigen.hpp">
656 |       <provider selected="true" editor-type-id="text-editor">
657 |         <state relative-caret-position="290">
658 |           <caret line="79" column="21" selection-start-line="79" selection-start-column="18" selection-end-line="79" selection-end-column="21" />
659 |           <folding>
660 |             <element signature="e#2645#2723#0" expanded="true" />
661 |           </folding>
662 |         </state>
663 |       </provider>
664 |     </entry>
665 |     <entry file="file:///usr/local/include/opencv2/core/types.hpp">
666 |       <provider selected="true" editor-type-id="text-editor">
667 |         <state relative-caret-position="142">
668 |           <caret line="270" column="15" selection-start-line="270" selection-start-column="8" selection-end-line="270" selection-end-column="15" />
669 |           <folding>
670 |             <element signature="e#7848#7862#0" expanded="true" />
671 |           </folding>
672 |         </state>
673 |       </provider>
674 |     </entry>
675 |     <entry file="file:///usr/include/eigen3/Eigen/src/Geometry/RotationBase.h">
676 |       <provider selected="true" editor-type-id="text-editor">
677 |         <state relative-caret-position="161">
678 |           <caret line="36" column="33" selection-start-line="36" selection-start-column="30" selection-end-line="36" selection-end-column="33" />
679 |         </state>
680 |       </provider>
681 |     </entry>
682 |     <entry file="file://$PROJECT_DIR$/Frame.cpp">
683 |       <provider selected="true" editor-type-id="text-editor">
684 |         <state relative-caret-position="165">
685 |           <caret line="18" column="42" selection-start-line="18" selection-start-column="31" selection-end-line="18" selection-end-column="42" />
686 |         </state>
687 |       </provider>
688 |     </entry>
689 |     <entry file="file://$PROJECT_DIR$/../../slambook/3rdparty/Sophus/sophus/se3.h">
690 |       <provider selected="true" editor-type-id="text-editor">
691 |         <state relative-caret-position="75">
692 |           <caret line="27" column="16" selection-start-line="27" selection-start-column="10" selection-end-line="27" selection-end-column="16" />
693 |         </state>
694 |       </provider>
695 |     </entry>
696 |     <entry file="file://$PROJECT_DIR$/common_include.h">
697 |       <provider selected="true" editor-type-id="text-editor">
698 |         <state relative-caret-position="345">
699 |           <caret line="25" selection-start-line="25" selection-end-line="25" />
700 |           <folding>
701 |             <element signature="e#458#475#0" expanded="true" />
702 |           </folding>
703 |         </state>
704 |       </provider>
705 |     </entry>
706 |     <entry file="file://$PROJECT_DIR$/CMakeLists.txt">
707 |       <provider selected="true" editor-type-id="text-editor">
708 |         <state relative-caret-position="451">
709 |           <caret line="54" column="26" selection-start-line="54" selection-start-column="10" selection-end-line="54" selection-end-column="26" />
710 |         </state>
711 |       </provider>
712 |     </entry>
713 |     <entry file="file:///usr/include/eigen3/Eigen/src/Geometry/AngleAxis.h">
714 |       <provider selected="true" editor-type-id="text-editor">
715 |         <state relative-caret-position="-295">
716 |           <caret line="96" column="32" lean-forward="true" selection-start-line="96" selection-start-column="32" selection-end-line="96" selection-end-column="32" />
717 |           <folding>
718 |             <element signature="e#1524#1542#0" expanded="true" />
719 |           </folding>
720 |         </state>
721 |       </provider>
722 |     </entry>
723 |     <entry file="file:///usr/local/include/ceres/rotation.h">
724 |       <provider selected="true" editor-type-id="text-editor">
725 |         <state relative-caret-position="16">
726 |           <caret line="522" column="26" selection-start-line="522" selection-start-column="5" selection-end-line="522" selection-end-column="26" />
727 |         </state>
728 |       </provider>
729 |     </entry>
730 |     <entry file="file:///usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h">
731 |       <provider selected="true" editor-type-id="text-editor">
732 |         <state relative-caret-position="150">
733 |           <caret line="804" selection-start-line="804" selection-end-line="804" />
734 |         </state>
735 |       </provider>
736 |     </entry>
737 |     <entry file="file:///usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h">
738 |       <provider selected="true" editor-type-id="text-editor">
739 |         <state relative-caret-position="150">
740 |           <caret line="31" selection-start-line="31" selection-end-line="31" />
741 |         </state>
742 |       </provider>
743 |     </entry>
744 |     <entry file="file:///usr/include/eigen3/Eigen/src/Core/Matrix.h">
745 |       <provider selected="true" editor-type-id="text-editor">
746 |         <state relative-caret-position="217">
747 |           <caret line="295" selection-start-line="295" selection-end-line="295" />
748 |         </state>
749 |       </provider>
750 |     </entry>
751 |     <entry file="file:///usr/local/include/opencv2/core/matx.hpp">
752 |       <provider selected="true" editor-type-id="text-editor">
753 |         <state relative-caret-position="421">
754 |           <caret line="211" column="8" selection-start-line="211" selection-start-column="8" selection-end-line="211" selection-end-column="8" />
755 |           <folding>
756 |             <element signature="e#3813#3841#0" expanded="true" />
757 |           </folding>
758 |         </state>
759 |       </provider>
760 |     </entry>
761 |     <entry file="file://$PROJECT_DIR$/MapPoint.h">
762 |       <provider selected="true" editor-type-id="text-editor">
763 |         <state relative-caret-position="172">
764 |           <caret line="45" column="30" selection-start-line="45" selection-start-column="30" selection-end-line="45" selection-end-column="30" />
765 |           <folding>
766 |             <element signature="e#105#132#0" expanded="true" />
767 |             <element signature="e#833#845#0" expanded="true" />
768 |             <element signature="e#989#1001#0" expanded="true" />
769 |           </folding>
770 |         </state>
771 |       </provider>
772 |     </entry>
773 |     <entry file="file://$PROJECT_DIR$/Camera.h">
774 |       <provider selected="true" editor-type-id="text-editor">
775 |         <state relative-caret-position="135">
776 |           <caret line="22" column="28" selection-start-line="22" selection-start-column="28" selection-end-line="22" selection-end-column="28" />
777 |           <folding>
778 |             <element signature="e#428#440#0" expanded="true" />
779 |           </folding>
780 |         </state>
781 |       </provider>
782 |     </entry>
783 |     <entry file="file://$PROJECT_DIR$/Map.cpp">
784 |       <provider selected="true" editor-type-id="text-editor">
785 |         <state relative-caret-position="160">
786 |           <caret line="22" column="14" selection-start-line="22" selection-start-column="14" selection-end-line="22" selection-end-column="14" />
787 |         </state>
788 |       </provider>
789 |     </entry>
790 |     <entry file="file://$PROJECT_DIR$/Map.h">
791 |       <provider selected="true" editor-type-id="text-editor">
792 |         <state relative-caret-position="330">
793 |           <caret line="22" column="18" selection-start-line="22" selection-start-column="18" selection-end-line="22" selection-end-column="18" />
794 |           <folding>
795 |             <element signature="e#95#116#0" expanded="true" />
796 |           </folding>
797 |         </state>
798 |       </provider>
799 |     </entry>
800 |     <entry file="file:///usr/include/c++/5/bits/stl_map.h">
801 |       <provider selected="true" editor-type-id="text-editor">
802 |         <state relative-caret-position="167">
803 |           <caret line="95" column="10" selection-start-line="95" selection-start-column="10" selection-end-line="95" selection-end-column="10" />
804 |         </state>
805 |       </provider>
806 |     </entry>
807 |     <entry file="file://$PROJECT_DIR$/Frame.h">
808 |       <provider selected="true" editor-type-id="text-editor">
809 |         <state relative-caret-position="230">
810 |           <caret line="55" column="61" selection-start-line="55" selection-start-column="61" selection-end-line="55" selection-end-column="61" />
811 |           <folding>
812 |             <element signature="e#99#126#0" expanded="true" />
813 |             <element signature="e#1322#1334#0" expanded="true" />
814 |             <element signature="e#1648#1660#0" expanded="true" />
815 |           </folding>
816 |         </state>
817 |       </provider>
818 |     </entry>
819 |     <entry file="file:///usr/local/include/opencv2/calib3d.hpp">
820 |       <provider selected="true" editor-type-id="text-editor">
821 |         <state relative-caret-position="293">
822 |           <caret line="1654" column="17" selection-start-line="1654" selection-start-column="17" selection-end-line="1654" selection-end-column="17" />
823 |         </state>
824 |       </provider>
825 |     </entry>
826 |     <entry file="file://$PROJECT_DIR$/SFM.h">
827 |       <provider selected="true" editor-type-id="text-editor">
828 |         <state relative-caret-position="260">
829 |           <caret line="60" column="15" selection-start-line="60" selection-start-column="15" selection-end-line="60" selection-end-column="15" />
830 |           <folding>
831 |             <element signature="e#95#122#0" expanded="true" />
832 |           </folding>
833 |         </state>
834 |       </provider>
835 |     </entry>
836 |     <entry file="file://$PROJECT_DIR$/SFM.cpp">
837 |       <provider selected="true" editor-type-id="text-editor">
838 |         <state relative-caret-position="309">
839 |           <caret line="254" column="52" selection-start-line="254" selection-start-column="52" selection-end-line="254" selection-end-column="52" />
840 |           <folding>
841 |             <element signature="e#46#62#0" expanded="true" />
842 |             <element signature="e#804#881#0" expanded="true" />
843 |           </folding>
844 |         </state>
845 |       </provider>
846 |     </entry>
847 |     <entry file="file://$PROJECT_DIR$/BA.cpp">
848 |       <provider selected="true" editor-type-id="text-editor">
849 |         <state relative-caret-position="120">
850 |           <caret line="8" selection-start-line="8" selection-end-line="8" />
851 |           <folding>
852 |             <element signature="e#87#1909#0" />
853 |             <element signature="e#1068#1297#0" />
854 |             <element signature="e#1372#1592#0" />
855 |             <element signature="e#1669#1892#0" />
856 |             <element signature="e#1919#4506#0" />
857 |             <element signature="e#4516#5052#0" />
858 |             <element signature="e#5062#5223#0" />
859 |             <element signature="e#4909#5345#0" />
860 |             <element signature="e#5323#5494#0" />
861 |           </folding>
862 |         </state>
863 |       </provider>
864 |     </entry>
865 |     <entry file="file://$PROJECT_DIR$/BA.h">
866 |       <provider selected="true" editor-type-id="text-editor">
867 |         <state relative-caret-position="-13">
868 |           <caret line="64" column="41" lean-forward="true" selection-start-line="64" selection-start-column="41" selection-end-line="64" selection-end-column="41" />
869 |           <folding>
870 |             <element signature="e#93#117#0" expanded="true" />
871 |             <element signature="e#333#1569#1" />
872 |             <element signature="e#509#1559#0" />
873 |           </folding>
874 |         </state>
875 |       </provider>
876 |     </entry>
877 |   </component>
878 | </project>


--------------------------------------------------------------------------------
/BA.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by pidan1231239 on 18-6-23.
  3 | //
  4 | 
  5 | #include "BA.h"
  6 | 
  7 | namespace sky{
  8 | 
  9 |     void BA::loadMap() {
 10 | #ifdef DEBUG
 11 |         cout << "BA::loadMap: " << endl;
 12 | #endif
 13 |         //加载mapPointsPos
 14 |         for (auto &mapPoints:map->mapPoints) {
 15 |             mapPointsPos[mapPoints] = mapPoints->getPosMatx13<double>();
 16 |         }
 17 |         //加载frameExtrinsics和cameraIntrinsics
 18 |         for (auto &frame:map->frames) {
 19 |             auto angleAxis = frame->getAngleAxisWcMatxCV<double>();
 20 |             auto t = frame->Tcw.translation();
 21 |             frameExtrinsics[frame] = Matx23d(angleAxis(0), angleAxis(1), angleAxis(2),
 22 |                                              t[0], t[1], t[2]);
 23 |             if (cameraIntrinsics.find(frame->camera) == cameraIntrinsics.end())
 24 |                 cameraIntrinsics[frame->camera] = Matx14d(
 25 |                         frame->camera->fx, frame->camera->fy, frame->camera->cx, frame->camera->cy);
 26 |         }
 27 | #ifdef DEBUG
 28 |         cout << "\t" << mapPointsPos.size() << " map points" << endl;
 29 | /*            int i = 0;
 30 |             for (auto &mapPoints:mapPointsPos) {
 31 |                 cout << mapPoints.second << endl;
 32 |                 ++i;
 33 |                 if (i >= 5)break;
 34 |             }
 35 |             cout << "..." << endl;*/
 36 | 
 37 |         cout << "\t" << frameExtrinsics.size() << " frames" << endl;
 38 | /*            i = 0;
 39 |             for (auto &frame:frameExtrinsics) {
 40 |                 cout << frame.second << endl;
 41 |                 ++i;
 42 |                 if (i >= 5)break;
 43 |             }
 44 |             cout << "..." << endl;*/
 45 | 
 46 |         cout << "\t" << cameraIntrinsics.size() << " cameras" << endl;
 47 | /*            i = 0;
 48 |             for (auto &camera:cameraIntrinsics) {
 49 |                 cout << camera.second << endl;
 50 |                 ++i;
 51 |                 if (i >= 5)break;
 52 |             }
 53 |             cout << "..." << endl;*/
 54 | #endif
 55 |     }
 56 | 
 57 |     void BA::bundleAdjustment() {
 58 | #ifdef DEBUG
 59 |         cout << "BA::bundleAdjustment: " << endl;
 60 | #endif
 61 |         ceres::Problem problem;
 62 | 
 63 | #ifdef DEBUG
 64 |         cout << "\tloading frameExtrinsics..." << endl;
 65 | #endif
 66 |         for (auto &frameExtrinsic:frameExtrinsics)
 67 |             problem.AddParameterBlock(frameExtrinsic.second.val, 6);
 68 |         problem.SetParameterBlockConstant(frameExtrinsics[map->frames.front()].val);
 69 | 
 70 | #ifdef DEBUG
 71 |         cout << "\tloading cameraIntrinsics..." << endl;
 72 | #endif
 73 |         for (auto &cameraIntrinsic:cameraIntrinsics)
 74 |             problem.AddParameterBlock(cameraIntrinsic.second.val, 4);
 75 | 
 76 | #ifdef DEBUG
 77 |         cout << "\tloading mapPointsPos..." << endl;
 78 | #endif
 79 |         ceres::LossFunction *lossFunction = new ceres::HuberLoss(4);
 80 |         for (auto &mapPointPos:mapPointsPos) {
 81 |             for (auto &observedFrame:mapPointPos.first->observedFrames) {
 82 |                 ceres::CostFunction *costFunction =
 83 |                         new ceres::AutoDiffCostFunction<ReprojectCost, 2, 4, 6, 3>(
 84 |                                 new ReprojectCost(observedFrame.second));
 85 |                 problem.AddResidualBlock(
 86 |                         costFunction,
 87 |                         lossFunction,
 88 |                         cameraIntrinsics[observedFrame.first->camera].val,            // Intrinsic
 89 |                         frameExtrinsics[observedFrame.first].val,  // View Rotation and Translation
 90 |                         mapPointPos.second.val          // Point in 3D space
 91 |                 );
 92 |             }
 93 |         }
 94 | 
 95 | #ifdef DEBUG
 96 |         cout << "\tsolving BA..." << endl;
 97 | #endif
 98 | 
 99 |         ceres::Solver::Summary summary;
100 |         ceres::Solve(ceres_config_options, &problem, &summary);
101 | 
102 |         if (!summary.IsSolutionUsable()) {
103 |             std::cout << "Bundle Adjustment failed." << std::endl;
104 |         } else {
105 |             // Display statistics about the minimization
106 |             std::cout << "Bundle Adjustment statistics (approximated RMSE):\n"
107 |                       << "    #views: " << frameExtrinsics.size() << "\n"
108 |                       << "    #residuals: " << summary.num_residuals << "\n"
109 |                       << "    Initial RMSE: " << std::sqrt(summary.initial_cost / summary.num_residuals) << "\n"
110 |                       << "    Final RMSE: " << std::sqrt(summary.final_cost / summary.num_residuals) << "\n"
111 |                       << "    Time (s): " << summary.total_time_in_seconds << "\n";
112 |         }
113 |     }
114 | 
115 |     void BA::writeMap() {
116 |         //写mapPointsPos
117 |         for (auto &mapPointPos:mapPointsPos) {
118 |             mapPointPos.first->setPos(mapPointPos.second);
119 |         }
120 |         //写frameExtrinsics
121 |         for (auto &frameExtrinsic:frameExtrinsics) {
122 |             frameExtrinsic.first->setTcw(frameExtrinsic.second);
123 |         }
124 |         //写cameraIntrinsics
125 |         for (auto &cameraIntrinsic:cameraIntrinsics) {
126 |             cameraIntrinsic.first->setIntrinsic(cameraIntrinsic.second);
127 |         }
128 |     }
129 | 
130 |     void BA::clear() {
131 |         map = nullptr;
132 | 
133 |         cameraIntrinsics.clear();
134 |         frameExtrinsics.clear();
135 |         mapPointsPos.clear();
136 |     }
137 | 
138 |     BA::BA() {
139 |         ceres_config_options.minimizer_progress_to_stdout = false;
140 |         ceres_config_options.logging_type = ceres::SILENT;
141 |         ceres_config_options.num_threads = 1;
142 |         ceres_config_options.preconditioner_type = ceres::JACOBI;
143 |         ceres_config_options.linear_solver_type = ceres::SPARSE_SCHUR;
144 |         ceres_config_options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;
145 |     }
146 | 
147 |     void BA::operator()(Map::Ptr &map) {
148 |         this->map = map;
149 |         loadMap();
150 |         bundleAdjustment();
151 |         writeMap();
152 |         clear();
153 |     }
154 | 
155 | }


--------------------------------------------------------------------------------
/BA.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-23.
 3 | //
 4 | 
 5 | #ifndef SFM_LEARN_BA_H
 6 | #define SFM_LEARN_BA_H
 7 | 
 8 | #include <ceres/ceres.h>
 9 | #include <ceres/rotation.h>
10 | #include <opencv2/opencv.hpp>
11 | #include "Map.h"
12 | #include "Frame.h"
13 | #include "common_include.h"
14 | #include <unordered_map>
15 | 
16 | namespace sky {
17 | 
18 |     using namespace std;
19 | 
20 |     class BA {
21 | 
22 |         class ReprojectCost {
23 |             cv::Point2d observation;
24 |         public:
25 |             ReprojectCost(const cv::Point2d &observation) : observation(observation) {}
26 | 
27 |             template<typename T>
28 |             bool
29 |             operator()(const T *const intrinsic, const T *const extrinsic, const T *const pos3d, T *residuals) const {
30 |                 const T *r = extrinsic;
31 |                 const T *t = &extrinsic[3];
32 | 
33 |                 T pos_proj[3];
34 |                 ceres::AngleAxisRotatePoint(r, pos3d, pos_proj);
35 | 
36 |                 // Apply the camera translation
37 |                 pos_proj[0] += t[0];
38 |                 pos_proj[1] += t[1];
39 |                 pos_proj[2] += t[2];
40 | 
41 |                 const T x = pos_proj[0] / pos_proj[2];
42 |                 const T y = pos_proj[1] / pos_proj[2];
43 | 
44 |                 const T fx = intrinsic[0];
45 |                 const T fy = intrinsic[1];
46 |                 const T cx = intrinsic[2];
47 |                 const T cy = intrinsic[3];
48 | 
49 |                 // Apply intrinsic
50 |                 const T u = fx * x + cx;
51 |                 const T v = fy * y + cy;
52 | 
53 |                 residuals[0] = u - T(observation.x);
54 |                 residuals[1] = v - T(observation.y);
55 | 
56 |                 return true;
57 |             }
58 |         };
59 | 
60 |         Map::Ptr map;
61 |         ceres::Solver::Options ceres_config_options;
62 | 
63 |         unordered_map<Camera::Ptr, Matx14d> cameraIntrinsics;
64 |         unordered_map<Frame::Ptr, Matx23d> frameExtrinsics;
65 |         unordered_map<MapPoint::Ptr, Matx13d> mapPointsPos;
66 | 
67 |         void loadMap();
68 | 
69 |         void bundleAdjustment();
70 | 
71 |         void writeMap();
72 | 
73 |         void clear();
74 | 
75 |     public:
76 | 
77 |         BA();
78 | 
79 |         void operator()(Map::Ptr &map);
80 | 
81 | 
82 |     };
83 | 
84 | }
85 | 
86 | 
87 | #endif //SFM_LEARN_BA_H
88 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | project(sfm_learn)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 14)
 5 | 
 6 | # (un)set: cmake -DCVV_DEBUG_MODE=OFF ..
 7 | OPTION(CVV_DEBUG_MODE "cvvisual-debug-mode" ON)
 8 | if(CVV_DEBUG_MODE MATCHES ON)
 9 |     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DCVVISUAL_DEBUGMODE")
10 | endif()
11 | 
12 | # (un)set: cmake -CLOUDVIEWER_DEBUG_MODE=OFF ..
13 | OPTION(CLOUDVIEWER_DEBUG_MODE "cloudviewer-debug-mode" ON)
14 | if(CLOUDVIEWER_DEBUG_MODE MATCHES ON)
15 |     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DCLOUDVIEWER_DEBUG")
16 | endif()
17 | 
18 | # (un)set: cmake -DDEBUG_MODE=OFF ..
19 | OPTION(DEBUG_MODE "debug-mode" ON)
20 | if(DEBUG_MODE MATCHES ON)
21 |     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DDEBUG")
22 | endif()
23 | 
24 | 
25 | 
26 | list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
27 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
28 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
29 | 
30 | # Eigen
31 | include_directories("/usr/include/eigen3")
32 | # OpenCV
33 | find_package(OpenCV 3.1 REQUIRED)
34 | include_directories(${OpenCV_INCLUDE_DIRS})
35 | # Sophus 
36 | find_package(Sophus REQUIRED)
37 | include_directories(${Sophus_INCLUDE_DIRS})
38 | # G2O
39 | find_package(G2O REQUIRED)
40 | include_directories(${G2O_INCLUDE_DIRS})
41 | # Ceres
42 | find_package(Ceres REQUIRED)
43 | include_directories(${CERES_INCLUDE_DIRS} )
44 | # Boost
45 | find_package(Boost      REQUIRED COMPONENTS system chrono filesystem unit_test_framework program_options)
46 | include_directories(${Boost_INCLUDE_DIR})
47 | # PCL
48 | find_package(PCL REQUIRED COMPONENTS common io visualization)
49 | include_directories(${PCL_INCLUDE_DIRS})
50 | link_directories(${PCL_LIBRARY_DIRS})
51 | add_definitions(${PCL_DEFINITIONS})
52 | 
53 | set(THIRD_PARTY_LIBS
54 |         ${OpenCV_LIBS}
55 |         ${Sophus_LIBRARIES}
56 |         g2o_core g2o_stuff g2o_types_sba
57 |         ${CERES_LIBRARIES}
58 |         ${Boost_LIBRARIES}
59 |         opencv_cvv
60 |         opencv_xfeatures2d
61 |         ${PCL_LIBRARIES}
62 |         )
63 | 
64 | add_executable(sfm_learn main.cpp Camera.cpp Camera.h common_include.h Frame.cpp Frame.h MapPoint.cpp MapPoint.h Map.cpp Map.h SFM.cpp SFM.h BA.cpp BA.h)
65 | target_link_libraries(sfm_learn ${THIRD_PARTY_LIBS})


--------------------------------------------------------------------------------
/CVVisual:
--------------------------------------------------------------------------------
 1 | [STFLEngine]
 2 | Overview\settings=type match, raw
 3 | Rawview\settings=group by keypoint_type
 4 | 
 5 | [default_views]
 6 | default_filter_view=DefaultFilterView
 7 | default_match_view=TranslationMatchView
 8 | 
 9 | [overview]
10 | imgsize=100
11 | imgzoom=46
12 | 


--------------------------------------------------------------------------------
/Camera.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #include "Camera.h"
 6 | 
 7 | namespace sky {
 8 | 
 9 |     Vector3d Camera::world2camera(const Vector3d &p_w, const SE3 &T_c_w) {
10 |         return T_c_w * p_w;
11 |     }
12 | 
13 |     Vector3d Camera::camera2world(const Vector3d &p_c, const SE3 &T_c_w) {
14 |         return T_c_w.inverse() * p_c;
15 |     }
16 | 
17 |     Vector2d Camera::camera2pixel(const Vector3d &p_c) {
18 |         return Vector2d(
19 |                 fx * p_c(0, 0) / p_c(2, 0) + cx,
20 |                 fy * p_c(1, 0) / p_c(2, 0) + cy
21 |         );
22 |     }
23 | 
24 |     Vector3d Camera::pixel2camera(const Vector2d &p_p, double depth) {
25 |         return Vector3d(
26 |                 (p_p(0, 0) - cx) * depth / fx,
27 |                 (p_p(1, 0) - cy) * depth / fy,
28 |                 depth
29 |         );
30 |     }
31 | 
32 |     Vector2d Camera::world2pixel(const Vector3d &p_w, const SE3 &T_c_w) {
33 |         return camera2pixel(world2camera(p_w, T_c_w));
34 |     }
35 | 
36 |     Vector3d Camera::pixel2world(const Vector2d &p_p, const SE3 &T_c_w, double depth) {
37 |         return camera2world(pixel2camera(p_p, depth), T_c_w);
38 |     }
39 | 
40 |     Point2f Camera::pixel2normal(const Point2d &p) const{
41 |         return Point2f
42 |                 (
43 |                         (p.x - cx) / fx,
44 |                         (p.y - cy) / fy
45 |                 );
46 |     }
47 | 
48 | 
49 | }
50 | 
51 | 


--------------------------------------------------------------------------------
/Camera.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #ifndef SFM_LEARN_CAMERA_H
 6 | #define SFM_LEARN_CAMERA_H
 7 | 
 8 | #include "common_include.h"
 9 | #include "opencv2/opencv.hpp"
10 | 
11 | namespace sky {
12 | 
13 |     using namespace cv;
14 | 
15 |     class Camera {
16 |     public:
17 |         typedef shared_ptr<Camera> Ptr;
18 |         float fx, fy, cx, cy;
19 | 
20 |         Camera(float fx, float fy, float cx, float cy) :
21 |                 fx(fx), fy(fy), cx(cx), cy(cy) {}
22 | 
23 |         template<typename T>
24 |         void setIntrinsic(Matx<T, 1, 4> intrinsic) {
25 |             fx=intrinsic(0);
26 |             fy=intrinsic(1);
27 |             cx=intrinsic(2);
28 |             cy=intrinsic(3);
29 |         }
30 | 
31 |         //坐标转换
32 | 
33 |         Vector3d world2camera(const Vector3d &p_w, const SE3 &T_c_w);
34 | 
35 |         Vector3d camera2world(const Vector3d &p_c, const SE3 &T_c_w);
36 | 
37 |         Vector2d camera2pixel(const Vector3d &p_c);
38 | 
39 |         Vector3d pixel2camera(const Vector2d &p_p, double depth = 1);
40 | 
41 |         Vector3d pixel2world(const Vector2d &p_p, const SE3 &T_c_w, double depth = 1);
42 | 
43 |         Vector2d world2pixel(const Vector3d &p_w, const SE3 &T_c_w);
44 | 
45 |         Point2f pixel2normal(const Point2d &p) const;
46 | 
47 |         //获取参数
48 |         float getFocalLength() {
49 |             return (fx + fy) / 2;
50 |         }
51 | 
52 |         cv::Point2d getPrincipalPoint() {
53 |             return cv::Point2d(cx, cy);
54 |         }
55 | 
56 |         cv::Matx<float, 3, 3> getKMatxCV() {
57 |             return cv::Matx<float, 3, 3>(fx, 0, cx, 0, fy, cy, 0, 0, 1);
58 |         }
59 | 
60 |         cv::Mat getKMatCV() {
61 |             Mat K = (Mat_<float>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
62 |             return K;
63 |         }
64 |     };
65 | 
66 | }
67 | 
68 | 
69 | #endif //SFM_LEARN_CAMERA_H
70 | 


--------------------------------------------------------------------------------
/Frame.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #include "Frame.h"
 6 | 
 7 | namespace sky {
 8 |     Frame::Frame()
 9 |             : camera(nullptr) {
10 | 
11 |     }
12 | 
13 |     Frame::~Frame() {
14 | 
15 |     }
16 | 
17 | 
18 |     Vector3d Frame::getCamCenterEigen() const {
19 |         return Tcw.inverse().translation();
20 |     }
21 | 
22 | 
23 |     bool Frame::isInFrame(const Vector3d &pt_world) {
24 |         // cout<<"pt_world = "<<endl<<pt_world<<endl;
25 |         Vector3d p_cam = camera->world2camera(pt_world, Tcw);
26 |         // cout<<"P_cam = "<<p_cam.transpose()<<endl;
27 |         if (p_cam(2, 0) < 0) return false;
28 |         Vector2d pixel = camera->world2pixel(pt_world, Tcw);
29 |         // cout<<"P_pixel = "<<pixel.transpose()<<endl<<endl;
30 |         return pixel(0, 0) > 0 && pixel(1, 0) > 0
31 |                && pixel(0, 0) < cols
32 |                && pixel(1, 0) < rows;
33 |     }
34 | 
35 | }
36 | 


--------------------------------------------------------------------------------
/Frame.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #ifndef SFM_LEARN_FRAME_H
 6 | #define SFM_LEARN_FRAME_H
 7 | 
 8 | #include "common_include.h"
 9 | #include "Camera.h"
10 | #include <opencv2/opencv.hpp>
11 | #include <opencv2/core/eigen.hpp>
12 | 
13 | namespace sky {
14 | 
15 |     class Frame {
16 |     public:
17 |         typedef shared_ptr<Frame> Ptr;
18 |         SE3 Tcw;      // transform from world to camera
19 |         Camera::Ptr camera;     // Pinhole RGBD Camera model
20 |         int cols, rows;
21 | 
22 |         Frame();
23 | 
24 |         Frame(const Camera::Ptr &camera, const Mat &image) :
25 |                 camera(camera), cols(image.cols), rows(image.rows) {}
26 | 
27 |         ~Frame();
28 | 
29 | 
30 |         Vector3d getCamCenterEigen() const;
31 | 
32 |         cv::Mat getTcwMatCV(int rtype) {
33 |             cv::Mat TcwCV, TcwCVR;
34 |             cv::eigen2cv(Tcw.matrix(), TcwCV);
35 |             TcwCV.convertTo(TcwCVR, rtype);
36 |             return TcwCVR;
37 |         }
38 | 
39 |         cv::Mat getTcw34MatCV(int rtype) {
40 |             auto TcwCV = getTcwMatCV(rtype);
41 |             Mat Tcw34;
42 |             TcwCV(cv::Range(0, 3), cv::Range(0, 4)).convertTo(Tcw34, rtype);
43 |             return Tcw34;
44 |         }
45 | 
46 |         cv::Mat getTwcMatCV(int rtype) {
47 |             cv::Mat TwcCV, TwcCVR;
48 |             cv::eigen2cv(Tcw.inverse().matrix(), TwcCV);
49 |             TwcCV.convertTo(TwcCVR, rtype);
50 |             return TwcCVR;
51 |         }
52 | 
53 |         template<typename T>
54 |         cv::Matx<T, 1, 3> getAngleAxisWcMatxCV() {
55 |             Sophus::AngleAxisd angleAxis(Tcw.so3().matrix());
56 |             auto axis = angleAxis.angle() * angleAxis.axis();
57 |             cv::Matx<T, 1, 3> angleAxisCV(axis[0],axis[1],axis[2]);
58 |             return angleAxisCV;
59 |         };
60 | 
61 |         template<typename T>
62 |         void setTcw(Matx<T,2,3> angleAxisAndTranslation){
63 |             Tcw.so3()=SO3(angleAxisAndTranslation(0,0),
64 |                             angleAxisAndTranslation(0,1),
65 |                             angleAxisAndTranslation(0,2));
66 |             Tcw.translation()=Vector3d(angleAxisAndTranslation(1,0),
67 |                                          angleAxisAndTranslation(1,1),
68 |                                          angleAxisAndTranslation(1,2));
69 |         }
70 | 
71 | 
72 | 
73 | 
74 | /*        cv::Mat getProjMatCV() {
75 |             return camera->getKMatCV()*getTcw34MatCV();
76 |         }*/
77 | 
78 | 
79 |         // check if a point is in this frame
80 |         bool isInFrame(const Vector3d &pt_world);
81 |     };
82 | 
83 | }
84 | 
85 | 
86 | #endif //SFM_LEARN_FRAME_H
87 | 


--------------------------------------------------------------------------------
/Map.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #include "Map.h"
 6 | #ifdef CLOUDVIEWER_DEBUG
 7 | #include <pcl/common/common_headers.h>
 8 | #include <pcl/visualization/cloud_viewer.h>
 9 | #include <pcl/common/common_headers.h>
10 | #include <pcl/io/pcd_io.h>
11 | #include <pcl/point_types.h>
12 | #include <pcl/point_cloud.h>
13 | #include <pcl/visualization/pcl_visualizer.h>
14 | #endif
15 | 
16 | namespace sky {
17 | 
18 |     void Map::addFrame(Frame::Ptr frame) {
19 |         if (frame)
20 |             frames.push_back(frame);
21 |     }
22 | 
23 |     void Map::addMapPoint(MapPoint::Ptr mapPoint) {
24 |         if (mapPoint)
25 |             mapPoints.push_back(mapPoint);
26 |     }
27 | 
28 | #ifdef CLOUDVIEWER_DEBUG
29 |     void Map::visInCloudViewer() {
30 | 
31 |         pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
32 |         for (auto point:mapPoints) {
33 |             pcl::PointXYZRGB pointXYZ(point->rgb[0], point->rgb[1], point->rgb[2]);
34 |             pointXYZ.x = point->pos(0);
35 |             pointXYZ.y = point->pos(1);
36 |             pointXYZ.z = point->pos(2);
37 |             cloud->push_back(pointXYZ);
38 |         }
39 | 
40 |         pcl::visualization::PCLVisualizer viewer("Viewer");
41 |         viewer.setBackgroundColor(50, 50, 50);
42 |         viewer.addPointCloud(cloud, "Triangulated Point Cloud");
43 |         viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
44 |                                                 3,
45 |                                                 "Triangulated Point Cloud");
46 |         viewer.addCoordinateSystem(1.0);
47 | 
48 |         int indexFrame = 0;
49 |         for (auto &frame:frames) {
50 |             Eigen::Matrix4f camPose;
51 |             auto T_c_w = frame->Tcw.inverse().matrix();
52 |             for (int i = 0; i < camPose.rows(); ++i)
53 |                 for (int j = 0; j < camPose.cols(); ++j)
54 |                     camPose(i, j) = T_c_w(i, j);
55 |             viewer.addCoordinateSystem(1.0, Eigen::Affine3f(camPose), "cam" + to_string(indexFrame++));
56 |         }
57 |         viewer.initCameraParameters ();
58 |         while (!viewer.wasStopped ()) {
59 |             viewer.spin();
60 |         }
61 | 
62 |     }
63 | #endif
64 | 
65 | }
66 | 
67 | 


--------------------------------------------------------------------------------
/Map.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #ifndef SFM_LEARN_MAP_H
 6 | #define SFM_LEARN_MAP_H
 7 | 
 8 | #include "MapPoint.h"
 9 | #include "Frame.h"
10 | 
11 | namespace sky {
12 | 
13 |     class Map {
14 |     public:
15 |         typedef shared_ptr<Map> Ptr;
16 |         list<MapPoint::Ptr> mapPoints;        // all landmarks
17 |         list<Frame::Ptr> frames;         // all key-frames
18 | 
19 |         Map() {}
20 | 
21 |         void addFrame(Frame::Ptr frame);
22 | 
23 |         void addMapPoint(MapPoint::Ptr mapPoint);
24 | 
25 |         void visInCloudViewer();
26 |     };
27 | 
28 | }
29 | 
30 | 
31 | #endif //SFM_LEARN_MAP_H
32 | 


--------------------------------------------------------------------------------
/MapPoint.cpp:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #include "MapPoint.h"
 6 | 
 7 | namespace sky {
 8 | 
 9 | 
10 | 
11 | }
12 | 


--------------------------------------------------------------------------------
/MapPoint.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #ifndef SFM_LEARN_MAPPOINT_H
 6 | #define SFM_LEARN_MAPPOINT_H
 7 | 
 8 | #include "common_include.h"
 9 | #include "Frame.h"
10 | #include <utility>
11 | 
12 | namespace sky {
13 | 
14 |     class MapPoint {
15 | 
16 |     public:
17 | 
18 |         typedef shared_ptr<MapPoint> Ptr;
19 |         Mat descriptor; // Descriptor for matching
20 |         list<std::pair<Frame::Ptr,cv::Point2d>> observedFrames;//观测帧和像素坐标
21 |         Vector3d pos;       // Position in world
22 |         Vec3b rgb;
23 | 
24 | 
25 |         MapPoint();
26 | 
27 |         MapPoint(const Vector3d &pos, const Mat &descriptor, const Vec3b &rgb) :
28 |                 pos(pos),
29 |                 descriptor(descriptor),
30 |                 rgb(rgb) {}
31 | 
32 |         template <typename T>
33 |         cv::Point3_<T> getPosPoint3_CV() const {
34 |             return cv::Point3_<T>(pos(0, 0), pos(1, 0), pos(2, 0));
35 |         }
36 | 
37 |         template <typename T>
38 |         cv::Matx<T,1,3> getPosMatx13() const{
39 |             return cv::Matx<T,1,3>(pos(0, 0), pos(1, 0), pos(2, 0));
40 |         };
41 | 
42 |         template <typename T>
43 |         void setPos(cv::Matx<T,1,3> posMatx13){
44 |             pos(0)=posMatx13(0);
45 |             pos(1)=posMatx13(1);
46 |             pos(2)=posMatx13(2);
47 |         }
48 | 
49 |         void addObervedFrame(const Frame::Ptr &observedFrame,const cv::Point2d &pixelCoor) {
50 |             if (observedFrame)
51 |                 observedFrames.push_back(
52 |                         std::pair<Frame::Ptr,cv::Point2d>(observedFrame,pixelCoor));
53 |         }
54 |     };
55 | 
56 | 
57 | }
58 | 
59 | 
60 | #endif //SFM_LEARN_MAPPOINT_H
61 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | # sfm_learn
  2 | 一个基于opencv的SFM简单实现，学习用途。欢迎光临[我的主页](https://chengzhag.github.io/)查看更多其他项目详情。
  3 | 
  4 | # 问题简化
  5 | * 假设图像有序
  6 | * 直接用前两帧做初始化，解2D-2D，三角化建立初始全局地图
  7 | * 之后每帧与全局地图中上一帧范围内的点匹配，解3D-2D，三角化后向全局地图添加新点
  8 | * 最后对全局地图进行BA
  9 | 
 10 | # 完成进度
 11 | 
 12 | 以下示例采用数据[fountain-P11](http://icwww.epfl.ch/multiview/denseMVS.html)。其11张照片位于datasets/fountain_dense_images目录下。
 13 | 1. 前两帧做2D-2D初始化：
 14 | ![2D2D](images/2D2D.png)
 15 | 2. 之后每帧与全局地图中上一帧范围内的点匹配，解3D-2D，三角化后向全局地图添加新点：
 16 | ![3D2D](images/3D2D.png)
 17 | 3. 进行Bundle Adjustment前：
 18 | ![3D2D](images/beforeBA.png)
 19 | 3. 进行Bundle Adjustment后：
 20 | ![3D2D](images/afterBA.png)
 21 | 
 22 | 计划：
 23 | * 如有需要，继续改进SFM的效果
 24 | * 从SFM的实现转向SLAM
 25 | 
 26 | 
 27 | 
 28 | # 实现
 29 | 
 30 | > 本实现的类设计参考了《视觉SLAM十四讲：从理论到实践》第9讲中对一个深度相机视觉里程计的实现。
 31 | 
 32 | ## 类设计
 33 | 
 34 | * Camera：相机-世界-像素坐标转换，相机参数
 35 | * Frame：判断点是否在视野内，图像、位姿、相机
 36 | * MapPoint：路标点，描述子、所在帧
 37 | * Map：地图，存储路标、帧
 38 | * SFM：顺序读取输入的路径，完成简化小节描述的步骤
 39 | 
 40 | ## 问题
 41 | 
 42 | * opencv:
 43 |     * 找不到/usr/local/include/opencv2/viz.hpp：
 44 |         * 原因是编译opencv前没有安装vtk
 45 |         * [ubuntu安装OPENCV的VIZ模块](https://blog.csdn.net/j_____j/article/details/78788155)
 46 |     * int flag如何快速找到？
 47 |     * [cmake: how to build opencv in both debug and release mode?](https://stackoverflow.com/questions/29634489/cmake-how-to-build-opencv-in-both-debug-and-release-mode)
 48 |     * 在一个函数中通过at<uin8_t>()对一个局部变量Mat循环赋值后，报错corrupted double-linked list或mat opencv invalid next size或return 11
 49 |         * 原来是因为越界写入，at<>()没有越界判断
 50 | 
 51 | 
 52 | # 改进
 53 | 
 54 | * 筛选匹配点
 55 |     * 以最短描述子距离的倍数为阈值选择最匹配的描述点：slambook ch7 2d2d
 56 |     * 用FLANN或KNN做特征点匹配：Trying to match two images using sift in OpenCv, but too many matches
 57 |     * 参考opencv官方教程做特征点匹配：Feature Matching
 58 |     * Ratio Test方法：OpenCV实现SfM（二）：双目三维重建
 59 | * 对每次添加帧解3D-2D后，做一次优化
 60 | * 建立全局和局部地图，局部地图用于匹配，减少匹配时间
 61 | 
 62 | # 参考资料
 63 | 
 64 | ## SLAM
 65 | 
 66 | * [ORB-SLAM](http://webdiis.unizar.es/~raulmur/orbslam/)：
 67 |     > ORB-SLAM is a versatile and accurate SLAM solution for Monocular, Stereo and RGB-D cameras.
 68 |     * [github](https://github.com/raulmur/ORB_SLAM)
 69 |     * [ORB-SLAM（一）简介](http://www.cnblogs.com/luyb/p/5215168.html)：系例中文介绍
 70 |     * [ORB-SLAM2源码详解 by 吴博](http://rosclub.cn/post-505.html)：原理、流程、结构
 71 |     * [ORB-SLAM代码详解之SLAM系统初始化](https://blog.csdn.net/c602273091/article/details/54933760)：系列代码详解
 72 |     教程：
 73 |     *   [SLAM Course - WS13/14](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_)
 74 | *   [SLAM Course - WS13/14](https://www.youtube.com/playlist?list=PLgnQpQtFTOGQrZ4O5QzbIHgl3b1JHimN_)
 75 | 
 76 | ## SFM
 77 | 
 78 | * [Structure from Motion Using OpenCV](https://blog.csdn.net/zkl99999/article/details/46467277)：不同的特征点匹配方法、用[SSBA](https://github.com/chzach/SSBA/tree/master/SSBA-4.0)做BA、调用PCL的API处理和显示点云
 79 |     * [三维重建-opencv实现sfm](https://blog.csdn.net/qq_26499769/article/details/51817254)：转载，完整单篇实现，不含公式，用ceres做BA，代码需积分，文章中代码不完整
 80 |     * [OpenCV实现SfM（一）：相机模型](https://blog.csdn.net/AIchipmunk/article/details/48132109)
 81 |     * [OpenCV实现SfM（二）：双目三维重建](https://blog.csdn.net/AIchipmunk/article/details/48157369)
 82 |     * [OpenCV实现SfM（三）：多目三维重建](https://blog.csdn.net/aichipmunk/article/details/51232861)
 83 |     * [OpenCV实现SfM（四）：Bundle Adjustment](https://blog.csdn.net/aichipmunk/article/details/52433884)
 84 | 
 85 | * 参考实现：
 86 |     * [Structure From Motion](https://docs.opencv.org/3.1.0/de/d7c/tutorial_table_of_content_sfm.html)：opencv sfm模块官方教程 
 87 |     * [Structure-from-Motion Toy Lib Upgrades to OpenCV 3](http://www.morethantechnical.com/2016/10/17/structure-from-motion-toy-lib-upgrades-to-opencv-3/)：[github](https://github.com/royshil/SfM-Toy-Library/)，完整的入门代码，使用Boost
 88 |     * [Structure from motion（SFM）原理 - 附我的实现结果](https://blog.csdn.net/Mahabharata_/article/details/70799695)：只有结果
 89 |     * [SFM WITH OPENCV + GTSAM + PMVS](http://nghiaho.com/?p=2379)
 90 |     * cashiwamochi/[simple_triangulation.cc](https://gist.github.com/cashiwamochi/8ac3f8bab9bf00e247a01f63075fedeb)
 91 | 
 92 | ## [视觉SLAM十四讲：从理论到实践](http://www.broadview.com.cn/book/4938)
 93 | 
 94 | > 依赖库：
 95 | 
 96 | *   [eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page)：a C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms.
 97 |     *   问题：
 98 |         *   第13讲例程将Eigen::Isometry3d存储到vector中时运行时报错：[Explanation of the assertion on unaligned arrays](http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html)、[运行PLSLAM时，遇到Eigen对齐问题](https://blog.csdn.net/wojiushixiangshi/article/details/78356271)
 99 | *   [Sophus](https://github.com/strasdat/Sophus)：C++ implementation of Lie Groups using Eigen.
100 | *   [pcl](http://pointclouds.org/)：The Point Cloud Library (or PCL) is a large scale, open project [1] for 2D/3D image and point cloud processing.
101 |     *   安装：
102 |         *   [Downloads](http://pointclouds.org/downloads/)
103 |         *   [Ubuntu安装PCL(Point Cloud Library)](https://blog.csdn.net/xs1102/article/details/74736298)：apt安装改用apt install libpcl-dev 
104 |         *   [Compiling PCL from source on POSIX compliant systems](http://www.pointclouds.org/documentation/tutorials/compiling_pcl_posix.php)：编译安装
105 |     *   问题：
106 |         *   通过apt install libpcl-dev安装的pcl在链接时遇到错误：[Link error: cannot find vtkproj4](https://github.com/PointCloudLibrary/pcl/issues/1594)
107 | *   [Ceres](https://github.com/ceres-solver/ceres-solver)：A large scale non-linear optimization library.
108 |     *   [Ceres Solver](http://ceres-solver.org/)：文档
109 |     *   [在Ubuntu16.04下安装Ceres](https://blog.csdn.net/jz1570551401/article/details/78207208)：cmake环节报错，可能缺少库
110 |     *   [ubuntu16.04下编译ceres-solver](https://www.cnblogs.com/dakewei/p/8421763.html)：包括上面缺少的库
111 | *   [g2o](https://github.com/RainerKuemmerle/g2o)：A General Framework for Graph Optimization.
112 |     *   [文档](https://github.com/RainerKuemmerle/g2o/blob/master/doc/g2o.pdf)(file://C:\Users\85057\Evernote\TEMP\enhtmlclip\g2o.pdf)
113 |     *   [openslam/g2o](https://openslam-org.github.io/g2o.html)：官网
114 |     *   书中用到的g2o版本采用普通指针，最新版本已更改为stl的智能指针，直接使用会报错
115 | *   [DBow3](https://github.com/rmsalinas/DBow3)：an open source C++ library for indexing and converting images into a bag-of-word representation.
116 | *   [octomap](https://octomap.github.io/)：An Efficient Probabilistic 3D Mapping Framework Based on Octrees
117 |     *   问题：
118 |         *   [Octovis在Ubuntu16.04下运行出现core dump的解决方案](http://www.cnblogs.com/hitlrk/p/6667253.html)
119 | 
120 | 


--------------------------------------------------------------------------------
/SFM.cpp:
--------------------------------------------------------------------------------
  1 | //
  2 | // Created by pidan1231239 on 18-6-13.
  3 | //
  4 | 
  5 | #include "SFM.h"
  6 | #include <opencv2/opencv.hpp>
  7 | #include <opencv2/cvv.hpp>
  8 | #include <opencv2/core/eigen.hpp>
  9 | 
 10 | using namespace cv;
 11 | 
 12 | namespace sky {
 13 | 
 14 |     void SFM::addImages(const vector<string> &imagesDir, Camera::Ptr camera) {
 15 | 
 16 |         auto imageDirIt = imagesDir.begin();
 17 |         Mat image1 = imread(*imageDirIt++);
 18 |         Mat image2 = imread(*imageDirIt++);
 19 | 
 20 |         init(image1, image2, camera);
 21 | 
 22 | 
 23 | /*#ifdef CLOUDVIEWER_DEBUG
 24 |         map->visInCloudViewer();
 25 | #endif*/
 26 | 
 27 |         //3D-2D
 28 | 
 29 |         for (; imageDirIt != imagesDir.end(); ++imageDirIt) {
 30 |             Mat image = imread(*imageDirIt);
 31 | #ifdef DEBUG
 32 |             cout << endl << "==============Adding image: " + *imageDirIt << "==============" << endl;
 33 | #endif
 34 | #ifdef CVVISUAL_DEBUGMODE
 35 |             cvv::showImage(image, CVVISUAL_LOCATION, "Adding image: " + *imageDirIt, "");
 36 | #endif
 37 | 
 38 |             step(image, camera);
 39 |         }
 40 | 
 41 |         //输出点云信息
 42 | /*#ifdef DEBUG
 43 |         for (auto &mapPoints:map->mapPoints) {
 44 |             cout << "MapPoint " << mapPoints->getPosPoint3_CV<float>() << endl;
 45 |             cout << " has " << mapPoints->observedFrames.size() << " oberved frames" << endl;
 46 |         }
 47 | #endif*/
 48 | 
 49 |         //可视化初始化点云
 50 | #ifdef CLOUDVIEWER_DEBUG
 51 |         map->visInCloudViewer();
 52 | #endif
 53 | 
 54 |         //BA
 55 |         BA ba;
 56 |         ba(map);
 57 | 
 58 |         //可视化初始化点云
 59 | #ifdef CLOUDVIEWER_DEBUG
 60 |         map->visInCloudViewer();
 61 | #endif
 62 |     }
 63 | 
 64 |     void SFM::init(Mat &image1, Mat &image2, Camera::Ptr camera) {
 65 |         //2D-2D
 66 | #ifdef DEBUG
 67 |         cout << endl << "==============2D-2D initializing==============" << endl;
 68 | #endif
 69 |         //检测特征点并匹配
 70 |         pushImage(image1, camera);
 71 |         saveFrame();
 72 |         detectAndCompute();
 73 |         pushImage(image2, camera);
 74 |         detectAndCompute();
 75 |         //筛选匹配点
 76 |         vector<DMatch> matches;
 77 |         matchWithFrameAndFilt(matches);
 78 | 
 79 | 
 80 |         //解对极约束并三角化
 81 | 
 82 |         vector<Point2f> matchPoints1, matchPoints2;
 83 |         for (auto match:matches) {
 84 |             matchPoints1.push_back(keyFrame1->keyPoints[match.queryIdx].pt);
 85 |             matchPoints2.push_back(keyFrame2->keyPoints[match.trainIdx].pt);
 86 |         }
 87 | 
 88 |         Mat essentialMatrix, inlierMask;;
 89 |         essentialMatrix = findEssentialMat(matchPoints1, matchPoints2,
 90 |                                            keyFrame2->frame->camera->getFocalLength(),
 91 |                                            keyFrame2->frame->camera->getPrincipalPoint(),
 92 |                                            RANSAC, 0.999, 1.0, inlierMask);
 93 | #ifdef DEBUG
 94 |         int nPointsFindEssentialMat = countNonZero(inlierMask);
 95 |         cout << "findEssentialMat: \n\t" << nPointsFindEssentialMat << " valid points, " <<
 96 |              (float) nPointsFindEssentialMat * 100 / matchPoints1.size()
 97 |              << "% of " << matchPoints1.size() << " points are used" << endl;
 98 | #endif
 99 |         //可视化用于三角化的点
100 | #ifdef CVVISUAL_DEBUGMODE
101 |         vector<DMatch> inlierMatches;
102 |         vector<cv::KeyPoint> inlierKeyPoints1, inlierKeyPoints2;
103 |         for (int i = 0; i < matches.size(); ++i) {
104 |             if (!inlierMask.at<uint8_t>(i, 0))
105 |                 continue;
106 |             inlierMatches.push_back(matches[i]);
107 |             inlierMatches.back().trainIdx = inlierKeyPoints1.size();
108 |             inlierMatches.back().queryIdx = inlierKeyPoints2.size();
109 |             inlierKeyPoints1.push_back(keyFrame1->keyPoints[matches[i].queryIdx]);
110 |             inlierKeyPoints2.push_back(keyFrame2->keyPoints[matches[i].trainIdx]);
111 |         }
112 |         cvv::debugDMatch(keyFrame1->image, inlierKeyPoints1, keyFrame2->image, inlierKeyPoints2, inlierMatches, CVVISUAL_LOCATION,
113 |                          "match used in triangulation");
114 | 
115 | #endif
116 | 
117 |         //解frame2的R、t并计算se3,三角化
118 |         Mat R, t, points4D;;
119 |         recoverPose(essentialMatrix, matchPoints1, matchPoints2,
120 |                     keyFrame2->frame->camera->getKMatxCV(), R, t, 100, inlierMask,
121 |                     points4D);
122 |         Eigen::Matrix3d eigenR2;
123 |         cv2eigen(R, eigenR2);
124 |         keyFrame2->frame->Tcw = SE3(
125 |                 eigenR2,
126 |                 Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))
127 |         );
128 | 
129 | 
130 | #ifdef DEBUG
131 |         int nPointsRecoverPose = countNonZero(inlierMask);
132 |         cout << "recoverPose: \n\t" << nPointsRecoverPose << " valid points, " <<
133 |              (float) nPointsRecoverPose * 100 / matchPoints1.size()
134 |              << "% of " << matchPoints1.size() << " points are used" << endl;
135 | /*        cout << "2D-2D frame2 R: " << R.size << endl << R << endl;
136 |         cout << "2D-2D frame2 t: " << t.size << endl << t << endl;
137 |         cout << "2D-2D frame2 SE3: " << endl << keyFrame2->frame->Tcw << endl;
138 |         cout << "2D-2D frame2 Tcw: " << endl << keyFrame2->frame->getTcwMatCV() << endl << endl;
139 |         cout << "2D-2D frame2 ProjMat: " << endl << keyFrame2->frame->getTcw34MatCV() << endl << endl;*/
140 | #endif
141 | 
142 | 
143 | 
144 |         //保存三角化后的点到地图
145 |         convAndAddMappoints(inlierMask, points4D, matches);
146 | 
147 |         //可视化重投影点
148 | #ifdef CVVISUAL_DEBUGMODE
149 | 
150 | #endif
151 |         saveFrame();
152 | 
153 |     }
154 | 
155 |     void SFM::step(Mat &image, const Camera::Ptr &camera) {
156 | 
157 |         pushImage(image, camera);
158 | 
159 |         //检测特征点
160 |         detectAndCompute();
161 |         //提取地图的特征点
162 |         Mat descriptorsMap;
163 |         vector<Point3f> points3D;
164 |         for (MapPoint::Ptr &point:map->mapPoints) {
165 |             if (keyFrame1->frame->isInFrame(point->pos)) {
166 |                 points3D.push_back(point->getPosPoint3_CV<float>());
167 |                 descriptorsMap.push_back(point->descriptor);
168 |             }
169 |         }
170 | #ifdef DEBUG
171 |         cout << "found " << points3D.size() << " 3D points in the last frame" << endl;
172 | #endif
173 |         //匹配地图特征点
174 |         vector<DMatch> matches;
175 |         matcher->match(descriptorsMap, keyFrame2->descriptors, matches, cv::noArray());
176 | #ifdef DEBUG
177 |         cout << "found " << matches.size() << " keypoints matched with 3D points" << endl;
178 | #endif
179 | 
180 |         //筛选匹配点
181 |         filtMatches(matches);
182 | 
183 |         //解PnP得相机位姿
184 |         vector<Point2f> points2DPnP;
185 |         vector<Point3f> points3DPnP;
186 |         for (auto match:matches) {
187 |             points2DPnP.push_back(keyFrame2->keyPoints[match.trainIdx].pt);
188 |             points3DPnP.push_back(points3D[match.queryIdx]);
189 |         }
190 |         Mat r, t, indexInliers;
191 |         solvePnPRansac(points3DPnP, points2DPnP, camera->getKMatxCV(),
192 |                        cv::noArray(), r, t, false, 100, 8.0, 0.99,
193 |                        indexInliers);
194 |         Mat R;
195 |         cv::Rodrigues(r, R);
196 | 
197 |         keyFrame2->frame->Tcw = SE3(
198 |                 SO3(r.at<double>(0, 0), r.at<double>(1, 0), r.at<double>(2, 0)),
199 |                 Vector3d(t.at<double>(0, 0), t.at<double>(1, 0), t.at<double>(2, 0))
200 |         );
201 | 
202 | #ifdef DEBUG
203 |         cout << "solvePnPRansac: \n\t" << indexInliers.rows << " valid points, " <<
204 |              (float) indexInliers.rows * 100 / points2DPnP.size()
205 |              << "% of " << points2DPnP.size() << " points are used" << endl;
206 | /*        cout << "2D-2D frame2 R: " << R.size << endl << R << endl;
207 |         cout << "2D-2D frame2 t: " << t.size << endl << t << endl;
208 |         cout << "2D-2D frame2 SE3: " << endl << keyFrame2->frame->Tcw << endl;
209 |         cout << "2D-2D frame2 Tcw: " << endl << keyFrame2->frame->getTcwMatCV() << endl << endl;
210 |         cout << "2D-2D frame2 ProjMat: " << endl << keyFrame2->frame->getTcw34MatCV() << endl << endl;*/
211 | #endif
212 | 
213 |         if ((float) indexInliers.rows < 30) {
214 |             cout << "current frame has bad matched points!" << endl;
215 |             return;
216 |         }
217 | 
218 | 
219 |         //匹配帧间特征点
220 |         matchWithFrameAndFilt(matches);
221 | 
222 |         //通过findEssentialMat筛选异常点
223 |         vector<Point2f> matchPoints1, matchPoints2;
224 |         for (auto match:matches) {
225 |             matchPoints1.push_back(keyFrame1->keyPoints[match.queryIdx].pt);
226 |             matchPoints2.push_back(keyFrame2->keyPoints[match.trainIdx].pt);
227 |         }
228 |         Mat inlierMask;
229 |         findEssentialMat(matchPoints1, matchPoints2,
230 |                          keyFrame2->frame->camera->getFocalLength(),
231 |                          keyFrame2->frame->camera->getPrincipalPoint(),
232 |                          RANSAC, 0.999, 1.0, inlierMask);
233 | #ifdef DEBUG
234 |         int nPointsFindEssentialMat = countNonZero(inlierMask);
235 |         cout << "findEssentialMat: \n\t" << nPointsFindEssentialMat << " valid points, " <<
236 |              (float) nPointsFindEssentialMat * 100 / matchPoints1.size()
237 |              << "% of " << matchPoints1.size() << " points are used" << endl;
238 | #endif
239 | 
240 |         //三角化
241 |         vector<Point2f> matchPointsNorm1, matchPointsNorm2;
242 |         matchPointsNorm1.reserve(matches.size());
243 |         matchPointsNorm2.reserve(matches.size());
244 |         for (auto &match:matches) {
245 |             matchPointsNorm1.push_back(keyFrame1->frame->camera->pixel2normal(keyFrame1->keyPoints[match.queryIdx].pt));
246 |             matchPointsNorm2.push_back(keyFrame2->frame->camera->pixel2normal(keyFrame2->keyPoints[match.trainIdx].pt));
247 | /*#ifdef DEBUG
248 |             if (i < 5) {
249 |                 cout << keyFrame1->matchPoints[i] << endl;
250 |                 cout << matchPointsNorm1.back() << endl << endl;
251 |             }
252 | #endif*/
253 |         }
254 |         Mat points4D;
255 |         triangulatePoints(keyFrame1->frame->getTcw34MatCV(CV_32F), keyFrame2->frame->getTcw34MatCV(CV_32F),
256 |                           matchPointsNorm1, matchPointsNorm2, points4D);
257 | /*        triangulatePoints(keyFrame1->frame->getProjMatCV(), keyFrame2->frame->getProjMatCV(),
258 |                           keyFrame1->matchPoints, keyFrame1->matchPoints, points4D);*/
259 | 
260 |         //转换齐次坐标点，保存到Map，并做局部BA
261 |         convAndAddMappoints(inlierMask, points4D, matches);
262 | 
263 |         saveFrame();
264 |     }
265 | 
266 |     void SFM::convAndAddMappoints(const Mat &inlierMask, const Mat &points4D,
267 |                                   const vector<DMatch> &matches) {//归一化齐次坐标点,转换Mat
268 | #ifdef DEBUG
269 |         cout << "convAndAddMappoints: " << endl;
270 | #endif
271 |         Map::Ptr localMap(new Map);
272 |         //建立小地图
273 |         if (keyFrame1 && keyFrame2) {
274 |             localMap->addFrame(keyFrame1->frame);
275 |             localMap->addFrame(keyFrame2->frame);
276 |         }
277 | #ifdef DEBUG
278 |         int numOldMappoints = map->mapPoints.size();
279 |         //cout << "showing 5 samples of 3D points:" << endl;
280 | #endif
281 |         for (int i = 0; i < points4D.cols; ++i) {
282 |             MapPoint::Ptr mapPoint;
283 | 
284 |             //如果是outlier，跳过
285 |             if (!inlierMask.empty() && !inlierMask.at<uint8_t>(i, 0))
286 |                 continue;
287 | 
288 |             //获取描述子
289 |             Mat descriptor = keyFrame2->descriptors.row(matches[i].trainIdx);
290 | 
291 |             //如果是上一帧加到地图中的点，更新描述子、加入观测帧后跳过
292 |             if (keyFrame1->inlierPoints.find(matches[i].queryIdx) != keyFrame1->inlierPoints.end()) {
293 |                 mapPoint = keyFrame1->inlierPoints[matches[i].queryIdx];
294 |                 //更新描述子
295 |                 mapPoint->descriptor = descriptor;
296 |                 //加入观测帧
297 |                 mapPoint->addObervedFrame(
298 |                         keyFrame2->frame, keyFrame2->keyPoints[matches[i].trainIdx].pt);
299 |                 //记录当前帧加入地图的mapPoint和特征点下标
300 |                 keyFrame2->inlierPoints[matches[i].trainIdx] = mapPoint;
301 | 
302 |             } else {
303 | 
304 |                 //转换齐次坐标
305 |                 Mat x = points4D.col(i);
306 | 
307 |                 //向地图增加点
308 |                 //获取颜色
309 |                 Vec3b rgb;
310 |                 if (keyFrame2->image.type() == CV_8UC3) {
311 |                     rgb = keyFrame2->image.at<Vec3b>(keyFrame2->keyPoints[matches[i].trainIdx].pt);
312 |                     swap(rgb[0], rgb[2]);
313 |                 } else if (keyFrame2->image.type() == CV_8UC1) {
314 |                     cvtColor(keyFrame2->image.at<uint8_t>(keyFrame2->keyPoints[matches[i].trainIdx].pt),
315 |                              rgb,
316 |                              COLOR_GRAY2RGB);
317 |                 }
318 | 
319 | 
320 |                 if (x.type() == CV_32FC1) {
321 |                     x /= x.at<float>(3, 0); // 归一化
322 |                     mapPoint = MapPoint::Ptr(new MapPoint(Vector3d(x.at<float>(0, 0),
323 |                                                                    x.at<float>(1, 0),
324 |                                                                    x.at<float>(2, 0)),
325 |                                                           descriptor, rgb
326 |                     ));
327 |                 } else if (x.type() == CV_64FC1) {
328 |                     x /= x.at<double>(3, 0);
329 |                     mapPoint = MapPoint::Ptr(new MapPoint(Vector3d(x.at<double>(0, 0),
330 |                                                                    x.at<double>(1, 0),
331 |                                                                    x.at<double>(2, 0)),
332 |                                                           descriptor, rgb
333 |                     ));
334 |                 }
335 | 
336 |                 //记录当前帧加入地图的mapPoint和特征点下标
337 |                 keyFrame2->inlierPoints[matches[i].trainIdx] = mapPoint;
338 | 
339 | /*#ifdef DEBUG
340 |             if (i < 5)
341 |                 cout << mapPoint->pos << endl << endl;
342 | #endif*/
343 |                 mapPoint->addObervedFrame(keyFrame1->frame, keyFrame1->keyPoints[matches[i].queryIdx].pt);
344 |                 mapPoint->addObervedFrame(keyFrame2->frame, keyFrame2->keyPoints[matches[i].trainIdx].pt);
345 |                 map->addMapPoint(mapPoint);
346 |                 //加入到小地图
347 |                 localMap->addMapPoint(mapPoint);
348 |             }
349 | 
350 | 
351 |         }
352 | #ifdef DEBUG
353 |         cout << "\t" << map->mapPoints.size() - numOldMappoints << " new 3D points added to the map" << endl
354 |              << "\t" << map->mapPoints.size() << " in total" << endl;
355 | #endif
356 |         BA ba;
357 |         ba(localMap);
358 |     }
359 | 
360 | 
361 |     void SFM::matchWithFrameAndFilt(vector<DMatch> &matches) {
362 | #ifdef DEBUG
363 |         cout << "matchWithFrameAndFilt keypoints: " << endl;
364 | #endif
365 |         matcher->match(keyFrame1->descriptors, keyFrame2->descriptors, matches, noArray());
366 | #ifdef DEBUG
367 |         cout << "\tfound " << matches.size() << " keypoints matched with last frame" << endl;
368 | #endif
369 |         //筛选匹配点
370 |         filtMatches(matches);
371 | 
372 | #ifdef CVVISUAL_DEBUGMODE
373 |         cvv::debugDMatch(keyFrame1->image, keyFrame1->keyPoints, keyFrame2->image, keyFrame2->keyPoints, matches, CVVISUAL_LOCATION,
374 |                          "2D-2D points matching");
375 | #endif
376 |     }
377 | 
378 |     void SFM::filtMatches(vector<DMatch> &matches) {
379 |         auto minMaxDis = minmax_element(
380 |                 matches.begin(), matches.end(),
381 |                 [](const DMatch &m1, const DMatch &m2) {
382 |                     return m1.distance < m2.distance;
383 |                 });
384 |         auto minDis = minMaxDis.first->distance;
385 |         auto maxDis = minMaxDis.second->distance;
386 |         vector<DMatch> goodMatches;
387 |         for (auto match:matches) {
388 |             if (match.distance <= 4 * minDis)
389 |                 goodMatches.push_back(match);
390 |         }
391 |         matches = goodMatches;
392 | #ifdef DEBUG
393 |         cout << "\tfound " << matches.size() << " good matches" << endl;
394 | #endif
395 |     }
396 | 
397 |     void SFM::detectAndCompute() {
398 | #ifdef DEBUG
399 |         cout << "detectAndCompute features: " << endl;
400 | #endif
401 |         feature2D->detect(keyFrame2->image, keyFrame2->keyPoints, noArray());
402 |         feature2D->compute(keyFrame2->image, keyFrame2->keyPoints, keyFrame2->descriptors);
403 |     }
404 | 
405 |     void SFM::pushImage(Mat &image, const Camera::Ptr &camera) {
406 |         //加载新帧
407 |         Frame::Ptr frame(new Frame(camera, image));
408 |         keyFrame2 = KeyFrame::Ptr(new KeyFrame(frame, image));
409 |     }
410 | 
411 |     void SFM::saveFrame() {
412 |         map->addFrame(keyFrame2->frame);
413 |         keyFrame1 = keyFrame2;
414 |     }
415 | }
416 | 


--------------------------------------------------------------------------------
/SFM.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #ifndef SFM_LEARN_SFM_H
 6 | #define SFM_LEARN_SFM_H
 7 | 
 8 | #include "common_include.h"
 9 | #include "Map.h"
10 | #include "Frame.h"
11 | #include <opencv2/opencv.hpp>
12 | #include <opencv2/cvv.hpp>
13 | #include <unordered_map>
14 | #include "BA.h"
15 | 
16 | 
17 | namespace sky {
18 | 
19 |     using namespace cv;
20 | 
21 |     class SFM {
22 | 
23 |         Map::Ptr map;
24 |         cv::Ptr<cv::Feature2D> feature2D;
25 |         cv::Ptr<DescriptorMatcher> matcher;
26 | 
27 |         //相邻关键帧变量
28 |         struct KeyFrame {
29 |         public:
30 |             typedef shared_ptr<KeyFrame> Ptr;
31 |             Frame::Ptr frame;
32 |             Mat image;
33 |             vector<cv::KeyPoint> keyPoints;
34 |             Mat descriptors;
35 |             unordered_map<int, MapPoint::Ptr> inlierPoints;//在descriptors或keyPoints中的序号和对应的地图点
36 | 
37 |             KeyFrame(const Frame::Ptr &frame, const Mat &image) :
38 |                     frame(frame), image(image) {}
39 |         };
40 | 
41 |         KeyFrame::Ptr keyFrame1, keyFrame2;
42 | 
43 |     public:
44 |         SFM(const cv::Ptr<cv::Feature2D> &feature2D,
45 |             const cv::Ptr<DescriptorMatcher> &matcher
46 |         ) :
47 |                 feature2D(feature2D),
48 |                 matcher(matcher),
49 |                 map(new Map) {}
50 | 
51 |         void addImages(const vector<string> &imagesDir, Camera::Ptr camera);
52 | 
53 |         ///2D-2D初始化
54 | 
55 |         //对两张图片提取、匹配、筛选特征点，求解对极约束，三角化
56 |         void init(Mat &image1, Mat &image2, Camera::Ptr camera);
57 | 
58 |         ///3D-2D求解
59 | 
60 |         //添加新帧，提取、匹配、筛选和Map的特征点，PnP求解，提取、匹配、筛选和前一帧的特征点，三角化
61 |         void step(Mat &image, const Camera::Ptr &camera);
62 | 
63 |         ///基本处理
64 | 
65 |         //加载新帧
66 |         void pushImage(Mat &image, const Camera::Ptr &camera);
67 | 
68 |         //存储新帧
69 |         void saveFrame();
70 | 
71 |         //检测特征点，提取描述子
72 |         void detectAndCompute();
73 | 
74 |         //匹配、筛选特征点
75 |         void matchWithFrameAndFilt(vector<DMatch> &matches);
76 | 
77 |         //转换齐次坐标点，保存到Map
78 |         void convAndAddMappoints(
79 |                 const Mat &inlierMask, const Mat &points4D, const vector<DMatch> &matches);
80 | 
81 |         //筛选匹配点
82 |         void filtMatches(vector<DMatch> &matches);
83 |     };
84 | 
85 | }
86 | 
87 | 
88 | #endif //SFM_LEARN_SFM_H
89 | 


--------------------------------------------------------------------------------
/cmake_modules/FindCSparse.cmake:
--------------------------------------------------------------------------------
 1 | # Look for csparse; note the difference in the directory specifications!
 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
 3 |   PATHS
 4 |   /usr/include/suitesparse
 5 |   /usr/include
 6 |   /opt/local/include
 7 |   /usr/local/include
 8 |   /sw/include
 9 |   /usr/include/ufsparse
10 |   /opt/local/include/ufsparse
11 |   /usr/local/include/ufsparse
12 |   /sw/include/ufsparse
13 |   )
14 | 
15 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
16 |   PATHS
17 |   /usr/lib
18 |   /usr/local/lib
19 |   /opt/local/lib
20 |   /sw/lib
21 |   )
22 | 
23 | include(FindPackageHandleStandardArgs)
24 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG
25 |   CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)
26 | 


--------------------------------------------------------------------------------
/cmake_modules/FindG2O.cmake:
--------------------------------------------------------------------------------
  1 | # Find the header files
  2 | 
  3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
  4 |   ${G2O_ROOT}/include
  5 |   $ENV{G2O_ROOT}/include
  6 |   $ENV{G2O_ROOT}
  7 |   /usr/local/include
  8 |   /usr/include
  9 |   /opt/local/include
 10 |   /sw/local/include
 11 |   /sw/include
 12 |   NO_DEFAULT_PATH
 13 |   )
 14 | 
 15 | # Macro to unify finding both the debug and release versions of the
 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
 17 | # macro.
 18 | 
 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
 20 | 
 21 |   FIND_LIBRARY("${MYLIBRARY}_DEBUG"
 22 |     NAMES "g2o_${MYLIBRARYNAME}_d"
 23 |     PATHS
 24 |     ${G2O_ROOT}/lib/Debug
 25 |     ${G2O_ROOT}/lib
 26 |     $ENV{G2O_ROOT}/lib/Debug
 27 |     $ENV{G2O_ROOT}/lib
 28 |     NO_DEFAULT_PATH
 29 |     )
 30 | 
 31 |   FIND_LIBRARY("${MYLIBRARY}_DEBUG"
 32 |     NAMES "g2o_${MYLIBRARYNAME}_d"
 33 |     PATHS
 34 |     ~/Library/Frameworks
 35 |     /Library/Frameworks
 36 |     /usr/local/lib
 37 |     /usr/local/lib64
 38 |     /usr/lib
 39 |     /usr/lib64
 40 |     /opt/local/lib
 41 |     /sw/local/lib
 42 |     /sw/lib
 43 |     )
 44 |   
 45 |   FIND_LIBRARY(${MYLIBRARY}
 46 |     NAMES "g2o_${MYLIBRARYNAME}"
 47 |     PATHS
 48 |     ${G2O_ROOT}/lib/Release
 49 |     ${G2O_ROOT}/lib
 50 |     $ENV{G2O_ROOT}/lib/Release
 51 |     $ENV{G2O_ROOT}/lib
 52 |     NO_DEFAULT_PATH
 53 |     )
 54 | 
 55 |   FIND_LIBRARY(${MYLIBRARY}
 56 |     NAMES "g2o_${MYLIBRARYNAME}"
 57 |     PATHS
 58 |     ~/Library/Frameworks
 59 |     /Library/Frameworks
 60 |     /usr/local/lib
 61 |     /usr/local/lib64
 62 |     /usr/lib
 63 |     /usr/lib64
 64 |     /opt/local/lib
 65 |     /sw/local/lib
 66 |     /sw/lib
 67 |     )
 68 |   
 69 |   IF(NOT ${MYLIBRARY}_DEBUG)
 70 |     IF(MYLIBRARY)
 71 |       SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
 72 |     ENDIF(MYLIBRARY)
 73 |   ENDIF( NOT ${MYLIBRARY}_DEBUG)
 74 |   
 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
 76 | 
 77 | # Find the core elements
 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
 80 | 
 81 | # Find the CLI library
 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
 83 | 
 84 | # Find the pluggable solvers
 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
 93 | 
 94 | # Find the predefined types
 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
102 | 
103 | # G2O solvers declared found if we found at least one solver
104 | SET(G2O_SOLVERS_FOUND "NO")
105 | IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
106 |   SET(G2O_SOLVERS_FOUND "YES")
107 | ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
108 | 
109 | # G2O itself declared found if we found the core libraries and at least one solver
110 | SET(G2O_FOUND "NO")
111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
112 |   SET(G2O_FOUND "YES")
113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
114 | 


--------------------------------------------------------------------------------
/common_include.h:
--------------------------------------------------------------------------------
 1 | //
 2 | // Created by pidan1231239 on 18-6-13.
 3 | //
 4 | 
 5 | #ifndef SFM_LEARN_COMMON_INCLUDE_H
 6 | #define SFM_LEARN_COMMON_INCLUDE_H
 7 | 
 8 | // define the commonly included file to avoid a long include list
 9 | // for Eigen
10 | #include <Eigen/Core>
11 | #include <Eigen/Geometry>
12 | using Eigen::Vector2d;
13 | using Eigen::Vector3d;
14 | 
15 | // for Sophus
16 | #include <sophus/se3.h>
17 | #include <sophus/so3.h>
18 | using Sophus::SO3;
19 | using Sophus::SE3;
20 | 
21 | // for cv
22 | #include <opencv2/core/core.hpp>
23 | using cv::Mat;
24 | 
25 | // std
26 | #include <vector>
27 | #include <list>
28 | #include <memory>
29 | #include <string>
30 | #include <iostream>
31 | #include <set>
32 | #include <unordered_map>
33 | #include <map>
34 | 
35 | using namespace std;
36 | #endif //SFM_LEARN_COMMON_INCLUDE_H
37 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0000.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.450927 -0.0945642 -0.887537 
 6 | -0.892535 -0.0401974 -0.449183 
 7 | 0.00679989 0.994707 -0.102528 
 8 | -7.28137 -7.57667 0.204446 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0001.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.582226 -0.0983866 -0.807052 
 6 | -0.813027 -0.0706383 -0.577925 
 7 | -0.000148752 0.992638 -0.121118 
 8 | -8.31326 -6.3181 0.16107 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0002.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.666779 -0.0831384 -0.740603 
 6 | -0.74495 -0.0459057 -0.665539 
 7 | 0.021334 0.99548 -0.0925429 
 8 | -9.46627 -5.58174 0.147736 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0003.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.795163 -0.050195 -0.604314 
 6 | -0.606377 -0.0736593 -0.791759 
 7 | -0.00477103 0.996019 -0.0890082 
 8 | -10.8142 -4.53704 0.122293 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0004.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.890856 -0.0211638 -0.453793 
 6 | -0.454283 -0.0449857 -0.889721 
 7 | -0.00158434 0.998763 -0.0496901 
 8 | -12.404 -3.81315 0.110559 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0005.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.962742 -0.0160548 -0.269944 
 6 | -0.270399 -0.0444283 -0.961723 
 7 | 0.00344709 0.998884 -0.0471142 
 8 | -14.1604 -3.32084 0.0862032 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0006.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.994915 -0.00462005 -0.100616 
 6 | -0.100715 -0.0339759 -0.994335 
 7 | 0.00117536 0.999412 -0.0342684 
 8 | -15.8818 -3.15083 0.0592619 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0007.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.995535 0.003721 0.0943235 
 6 | 0.0943815 -0.02118 -0.995311 
 7 | -0.00170578 0.999769 -0.0214366 
 8 | -17.6302 -3.36186 0.0325247 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0008.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.928942 0.000956298 0.370223 
 6 | 0.370116 -0.0265758 -0.928605 
 7 | 0.00895096 0.999646 -0.0250414 
 8 | -19.6309 -3.81958 -0.00781603 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0009.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.84137 0.00860137 0.540391 
 6 | 0.540262 -0.0403709 -0.840528 
 7 | 0.0145864 0.999148 -0.0386139 
 8 | -20.9553 -4.61897 -0.0303931 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_cameras/0010.png.camera:
--------------------------------------------------------------------------------
 1 | 2759.48 0 1520.69 
 2 | 0 2764.16 1006.81 
 3 | 0 0 1 
 4 | 0 0 0
 5 | 0.70735 0.0218279 0.706526 
 6 | 0.706534 -0.0523403 -0.705741 
 7 | 0.0215749 0.998391 -0.0524451 
 8 | -21.9937 -5.82033 -0.0463931 
 9 | 3072 2048
10 | 


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0000.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0000.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0001.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0001.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0002.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0002.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0003.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0003.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0004.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0004.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0005.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0005.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0006.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0006.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0007.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0007.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0008.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0008.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0009.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0009.png


--------------------------------------------------------------------------------
/datasets/fountain_dense_images/0010.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/datasets/fountain_dense_images/0010.png


--------------------------------------------------------------------------------
/images/2D2D.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/images/2D2D.png


--------------------------------------------------------------------------------
/images/3D2D.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/images/3D2D.png


--------------------------------------------------------------------------------
/images/afterBA.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/images/afterBA.png


--------------------------------------------------------------------------------
/images/beforeBA.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/chengzhag/sfm_learn/9f5dd56eff52deb87720f7573243b6250cdb6c87/images/beforeBA.png


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include "SFM.h"
 3 | #include "common_include.h"
 4 | 
 5 | #include <boost/algorithm/string.hpp>
 6 | #include <boost/range/adaptor/transformed.hpp>
 7 | #include <boost/filesystem.hpp>
 8 | 
 9 | #include <opencv2/xfeatures2d.hpp>
10 | 
11 | using namespace sky;
12 | using namespace cv;
13 | 
14 | int main() {
15 |     //SFM sfm(ORB::create(500,1.2,8));
16 |     SFM sfm(xfeatures2d::SIFT::create(0, 3, 0.04, 10),
17 |             DescriptorMatcher::create("BruteForce"));
18 | 
19 |     string imagesFolder("datasets/fountain_dense_images");
20 | #ifdef DEBUG
21 |     std::cout << "Adding images from: " + imagesFolder << std::endl;
22 | #endif
23 | 
24 |     vector<string> imagesDir;
25 | 
26 |     if (!imagesFolder.empty()) {
27 |         using namespace boost::filesystem;
28 | 
29 |         path dirPath(imagesFolder);
30 |         if (not exists(dirPath) or not is_directory(dirPath)) {
31 |             cerr << "Cannot open directory: " << imagesFolder << endl;
32 |             return false;
33 |         }
34 | 
35 |         for (directory_entry &x : directory_iterator(dirPath)) {
36 |             string extension = x.path().extension().string();
37 |             boost::algorithm::to_lower(extension);
38 |             if (extension == ".jpg" or extension == ".png") {
39 |                 imagesDir.push_back(x.path().string());
40 |             }
41 |         }
42 | 
43 |         if (imagesDir.size() <= 0) {
44 |             cerr << "Unable to find valid files in images directory (\"" << imagesFolder << "\")." << endl;
45 |             return false;
46 |         }
47 | 
48 |         sort(imagesDir.begin(), imagesDir.end());
49 |     }
50 | 
51 |     sfm.addImages(imagesDir, Camera::Ptr(new Camera(
52 |             2759.48, 2764.16, 1520.69, 1006.81
53 |     )));
54 | 
55 |     cvv::finalShow();
56 | 
57 |     return 0;
58 | }


--------------------------------------------------------------------------------