G2O 3D SLAM with calibration test

bunny.ply

ply
format ascii 1.0
comment VCGLIB generated
element vertex 148
property float x
property float y
property float z
property float nx
property float ny
property float nz
property int flags
element face 292
property list uchar int vertex_indices
end_header
55.2598 -29.1207 14.605 0.973919 -1.12931 -0.729613 0 
31.1172 26.4994 112.434 -0.52155 0.019812 0.714006 0 
29.227 18.7072 44.5395 0.0119161 1.15611 -0.358441 0 
10.0612 -10.7915 10.2174 -0.424335 0.0605094 -0.0586343 0 
28.565 26.4273 32.5673 0.147685 0.790805 -0.231613 0 
-0.456871 -12.8818 5.25388 -0.169482 -0.995182 0.778251 0 
31.863 17.0913 15.0763 -0.154309 0.291953 0.47583 0 
9.65761 7.29352 79.9358 0.625702 1.14377 0.431724 0 
16.2198 41.0042 112.514 0.202241 1.03554 0.309411 0 
8.38038 43.9843 107.664 0.258673 1.32449 0.395747 0 
3.33072 19.6172 82.5374 1.22729 0.229772 -0.0902912 0 
7.05195 -3.02998 92.853 0.21734 -0.972379 0.496764 0 
17.848 -29.9476 17.2534 -0.238602 -0.907962 0.594866 0 
57.8143 13.2049 53.8334 0.558248 1.49702 0.116831 0 
62.428 -28.3823 60.189 0.739328 -1.33397 0.404881 0 
77.4438 -9.29252 31.7864 1.21759 0.0160004 0.0631283 0 
63.7695 -6.07431 69.4726 0.248171 0.146569 0.763852 0 
16.8818 -3.67468 71.5072 0.436077 0.241168 0.0599277 0 
-2.56485 12.4585 77.9485 0.264661 0.4412 -1.0283 0 
-2.58205 3.29196 90.2996 -0.0626836 0.325438 1.04226 0 
4.80431 14.3531 89.1712 -0.128973 0.170127 0.82342 0 
63.0925 13.9103 22.043 0.64744 0.606298 -0.653458 0 
-7.55354 1.50641 24.9849 -0.510947 0.544551 -0.370438 0 
-5.46256 11.1365 58.6369 -0.341499 1.1168 0.630477 0 
13.4674 17.5026 8.85838 -0.701017 0.237011 0.0904952 0 
54.832 12.9908 5.35187 0.00160658 -0.0032007 -1.74744 0 
22.3958 18.5504 5.31187 0.000326077 -0.000649626 -0.354667 0 
50.7957 1.5085 72.2327 0.524549 0.481524 1.14275 0 
-14.1174 -16.907 88.406 -0.457813 -0.681879 0.706542 0 
48.752 -10.7503 75.2489 -0.370286 -0.215195 0.941038 0 
6.38619 8.42578 90.6436 1.11027 0.269957 -0.106091 0 
54.9234 -0.264993 5.37624 0.000675698 -0.00182802 -0.875791 0 
-15.6197 -19.235 41.2536 -0.446213 -0.509677 0.151736 0 
82.0118 -22.4488 25.6991 0.325436 -0.264849 -0.192571 0 
-17.3628 -2.17747 54.5558 -0.788314 0.450641 0.0275924 0 
-20.5117 -9.21455 46.4417 -1.21755 -0.141811 0.59549 0 
-19.3417 -1.29074 78.1619 -1.14322 0.94518 0.217871 0 
-9.31655 8.58915 87.9046 -1.25063 0.532278 -0.254414 0 
-22.557 -7.86388 60.1161 -0.580265 0.0308894 0.115869 0 
56.6766 18.9335 42.754 0.220746 0.535071 0.493404 0 
4.80457 10.3277 78.931 0.491333 1.87142 0.373354 0 
5.59186 20.2002 48.5694 -0.0963775 0.374904 0.206399 0 
1.50083 -25.9176 65.0045 1.57569 -1.58454 0.532267 0 
13.1848 0.510763 81.5657 0.428409 -0.194403 0.238355 0 
-7.09631 11.3793 82.828 -0.888544 0.833219 0.0693505 0 
5.31533 -16.9999 80.2601 1.34471 -1.11571 0.824896 0 
-17.0163 6.63105 62.738 -0.541534 0.251908 -0.248264 0 
68.2421 9.49475 51.547 1.00232 0.795006 0.390266 0 
-4.71178 -24.599 55.313 -0.0827027 -0.96791 0.504701 0 
42.5512 -31.0914 64.3516 -0.438215 -0.63134 0.526125 0 
-1.42583 -28.6978 35.9278 -0.268953 -0.653971 -0.490971 0 
23.362 20.3964 106.939 -0.563421 1.1167 -0.233387 0 
36.5189 26.3574 43.7216 -0.0327815 1.02092 0.0297722 0 
4.5982 36.3585 84.129 0.134471 0.371111 -0.394598 0 
6.27608 42.7545 90.716 0.534478 1.47504 -1.5684 0 
18.3093 45.1973 97.1142 0.618625 0.347741 0.17863 0 
9.6797 4.99755 100.125 -1.38272 0.875896 0.797034 0 
55.1522 -35.4013 50.3492 0.328081 -2.11318 1.26481 0 
-22.7219 -26.3611 64.2214 -0.682871 -0.624554 0.231109 0 
64.2776 -29.0771 17.0897 0.364679 -0.895484 -0.197414 0 
28.9024 20.6026 94.5502 -0.0792485 1.12158 -1.22749 0 
30.0411 30.6012 103.613 -0.0232063 0.328432 -0.359446 0 
32.6508 16.5741 59.5619 -0.101911 0.649989 1.04296 0 
-1.69333 -30.4149 44.6539 -0.384592 -0.501533 -0.110479 0 
10.3975 -3.88809 89.7095 0.952305 -0.168363 0.234985 0 
79.9774 -3.08799 21.7587 0.66658 0.377355 0.0508098 0 
2.11345 16.7165 32.4448 0.037988 0.806449 -0.907905 0 
21.0509 14.744 31.4851 0.02759 0.585709 -0.659395 0 
16.2963 -30.7114 28.9017 -0.172339 -0.52486 0.0408164 0 
23.5493 -31.6159 47.8953 -0.488516 -1.48778 0.115699 0 
8.90513 5.3768 5.27491 -0.426026 1.52334 -0.052837 0 
29.5429 11.7657 111.693 -0.591394 0.102731 0.955333 0 
38.1957 23.4682 110.779 0.264067 -0.0936618 1.30108 0 
-14.4832 -32.9049 70.881 -0.560206 -1.06061 -0.465707 0 
22.6169 2.45895 96.7873 0.444999 -2.01583 1.05186 0 
82.9968 -9.75807 31.6852 0.0576289 0.568211 0.548064 0 
29.2369 1.37228 68.7024 -0.465482 -0.162348 1.08696 0 
27.1611 -20.2105 64.5898 -0.402575 -0.140408 0.940065 0 
-4.46217 33.0562 103.48 -0.6588 0.933814 -0.416904 0 
3.26982 34.3156 94.0829 -0.653596 0.926437 -0.413611 0 
46.16 16.6259 49.9615 0.408497 2.05476 0.585912 0 
7.33785 -24.8425 15.4258 -0.743063 -1.11774 1.15097 0 
-1.81486 -7.48487 11.8594 -0.479 0.64275 1.76775 0 
-6.7721 -27.3672 83.0704 0.36402 -0.438847 -0.0532387 0 
3.63042 -19.4018 88.5387 0.954583 -1.15081 -0.13961 0 
63.9359 15.7362 34.6951 0.524824 1.09517 -0.193032 0 
27.7241 -39.7768 41.869 -0.152358 -0.614671 0.337939 0 
-8.19249 14.4554 44.9015 -0.621343 0.594135 -0.406217 0 
19.8744 -37.0502 7.22532 0.122661 -0.664462 0.0137447 0 
31.4526 -34.7439 15.3895 0.354261 -1.91906 0.0396967 0 
10.3931 6.12479 14.8424 -1.06737 0.370058 -0.557528 0 
67.266 -25.7619 5.33583 0.188857 -0.612004 0.0228543 0 
55.1358 -33.819 21.7115 0.145321 -0.780905 -0.513738 0 
67.7567 -19.7853 64.5325 0.219173 -0.146324 0.512053 0 
12.6977 -28.1816 5.31038 0.197695 -0.829064 0.378422 0 
21.2407 36.329 104.227 0.649779 0.155597 -0.0737897 0 
12.4604 -27.6314 52.3115 0.138381 -1.20055 0.180631 0 
74.1775 6.97171 41.443 1.07326 0.867702 -0.501926 0 
-16.0818 1.18279 36.2723 -0.975224 -0.132817 -0.560606 0 
74.1037 -2.43334 25.0263 0.491928 0.317087 -0.0785521 0 
71.8478 -25.795 51.5126 0.965964 -0.23915 0.413907 0 
38.3743 19.9742 16.789 0.106494 0.971008 -2.03925 0 
56.4394 12.5683 14.206 0.0107291 0.0978281 -0.205453 0 
47.8806 -23.292 72.038 0.199902 -0.316631 0.381451 0 
-5.8182 -33.935 62.8038 0.732265 -0.520207 -0.540189 0 
6.23926 4.2788 21.5696 -0.402799 0.753745 -1.01485 0 
35.3632 30.6435 105.769 0.330857 0.990243 -0.836085 0 
-7.13561 -15.087 23.8353 -0.744322 -1.14612 -1.22942 0 
75.2355 -20.4616 30.3582 0.29132 -0.337842 -0.0212223 0 
-4.24593 23.641 94.4699 -1.23374 -0.301025 0.284942 0 
39.4984 -40.3041 46.2183 -0.0179421 -0.609592 0.0397996 0 
31.1809 -41.3066 27.1143 -0.0258199 -0.877242 0.0572742 0 
-15.2951 -17.3399 55.1727 -0.440076 -0.0800709 -0.492996 0 
1.37167 -29.6384 8.03567 -0.310022 -1.07351 -1.86227 0 
65.3927 1.68511 12.9791 0.564568 0.568537 -0.923252 0 
70.4716 -5.59137 5.29483 0.301028 0.127977 -0.193266 0 
59.223 11.8868 63.9912 -0.0244062 0.63447 0.774014 0 
43.6051 11.6081 63.7272 -0.0262085 0.681324 0.831174 0 
41.2332 26.832 32.6388 1.25747 1.68389 0.606999 0 
-5.43409 1.44832 7.63063 -0.351067 0.0513166 0.737186 0 
19.4853 38.6301 93.6212 0.750584 0.871454 -1.38573 0 
-21.5937 -5.90878 86.3794 -0.548441 -0.283718 0.483532 0 
-20.0183 -24.4914 77.2628 -0.490887 -0.253944 0.43279 0 
-19.512 -24.398 57.5565 -0.2675 -0.668217 -0.325657 0 
49.2078 -39.9662 41.9724 0.336794 -1.61589 0.641578 0 
62.8857 -34.1177 37.3827 0.333883 -0.751426 -0.179439 0 
42.7133 -41.4276 30.4594 0.347017 -0.780985 -0.186497 0 
10.6081 28.9536 101.517 0.251314 -0.160509 0.0716812 0 
13.9773 -17.5217 65.4339 -0.0355738 -0.63238 1.45882 0 
77.1601 -20.2484 40.0356 1.22289 -0.287919 0.426893 0 
76.1413 -2.21627 55.116 0.539973 -0.127132 0.188496 0 
1.88037 37.0664 110.211 -0.385872 -0.192567 0.478313 0 
-10.108 -20.9836 31.132 -0.412371 -0.897886 -0.697734 0 
7.37312 -20.2731 19.886 -0.21243 -0.462539 -0.359432 0 
84.2329 -10.8092 23.2742 0.432315 0.148601 -0.287767 0 
74.378 -16.983 5.28087 1.15737 0.397826 -0.770394 0 
49.9605 21.4943 29.3667 0.745837 1.87018 0.064804 0 
-4.20537 -8.04033 93.7404 0.0858664 -0.168588 0.137411 0 
6.41978 -6.82797 13.8519 -2.10719 -0.10422 0.100697 0 
17.3738 15.5957 62.4346 0.191374 1.77494 1.62224 0 
-23.8899 -7.02868 67.7221 -1.11864 0.616981 0.148234 0 
46.4701 -32.1801 5.31205 0.48457 -0.68425 -0.23306 0 
-6.45279 -22.7646 5.37584 0.000253589 0.00210013 -0.0995928 0 
46.7509 -33.6833 10.309 0.314751 -0.790812 -0.240864 0 
-2.1548 20.6236 100.336 0.236595 -0.45651 0.663443 0 
11.3227 10.6954 68.51 1.03525 0.460552 0.287992 0 
35.53 14.8194 95.2454 1.27645 -0.709287 -1.34314 0 
36.3632 19.8498 93.3807 0.645646 -0.358766 -0.679376 0 
3 2 80 52 
3 143 0 92 
3 1 51 71 
3 128 96 77 
3 2 67 41 
3 39 85 52 
3 123 58 38 
3 21 99 114 
3 9 55 54 
3 102 136 21 
3 133 3 81 
3 136 101 4 
3 82 5 3 
3 90 6 24 
3 40 7 145 
3 75 33 134 
3 8 55 9 
3 40 10 20 
3 140 46 38 
3 64 74 11 
3 88 89 12 
3 60 147 7 
3 116 47 13 
3 129 59 108 
3 72 147 106 
3 108 33 75 
3 57 100 14 
3 130 129 15 
3 35 32 112 
3 29 16 27 
3 98 107 132 
3 116 130 47 
3 43 17 7 
3 44 54 53 
3 34 46 23 
3 41 87 23 
3 10 40 18 
3 131 8 9 
3 19 11 56 
3 137 11 19 
3 20 19 30 
3 121 28 137 
3 140 122 36 
3 130 15 97 
3 84 28 83 
3 21 114 102 
3 98 87 22 
3 145 41 23 
3 68 133 12 
3 70 90 24 
3 25 31 26 
3 34 98 35 
3 27 16 116 
3 83 28 122 
3 103 29 77 
3 30 40 20 
3 49 14 103 
3 26 31 142 
3 9 78 131 
3 62 80 2 
3 67 6 105 
3 48 32 63 
3 30 60 7 
3 135 33 91 
3 130 116 16 
3 13 47 39 
3 119 70 5 
3 26 24 6 
3 25 102 31 
3 49 103 77 
3 130 16 93 
3 126 125 124 
3 86 111 110 
3 52 4 2 
3 34 87 98 
3 6 4 101 
3 76 29 27 
3 35 112 34 
3 4 6 67 
3 1 72 106 
3 24 26 70 
3 37 36 121 
3 113 81 142 
3 109 44 37 
3 58 122 38 
3 48 96 128 
3 11 71 56 
3 122 73 83 
3 39 52 80 
3 18 40 145 
3 5 82 119 
3 20 10 120 
3 145 139 41 
3 142 3 5 
3 117 76 27 
3 120 95 20 
3 45 104 42 
3 43 128 17 
3 37 44 36 
3 45 128 64 
3 111 143 126 
3 46 34 38 
3 130 97 47 
3 91 142 115 
3 31 114 115 
3 100 125 129 
3 96 48 63 
3 41 62 2 
3 77 69 49 
3 50 133 68 
3 51 60 30 
3 118 4 52 
3 55 53 54 
3 8 95 55 
3 37 121 19 
3 75 65 99 
3 56 51 30 
3 57 14 110 
3 122 58 73 
3 92 59 125 
3 45 42 128 
3 14 49 110 
3 147 60 61 
3 62 76 117 
3 49 69 86 
3 5 26 142 
3 44 46 36 
3 50 63 132 
3 64 128 43 
3 108 75 15 
3 75 134 65 
3 69 68 86 
3 76 62 145 
3 141 142 91 
3 66 67 105 
3 68 69 96 
3 70 119 90 
3 91 33 108 
3 118 136 4 
3 51 56 71 
3 72 1 71 
3 18 23 44 
3 123 104 73 
3 1 106 61 
3 111 86 68 
3 45 83 104 
3 56 30 19 
3 97 15 99 
3 74 71 11 
3 99 15 75 
3 102 25 6 
3 94 12 81 
3 33 135 134 
3 133 138 3 
3 29 76 77 
3 88 94 141 
3 31 115 142 
3 121 36 122 
3 2 4 67 
3 78 9 79 
3 121 137 19 
3 96 69 77 
3 62 13 80 
3 127 8 131 
3 141 143 89 
3 12 133 81 
3 119 82 138 
3 83 45 84 
3 85 21 136 
3 110 126 124 
3 49 86 110 
3 116 13 117 
3 66 22 87 
3 88 141 89 
3 45 64 84 
3 78 79 109 
3 70 26 5 
3 93 14 100 
3 50 68 63 
3 105 90 138 
3 0 141 91 
3 90 105 6 
3 92 0 59 
3 145 17 76 
3 93 29 103 
3 81 113 94 
3 85 39 47 
3 35 132 32 
3 48 128 42 
3 29 93 16 
3 18 145 23 
3 129 108 15 
3 112 32 48 
3 41 66 87 
3 95 120 55 
3 68 96 63 
3 99 85 97 
3 53 18 44 
3 98 22 107 
3 35 98 132 
3 127 95 8 
3 64 137 84 
3 10 18 53 
3 99 21 85 
3 79 54 44 
3 93 100 130 
3 3 142 81 
3 101 102 6 
3 14 93 103 
3 48 42 104 
3 23 87 34 
3 22 66 105 
3 61 106 147 
3 74 72 71 
3 144 109 37 
3 65 115 99 
3 132 107 133 
3 12 94 88 
3 91 108 59 
3 64 43 74 
3 78 109 144 
3 147 43 7 
3 135 91 115 
3 110 111 126 
3 112 38 34 
3 113 142 94 
3 9 54 79 
3 53 120 10 
3 3 138 82 
3 102 114 31 
3 65 134 115 
3 22 105 107 
3 129 125 59 
3 144 37 19 
3 76 17 77 
3 12 89 111 
3 66 41 67 
3 117 13 62 
3 27 116 117 
3 52 136 118 
3 60 51 61 
3 119 138 90 
3 120 53 55 
3 111 68 12 
3 121 122 28 
3 58 123 73 
3 57 110 124 
3 85 47 97 
3 79 44 109 
3 125 126 92 
3 74 43 146 
3 19 20 127 
3 17 128 77 
3 146 72 74 
3 99 115 114 
3 122 140 38 
3 105 133 107 
3 100 129 130 
3 144 131 78 
3 20 95 127 
3 48 123 112 
3 136 102 101 
3 111 89 143 
3 137 28 84 
3 132 133 50 
3 57 125 100 
3 123 38 112 
3 57 124 125 
3 134 135 115 
3 44 23 46 
3 85 136 52 
3 62 41 139 
3 11 137 64 
3 48 104 123 
3 138 133 105 
3 139 145 62 
3 6 25 26 
3 30 7 40 
3 36 46 140 
3 143 141 0 
3 32 132 63 
3 104 83 73 
3 144 19 127 
3 94 142 141 
3 13 39 80 
3 143 92 126 
3 144 127 131 
3 61 51 1 
3 0 91 59 
3 7 17 145 
3 146 43 147 
3 72 146 147

CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
find_package(PCL 1.8 REQUIRED)
find_package(G2O REQUIRED)
find_package(Eigen3 REQUIRED)

include_directories(
  ${PCL_INCLUDE_DIRS}
  ${G2O_INCLUDE_DIR}
  ${EIGEN3_INCLUDE_DIR}
)

add_executable(convergence_test code.cpp)
target_link_libraries(convergence_test PUBLIC
  ${PCL_LIBRARIES}
  ${G2O_CORE_LIBRARY}
  ${G2O_STUFF_LIBRARY}
  ${G2O_TYPES_ICP}
  ${G2O_TYPES_SLAM3D}
  ${G2O_SOLVER_CSPARSE}
)

code.cpp

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/slam3d/vertex_se3.h>
#include <g2o/types/slam3d/edge_se3_prior.h>
#include <g2o/types/slam3d_addons/edge_se3_calib.h>
#include <g2o/types/icp/types_icp.h>

#include <pcl/io/ply_io.h>
#include <cmath>
#include <memory>
#include <cstring>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <pcl/common/transforms.h>

#include <cstdio>
#include <memory>
#include <vector>

using Stats = std::pair<Eigen::Vector3d, Eigen::Vector3d>;
using Cloud = pcl::PointCloud<pcl::PointNormal>;
using CloudPtr = Cloud::Ptr;
using Vector6d = Eigen::Matrix<double, 6, 1>;
using Matrix6d = Eigen::Matrix<double, 6, 6>;
char BUNNY_FILEPATH[] = "/tmp/bunny.ply";

