Skip to content

Instantly share code, notes, and snippets.

@wf34
Created September 3, 2018 18:02
Show Gist options
  • Save wf34/7cdd74e4790ed5a7923ab54f81a57bdc to your computer and use it in GitHub Desktop.
Save wf34/7cdd74e4790ed5a7923ab54f81a57bdc to your computer and use it in GitHub Desktop.
G2O 3D SLAM with calibration test
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
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}
)
#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));
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment