Implemented PoseOptimizationSLAM3D

SLAM/PoseOptimizationSLAM3D/README.md

@@ -0,0 +1,21 @@
+# PoseOptimizationSLAM3D
+3D (x, y, z, qw, qx, qy, qz) pose optimization SLAM
+
+## How to use 
+1. Download data  
+
+~~~
+python data_downloader.py
+~~~
+
+2. run SLAM 
+
+~~~
+python pose_optimization_slam_3d.py
+~~~
+
+## Reference 
+[A Compact and Portable Implementation of Graph\-based SLAM](https://www.researchgate.net/publication/321287640_A_Compact_and_Portable_Implementation_of_Graph-based_SLAM)  
+[GitHub \- furo\-org/p2o: Single header 2D/3D graph\-based SLAM library](https://github.com/furo-org/p2o)  
+[GitHub \- AtsushiSakai/PythonRobotics
+/SLAM/PoseOptimizationSLAM](https://github.com/AtsushiSakai/PythonRobotics/tree/master/SLAM/PoseOptimizationSLAM)

SLAM/PoseOptimizationSLAM3D/data_downloader.py

@@ -0,0 +1,21 @@
+"""
+
+Data down loader for pose optimization
+
+author: Atsushi Sakai
+
+"""
+
+
+import subprocess
+def main():
+    print("start!!")
+
+    cmd = "wget https://www.dropbox.com/s/zu23p8d522qccor/parking-garage.g2o?dl=0 -O parking-garage.g2o -nc"
+    subprocess.call(cmd, shell=True)
+
+    print("done!!")
+
+
+if __name__ == '__main__':
+    main()