struct data {
  Eigen::Isometry3d t_b_s;
  std::vector<Eigen::Isometry3d> global;
  std::vector<Eigen::Isometry3d> relative;
};

namespace CalibCases {
enum CalibCases {
  ID = 0,
  YAW_180,
  SMALL_NOIZE,
  MEDIUM_NOIZE,
  HUGE_NOIZE
};
}  // CalibCases

Stats get_cloud_stats(const CloudPtr& cloud) {
  Eigen::Vector3d mean_(Eigen::Vector3d::Zero());
  Eigen::Vector3d stds(Eigen::Vector3d::Zero());

  for (const auto& point : *cloud) {
    const Eigen::Vector3d entry = point.getVector3fMap().cast<double>();
    mean_ += entry / cloud->size();
  }
  for (const auto& point : *cloud) {
    const Eigen::Vector3d entry = point.getVector3fMap().cast<double>();
      stds += (entry - mean_).cwiseAbs2() / cloud->size();
  }
  stds = stds.cwiseSqrt();
  return { mean_, stds };
}

void print_stats(const std::string& name, const Stats& s) {
  std::cout << name << std::endl;
  printf("mean vector is: %.6f %.6f %.6f \n", s.first(0), s.first(1), s.first(2));
  printf("stds vector is: %.6f %.6f %.6f \n", s.second(0), s.second(1), s.second(2));
}

CloudPtr normalize_cloud(
  const CloudPtr& cloud,
  const Stats& stats) {
  CloudPtr out(new Cloud());
  double average_std = stats.second.sum() / 3.;

  for (size_t i = 0; i < cloud->size(); ++i) {
    pcl::PointNormal other((*cloud)[i]);
    const Eigen::Vector3d p = (*cloud)[i].getVector3fMap().cast<double>();
    other.getVector3fMap() = ((p - stats.first) / average_std).cast<float>();
    out->push_back(other);
  }
  return out;
}

CloudPtr load_bunny() {
  CloudPtr cloud(new Cloud());

  pcl::io::loadPLYFile(BUNNY_FILEPATH, *cloud);
  const Stats sb = get_cloud_stats(cloud);
  return normalize_cloud(cloud, sb);
}

pcl::PolygonMesh::Ptr load_bunny_full() {
  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh());
  pcl::io::loadPLYFile(BUNNY_FILEPATH, *mesh);
  return mesh;
}



CloudPtr transform_pointcloud(
  const CloudPtr& source,
  const Eigen::Isometry3d& destination_from_souce)
{
  CloudPtr destination(new Cloud());

  pcl::transformPointCloudWithNormals(*source,
                                      *destination,
                                      destination_from_souce.matrix());
  return destination;
}

