├── Dockerfile
├── README.md
├── result
├── error_function.gif
├── initial_pose.png
├── optimized_pose.png
└── optimzied_pose_solution.png
├── slam_tutorial-solution
├── CMakeLists.txt
├── data
│ ├── constraints.csv
│ ├── initial_poses.csv
│ └── optimized_poses.csv
├── matlab
│ ├── plot_poses.m
│ └── visualize_results.m
├── package.xml
└── src
│ ├── ceres_spg.cpp
│ └── gtsam_spg.cpp
└── slam_tutorial
├── CMakeLists.txt
├── data
├── constraints.csv
├── initial_poses.csv
└── optimized_poses.csv
├── matlab
├── plot_poses.m
└── visualize_results.m
├── package.xml
└── src
├── ceres_spg.cpp
└── gtsam_spg.cpp
/Dockerfile:
--------------------------------------------------------------------------------
1 | FROM ros:melodic
2 | WORKDIR /root/
3 |
4 | RUN apt-get update && apt-get install -y wget unzip vim libeigen3-dev libboost-all-dev
5 |
6 | # install Ceres solver (latest stable version by 06/20/2020)
7 | RUN apt-get update && apt-get install -y cmake libgoogle-glog-dev libatlas-base-dev libsuitesparse-dev
8 | RUN wget http://ceres-solver.org/ceres-solver-1.14.0.tar.gz && tar zxf ceres-solver-1.14.0.tar.gz && rm ceres-solver-1.14.0.tar.gz && \
9 | cd ceres-solver-1.14.0 && mkdir build && cd build && cmake .. && make -j5 && make test && make install && cd ../.. && rm -rf ceres-solver-1.14.0
10 |
11 |
12 | # install gtsam 4.0.3 (latest release version by 06/20/2020)
13 | RUN git clone https://github.com/borglab/gtsam.git && cd gtsam && mkdir build && cd build && cmake -DGTSAM_WITH_TBB=Off .. && make -j5 && make install && \
14 | cd ../../ && rm -rf gtsam
15 |
16 | # setup base ros env
17 | RUN /bin/bash -c "source /opt/ros/melodic/setup.bash && mkdir -p /root/slam_ws/src && cd /root/slam_ws/src && catkin_init_workspace && cd .. && catkin_make"
18 | RUN echo "source /opt/ros/melodic/setup.bash" >> /root/.bashrc && echo "source /root/slam_ws/devel/setup.bash" >> /root/.bashrc
19 | RUN echo "export LD_LIBRARY_PATH=/root/slam_ws/devel/lib:/opt/ros/melodic/lib:/lib:/usr/lib:/usr/local/lib" >> /root/.bashrc
20 |
21 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # SLAM-backend-tutorial
2 |
3 | You could see the Video lecture on [Youtube](https://youtu.be/FhwFyA0NQkE).
4 |
5 | The original code, slides, and notes are available in [Google drive](https://drive.google.com/drive/folders/1_MLRQMvRuXvyxVl6514M20zcFY4-O4pI?usp=sharing).
6 |
7 | `slam_tutorial/` folder involves my personal solution,
8 | and `slam_tutorial_solution/` involves given solution.
9 | The contents are almost similar.
10 |
11 | I test this repository on Ubuntu 18.04.
12 |
13 | ### Contents
14 | - [How to make same enviornment with Docker.](#how-to-make-same-enviornment-with-docker)
15 | - [How to execute this code.](#how-to-execute-this-code)
16 | - [Result](#result)
17 | - [Contact](#contact)
18 |
19 | ## How to make same enviornment with Docker.
20 |
21 | First you have to install [Docker](https://www.docker.com/).
22 |
23 | **1. Just clone this repository and change the directory path.**
24 | ```
25 | git clone https://github.com/Taeyoung96/SLAM-backend-tutorial.git
26 | cd SLAM-backend-tutorial
27 | ```
28 |
29 | **2. Build the SLAM docker image.**
30 | This docker image includes ubuntu 18.04, ros melodic, [ceras-solver](http://ceres-solver.org/), and [gtsam](https://gtsam.org/).
31 | ```
32 | docker build -t tutorial-week-2020/slam:latest .
33 | ```
34 |
35 | When you are finished, check the image.
36 | ```
37 | docker images
38 | ```
39 | You can check that the image is created as below in your terminal, the IMAGE ID and CREATED TIME may be different.
40 | ```
41 | REPOSITORY TAG IMAGE ID CREATED SIZE
42 | tutorial-week-2020/slam latest 18d7778e6a07 22 hours ago 1.46GB
43 | ros melodic 2529d0ef4064 2 weeks ago 1.28GB
44 | ```
45 |
46 | **3. Start a Docker container while sharing the container's volume directory with a local directory.**
47 | ```
48 | docker run -v ${CODE_DIR}/slam_tutorial:/root/slam_ws/src/slam_tutorial -n slam_tutorial -it tutorial-week-2020/slam:latest
49 | ```
50 |
51 | When you following above command, you share the `slam_tutorial/` folder.
52 | If you want to change with `slam_tutorial_solution/`,
53 | just change the directory `slam_tutorial/` to `slam_tutorial_solution/` when you run the above command.
54 |
55 | In my case, ${CODE_DIR} is `/home/taeyoung/Desktop/SLAM-backend-tutorial/`, it may depend on where you clone the repository.
56 | For example,
57 | ```
58 | docker run -v /home/taeyoung/Desktop/SLAM-backend-tutorial/slam_tutorial:/root/slam_ws/src/slam_tutorial -n slam_tutorial -it tutorial-week-2020/slam:latest
59 | ```
60 | When the container creation is completed, the terminal path is changed and you can see that you are connected to the docker container.
61 | ```
62 | root@bf318ac3a3d7:~#
63 | ```
64 | Check the directory in the container and the local directory are shared.
65 | ```
66 | root@bf318ac3a3d7:~# cd slam_ws/
67 | root@bf318ac3a3d7:~/slam_ws# cd src
68 | root@bf318ac3a3d7:~/slam_ws/src# ls
69 | CMakeLists.txt slam_tutorial
70 | ```
71 | ## How to execute this code.
72 |
73 | **1. Just build the code with `catkin_make`**
74 |
75 | ```
76 | root@bf318ac3a3d7:~/slam_ws# catkin_make
77 | ```
78 | After the build is complete, you can see the output as below and a build folder and a devel folder are created.
79 | ```
80 | Scanning dependencies of target ceres_spg
81 | Scanning dependencies of target gtsam_spg
82 | [ 25%] Building CXX object slam_tutorial/CMakeFiles/ceres_spg.dir/src/ceres_spg.cpp.o
83 | [ 50%] Building CXX object slam_tutorial/CMakeFiles/gtsam_spg.dir/src/gtsam_spg.cpp.o
84 | [ 75%] Linking CXX executable /root/slam_ws/devel/lib/slam_tutorial/ceres_spg
85 | [ 75%] Built target ceres_spg
86 | [100%] Linking CXX executable /root/slam_ws/devel/lib/slam_tutorial/gtsam_spg
87 | [100%] Built target gtsam_spg
88 | ```
89 |
90 | **2. Execute the code with `rosrun` command.**
91 |
92 | You could execute two different node. One is for Ceres-solver example, the other is for gtsam example.
93 |
94 | If you want to run the ceres-solver example node, you enter the following command.
95 | ```
96 | rosrun slam_tutorial ceres_spg
97 | ```
98 |
99 | If you want to run the gtsam example node, you enter the following command.
100 | ```
101 | rosrun slam_tutorial gtsam_spg
102 | ```
103 |
104 | ## Result
105 |
106 | - Result output when you run ceres-solver example node
107 |
108 | You could visualize `.csv` files, with [Matlab](https://kr.mathworks.com/products/matlab.html).
109 | I got different results because I designed the error function differently.
110 |

111 |
112 | - Initial pose
113 |
114 | 
115 |
116 | - Optimized_pose with my personal solution
117 |
118 | 
119 |
120 | - Optimized_pose with given soltion
121 |
122 | 
123 |
124 | ```
125 | Solver Summary (v 1.14.0-eigen-(3.3.4)-lapack-suitesparse-(5.1.2)-cxsparse-(3.1.9)-eigensparse-openmp-no_tbb)
126 |
127 | Original Reduced
128 | Parameter blocks 552 550
129 | Parameters 1932 1925
130 | Effective parameters 1656 1650
131 | Residual blocks 1073 1073
132 | Residuals 6438 6438
133 |
134 | Minimizer TRUST_REGION
135 |
136 | Sparse linear algebra library SUITE_SPARSE
137 | Trust region strategy LEVENBERG_MARQUARDT
138 |
139 | Given Used
140 | Linear solver SPARSE_NORMAL_CHOLESKY SPARSE_NORMAL_CHOLESKY
141 | Threads 1 1
142 | Linear solver ordering AUTOMATIC 550
143 |
144 | Cost:
145 | Initial 2.716861e+02
146 | Final 5.204096e+00
147 | Change 2.664820e+02
148 |
149 | Minimizer iterations 161
150 | Successful steps 149
151 | Unsuccessful steps 12
152 |
153 | Time (in seconds):
154 | Preprocessor 0.000880
155 |
156 | Residual only evaluation 0.026268 (161)
157 | Jacobian & residual evaluation 0.265551 (149)
158 | Linear solver 0.838873 (161)
159 | Minimizer 1.193949
160 |
161 | Postprocessor 0.000041
162 | Total 1.194870
163 |
164 | Termination: CONVERGENCE (Function tolerance reached. |cost_change|/cost: 8.268293e-07 <= 1.000000e-06)
165 |
166 | ```
167 |
168 |
169 | - Result output when you run gtsam example node
170 |
171 | ```
172 | Factor Graph:
173 | size: 6
174 |
175 | Factor 0: PriorFactor on x1
176 | prior mean: (0, 0, 0)
177 | noise model: diagonal sigmas[1; 1; 0.1];
178 |
179 | Factor 1: BetweenFactor(x1,x2)
180 | measured: (5, 0, 0)
181 | noise model: diagonal sigmas[0.5; 0.5; 0.1];
182 |
183 | Factor 2: BetweenFactor(x2,x3)
184 | measured: (5, 0, -1.57079633)
185 | noise model: diagonal sigmas[0.5; 0.5; 0.1];
186 |
187 | Factor 3: BetweenFactor(x3,x4)
188 | measured: (5, 0, -1.57079633)
189 | noise model: diagonal sigmas[0.5; 0.5; 0.1];
190 |
191 | Factor 4: BetweenFactor(x4,x5)
192 | measured: (5, 0, -1.57079633)
193 | noise model: diagonal sigmas[0.5; 0.5; 0.1];
194 |
195 | Factor 5: BetweenFactor(x5,x2)
196 | measured: (5, 0, -1.57079633)
197 | noise model: diagonal sigmas[0.5; 0.5; 0.1];
198 |
199 |
200 | Initial Values:
201 |
202 | Values with 5 values:
203 | Value x1: (gtsam::Pose2)
204 | (0.2, -0.3, 0.2)
205 |
206 | Value x2: (gtsam::Pose2)
207 | (5.1, 0.3, -0.1)
208 |
209 | Value x3: (gtsam::Pose2)
210 | (9.9, -0.1, -1.77079633)
211 |
212 | Value x4: (gtsam::Pose2)
213 | (10.2, -5, -3.04159265)
214 |
215 | Value x5: (gtsam::Pose2)
216 | (5.1, -5.1, 1.47079633)
217 |
218 | Initial error: 18.510326
219 | newError: 0.122934358
220 | errorThreshold: 0.122934358 > 0
221 | absoluteDecrease: 18.3873916591 >= 1e-05
222 | relativeDecrease: 0.993358606565 >= 1e-05
223 | newError: 8.85829965247e-06
224 | errorThreshold: 8.85829965247e-06 > 0
225 | absoluteDecrease: 0.12292549938 >= 1e-05
226 | relativeDecrease: 0.999927942848 >= 1e-05
227 | newError: 3.68234840295e-15
228 | errorThreshold: 3.68234840295e-15 > 0
229 | absoluteDecrease: 8.85829964879e-06 < 1e-05
230 | relativeDecrease: 0.999999999584 >= 1e-05
231 | converged
232 | errorThreshold: 3.68234840295e-15 0
233 | absoluteDecrease: 8.85829964879e-06 1e-05
234 | relativeDecrease: 0.999999999584 1e-05
235 | iterations: 3 >? 100
236 | Final Result:
237 |
238 | Values with 5 values:
239 | Value x1: (gtsam::Pose2)
240 | (-1.27918496267e-18, 1.66618678917e-19, -3.14163502147e-20)
241 |
242 | Value x2: (gtsam::Pose2)
243 | (5, 5.11915975726e-20, -7.11636343752e-20)
244 |
245 | Value x3: (gtsam::Pose2)
246 | (10.0000000015, -4.40576423166e-09, -1.5707963267)
247 |
248 | Value x4: (gtsam::Pose2)
249 | (10.0000000114, -5.00000003139, 3.14159265352)
250 |
251 | Value x5: (gtsam::Pose2)
252 | (4.99999999784, -5.00000000264, 1.57079632663)
253 |
254 | x1 covariance:
255 | 1 -1.71241483502e-17 -5.65095102146e-17
256 | -1.71241483502e-17 1 1.7763568394e-16
257 | -5.65095102146e-17 1.7763568394e-16 0.01
258 | x2 covariance:
259 | 1.25 -3.79952940193e-16 -1.40205920175e-16
260 | -3.79952940193e-16 1.5 0.05
261 | -1.40205920175e-16 0.05 0.02
262 | x3 covariance:
263 | 2.70000000047 -8.21536053911e-10 -0.155000000029
264 | -8.21536047854e-10 1.45000000006 -0.00499999990562
265 | -0.155000000029 -0.00499999990562 0.0264999999985
266 | x4 covariance:
267 | 2.1125000074 0.800000006448 -0.120000000784
268 | 0.800000006448 2.80000000387 -0.170000000296
269 | -0.120000000784 -0.170000000296 0.0279999999952
270 | x5 covariance:
271 | 1.69999999991 -0.224999999954 0.0449999999659
272 | -0.224999999954 2.06250000049 -0.127500000037
273 | 0.0449999999659 -0.127500000037 0.0264999999968
274 |
275 | ```
276 |
277 | ## Contact
278 |
279 | If you have any question, feel free to send an email.
280 |
281 | - **TaeYoung Kim** : tyoung96@yonsei.ac.kr
282 |
283 |
284 |
285 |
286 |
287 |
--------------------------------------------------------------------------------
/result/error_function.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Taeyoung96/SLAM-backend-tutorial/7e52d5019e0e0d016fee5a4e3542710d8dae1390/result/error_function.gif
--------------------------------------------------------------------------------
/result/initial_pose.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Taeyoung96/SLAM-backend-tutorial/7e52d5019e0e0d016fee5a4e3542710d8dae1390/result/initial_pose.png
--------------------------------------------------------------------------------
/result/optimized_pose.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Taeyoung96/SLAM-backend-tutorial/7e52d5019e0e0d016fee5a4e3542710d8dae1390/result/optimized_pose.png
--------------------------------------------------------------------------------
/result/optimzied_pose_solution.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/Taeyoung96/SLAM-backend-tutorial/7e52d5019e0e0d016fee5a4e3542710d8dae1390/result/optimzied_pose_solution.png
--------------------------------------------------------------------------------
/slam_tutorial-solution/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 3.0.2)
2 | project(slam_tutorial)
3 |
4 | add_compile_options(-std=c++11)
5 |
6 | find_package(catkin REQUIRED COMPONENTS
7 | roscpp
8 | std_msgs
9 | )
10 | include_directories(${catkin_INCLUDE_DIRS})
11 | catkin_package(
12 | # INCLUDE_DIRS include
13 | # LIBRARIES shimizu_abstraction
14 | # CATKIN_DEPENDS roscpp rospy std_msgs
15 | # DEPENDS system_lib
16 | )
17 | find_package(Eigen3 REQUIRED)
18 | include_directories(${Eigen3_INCLUDE_DIRS})
19 |
20 | find_package(Ceres REQUIRED)
21 | include_directories(${CERES_INCLUDE_DIRS})
22 |
23 | find_package(GTSAM REQUIRED)
24 | include_directories(${GTSAM_INCLUDE_DIR})
25 | set(GTSAM_LIBRARIES gtsam)
26 |
27 | find_package(GTSAMCMakeTools)
28 | include(GtsamMakeConfigFile)
29 | include(GtsamBuildTypes)
30 | include(GtsamTesting)
31 |
32 | find_package(Boost REQUIRED)
33 | include_directories(${Boost_INCLUDE_DIR})
34 |
35 | add_executable(ceres_spg src/ceres_spg.cpp)
36 | target_link_libraries(ceres_spg ${catkin_LIBRARIES} ${CERES_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES})
37 |
38 | add_executable(gtsam_spg src/gtsam_spg.cpp)
39 | target_link_libraries(gtsam_spg ${catkin_LIBRARIES} ${GTSAM_LIBRARIES} ${Boost_LIBRARIES})
--------------------------------------------------------------------------------
/slam_tutorial-solution/data/initial_poses.csv:
--------------------------------------------------------------------------------
1 | 0, 1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000
2 | 1, 0.987383, -0.157967, 0.011000, 0.000429, 0.002774, 0.072733, 0.034876
3 | 2, 0.988921, 0.148418, 0.002620, 0.000498, 0.007340, 0.030627, -0.025935
4 | 3, 0.962742, -0.145776, 0.226320, -0.025618, -1.082510, -0.435961, 0.752557
5 | 4, 0.950384, -0.148409, -0.271818, 0.029336, 1.525530, -0.310588, 1.203260
6 | 5, 0.999871, 0.001501, -0.015985, 0.000033, 1.087240, -0.000941, 0.158052
7 | 6, 0.988207, -0.152094, -0.017225, 0.004213, 1.086660, 0.125640, 0.178123
8 | 7, 0.987413, -0.156873, 0.019812, -0.003679, 1.988380, 0.056329, 0.071263
9 | 8, 0.999744, 0.000313, 0.022494, -0.002391, 1.973620, -0.007095, 0.049869
10 | 9, 0.999584, 0.002711, 0.028416, -0.004077, 2.838719, -0.001637, 0.024915
11 | 10, 0.986053, -0.163291, 0.028397, -0.015138, 2.828501, 0.008920, 0.054197
12 | 11, 0.987261, -0.158477, 0.013848, -0.003032, 3.787225, 0.061882, 0.134072
13 | 12, 0.999896, 0.003399, 0.013290, -0.004434, 3.785436, 0.035791, 0.081807
14 | 13, 0.999485, 0.007993, 0.030999, -0.001901, 4.717825, 0.020000, 0.090090
15 | 14, 0.987267, -0.156305, 0.029142, 0.004889, 4.734741, 0.056230, 0.117771
16 | 15, 0.987334, -0.157226, 0.020934, -0.003683, 5.701466, 0.062535, 0.146697
17 | 16, 0.999663, 0.003842, 0.024562, -0.007375, 5.679881, 0.044334, 0.086133
18 | 17, 0.998892, 0.007646, 0.046348, -0.002649, 6.625543, 0.033553, 0.109867
19 | 18, 0.987879, -0.149026, 0.043400, 0.001382, 6.638926, 0.086318, 0.138704
20 | 19, 0.986936, -0.158621, 0.026781, -0.009005, 7.510689, 0.052257, 0.247152
21 | 20, 0.999620, 0.000312, 0.024881, -0.011871, 7.523980, 0.011145, 0.193712
22 | 21, 0.999538, 0.002610, 0.030014, -0.004102, 8.413233, 0.049449, 0.252705
23 | 22, 0.987729, -0.153763, 0.026258, 0.007595, 8.423466, 0.099007, 0.280946
24 | 23, 0.987269, -0.157972, 0.013349, -0.012979, 9.437891, 0.046061, 0.278433
25 | 24, 0.999764, -0.004791, 0.009853, -0.018771, 9.450453, 0.026128, 0.222670
26 | 25, 0.999264, 0.009940, 0.036629, -0.005575, 10.417958, 0.094002, 0.271574
27 | 26, 0.986720, -0.157428, 0.039763, -0.004298, 10.443458, 0.119002, 0.275702
28 | 27, 0.985335, -0.164704, 0.041918, -0.015227, 11.488409, 0.102192, 0.239076
29 | 28, 0.998488, 0.006649, 0.054483, -0.003008, 11.427452, 0.107048, 0.318674
30 | 29, 0.999005, 0.005208, 0.043864, -0.006086, 12.405828, 0.116187, 0.331689
31 | 30, 0.998570, 0.005798, 0.052852, -0.005610, 12.335537, 0.116948, 0.238648
32 | 31, 0.986382, -0.163076, 0.019587, -0.008628, 13.360458, 0.130423, 0.209416
33 | 32, 0.987367, -0.154692, 0.034323, -0.000062, 12.449362, 0.167203, 0.382175
34 | 33, 0.999072, -0.003632, 0.038206, -0.019617, 14.811030, 0.039783, 0.376366
35 | 34, 0.985391, -0.165451, 0.036615, -0.017113, 14.814629, 0.136156, 0.450653
36 | 35, 0.983270, -0.167118, -0.065173, 0.031726, 15.660948, 0.132021, 0.924434
37 | 36, 0.996598, -0.005967, -0.080583, 0.016291, 15.646933, -0.112019, 0.872742
38 | 37, 0.975543, -0.007532, -0.211313, 0.060060, 16.114095, -0.352728, 1.602589
39 | 38, 0.962475, -0.160503, -0.196120, 0.097054, 16.104036, 0.111857, 1.737696
40 | 39, 0.925912, -0.153772, -0.342070, 0.045069, 16.965180, 0.517381, 1.533328
41 | 40, 0.937161, -0.015055, -0.347913, -0.021483, 16.989815, 0.116281, 1.552370
42 | 41, 0.897079, 0.002473, -0.438175, 0.056997, 17.552636, -0.530215, 1.186043
43 | 42, 0.882268, -0.152487, -0.441623, 0.057638, 17.580076, -0.397872, 1.197719
44 | 43, 0.825364, -0.188798, -0.515666, 0.131225, 18.485457, 0.563734, 0.652686
45 | 44, 0.839395, -0.037283, -0.519268, 0.156174, 18.449367, 0.430971, 0.597080
46 | 45, 0.751957, 0.028049, -0.654256, 0.075659, 19.471132, -0.807549, -0.301909
47 | 46, 0.743629, -0.122307, -0.655767, 0.045038, 19.504487, -0.564284, -0.318366
48 | 47, 0.757899, -0.104286, -0.626433, 0.149325, 19.408196, -0.074772, -1.642797
49 | 48, 0.764586, 0.032774, -0.607508, 0.212769, 19.374044, -0.092660, -1.646127
50 | 49, 0.756165, 0.035211, -0.613100, 0.226024, 19.722625, -0.329122, -2.787635
51 | 50, 0.748600, -0.086665, -0.643979, 0.131837, 19.804939, -0.223639, -2.787221
52 | 51, 0.730391, -0.082242, -0.665534, 0.129738, 20.088929, -0.381554, -4.017754
53 | 52, 0.745566, 0.047222, -0.629226, 0.214425, 19.922281, -0.827588, -3.962502
54 | 53, 0.739742, 0.073703, -0.634172, 0.212557, 20.141208, -1.355883, -5.089526
55 | 54, 0.758272, -0.056783, -0.638281, 0.119988, 20.073017, -0.494207, -5.327158
56 | 55, 0.757050, -0.031382, -0.638638, 0.134295, 20.074427, -1.718330, -5.999400
57 | 56, 0.748803, 0.081692, -0.614704, 0.234012, 19.946667, -1.624906, -6.099268
58 | 57, 0.761584, 0.087716, -0.583197, 0.268664, 20.238503, -1.906140, -7.609684
59 | 58, 0.777467, -0.026477, -0.607359, 0.161130, 20.235542, -1.925874, -7.625070
60 | 59, 0.753130, 0.043216, -0.628458, 0.189661, 20.510167, -2.481051, -8.611791
61 | 60, 0.738956, 0.158332, -0.596847, 0.269540, 20.479641, -2.538036, -8.580638
62 | 61, 0.763592, 0.130727, -0.573172, 0.267047, 20.834598, -2.961954, -9.686624
63 | 62, 0.774666, 0.020831, -0.609064, 0.168822, 20.893518, -2.864712, -9.712503
64 | 63, 0.715164, -0.036315, -0.690831, 0.099884, 22.139056, -2.859452, -10.131215
65 | 64, 0.737594, 0.076203, -0.642936, 0.191789, 21.980814, -3.290550, -10.083999
66 | 65, 0.780442, 0.115036, -0.584129, 0.190981, 21.943655, -4.093076, -10.780718
67 | 66, 0.784407, -0.001135, -0.611768, 0.102210, 22.005944, -3.848378, -10.969376
68 | 67, 0.758208, -0.135827, -0.616894, 0.161606, 22.494447, -4.481352, -12.251374
69 | 68, 0.765955, -0.024708, -0.588470, 0.257701, 22.408015, -4.584532, -12.239449
70 | 69, 0.765789, 0.012384, -0.608181, 0.208648, 22.888056, -5.231176, -12.939223
71 | 70, 0.778552, -0.113319, -0.606133, 0.116711, 22.914991, -5.125923, -12.975304
72 | 71, 0.760623, -0.113216, -0.620819, 0.152391, 23.137815, -5.574346, -14.169809
73 | 72, 0.753522, 0.006128, -0.606507, 0.253613, 22.993529, -5.426286, -14.352238
74 | 73, 0.762177, 0.012886, -0.609081, 0.218963, 23.338051, -6.109990, -15.332069
75 | 74, 0.755951, -0.102041, -0.634207, 0.126137, 23.386404, -6.073087, -15.352964
76 | 75, 0.748495, -0.111427, -0.641321, 0.126690, 23.653552, -5.833352, -16.869117
77 | 76, 0.761261, 0.005533, -0.611012, 0.217071, 23.625078, -5.852774, -16.838451
78 | 77, 0.768992, -0.014973, -0.605288, 0.205082, 23.742682, -7.038896, -17.961445
79 | 78, 0.755734, -0.129957, -0.632436, 0.109570, 23.834822, -5.890882, -18.195865
80 | 79, 0.734981, -0.115843, -0.655769, 0.127884, 23.892374, -6.003641, -19.643062
81 | 80, 0.754942, 0.002913, -0.620796, 0.211354, 23.882384, -6.771476, -19.427553
82 | 81, 0.749407, -0.021160, -0.627942, 0.208888, 23.966150, -7.759137, -20.619628
83 | 82, 0.732605, -0.145450, -0.653969, 0.120260, 23.998981, -6.037497, -20.968076
84 | 83, 0.740109, -0.109680, -0.653655, 0.113786, 23.940847, -8.016276, -21.687463
85 | 84, 0.744696, -0.015191, -0.630825, 0.217397, 23.859030, -6.060989, -22.067665
86 | 85, 0.760517, 0.003002, -0.609714, 0.223289, 23.957832, -8.299043, -22.969022
87 | 86, 0.750870, -0.107677, -0.640067, 0.122139, 24.000292, -8.048327, -23.022503
88 | 87, 0.764878, -0.118950, -0.622821, 0.113630, 24.207017, -8.655693, -24.098978
89 | 88, 0.775780, -0.010129, -0.596045, 0.206875, 24.152752, -8.720719, -24.072516
90 | 89, 0.803973, -0.003134, -0.563740, 0.189260, 24.331156, -8.722860, -25.479579
91 | 90, 0.791034, -0.115986, -0.593488, 0.092676, 24.393497, -8.714713, -25.489083
92 | 91, 0.764029, -0.124289, -0.626640, 0.090216, 24.739266, -8.976296, -26.441388
93 | 92, 0.777414, -0.003037, -0.605437, 0.170497, 24.694244, -9.052096, -26.428466
94 | 93, 0.764531, 0.019356, -0.609948, 0.207572, 25.193270, -9.375030, -27.381792
95 | 94, 0.756342, -0.108499, -0.632186, 0.128528, 25.235185, -9.249753, -27.422370
96 | 95, 0.767328, -0.071954, -0.625953, 0.119239, 25.347274, -9.731641, -28.502939
97 | 96, 0.771375, 0.038936, -0.598389, 0.213075, 25.306256, -9.761689, -28.495527
98 | 97, 0.789582, 0.106970, -0.585058, 0.151098, 25.531685, -9.948086, -29.638702
99 | 98, 0.776718, -0.004306, -0.627050, 0.059196, 25.615616, -9.908654, -29.645022
100 | 99, 0.772538, -0.025594, -0.631012, 0.066032, 25.859362, -9.952541, -30.697864
101 | 100, 0.779687, 0.088747, -0.600329, 0.154345, 25.846762, -10.327457, -30.617884
102 | 101, 0.801827, 0.040979, -0.566820, 0.184704, 26.247003, -10.795277, -31.935055
103 | 102, 0.805918, -0.062002, -0.583513, 0.078563, 26.201054, -10.004852, -32.124834
104 | 103, 0.790428, -0.081054, -0.599106, 0.098651, 26.632166, -11.184182, -33.104917
105 | 104, 0.794653, 0.037876, -0.577550, 0.183126, 26.592109, -11.232337, -33.089382
106 | 105, 0.789423, 0.034502, -0.581540, 0.193489, 26.857379, -11.653954, -34.357683
107 | 106, 0.786001, -0.081839, -0.603818, 0.104472, 26.899233, -11.588782, -34.369118
108 | 107, 0.780701, -0.076752, -0.611157, 0.105391, 27.194561, -12.032754, -35.451815
109 | 108, 0.783999, 0.042361, -0.587927, 0.194677, 27.153254, -12.082214, -35.445008
110 | 109, 0.781910, 0.043691, -0.588971, 0.199566, 27.400087, -12.474849, -36.553842
111 | 110, 0.797014, -0.078718, -0.590014, 0.102287, 27.356736, -12.125655, -36.606714
112 | 111, 0.633435, -0.050797, -0.749906, 0.183915, 28.164230, -13.093126, -38.676315
113 | 112, 0.806664, -0.041894, -0.575377, 0.128390, 28.222160, -13.112096, -38.757274
114 | 113, 0.794386, -0.004057, -0.597747, 0.107878, 29.658978, -13.378169, -39.794013
115 | 114, 0.792875, 0.117015, -0.567086, 0.189937, 29.545294, -13.707894, -39.779522
116 | 115, 0.756521, 0.086496, -0.623383, 0.177739, 30.373342, -13.660206, -40.733387
117 | 116, 0.772199, -0.038855, -0.627892, 0.089189, 30.402987, -13.475293, -40.788729
118 | 117, 0.754896, -0.030223, -0.647464, 0.100073, 30.888722, -13.663657, -41.838806
119 | 118, 0.750451, 0.095012, -0.627912, 0.183107, 30.830177, -13.896355, -41.810656
120 | 119, 0.750233, 0.093508, -0.633967, 0.162779, 31.149763, -14.609628, -42.743950
121 | 120, 0.752953, -0.020888, -0.654213, 0.068092, 31.192884, -14.447508, -42.820676
122 | 121, 0.821014, -0.052066, -0.565194, 0.061530, 31.268113, -14.988436, -43.877206
123 | 122, 0.814122, 0.050652, -0.552404, 0.171735, 31.240566, -15.020311, -43.866313
124 | 123, 0.801116, 0.057245, -0.573046, 0.162969, 31.471704, -15.415067, -44.774736
125 | 124, 0.803598, -0.034838, -0.592732, 0.041111, 31.495748, -14.628773, -45.001263
126 | 125, 0.791637, -0.074580, -0.602819, 0.066061, 32.117560, -15.077773, -46.106408
127 | 126, 0.792869, 0.052971, -0.589334, 0.145752, 32.075507, -15.110679, -46.110582
128 | 127, 0.780322, 0.037451, -0.601253, 0.167912, 32.299491, -16.059679, -46.898278
129 | 128, 0.780130, -0.083644, -0.614811, 0.080078, 32.039662, -16.113631, -46.976570
130 | 129, 0.739566, -0.043987, -0.668537, 0.064571, 32.722009, -16.566442, -47.752179
131 | 130, 0.745348, 0.083554, -0.645929, 0.142325, 32.636323, -16.653507, -47.719554
132 | 131, 0.632523, 0.084506, -0.758630, 0.131365, 33.902957, -16.915332, -48.174242
133 | 132, 0.630799, -0.095217, -0.758660, 0.132159, 33.608556, -15.048960, -48.862739
134 | 133, 0.551876, 0.020148, -0.833614, -0.010904, 33.947625, -17.298712, -49.421427
135 | 134, 0.548473, 0.116738, -0.821742, 0.101455, 33.963458, -17.421138, -49.510885
136 | 135, 0.391034, 0.086405, -0.906483, 0.133862, 33.552477, -17.384232, -50.398829
137 | 136, 0.392817, -0.079812, -0.897444, 0.184187, 33.897964, -15.259127, -51.108182
138 | 137, 0.354357, -0.030074, -0.933965, 0.035208, 33.258295, -17.314199, -51.293536
139 | 138, 0.338181, 0.074409, -0.928096, 0.136886, 33.225317, -17.395420, -51.275922
140 | 139, 0.246160, 0.141905, -0.956042, 0.072494, 33.258066, -18.308806, -52.339435
141 | 140, 0.263065, -0.067457, -0.961094, 0.050491, 33.146399, -17.180267, -52.392437
142 | 141, 0.179887, 0.008324, -0.983630, -0.006982, 31.748047, -18.007549, -52.743633
143 | 142, 0.173633, 0.094885, -0.973794, 0.112154, 31.763565, -18.148729, -52.690621
144 | 143, 0.188672, -0.006487, -0.965441, 0.179690, 30.981646, -17.266173, -53.367540
145 | 144, 0.192822, -0.083024, -0.976480, 0.049171, 31.007125, -17.203217, -53.404028
146 | 145, 0.182701, 0.017220, -0.982568, -0.029814, 30.146359, -18.358408, -53.739766
147 | 146, 0.176417, 0.089729, -0.975207, 0.099002, 30.124051, -18.395429, -53.708205
148 | 147, 0.194399, -0.018814, -0.968655, 0.153511, 28.988921, -17.274306, -53.985769
149 | 148, 0.188889, -0.044875, -0.980953, 0.006427, 28.974803, -17.280859, -54.015652
150 | 149, 0.164096, 0.006684, -0.985353, -0.045945, 27.989385, -18.462658, -54.578580
151 | 150, 0.161776, 0.035493, -0.981028, 0.100781, 27.987862, -18.494725, -54.525210
152 | 151, 0.189226, 0.025239, -0.975277, 0.111336, 26.896571, -18.526303, -54.848514
153 | 152, 0.195108, -0.034658, -0.980113, 0.010646, 26.767409, -17.098650, -54.795864
154 | 153, 0.200666, 0.006896, -0.978981, -0.035853, 25.962881, -18.487888, -55.172140
155 | 154, 0.201186, 0.025613, -0.972494, 0.114571, 25.981121, -18.491942, -55.124620
156 | 155, 0.193558, 0.022984, -0.974539, 0.110832, 24.908392, -18.660796, -55.406091
157 | 156, 0.193417, -0.003950, -0.980447, -0.036084, 24.902200, -18.679646, -55.435000
158 | 157, 0.203490, 0.010675, -0.978134, -0.041660, 23.868282, -18.620026, -55.737411
159 | 158, 0.181747, 0.034722, -0.977266, 0.103517, 23.882220, -18.579888, -55.700396
160 | 159, 0.159875, 0.041444, -0.981004, 0.101770, 22.849182, -18.768178, -56.013745
161 | 160, 0.159818, 0.002507, -0.986061, -0.046273, 22.857939, -18.832178, -55.965635
162 | 161, 0.161816, -0.008954, -0.984872, -0.061367, 21.759189, -18.740045, -56.367921
163 | 162, 0.159851, 0.027954, -0.983087, 0.084911, 21.751420, -18.703395, -56.309119
164 | 163, 0.148895, 0.055840, -0.984018, 0.080152, 20.681265, -19.100403, -56.784151
165 | 164, 0.141563, 0.002962, -0.987104, -0.074701, 20.638277, -18.701044, -56.795848
166 | 165, 0.316212, 0.068370, -0.943014, -0.077872, 20.209589, -19.048708, -56.094470
167 | 166, 0.338225, 0.036540, -0.935260, 0.097775, 20.328041, -19.070202, -56.451462
168 | 167, 0.433721, 0.146785, -0.885494, 0.079020, 20.060576, -19.148088, -55.996000
169 | 168, 0.446451, 0.124310, -0.883891, -0.063018, 20.315923, -18.946001, -56.360483
170 | 169, 0.137710, -0.005938, -0.989336, -0.047107, 18.216636, -19.027216, -56.669998
171 | 170, 0.109396, 0.068871, -0.988834, 0.074159, 18.335271, -19.381762, -56.774016
172 | 171, 0.020549, 0.179475, -0.983355, 0.019600, 17.920739, -20.473545, -59.160532
173 | 172, 0.049114, -0.009488, -0.997897, -0.041268, 18.115615, -19.507821, -59.776096
174 | 173, -0.167138, -0.083490, -0.982098, -0.024124, 19.393696, -18.511707, -61.237117
175 | 174, -0.167277, 0.088616, -0.976376, 0.104200, 18.453881, -20.043237, -61.043517
176 | 175, -0.321709, 0.108004, -0.933123, -0.118839, 17.530822, -20.862225, -60.352755
177 | 176, -0.327431, 0.111683, -0.935914, -0.066202, 17.562384, -20.847037, -60.271455
178 | 177, -0.436657, 0.042590, -0.893032, -0.100067, 16.934895, -21.308536, -59.554635
179 | 178, -0.399678, 0.080610, -0.909084, -0.085595, 16.659530, -21.542979, -58.615641
180 | 179, -0.445941, 0.047335, -0.888026, -0.101522, 16.345218, -21.807453, -57.910645
181 | 180, -0.444760, 0.050442, -0.888430, -0.101667, 15.978236, -22.063572, -57.210099
182 | 181, -0.371557, 0.078169, -0.921767, -0.078621, 15.601373, -22.365758, -56.264254
183 | 182, -0.378724, 0.078236, -0.918346, -0.084188, 15.127098, -22.639484, -55.509873
184 | 183, -0.469204, 0.042279, -0.876388, -0.100021, 14.383156, -22.910472, -54.691536
185 | 184, -0.457673, 0.040444, -0.882690, -0.098784, 13.654893, -23.210144, -53.414478
186 | 185, -0.453549, 0.044839, -0.884571, -0.099083, 13.098504, -23.405260, -52.533471
187 | 186, -0.446984, 0.045058, -0.889343, -0.085114, 12.498544, -23.681156, -51.467366
188 | 187, -0.457681, 0.036954, -0.884213, -0.085617, 11.863199, -23.859292, -50.578232
189 | 188, -0.492830, -0.037816, -0.868419, 0.039212, 11.055256, -23.425253, -49.426360
190 | 189, -0.488346, 0.027542, -0.866703, -0.097914, 11.285431, -23.427064, -49.296647
191 | 190, -0.491942, 0.038870, -0.863575, -0.103536, 10.536030, -23.257730, -48.477682
192 | 191, -0.493364, -0.035204, -0.868764, 0.024503, 10.555455, -23.298313, -48.460241
193 | 192, -0.483742, -0.027472, -0.874185, 0.032242, 10.000300, -23.506485, -47.557232
194 | 193, -0.488567, 0.045285, -0.865895, -0.097353, 9.952479, -23.439204, -47.553691
195 | 194, -0.503138, 0.041040, -0.857580, -0.098616, 9.912354, -23.440219, -47.533485
196 | 195, -0.496741, 0.042865, -0.860895, -0.101353, 9.351053, -23.342944, -46.640155
197 | 196, -0.497422, -0.027416, -0.866534, 0.030645, 9.369842, -23.417931, -46.620916
198 | 197, -0.492187, -0.031296, -0.869405, 0.030168, 8.808558, -23.586604, -45.746747
199 | 198, -0.484459, 0.048147, -0.867998, -0.097782, 8.788659, -23.528907, -45.730620
200 | 199, -0.478695, 0.047823, -0.871604, -0.094190, 8.256035, -23.426072, -44.910775
201 | 200, -0.478083, -0.026156, -0.877337, 0.032147, 8.275910, -23.503418, -44.893768
202 | 201, -0.464344, -0.027229, -0.884665, 0.031831, 7.748532, -23.661429, -44.009198
203 | 202, -0.462712, 0.045720, -0.880705, -0.090369, 7.715470, -23.620145, -44.023631
204 | 203, -0.499324, 0.047095, -0.859830, -0.095666, 6.992666, -23.818957, -43.139877
205 | 204, -0.496824, -0.019940, -0.867096, 0.030232, 7.046687, -23.886156, -43.122512
206 | 205, -0.513531, -0.009408, -0.857227, 0.036892, 6.682148, -23.498206, -42.153470
207 | 206, -0.535256, 0.050285, -0.837173, -0.100577, 6.581263, -23.466340, -42.212576
208 | 207, -0.597801, 0.038929, -0.795206, -0.093632, 4.789802, -22.938193, -42.985847
209 | 208, -0.589754, -0.014094, -0.806474, 0.039898, 4.525305, -23.370005, -43.017015
210 | 209, -0.447875, 0.068226, -0.886182, -0.097147, 5.192699, -23.775185, -41.873181
211 | 210, -0.710681, -0.044173, -0.686245, -0.148498, 3.423760, -22.278683, -40.794506
212 | 211, -0.730903, -0.014377, -0.680297, 0.052657, 2.312738, -22.808140, -42.676051
213 | 212, -0.538096, -0.038039, -0.841206, 0.037157, 3.833087, -23.732499, -39.418232
214 | 213, -0.544904, 0.008787, -0.830656, -0.114087, 3.871839, -23.769020, -39.365832
215 | 214, -0.490300, -0.046023, -0.869721, 0.032808, 3.443815, -23.584492, -38.226039
216 | 215, -0.500481, 0.040553, -0.860007, -0.090907, 3.403807, -23.555623, -38.251819
217 | 216, -0.477087, 0.029068, -0.873483, -0.092587, 2.997726, -23.441612, -37.194192
218 | 217, -0.474538, -0.045643, -0.878352, 0.035079, 3.025122, -23.454183, -37.177979
219 | 218, -0.477575, -0.039748, -0.876656, 0.042659, 2.431677, -23.330458, -36.045295
220 | 219, -0.480282, 0.036521, -0.871638, -0.090808, 2.391556, -23.372779, -36.042778
221 | 220, -0.484216, 0.039838, -0.870254, -0.081296, 1.776584, -23.805914, -35.232270
222 | 221, -0.485273, -0.027073, -0.872767, 0.045358, 1.780073, -23.892582, -35.195405
223 | 222, -0.464142, -0.030838, -0.884215, 0.042270, 1.158039, -23.082163, -34.191523
224 | 223, -0.466556, 0.042054, -0.879104, -0.087957, 1.137199, -23.002591, -34.205571
225 | 224, -0.462656, 0.049630, -0.880770, -0.087939, 0.562567, -24.194182, -33.145829
226 | 225, -0.464230, -0.025319, -0.884388, 0.041366, 0.585771, -24.223642, -33.120364
227 | 226, -0.441317, -0.014359, -0.896143, 0.044301, -0.085421, -24.489189, -31.932900
228 | 227, -0.440434, 0.057507, -0.891844, -0.085601, -0.124857, -24.473878, -31.952378
229 | 228, -0.425925, 0.065668, -0.898179, -0.086912, -0.809515, -24.692700, -30.942128
230 | 229, -0.425827, -0.013309, -0.903769, 0.041213, -0.873015, -24.403693, -30.955100
231 | 230, -0.476421, -0.009080, -0.878024, 0.044921, -1.662661, -24.348515, -30.130037
232 | 231, -0.478137, 0.061751, -0.869898, -0.104165, -1.730306, -24.302974, -30.160304
233 | 232, -0.502245, 0.057642, -0.857251, -0.097734, -2.470916, -24.149594, -29.578566
234 | 233, -0.513863, -0.003139, -0.856598, 0.046667, -2.531458, -24.315793, -29.533952
235 | 234, -0.511559, -0.011144, -0.858288, 0.039083, -3.031384, -25.347678, -28.690367
236 | 235, -0.503931, 0.042225, -0.856200, -0.105807, -3.064183, -25.210597, -28.688067
237 | 236, -0.460328, 0.068155, -0.881253, -0.082761, -3.496484, -24.583269, -27.722817
238 | 237, -0.460843, -0.014778, -0.886180, 0.045754, -3.456768, -24.616414, -27.730988
239 | 238, -0.471889, -0.011980, -0.880444, 0.044698, -4.035158, -25.209523, -26.672250
240 | 239, -0.473417, 0.070429, -0.874198, -0.081828, -4.088204, -25.180409, -26.659300
241 | 240, -0.474084, 0.066306, -0.874182, -0.081588, -4.837940, -24.948593, -26.083814
242 | 241, -0.476348, -0.000603, -0.877757, 0.051366, -4.788948, -25.041039, -26.097658
243 | 242, -0.498477, -0.001727, -0.865679, 0.046045, -5.314973, -25.245171, -25.358816
244 | 243, -0.495402, 0.061311, -0.861876, -0.089398, -5.370601, -25.154659, -25.341975
245 | 244, -0.467943, 0.063637, -0.876580, -0.092682, -5.937819, -25.435848, -24.063668
246 | 245, -0.472531, -0.008313, -0.880447, 0.038223, -5.900034, -25.420830, -24.046737
247 | 246, -0.444040, 0.000878, -0.894959, 0.043338, -6.643660, -25.736330, -23.101446
248 | 247, -0.441131, 0.074487, -0.890422, -0.083704, -6.678191, -25.640184, -23.109265
249 | 248, -0.453427, 0.066105, -0.884261, -0.090105, -7.338678, -25.638995, -22.213344
250 | 249, -0.451655, -0.009190, -0.891226, 0.040539, -7.248762, -25.313241, -22.155705
251 | 250, -0.460733, 0.006845, -0.886368, 0.045071, -7.998631, -25.667203, -21.348502
252 | 251, -0.462168, 0.075997, -0.878141, -0.097453, -8.049133, -25.586683, -21.380545
253 | 252, -0.462302, 0.069778, -0.878578, -0.097527, -8.681618, -26.339353, -20.456052
254 | 253, -0.468536, -0.001418, -0.882140, 0.047973, -8.687802, -25.538390, -20.357706
255 | 254, -0.488789, -0.007177, -0.871257, 0.044154, -9.313348, -25.123544, -19.567963
256 | 255, -0.486664, 0.050821, -0.866781, -0.096269, -9.372065, -26.242987, -19.640574
257 | 256, -0.465655, 0.063490, -0.878129, -0.089595, -9.887955, -26.852578, -18.599908
258 | 257, -0.468443, 0.002931, -0.882540, 0.040950, -9.822325, -25.538603, -18.439473
259 | 258, -0.449858, -0.007310, -0.892120, 0.041233, -10.160383, -24.968316, -17.502178
260 | 259, -0.445487, 0.069837, -0.888347, -0.086641, -10.187233, -25.030323, -17.514345
261 | 260, -0.461930, 0.058323, -0.880673, -0.087394, -10.915289, -27.352870, -16.684914
262 | 261, -0.466205, -0.012433, -0.883543, 0.043057, -10.878898, -27.328799, -16.658863
263 | 262, -0.593282, -0.030786, -0.803953, 0.027037, -12.155996, -27.578928, -16.089994
264 | 263, -0.582643, 0.014162, -0.805397, -0.108008, -12.176284, -27.379211, -16.051023
265 | 264, -0.584510, -0.006709, -0.802674, -0.118408, -12.447818, -27.514293, -15.160064
266 | 265, -0.592824, -0.057299, -0.802942, 0.023736, -12.420688, -27.592623, -15.191554
267 | 266, -0.753593, 0.004455, -0.656048, 0.041015, -13.320680, -27.155797, -14.465074
268 | 267, -0.722578, -0.070860, -0.668701, -0.160322, -13.347604, -27.595909, -14.435970
269 | 268, -0.830813, 0.103700, -0.542654, -0.067283, -13.163300, -27.039332, -13.463614
270 | 269, -0.850221, 0.000247, -0.523857, 0.051969, -13.177554, -27.099561, -13.468226
271 | 270, 0.969285, -0.006054, 0.245671, 0.009814, -1.893900, -0.040817, 1.588940
272 | 271, -0.846483, 0.089922, -0.520157, -0.069433, -12.779609, -27.278525, -12.458822
273 | 272, 0.988458, -0.151223, 0.009131, 0.000002, -0.758147, 0.038092, 0.701909
274 | 273, 0.999935, -0.002788, 0.010957, 0.001432, -0.731500, -0.019048, 0.704759
275 | 274, -0.745735, 0.000826, -0.663810, 0.056908, -17.125693, -27.468899, -13.738050
276 | 275, -0.744438, 0.033115, -0.659060, -0.101777, -16.566757, -26.304171, -15.133093
277 |
--------------------------------------------------------------------------------
/slam_tutorial-solution/data/optimized_poses.csv:
--------------------------------------------------------------------------------
1 | 0, 1, 0, 0, 0, 0, 0, 5.58294e-322
2 | 1, 0.987301, 0.158543, -0.00999463, -0.000446507, -0.00485316, -0.0543497, 5.58294e-322
3 | 2, 0.988952, -0.148215, -0.0024255, -0.000798271, -0.00853961, 0.034821, 5.58294e-322
4 | 3, 0.962263, 0.146891, -0.227647, 0.0254852, 1.24504, -0.0402788, 5.58294e-322
5 | 4, 0.950313, 0.149438, 0.271505, -0.0293122, -1.86116, -0.0858317, 5.58294e-322
6 | 5, 0.999856, -0.00110548, 0.0169101, -0.000485505, -1.09005, -0.119036, 5.58294e-322
7 | 6, 0.987923, 0.153804, 0.0181361, -0.00492611, -1.09269, -0.163181, 5.58294e-322
8 | 7, 0.988159, 0.152437, -0.0167804, 0.00490578, -1.98549, -0.16395, 5.58294e-322
9 | 8, 0.999834, -0.000234814, -0.0181158, 0.00175299, -1.9808, -0.12208, 5.58294e-322
10 | 9, 0.999653, -0.00248118, -0.0259552, 0.00375223, -2.84033, -0.131827, 5.58294e-322
11 | 10, 0.986797, 0.159531, -0.0248871, 0.0127101, -2.84163, -0.178585, 5.58294e-322
12 | 11, 0.987935, 0.154421, -0.0106065, 0.00520119, -3.77435, -0.193595, 5.58294e-322
13 | 12, 0.999922, -0.00650132, -0.0105925, 0.00147235, -3.77094, -0.147051, 5.58294e-322
14 | 13, 0.99969, -0.00703612, -0.0238705, 0.000235742, -4.7257, -0.180028, 5.58294e-322
15 | 14, 0.987818, 0.153703, -0.0236767, 0.00548988, -4.74137, -0.228341, 5.58294e-322
16 | 15, 0.988027, 0.153141, -0.0169504, 0.00789529, -5.68, -0.217194, 5.58294e-322
17 | 16, 0.999809, -0.00646855, -0.0180467, 0.00375237, -5.67958, -0.17421, 5.58294e-322
18 | 17, 0.999263, -0.00732474, -0.0376668, 0.000864609, -6.63237, -0.225622, 5.58294e-322
19 | 18, 0.987839, 0.150949, -0.036957, 0.00465576, -6.63318, -0.270527, 5.58294e-322
20 | 19, 0.987953, 0.152877, -0.0219669, 0.00976929, -7.49172, -0.32618, 5.58294e-322
21 | 20, 0.99973, -0.00421681, -0.0224645, 0.00426549, -7.4956, -0.284654, 5.58294e-322
22 | 21, 0.999834, -0.00253482, -0.0178267, 0.00269194, -8.43389, -0.267207, 5.58294e-322
23 | 22, 0.988137, 0.152758, -0.0151333, 0.00463251, -8.43462, -0.305829, 5.58294e-322
24 | 23, 0.988239, 0.151722, -0.00866304, 0.0170194, -9.41631, -0.292097, 5.58294e-322
25 | 24, 0.999837, -0.00183985, -0.0103047, 0.0147361, -9.41818, -0.248837, 5.58294e-322
26 | 25, 0.999803, -0.00975817, -0.0171806, 0.001686, -10.4465, -0.239268, 5.58294e-322
27 | 26, 0.987068, 0.158327, -0.0235993, 0.00851062, -10.457, -0.288757, 5.58294e-322
28 | 27, 0.986808, 0.158338, -0.0325996, 0.00874907, -11.4611, -0.328656, 5.58294e-322
29 | 28, 0.999429, -0.00656217, -0.0331137, 0.0015771, -11.4638, -0.281343, 5.58294e-322
30 | 29, 0.999734, -0.00554904, -0.0222956, 0.00205286, -12.4368, -0.242974, 5.58294e-322
31 | 30, 0.999515, -0.00605916, -0.0304944, 0.00179266, -12.3714, -0.184834, 5.58294e-322
32 | 31, 0.98762, 0.156381, -0.00793938, 0.00945207, -13.3165, -0.16771, 5.58294e-322
33 | 32, 0.987848, 0.154311, -0.0174156, 0.00644083, -12.4453, -0.287803, 5.58294e-322
34 | 33, 0.999675, -0.00196502, -0.023962, 0.00851164, -14.7716, -0.372402, 5.58294e-322
35 | 34, 0.987178, 0.157467, -0.0227358, 0.0129095, -14.766, -0.420745, 5.58294e-322
36 | 35, 0.983401, 0.157779, 0.0895938, -0.0012995, -15.717, -0.367305, 5.58294e-322
37 | 36, 0.995006, -4.00264e-05, 0.0991127, 0.0117833, -15.7063, -0.321625, 5.58294e-322
38 | 37, 0.97191, -0.000839112, 0.235056, 0.0117665, -16.4347, -0.10739, 5.58294e-322
39 | 38, 0.960375, 0.150162, 0.233642, -0.023274, -16.455, -0.149901, 5.58294e-322
40 | 39, 0.919743, 0.144635, 0.362047, -0.0455601, -17.1499, 0.381916, 5.58294e-322
41 | 40, 0.93158, -0.00119907, 0.363335, 0.0120366, -17.1231, 0.418153, 5.58294e-322
42 | 41, 0.886604, 0.00186955, 0.462375, 0.011807, -17.4703, 0.99017, 5.58294e-322
43 | 42, 0.87602, 0.140543, 0.457696, -0.0578869, -17.5084, 0.961409, 5.58294e-322
44 | 43, 0.81369, 0.148692, 0.555154, -0.0871946, -18.0822, 1.8758, 5.58294e-322
45 | 44, 0.82635, 0.0205038, 0.562772, -0.00369218, -18.0397, 1.89585, 5.58294e-322
46 | 45, 0.734049, -0.0242328, 0.678619, 0.00783205, -18.2621, 3.30607, 5.58294e-322
47 | 46, 0.728829, 0.0879193, 0.672285, -0.0954536, -18.3041, 3.30507, 5.58294e-322
48 | 47, 0.744746, 0.0646857, 0.659728, -0.0769966, -18.2555, 4.37002, 5.58294e-322
49 | 48, 0.745603, -0.0494426, 0.664164, 0.0227766, -18.2173, 4.37046, 5.58294e-322
50 | 49, 0.731516, -0.0332353, 0.679324, 0.0479408, -18.5086, 5.55665, 5.58294e-322
51 | 50, 0.729899, 0.0830852, 0.675962, -0.0584867, -18.5457, 5.55308, 5.58294e-322
52 | 51, 0.708208, 0.0901319, 0.698128, -0.0541748, -18.5761, 6.83621, 5.58294e-322
53 | 52, 0.71229, -0.0244796, 0.699241, 0.0557285, -18.5367, 6.8367, 5.58294e-322
54 | 53, 0.699948, -0.0269344, 0.70954, 0.0768158, -18.5314, 8.12234, 5.58294e-322
55 | 54, 0.734154, 0.0909058, 0.672264, -0.0285557, -18.5821, 8.14977, 5.58294e-322
56 | 55, 0.711389, 0.0614835, 0.69981, -0.0202737, -18.3773, 9.09224, 5.58294e-322
57 | 56, 0.71405, -0.0472486, 0.692508, 0.0912883, -18.3442, 9.09808, 5.58294e-322
58 | 57, 0.72217, -0.0877013, 0.680631, 0.0867276, -18.3677, 10.7301, 5.58294e-322
59 | 58, 0.732553, 0.0342925, 0.679728, -0.0125984, -18.4073, 10.7349, 5.58294e-322
60 | 59, 0.695697, -0.00326142, 0.717028, 0.0431861, -18.3793, 11.9148, 5.58294e-322
61 | 60, 0.680591, -0.105014, 0.710675, 0.143903, -18.3536, 11.9046, 5.58294e-322
62 | 61, 0.712133, -0.0878511, 0.684225, 0.130328, -18.5948, 13.1172, 5.58294e-322
63 | 62, 0.714825, 0.0219796, 0.698336, 0.0294731, -18.6161, 13.1128, 5.58294e-322
64 | 63, 0.64369, 0.0877226, 0.75918, -0.0401658, -19.4746, 13.8483, 5.58294e-322
65 | 64, 0.672283, -0.0105234, 0.736811, 0.0709518, -19.4366, 13.8551, 5.58294e-322
66 | 65, 0.702987, -0.00771466, 0.699467, 0.128431, -19.407, 14.8221, 5.58294e-322
67 | 66, 0.703457, 0.0991949, 0.703474, 0.0208285, -19.4373, 14.817, 5.58294e-322
68 | 67, 0.702352, 0.0527535, 0.698773, -0.125038, -19.5841, 16.4086, 5.58294e-322
69 | 68, 0.72838, -0.0523561, 0.682941, -0.0176903, -19.5411, 16.4065, 5.58294e-322
70 | 69, 0.704091, -0.0146747, 0.709694, 0.0193547, -19.7084, 17.4207, 5.58294e-322
71 | 70, 0.703872, 0.0972739, 0.698705, -0.0831417, -19.7891, 17.4224, 5.58294e-322
72 | 71, 0.706036, 0.0555916, 0.699182, -0.0978115, -19.7347, 18.7456, 5.58294e-322
73 | 72, 0.706297, -0.0511177, 0.706051, 0.00477432, -19.7004, 18.7424, 5.58294e-322
74 | 73, 0.699446, -0.0238402, 0.714071, 0.0176333, -19.6655, 20.0127, 5.58294e-322
75 | 74, 0.695018, 0.0810239, 0.709157, -0.0864963, -19.7021, 20.0192, 5.58294e-322
76 | 75, 0.691074, 0.0850087, 0.711597, -0.0939148, -19.6651, 21.5032, 5.58294e-322
77 | 76, 0.693288, -0.0200043, 0.720293, 0.0114007, -19.6331, 21.4954, 5.58294e-322
78 | 77, 0.708346, -0.00852469, 0.705723, -0.0113, -19.6121, 22.8367, 5.58294e-322
79 | 78, 0.700287, 0.0976922, 0.69813, -0.112557, -19.6529, 22.8458, 5.58294e-322
80 | 79, 0.678071, 0.0872604, 0.722904, -0.100071, -19.3143, 24.2464, 5.58294e-322
81 | 80, 0.684132, -0.0147455, 0.729192, 0.00511263, -19.2799, 24.2417, 5.58294e-322
82 | 81, 0.686546, -0.00860025, 0.726656, -0.0234867, -19.1515, 25.56, 5.58294e-322
83 | 82, 0.677707, 0.0936961, 0.717807, -0.129181, -19.1936, 25.5647, 5.58294e-322
84 | 83, 0.677667, 0.0931972, 0.72149, -0.1074, -18.9156, 26.624, 5.58294e-322
85 | 84, 0.684314, -0.00791689, 0.729138, -0.0030804, -18.8737, 26.614, 5.58294e-322
86 | 85, 0.69409, -0.0279243, 0.719345, -0.00138069, -18.7278, 27.9423, 5.58294e-322
87 | 86, 0.689547, 0.0769002, 0.712164, -0.106924, -18.77, 27.9526, 5.58294e-322
88 | 87, 0.700906, 0.0940387, 0.698236, -0.11115, -18.781, 29.1462, 5.58294e-322
89 | 88, 0.706811, -0.0100242, 0.707292, -0.00740499, -18.7387, 29.1434, 5.58294e-322
90 | 89, 0.736421, -0.00727379, 0.676473, -0.00391313, -18.7648, 30.5402, 5.58294e-322
91 | 90, 0.728974, 0.10431, 0.668302, -0.105303, -18.8076, 30.5423, 5.58294e-322
92 | 91, 0.691445, 0.117383, 0.703817, -0.112984, -18.8461, 31.6116, 5.58294e-322
93 | 92, 0.700815, 0.0127172, 0.713164, -0.00969401, -18.8017, 31.6129, 5.58294e-322
94 | 93, 0.68875, -0.0278531, 0.724462, 0.00125823, -18.8845, 32.7307, 5.58294e-322
95 | 94, 0.685649, 0.0762132, 0.71573, -0.108661, -18.929, 32.7408, 5.58294e-322
96 | 95, 0.689694, 0.0678965, 0.716042, -0.0836412, -18.785, 33.9097, 5.58294e-322
97 | 96, 0.692336, -0.0353309, 0.720316, 0.0238023, -18.7482, 33.9068, 5.58294e-322
98 | 97, 0.709063, 0.0152169, 0.698779, 0.0933014, -18.8006, 35.085, 5.58294e-322
99 | 98, 0.6957, 0.121552, 0.7079, -0.0102085, -18.8343, 35.0798, 5.58294e-322
100 | 99, 0.689931, 0.11811, 0.713523, -0.0304955, -18.8306, 36.1595, 5.58294e-322
101 | 100, 0.699159, 0.0132646, 0.711101, 0.0730452, -18.793, 36.1665, 5.58294e-322
102 | 101, 0.732495, -0.00469362, 0.680117, 0.0294819, -18.9584, 37.6149, 5.58294e-322
103 | 102, 0.725459, 0.103662, 0.676637, -0.0715954, -18.9998, 37.6137, 5.58294e-322
104 | 103, 0.71501, 0.104156, 0.687012, -0.0769804, -19.0265, 38.8993, 5.58294e-322
105 | 104, 0.721393, -0.004432, 0.692126, 0.0231145, -18.9851, 38.9001, 5.58294e-322
106 | 105, 0.711274, -0.0146586, 0.70256, 0.0168607, -18.8932, 40.2596, 5.58294e-322
107 | 106, 0.704405, 0.0924978, 0.698325, -0.0871794, -18.9325, 40.2586, 5.58294e-322
108 | 107, 0.696822, 0.0893351, 0.706899, -0.0821708, -18.9177, 41.4728, 5.58294e-322
109 | 108, 0.70209, -0.0184049, 0.71151, 0.0220053, -18.8786, 41.4696, 5.58294e-322
110 | 109, 0.701614, -0.0229192, 0.711842, 0.0222126, -18.7972, 42.6497, 5.58294e-322
111 | 110, 0.714294, 0.0876698, 0.68932, -0.0832811, -18.8513, 42.6423, 5.58294e-322
112 | 111, 0.543689, -0.00875858, 0.831137, -0.116344, -18.0443, 44.8642, 5.58294e-322
113 | 112, 0.730196, 0.0136231, 0.679357, -0.0714309, -18.1211, 44.93, 5.58294e-322
114 | 113, 0.709599, 0.0294628, 0.703258, -0.0320922, -19.1761, 46.3767, 5.58294e-322
115 | 114, 0.706245, -0.0775614, 0.700277, 0.0693941, -19.1452, 46.3773, 5.58294e-322
116 | 115, 0.669929, -0.052107, 0.73938, 0.0423984, -19.5127, 47.5406, 5.58294e-322
117 | 116, 0.682835, 0.0547619, 0.72554, -0.065798, -19.5602, 47.563, 5.58294e-322
118 | 117, 0.663988, 0.0440546, 0.743996, -0.0604141, -19.549, 48.7491, 5.58294e-322
119 | 118, 0.662597, -0.0584117, 0.745083, 0.049049, -19.5138, 48.7435, 5.58294e-322
120 | 119, 0.657718, -0.0385893, 0.750801, 0.0470728, -19.6309, 49.8986, 5.58294e-322
121 | 120, 0.657394, 0.061197, 0.748226, -0.0651623, -19.6651, 49.9067, 5.58294e-322
122 | 121, 0.734433, 0.0799877, 0.669805, -0.074641, -19.6924, 51.0882, 5.58294e-322
123 | 122, 0.73765, -0.0334943, 0.673889, 0.0249865, -19.6528, 51.0866, 5.58294e-322
124 | 123, 0.717667, -0.0289857, 0.69526, 0.0269864, -19.7097, 52.1182, 5.58294e-322
125 | 124, 0.714337, 0.0810453, 0.690948, -0.075797, -19.7467, 52.1217, 5.58294e-322
126 | 125, 0.700013, 0.0860724, 0.702494, -0.095267, -19.6906, 53.4747, 5.58294e-322
127 | 126, 0.705027, -0.0206152, 0.708824, 0.00898752, -19.651, 53.4723, 5.58294e-322
128 | 127, 0.698411, -0.0290181, 0.715106, 0.00154202, -19.4938, 54.4748, 5.58294e-322
129 | 128, 0.694441, 0.0769837, 0.707972, -0.102963, -19.5333, 54.4831, 5.58294e-322
130 | 129, 0.641757, 0.0804331, 0.759117, -0.0736138, -19.4193, 55.52, 5.58294e-322
131 | 130, 0.647144, -0.0193244, 0.761226, 0.0369478, -19.384, 55.5079, 5.58294e-322
132 | 131, 0.520091, -0.0203457, 0.853809, 0.0101038, -18.8936, 56.4918, 5.58294e-322
133 | 132, 0.517477, 0.0586651, 0.845704, -0.116452, -18.9287, 56.5144, 5.58294e-322
134 | 133, 0.417932, 0.0784966, 0.898465, -0.109227, -18.2961, 57.6497, 5.58294e-322
135 | 134, 0.424803, 0.0144521, 0.904827, 0.0249198, -18.2627, 57.6267, 5.58294e-322
136 | 135, 0.262349, 0.0051511, 0.964062, 0.0416037, -17.3943, 58.2001, 5.58294e-322
137 | 136, 0.259025, 0.0505962, 0.95822, -0.110275, -17.4237, 58.2347, 5.58294e-322
138 | 137, 0.202839, 0.0330888, 0.973167, -0.103478, -16.5127, 58.5396, 5.58294e-322
139 | 138, 0.206148, 0.00183323, 0.977696, 0.0401169, -16.4984, 58.5074, 5.58294e-322
140 | 139, 0.104329, -0.00863145, 0.993929, 0.0338458, -15.4077, 58.6899, 5.58294e-322
141 | 140, 0.104236, 0.00808243, 0.987911, -0.11446, -15.4135, 58.7264, 5.58294e-322
142 | 141, 0.0386665, -0.0134111, 0.992301, -0.116893, -14.43, 58.795, 5.58294e-322
143 | 142, 0.0374745, -0.0208787, 0.998596, 0.03109, -14.4311, 58.7585, 5.58294e-322
144 | 143, 0.0323386, -0.0121964, 0.998943, 0.0302928, -13.4413, 58.7842, 5.58294e-322
145 | 144, 0.0341071, -0.00463629, 0.992103, -0.120613, -13.4388, 58.8247, 5.58294e-322
146 | 145, 0.0362505, 0.00775677, 0.990575, -0.13186, -12.4477, 58.8158, 5.58294e-322
147 | 146, 0.0361599, 0.000284852, 0.999216, 0.016135, -12.45, 58.7761, 5.58294e-322
148 | 147, 0.0387855, -0.0174509, 0.999093, 0.00194299, -11.3494, 58.7627, 5.58294e-322
149 | 148, 0.0408655, -0.00908829, 0.988186, -0.147431, -11.3457, 58.8047, 5.58294e-322
150 | 149, 0.0169515, 0.00468421, 0.988754, -0.148514, -10.1339, 58.8861, 5.58294e-322
151 | 150, 0.0179053, 0.00069735, 0.999839, -0.000757339, -10.1356, 58.8478, 5.58294e-322
152 | 151, 0.0451005, -0.0106715, 0.998881, 0.00938787, -9.02003, 58.8841, 5.58294e-322
153 | 152, 0.0489869, -0.00167301, 0.988826, -0.140784, -9.00388, 58.9307, 5.58294e-322
154 | 153, 0.0541693, -0.00162474, 0.988786, -0.139162, -8.03515, 59.0085, 5.58294e-322
155 | 154, 0.053018, -0.0147432, 0.99844, 0.00948901, -8.03794, 58.9707, 5.58294e-322
156 | 155, 0.0506558, -0.0128074, 0.9986, 0.00824173, -6.94666, 58.8798, 5.58294e-322
157 | 156, 0.0521876, -0.00287167, 0.988827, -0.139604, -6.9454, 58.9203, 5.58294e-322
158 | 157, 0.0613438, 0.00283746, 0.987717, -0.143683, -5.89268, 58.9296, 5.58294e-322
159 | 158, 0.0375432, -0.00847152, 0.999253, 0.00345774, -5.89781, 58.8305, 5.58294e-322
160 | 159, 0.0112195, -0.00153018, 0.999936, -0.000137107, -4.82019, 58.6429, 5.58294e-322
161 | 160, 0.0122086, 0.00162963, 0.988655, -0.149698, -4.82078, 58.6874, 5.58294e-322
162 | 161, 0.0162814, -0.000769226, 0.986013, -0.165868, -3.69723, 58.7158, 5.58294e-322
163 | 162, 0.017019, -0.00504446, 0.999703, -0.0166732, -3.70088, 58.6748, 5.58294e-322
164 | 163, -0.00117761, 0.0137614, 0.999709, -0.0197671, -2.49451, 58.7197, 5.58294e-322
165 | 164, 0.000410799, 0.0168899, 0.984221, -0.176135, -2.50529, 58.7642, 5.58294e-322
166 | 165, 0.189401, 0.0394119, 0.96697, -0.165961, -2.12556, 58.4629, 5.58294e-322
167 | 166, 0.193717, 0.00668315, 0.980923, -0.0148305, -2.109, 58.419, 5.58294e-322
168 | 167, 0.33676, 0.00179454, 0.941542, -0.00935609, -1.96234, 58.3575, 5.58294e-322
169 | 168, 0.337949, 0.0535712, 0.927511, -0.150478, -2.01152, 58.4543, 5.58294e-322
170 | 169, -0.00832987, 0.00569834, 0.988515, -0.150788, -0.220124, 57.8546, 5.58294e-322
171 | 170, -0.00745189, 0.0067306, 0.999946, -0.00260592, -0.213172, 57.8118, 5.58294e-322
172 | 171, -0.0881521, 0.00740648, 0.996046, 0.00819805, 2.05236, 57.9006, 5.58294e-322
173 | 172, -0.0883558, -0.00465614, 0.986447, -0.138177, 2.06223, 57.9441, 5.58294e-322
174 | 173, -0.310152, -0.0394349, 0.941884, -0.122904, 2.92067, 57.7618, 5.58294e-322
175 | 174, -0.312615, 0.00165621, 0.949522, 0.0260291, 2.89726, 57.7136, 5.58294e-322
176 | 175, 0.472264, 0.095861, -0.861892, 0.15786, 3.1924, 56.4818, 5.58294e-322
177 | 176, 0.476415, 0.0689146, -0.869215, 0.11289, 3.17678, 56.4722, 5.58294e-322
178 | 177, 0.587256, 0.0853987, -0.797162, 0.111225, 3.18883, 55.3913, 5.58294e-322
179 | 178, 0.549351, 0.084767, -0.824398, 0.106752, 3.16472, 54.4608, 5.58294e-322
180 | 179, 0.597967, 0.103823, -0.789218, 0.0937576, 3.10406, 53.6501, 5.58294e-322
181 | 180, 0.59753, 0.10341, -0.78929, 0.0963574, 3.09851, 52.816, 5.58294e-322
182 | 181, -0.531993, -0.0869545, 0.836665, -0.0970263, 3.2504, 51.7659, 5.58294e-322
183 | 182, -0.543646, -0.0906892, 0.828187, -0.101644, 3.38221, 50.8442, 5.58294e-322
184 | 183, -0.634134, -0.102403, 0.760706, -0.0933461, 3.50627, 49.7136, 5.58294e-322
185 | 184, -0.630163, -0.105062, 0.764135, -0.0891917, 3.65446, 48.2222, 5.58294e-322
186 | 185, -0.63191, -0.105476, 0.762214, -0.0927057, 3.77628, 47.17, 5.58294e-322
187 | 186, -0.632522, -0.095146, 0.763829, -0.086185, 3.84182, 45.9187, 5.58294e-322
188 | 187, -0.649709, -0.0978935, 0.749471, -0.0811644, 3.96332, 44.8189, 5.58294e-322
189 | 188, -0.690666, -0.00394152, 0.722652, 0.0271979, 4.06208, 43.4653, 5.58294e-322
190 | 189, -0.680062, -0.107631, 0.720435, -0.0830979, 4.01036, 43.4916, 5.58294e-322
191 | 190, -0.682279, -0.118971, 0.716092, -0.0869123, 4.08007, 42.371, 5.58294e-322
192 | 191, -0.692125, -0.0169522, 0.721291, 0.0203689, 4.03952, 42.3706, 5.58294e-322
193 | 192, -0.686701, -0.00938369, 0.726664, 0.0176993, 4.11402, 41.2728, 5.58294e-322
194 | 193, -0.678483, -0.112252, 0.720234, -0.0912312, 4.15589, 41.272, 5.58294e-322
195 | 194, -0.691042, -0.112327, 0.708216, -0.0909615, 4.14206, 41.2342, 5.58294e-322
196 | 195, -0.690572, -0.116341, 0.708189, -0.0896898, 4.21587, 40.1411, 5.58294e-322
197 | 196, -0.699428, -0.0117756, 0.714401, 0.0171208, 4.17377, 40.143, 5.58294e-322
198 | 197, -0.698586, -0.0110178, 0.715212, 0.0181265, 4.24421, 39.0709, 5.58294e-322
199 | 198, -0.681044, -0.11394, 0.717551, -0.0911998, 4.28491, 39.063, 5.58294e-322
200 | 199, -0.681084, -0.108859, 0.718156, -0.0923409, 4.3524, 38.0646, 5.58294e-322
201 | 200, -0.689351, -0.00602219, 0.724276, 0.0135072, 4.31299, 38.062, 5.58294e-322
202 | 201, -0.678122, -0.0146485, 0.734539, 0.0196939, 4.36453, 37.0064, 5.58294e-322
203 | 202, -0.669412, -0.108052, 0.729845, -0.0868266, 4.40487, 37.0068, 5.58294e-322
204 | 203, -0.697836, -0.105008, 0.701837, -0.0970657, 4.44154, 35.8651, 5.58294e-322
205 | 204, -0.702807, -0.00736475, 0.711321, 0.0054684, 4.40455, 35.8627, 5.58294e-322
206 | 205, -0.720694, -0.0071409, 0.693188, 0.00636269, 4.10361, 34.8641, 5.58294e-322
207 | 206, -0.727708, -0.115177, 0.669182, -0.0968078, 4.13263, 34.8699, 5.58294e-322
208 | 207, -0.77614, -0.120066, 0.613058, -0.0857392, 5.62876, 34.059, 5.58294e-322
209 | 208, -0.781954, -0.00404808, 0.623293, 0.00617877, 5.58975, 34.0627, 5.58294e-322
210 | 209, -0.653787, -0.106221, 0.741431, -0.10752, 5.67751, 34.0348, 5.58294e-322
211 | 210, -0.872029, -0.138468, 0.465551, -0.0604528, 5.06439, 31.3795, 5.58294e-322
212 | 211, -0.883309, -0.00577683, 0.468442, 0.0171491, 5.02782, 31.4064, 5.58294e-322
213 | 212, -0.737507, -0.00461554, 0.674831, 0.0257995, 5.09769, 31.3324, 5.58294e-322
214 | 213, -0.742259, -0.122636, 0.654288, -0.0769329, 5.12355, 31.3204, 5.58294e-322
215 | 214, -0.701149, -0.00443462, 0.712341, 0.0306598, 4.96917, 30.0827, 5.58294e-322
216 | 215, -0.701156, -0.115025, 0.699744, -0.0742205, 4.99965, 30.0795, 5.58294e-322
217 | 216, -0.682378, -0.104025, 0.719594, -0.0756498, 4.87668, 28.9555, 5.58294e-322
218 | 217, -0.68963, -0.00322378, 0.723426, 0.0324728, 4.83858, 28.9586, 5.58294e-322
219 | 218, -0.693072, 0.0060803, 0.720244, 0.0293544, 4.79837, 27.6694, 5.58294e-322
220 | 219, -0.68515, -0.0963446, 0.717417, -0.0812411, 4.83748, 27.6594, 5.58294e-322
221 | 220, -0.686241, -0.0919265, 0.716663, -0.0837689, 4.91289, 26.5941, 5.58294e-322
222 | 221, -0.693175, 0.0125826, 0.7204, 0.019338, 4.87465, 26.5914, 5.58294e-322
223 | 222, -0.683845, 0.00913765, 0.729287, 0.0203092, 5.00514, 25.4151, 5.58294e-322
224 | 223, -0.677734, -0.0928028, 0.723988, -0.0889098, 5.04473, 25.4117, 5.58294e-322
225 | 224, -0.669786, -0.0981073, 0.730279, -0.0919473, 5.03084, 24.1386, 5.58294e-322
226 | 225, -0.677372, 0.00271238, 0.735448, 0.016616, 4.99058, 24.1351, 5.58294e-322
227 | 226, -0.659888, 0.000486419, 0.751187, 0.0162892, 5.03744, 22.7337, 5.58294e-322
228 | 227, -0.653598, -0.0975979, 0.744664, -0.0935922, 5.07642, 22.7347, 5.58294e-322
229 | 228, -0.640597, -0.0991125, 0.754926, -0.0994949, 5.30251, 21.5114, 5.58294e-322
230 | 229, -0.647261, -0.00514843, 0.762098, 0.0152934, 5.26382, 21.5064, 5.58294e-322
231 | 230, -0.690241, -0.00158945, 0.723459, 0.0131461, 5.29752, 20.3553, 5.58294e-322
232 | 231, -0.684918, -0.100542, 0.713265, -0.109685, 5.34866, 20.3555, 5.58294e-322
233 | 232, -0.709234, -0.109521, 0.689127, -0.10048, 5.42239, 19.3196, 5.58294e-322
234 | 233, -0.720456, -0.00207327, 0.693422, 0.0102018, 5.37971, 19.3174, 5.58294e-322
235 | 234, -0.71452, 0.000908543, 0.699568, 0.00803421, 5.48855, 18.2987, 5.58294e-322
236 | 235, -0.707443, -0.108202, 0.691334, -0.0993729, 5.52734, 18.2978, 5.58294e-322
237 | 236, -0.675233, -0.101929, 0.723953, -0.0977922, 5.53182, 17.1492, 5.58294e-322
238 | 237, -0.6838, 0.00155821, 0.729555, 0.012844, 5.49172, 17.1662, 5.58294e-322
239 | 238, -0.685576, 0.00414156, 0.727909, 0.0108329, 5.54846, 16.0283, 5.58294e-322
240 | 239, -0.680113, -0.0970382, 0.719903, -0.0988402, 5.58907, 16.0147, 5.58294e-322
241 | 240, -0.68862, -0.10005, 0.711573, -0.0972466, 5.75539, 15.0149, 5.58294e-322
242 | 241, -0.694802, 0.00333478, 0.719141, 0.0087176, 5.71483, 15.0223, 5.58294e-322
243 | 242, -0.713996, 0.004164, 0.700126, 0.00412493, 5.59046, 14.0661, 5.58294e-322
244 | 243, -0.706756, -0.0991905, 0.693058, -0.101629, 5.63321, 14.0554, 5.58294e-322
245 | 244, -0.68516, -0.111851, 0.71321, -0.0968298, 5.6457, 12.6716, 5.58294e-322
246 | 245, -0.693014, -0.0096501, 0.720802, 0.00907714, 5.60571, 12.6705, 5.58294e-322
247 | 246, -0.672255, -0.00662723, 0.740222, 0.0100567, 5.79553, 11.4674, 5.58294e-322
248 | 247, -0.66501, -0.108595, 0.732493, -0.0970761, 5.83719, 11.4652, 5.58294e-322
249 | 248, -0.677591, -0.106612, 0.721038, -0.0980261, 5.88821, 10.3358, 5.58294e-322
250 | 249, -0.68162, -0.00496934, 0.73162, 0.0101105, 5.85091, 10.3413, 5.58294e-322
251 | 250, -0.688648, -0.00244639, 0.725062, 0.00656093, 5.92764, 9.25401, 5.58294e-322
252 | 251, -0.684323, -0.111544, 0.712151, -0.110002, 5.96774, 9.24908, 5.58294e-322
253 | 252, -0.684496, -0.115669, 0.712382, -0.102943, 5.92706, 8.05524, 5.58294e-322
254 | 253, -0.693508, -0.00854952, 0.720251, 0.0145654, 5.88525, 8.05428, 5.58294e-322
255 | 254, -0.71523, -0.00521786, 0.698771, 0.0117592, 5.90925, 6.98533, 5.58294e-322
256 | 255, -0.707222, -0.110074, 0.692393, -0.0911737, 5.94798, 6.97967, 5.58294e-322
257 | 256, -0.688377, -0.111499, 0.710514, -0.0942006, 5.90177, 5.78831, 5.58294e-322
258 | 257, -0.695098, -0.00741027, 0.718802, 0.0103256, 5.86535, 5.79525, 5.58294e-322
259 | 258, -0.685986, -0.00758867, 0.72749, 0.0111377, 5.68237, 4.75329, 5.58294e-322
260 | 259, -0.677273, -0.107134, 0.721336, -0.0974636, 5.72142, 4.74801, 5.58294e-322
261 | 260, -0.684994, -0.110379, 0.714513, -0.0898368, 5.72702, 3.58541, 5.58294e-322
262 | 261, -0.693938, -0.00931384, 0.719749, 0.0180118, 5.68671, 3.5896, 5.58294e-322
263 | 262, -0.793729, -0.00208906, 0.608257, 0.00372832, 5.41803, 2.39236, 5.58294e-322
264 | 263, -0.784947, -0.115788, 0.603363, -0.0800235, 5.44672, 2.37277, 5.58294e-322
265 | 264, -0.792918, -0.129074, 0.592037, -0.064127, 4.84497, 1.54651, 5.58294e-322
266 | 265, -0.802733, -0.0126534, 0.595644, 0.0258424, 4.80778, 1.56489, 5.58294e-322
267 | 266, -0.906692, -0.0180997, 0.421256, 0.0111854, 4.2167, 0.647395, 5.58294e-322
268 | 267, -0.89304, -0.157614, 0.418462, -0.0502712, 4.24027, 0.615573, 5.58294e-322
269 | 268, -0.945176, -0.155162, 0.285409, -0.0332961, 3.36306, 0.0885194, 5.58294e-322
270 | 269, -0.96378, -0.0040615, 0.266359, 0.0128335, 3.34649, 0.132214, 5.58294e-322
271 | 270, 0.968226, 0.00496042, -0.249839, -0.00966752, 2.42809, -0.476546, 5.58294e-322
272 | 271, -0.954774, -0.146366, 0.257237, -0.0285018, 2.44342, -0.51732, 5.58294e-322
273 | 272, 0.988599, 0.150024, -0.0127987, 0.00092082, 0.761279, -0.720746, 5.58294e-322
274 | 273, 0.999878, 0.00330812, -0.0152638, -0.000620657, 0.756444, -0.680824, 5.58294e-322
275 | 274, -0.902854, -0.00188077, 0.42988, 0.00737763, 7.11256, -1.94059, 5.58294e-322
276 | 275, -0.892262, -0.135626, 0.426821, -0.05743, 7.14127, -1.96395, 5.58294e-322
277 |
--------------------------------------------------------------------------------
/slam_tutorial-solution/matlab/plot_poses.m:
--------------------------------------------------------------------------------
1 | function h = plot_poses(pose_data)
2 |
3 | n_poses = size(pose_data, 1);
4 |
5 | for i=1:n_poses
6 | id = pose_data(i, 1);
7 | q = pose_data(i, 2:5);
8 | t = pose_data(i, 6:8);
9 |
10 | draw_frame(q, t, 0.5);
11 | text(t(1), t(2), t(3), [num2str(id)]);
12 | end
13 |
14 | end
15 |
16 | function draw_frame(q, t, s)
17 |
18 | if min(size(q)) == 1
19 | q = reshape(q, [1,4]);
20 | R = quat2rotm(q);
21 | end
22 |
23 | if size(t, 1) == 1
24 | t = t';
25 | end
26 |
27 | C = repmat(t, 1, 3);
28 | E = s * R + C;
29 | plot3([t(1), E(1,1)],[t(2), E(2,1)],[t(3), E(3,1)],'-r','linewidth',2); hold on;
30 | plot3([t(1), E(1,2)],[t(2), E(2,2)],[t(3), E(3,2)],'-g','linewidth',2);
31 | plot3([t(1), E(1,3)],[t(2), E(2,3)],[t(3), E(3,3)],'-b','linewidth',2);
32 |
33 | axis equal; grid on;
34 |
35 | end
36 |
--------------------------------------------------------------------------------
/slam_tutorial-solution/matlab/visualize_results.m:
--------------------------------------------------------------------------------
1 | clear; close all; clc;
2 |
3 | %% show initial poses
4 | init_data = importdata('../data/initial_poses.csv');
5 |
6 | subplot(1,2,1);
7 | plot_poses(init_data);
8 |
9 |
10 | %% show optimized poses
11 | opt_data = importdata('../data/optimized_poses.csv');
12 |
13 | subplot(1,2,2);
14 | plot_poses(opt_data);
15 |
16 |
--------------------------------------------------------------------------------
/slam_tutorial-solution/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | slam_tutorial
4 | 0.0.0
5 | The slam_tutorial package
6 |
7 | Weikun Zhen
8 |
9 | TODO
10 |
11 | catkin
12 | roscpp
13 | std_msgs
14 | roscpp
15 | std_msgs
16 | roscpp
17 | std_msgs
18 |
19 |
20 |
21 |
22 |
23 |
24 |
25 |
26 |
--------------------------------------------------------------------------------
/slam_tutorial-solution/src/ceres_spg.cpp:
--------------------------------------------------------------------------------
1 | /** Simple sparse pose graph example
2 | *
3 | *
4 | */
5 |
6 | #include
7 | #include
8 | #include
9 | #include