pcl::PolygonMesh::Ptr merge_meshes(const pcl::PolygonMesh::Ptr& source_mesh,
                                   const pcl::PolygonMesh::Ptr& base_mesh,
                                   const CloudPtr& current_cloud) {
  pcl::PolygonMesh::Ptr merged_mesh(new pcl::PolygonMesh());

  merged_mesh->polygons = source_mesh->polygons;
  CloudPtr cloud(new Cloud());
  pcl::fromPCLPointCloud2(source_mesh->cloud, *cloud);
  std::vector<pcl::Vertices> new_faces(base_mesh->polygons);

  const size_t offset = cloud->size();
  for (pcl::Vertices& face : new_faces) {
    for (uint32_t& i : face.vertices) {
      i += offset;
    }
  }
  std::copy(new_faces.cbegin(), new_faces.cend(), std::back_inserter(merged_mesh->polygons));

  (*cloud) += *current_cloud;
  pcl::toPCLPointCloud2(*cloud, merged_mesh->cloud);
  return merged_mesh;
}

Eigen::Isometry3d do_exponential_map_se3(const Vector6d& v);

Eigen::Isometry3d amplify_transform(const Eigen::Isometry3d& center, double angular_var,
                                double linear_var) {
  /*
     Apply independent gaussian noize around *center*
   * angular_var* - variance of rotational part, in degrees
   * linear_var* - variance of linear part, in meters
   */
  static size_t seed(34);
  static std::default_random_engine engine_(seed);
  std::normal_distribution<double> angular_noize(0., angular_var* M_PI / 180.);
  std::normal_distribution<double> linear_noize(0., linear_var);
  Vector6d twist;
  twist << angular_noize(engine_),
           angular_noize(engine_),
           angular_noize(engine_),
           linear_noize(engine_),
           linear_noize(engine_),
           linear_noize(engine_);
  return center * do_exponential_map_se3(twist);
}

using Stream = std::vector<double>;

struct sin_1d_generator {
  sin_1d_generator(size_t period_, double magnitude_, size_t offset_) 
    : period(period_)
    , magnitude(magnitude_)
    , offset(offset_) {
    }

  Stream generate(size_t n);
  size_t period;
  double magnitude;
  size_t offset;
};

Stream sin_1d_generator::generate(size_t n) {
  Stream s;
  for (size_t i = 0; i < n; ++i) {
    double x = std::fmod(((offset + i) % period) * M_PI * 2. / period, M_PI * 2.);
    s.emplace_back(std::sin(x) * magnitude);
  }
  assert(s.size() == n);
  return s;
}

struct circle_2d_generator {
  circle_2d_generator(double r_)
    : r(r_) {}

  std::pair<Stream, Stream> generate(size_t n);
  double r;
};

std::pair<Stream, Stream> circle_2d_generator::generate(size_t n) {
  std::pair<Stream, Stream> result;
  for (size_t i = 0; i < n; ++i) {
    double angle = i * M_PI * 2. / n;
    double x = std::cos(angle) * r;
    double y = std::sin(angle) * r;
    result.first.emplace_back(x);
    result.second.emplace_back(y);
  }
  assert(result.first.size() == n && result.second.size() == n);
  return result;
}

void generator(const CalibCases::CalibCases& c,
               size_t noize_intensity_level,
               data* gt, data* measured) {
  assert(gt);
  assert(measured);
  size_t n(30);
  size_t roll_period(5);
  size_t pitch_period(10);
  size_t yaw_period(n);
  size_t z_period(40);

  size_t roll_offset(1);
  size_t pitch_offset(5);
  size_t yaw_offset(0);
  size_t z_offset(0);

  double roll_magnitude(15. * M_PI / 180.);
  double pitch_magnitude(30. * M_PI / 180.);
  double yaw_magnitude(M_PI / 2);
  double z_magnitude(0.5);

  sin_1d_generator roll(roll_period, roll_magnitude, roll_offset);
  sin_1d_generator pitch(pitch_period, pitch_magnitude, pitch_offset);
  sin_1d_generator yaw(yaw_period, yaw_magnitude, yaw_offset);
  sin_1d_generator z(z_period, z_magnitude, z_offset);
  circle_2d_generator circle(2.);

  Stream rx = roll.generate(n);
  Stream ry = pitch.generate(n);
  Stream rz = yaw.generate(n);
  Stream tz = z.generate(n);
  Stream tx, ty;
  std::tie(tx, ty) = circle.generate(n);

  auto perturb_pose = [](const Eigen::Isometry3d& center, size_t intensity_level) {
    assert(intensity_level < 4);
    if (intensity_level == 0) {
      return center;
    } else {
      double ang_var, lin_var;
      if (intensity_level == 1) {
        ang_var = 1.;
        lin_var = 0.01;
      } else if (intensity_level == 2) {
        ang_var = 3.;
        lin_var = 0.03;
      } else if (intensity_level == 3) {
        ang_var = 10.;
        lin_var = 0.03;
      } else {
        assert(false);
      }
      return amplify_transform(center, ang_var, lin_var);
    }
  };
  for (size_t i = 0; i < n; ++i) {
    Vector6d sample;
    sample << rx.at(i), ry.at(i), rz.at(i), tx.at(i), ty.at(i), tz.at(i);
    const Eigen::Isometry3d t_w_b = do_exponential_map_se3(sample);
    gt->global.push_back(t_w_b);
    measured->global.push_back(perturb_pose(t_w_b, 0));
  }

  for (size_t i = 0; i < n; ++i) {
    const auto& prev = gt->global.at(i) ;
    const auto& curr = gt->global.at((i + 1) % n);
    const auto prev_from_curr = prev.inverse() * curr;
    gt->relative.push_back(prev_from_curr);
    measured->relative.push_back(perturb_pose(prev_from_curr, noize_intensity_level));
  }

  switch(c) {
    case CalibCases::ID : {
      gt->t_b_s = Eigen::Isometry3d::Identity();
      break;
    }
    case CalibCases::YAW_180 : {
      Vector6d point;
      point << 0., M_PI, 0., 0., 0., 0.;
      gt->t_b_s = do_exponential_map_se3(point);
      break;
    }
    default: {
      gt->t_b_s = perturb_pose(Eigen::Isometry3d::Identity(), c - 1);
      break;
    }
  }
  measured->t_b_s = Eigen::Isometry3d::Identity();

  assert(measured->global.size() == measured->relative.size() && 
         gt->global.size() == gt->relative.size() &&
         measured->global.size() == gt->global.size());
}



Vector6d do_logarithmic_map_se3(const Eigen::Isometry3d& p) {
  Eigen::Quaterniond q(p.rotation());
  Eigen::AngleAxisd angle_axis(q);
  Eigen::Vector3d r(angle_axis.axis() * angle_axis.angle());
  Vector6d v;
  v.head<3>() = r;
  v.tail<3>() = p.translation();
  return v;
}

std::string stringify(const Vector6d& _6Dof) {
  char a[512];

  memset(a, '\n', 512);
  sprintf(a,
          "% 02.3f % 02.3f % 02.3f % 02.3f % 02.3f % 02.3f",
          _6Dof(0),
          _6Dof(1),
          _6Dof(2),
          _6Dof(3),
          _6Dof(4),
          _6Dof(5));
  return std::string(a);
}

void print_discrepancy_report(const std::vector<Eigen::Vector3d>& deltas) {
  const CloudPtr cloud(new Cloud());

  for (const Eigen::Vector3d& d : deltas) {
    pcl::PointNormal p;
    p.x = d(0);
    p.y = d(1);
    p.z = d(2);
    cloud->push_back(p);
  }
  const Stats s = get_cloud_stats(cloud);
  print_stats("post-optimization discrepancy in ICP and GNSS locations:", s);
}

Eigen::Isometry3d do_exponential_map_se3(const Vector6d& v) {
  double angle = v.head<3>().norm();
  Eigen::Isometry3d pose;
  if (angle > 1e-3) {
    Eigen::Vector3d axis = v.head<3>() / angle;
    pose = (Eigen::Isometry3d)Eigen::Quaterniond(Eigen::AngleAxisd(angle, axis)).toRotationMatrix();
  } else {
    pose = (Eigen::Isometry3d)Eigen::Quaterniond::Identity().toRotationMatrix();
  }
  pose.translation() = v.tail<3>();
  char a[512];
  memset(a, '\n', 512);
  sprintf(a, "rx %.3f ry %.3f rz %.3f", v(0), v(1), v(2));
  return pose;
}

using OptPtr = std::shared_ptr<g2o::SparseOptimizer>;
OptPtr get_optimizer() {
  OptPtr optimizer = std::make_shared<g2o::SparseOptimizer>();
  auto linear_solver =
    std::make_unique<g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType> >();
  auto solver = std::make_unique<g2o::BlockSolverX>(std::move(linear_solver));
  auto solver_algorithm = new g2o::OptimizationAlgorithmLevenberg(std::move(solver));
  optimizer->setAlgorithm(solver_algorithm);
  return optimizer;
}

g2o::VertexSE3* get_vertex(const OptPtr& o, size_t vertex_id) {
  return dynamic_cast<g2o::VertexSE3*>(o->vertex(vertex_id));
}

const std::vector<Eigen::Isometry3d> store_poses(const OptPtr& o) {
  std::vector<Eigen::Isometry3d> poses;
  for (size_t i = 0; i < o->vertices().size(); ++i) {
    const auto t_w_b = get_vertex(o, i)->estimate();
    poses.emplace_back(t_w_b);
  }
  return poses;
}


class SimpleGlobalEdge : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3> {
 public:
  void computeError() override {
    const Eigen::Isometry3d t_w_b = static_cast<const g2o::VertexSE3*>(_vertices[0])->estimate();
    _error = t_w_b.translation() - _measurement;
  }
  bool read(std::istream&  /*is*/) override { assert(false); return false; };
  bool write(std::ostream&  /*os*/) const override { assert(false); return false; };
};

class Edge_V_V_GICP_Calib : public g2o::BaseMultiEdge<3, g2o::EdgeGICP>{
 public:
  Edge_V_V_GICP_Calib() : BaseMultiEdge<3, g2o::EdgeGICP>() {
    resize(3);
  }
  void computeError() override {
    const Eigen::Isometry3d t_w_b0 = static_cast<const g2o::VertexSE3*>(_vertices[0])->estimate();
    const Eigen::Isometry3d t_w_b1 = static_cast<const g2o::VertexSE3*>(_vertices[1])->estimate();
    const Eigen::Isometry3d t_b_s = static_cast<const g2o::VertexSE3*>(_vertices[2])->estimate();

    // get vp1 point into vp0 frame
    // could be more efficient if we computed this transform just once
    const g2o::Vector3 point1_w = t_w_b1 * t_b_s * measurement().pos1;
    const Eigen::Isometry3d t_s0_w = (t_w_b0 * t_b_s).inverse();
    const g2o::Vector3 point1_s0 = t_s0_w * point1_w;

    _error = point1_s0 - measurement().pos0;
  }
  bool read(std::istream&  /*is*/) override { assert(false); return false; };
  bool write(std::ostream&  /*os*/) const override { assert(false); return false; };
};

int main(int argc, char** argv) {
  assert (argc == 2);
  std::string mode(argv[1]);
  assert(mode == "ideal" || mode == "with_noize");
  data gt, measured;
  OptPtr o = get_optimizer();
  if ("ideal" == mode) {
    generator(CalibCases::ID, 0, &gt, &measured);
  } else if ("with_noize" == mode) {
    generator(CalibCases::SMALL_NOIZE, 1, &gt, &measured);
  }

  const size_t calib_edge_id = measured.global.size();
  auto calib_vert = new g2o::VertexSE3();
  calib_vert->setEstimate(measured.t_b_s);
  calib_vert->setId(calib_edge_id);
  o->addVertex(calib_vert);

  const CloudPtr bunny = load_bunny();
  std::vector<CloudPtr> local_clouds;
  for (size_t i = 0; i < measured.global.size(); ++i) {
    const Eigen::Isometry3d t_w_b(gt.global.at(i));
    const Eigen::Isometry3d mt_w_b(measured.global.at(i));
    const Eigen::Isometry3d t_w_s = t_w_b * gt.t_b_s;
    auto body_vert = new g2o::VertexSE3();
    body_vert->setEstimate(mt_w_b);
    body_vert->setId(i);
    local_clouds.emplace_back(transform_pointcloud(bunny, t_w_s.inverse()));
    o->addVertex(body_vert);

    auto global_edge = new SimpleGlobalEdge();
    global_edge->setVertex(0, body_vert);
    global_edge->setMeasurement(mt_w_b.translation());
    global_edge->setInformation(Eigen::Matrix3d::Identity());
    o->addEdge(global_edge);
    global_edge->computeError();
    printf("glo edge error: %.6f\n", global_edge->error().norm());
  }

  for (size_t i = 0; i < measured.relative.size(); ++i) {
    size_t source_id = (i + 1) % measured.relative.size();
    size_t dst_id = i;
    const auto source_vertex = get_vertex(o, source_id);
    const auto destination_vertex = get_vertex(o, dst_id);
    const Eigen::Isometry3d t_bi_bj(measured.relative.at(i));
    auto local_edge = new g2o::EdgeSE3();
    local_edge->setVertex(1, source_vertex);
    local_edge->setVertex(0, destination_vertex);
    local_edge->setMeasurement(t_bi_bj);
    local_edge->setInformation(Matrix6d::Identity());
    o->addEdge(local_edge);
    local_edge->computeError();
    printf("rel edge error: %.6f\n", local_edge->error().norm());

    double icp_error{0};
    for (size_t j = 0; j < bunny->size(); ++j) {
      auto graph_edge = new Edge_V_V_GICP_Calib();
      graph_edge->setVertex(1, source_vertex);
      graph_edge->setVertex(0, destination_vertex);
      graph_edge->setVertex(2, calib_vert);
      
      const auto& point_from_target = (*local_clouds.at(dst_id))[j];
      const auto& point_from_source = (*local_clouds.at(source_id))[j];
      g2o::EdgeGICP measurement;
      measurement.pos0 = point_from_target.getVector3fMap().cast<double>();
      measurement.pos1 = point_from_source.getVector3fMap().cast<double>();
      measurement.normal0 = point_from_target.getNormalVector3fMap().cast<double>();
      measurement.normal1 = point_from_source.getNormalVector3fMap().cast<double>();
      graph_edge->setMeasurement(measurement);
      graph_edge->setInformation(Eigen::Matrix3d::Identity());
      o->addEdge(graph_edge);
      graph_edge->computeError();
      icp_error += graph_edge->error().norm() / bunny->size();
    }
    printf("ICP edge error: %.6f\n", icp_error);
  }

  o->setVerbose(true);
  o->initializeOptimization();
  o->computeActiveErrors(); 
  printf("Vertices: %ld.  Active: %ld\n", o->vertices().size(), o->activeVertices().size());
  printf("Edges: %ld. Active: %ld\n", o->edges().size(), o->activeEdges().size());
  printf("Prior chi2: %f\n", o->activeChi2());
  o->optimize(50);
  printf("Resulting chi2: %f\n", o->activeChi2());
  const auto r_post = store_poses(o);
  const auto v = do_logarithmic_map_se3(calib_vert->estimate());
  printf("Resulting calib est: %.3f %.3f %.3f%.3f %.3f %.3f \n", v(0), v(1), v(2), v(3), v(4), v(5));
}