├── CMakeLists.txt
├── README.md
├── backup
    └── 3 rotating and calc cossim disp ver
    │   ├── CMakeLists.txt
    │   ├── do_build.sh
    │   ├── include
    │       ├── pointcloud.h
    │       ├── scancontext.h
    │       └── utils.h
    │   ├── main.cpp
    │   └── src
    │       ├── scancontext.cpp
    │       └── utils.cpp
├── do_build.sh
├── include
    ├── pointcloud.h
    ├── scancontext.h
    └── utils.h
├── main.cpp
└── src
    ├── scancontext.cpp
    └── utils.cpp


/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | project(ScanContext)
 3 | set (CMAKE_CXX_STANDARD 14)
 4 | 
 5 | 
 6 | find_package(nanoflann)
 7 | find_package (Eigen3 3.3 REQUIRED NO_MODULE)
 8 | 
 9 | set(INCLUDE_DIR include)
10 | set(SRC_DIR src)
11 | set(SRC
12 |     main.cpp
13 |     ${INCLUDE_DIR}/utils.h ${SRC_DIR}/utils.cpp
14 |     ${INCLUDE_DIR}/scancontext.h ${SRC_DIR}/scancontext.cpp
15 |     ${INCLUDE_DIR}/pointcloud.h 
16 | )
17 | message(${SRC})
18 | 
19 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/${INCLUDE_DIR})
20 | 
21 | # Executable file 
22 | add_executable(main ${SRC})
23 | target_link_libraries (main Eigen3::Eigen)
24 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # scancontext-cpp
2 | ## NEWS (April, 2020) 
3 | - THe C++ implementation is done!
4 | - This repository is deactivate and please follow the code of version at https://github.com/irapkaist/scancontext
5 | - go to https://github.com/irapkaist/SC-LeGO-LOAM
6 | - go to https://github.com/irapkaist/scancontext
7 | 


--------------------------------------------------------------------------------
/backup/3 rotating and calc cossim disp ver/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | project(ScanContext)
 3 | set (CMAKE_CXX_STANDARD 14)
 4 | 
 5 | 
 6 | find_package(nanoflann)
 7 | find_package (Eigen3 3.3 REQUIRED NO_MODULE)
 8 | 
 9 | set(INCLUDE_DIR include)
10 | set(SRC_DIR src)
11 | set(SRC
12 |     main.cpp
13 |     ${INCLUDE_DIR}/utils.h ${SRC_DIR}/utils.cpp
14 |     ${INCLUDE_DIR}/scancontext.h ${SRC_DIR}/scancontext.cpp
15 |     ${INCLUDE_DIR}/pointcloud.h 
16 | )
17 | message(${SRC})
18 | 
19 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/${INCLUDE_DIR})
20 | 
21 | # Executable file 
22 | add_executable(main ${SRC})
23 | target_link_libraries (main Eigen3::Eigen)
24 | 


--------------------------------------------------------------------------------
/backup/3 rotating and calc cossim disp ver/do_build.sh:
--------------------------------------------------------------------------------
1 | cd build
2 | rm rf *
3 | cmake ..
4 | make 
5 | ./main
6 | cd ..


--------------------------------------------------------------------------------
/backup/3 rotating and calc cossim disp ver/include/pointcloud.h:
--------------------------------------------------------------------------------
 1 | #ifndef _POINTCLOUD_H_
 2 | #define _POINTCLOUD_H_
 3 | 
 4 | #include <iostream>
 5 | #include <vector>
 6 | #include <string>
 7 | using namespace std;
 8 | 
 9 | namespace pointcloud
10 | {
11 | 
12 | struct Point 
13 | {
14 |     float x;
15 |     float y;
16 |     float z;
17 | }; // Class Point
18 | 
19 | struct Pointcloud
20 | {
21 |     std::vector<Point> points;
22 | }; // Class Pointcloud
23 | 
24 | 
25 | 
26 | } // namespace pointcloud 
27 | #endif // _POINTCLOUD_H_
28 | 
29 | 


--------------------------------------------------------------------------------
/backup/3 rotating and calc cossim disp ver/include/scancontext.h:
--------------------------------------------------------------------------------
 1 | #ifndef _SCANCONTEXT_H_
 2 | #define _SCANCONTEXT_H_
 3 | 
 4 | #include <iostream>
 5 | #include <vector>
 6 | #include <string>
 7 | #include <algorithm>
 8 | #include <numeric>
 9 | #include <cmath>
10 | // #include <complex>
11 | 
12 | #include <Eigen/Dense>
13 | 
14 | #include "pointcloud.h"
15 | #include "utils.h"
16 | 
17 | #define lc cout << endl
18 | 
19 | using namespace std;
20 | using namespace Eigen;
21 | 
22 | namespace scancontext
23 | {
24 | 
25 | using Ring = vector<double>;
26 | using Sector = vector<double>;
27 | 
28 | using Descriptor = vector<Sector>;
29 | using Ringkey = vector<double>;
30 | 
31 | class Scancontext
32 | {
33 | public:
34 |     Scancontext();
35 |     Scancontext(int, int, double = 80.0);
36 | 
37 |     void ptcloud2desc(pointcloud::Pointcloud ptcloud); // TBD
38 |     void desc2rk(void); // TBD
39 | 
40 | public:
41 |     // param default: original paper's setting
42 |     const int num_rings = 20;
43 |     const int num_sectors = 60;
44 |     const double max_len = 80; 
45 | 
46 |     Descriptor desc;
47 |     Ringkey ringkey;
48 | };
49 | 
50 | 
51 | double distance(const Descriptor& sc1, const Descriptor& sc2);
52 | 
53 | class ScancontextManager
54 | {
55 |     
56 | };
57 | 
58 | 
59 | } // namespace ScanContext 
60 | 
61 | // misc 
62 | scancontext::Descriptor eigen2desc(const MatrixXd& eigm);
63 | 
64 | template<class T>
65 | void printVector(const std::vector<T> & vec_)
66 | {
67 |     for(auto & v_: vec_)
68 |     {
69 |         cout << v_ << ", ";
70 |     }
71 |     cout << endl;
72 | }
73 | 
74 | void printDesc(const scancontext::Descriptor & desc_);
75 | 
76 | #endif // _SCANCONTEXT_H_
77 | 
78 | 


--------------------------------------------------------------------------------
/backup/3 rotating and calc cossim disp ver/include/utils.h:
--------------------------------------------------------------------------------
 1 | #ifndef _UTILS_H_
 2 | #define _UTILS_H_
 3 | 
 4 | #include <iostream>
 5 | #include <vector>
 6 | #include <string>
 7 | 
 8 | #include "scancontext.h"
 9 | 
10 | using namespace std;
11 | 
12 | namespace utils
13 | {
14 | 
15 | template<class T>
16 | void disp(T msg_)
17 | {
18 |     cout << msg_ << endl;
19 | }
20 | 
21 | void dispInt (int& msg_);
22 | 
23 | 
24 | } // namespace utils 
25 | #endif // _UTILS_H_
26 | 
27 | 


--------------------------------------------------------------------------------
/backup/3 rotating and calc cossim disp ver/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include <string>
 3 | #include <memory>
 4 | 
 5 | #include <Eigen/Dense>
 6 | 
 7 | #include "utils.h"
 8 | #include "scancontext.h"
 9 | #include "pointcloud.h"
10 | 
11 | using std::cout;
12 | using std::endl;
13 | using std::string;
14 | using namespace Eigen;
15 | 
16 | using namespace utils;
17 | using SC = scancontext::Scancontext;
18 | 
19 | #define lc cout << endl
20 | 
21 | auto main (void) -> int 
22 | {
23 |     // @ start 
24 |     auto disp_start = [](string str_){
25 |         auto str = std::make_unique<string>(str_);
26 |         cout << *str << endl;
27 |         disp(*str + "(printed by using utils)");
28 |     }; 
29 |     disp_start("Scan Context test Starts"); lc;
30 | 
31 |     // @ make
32 |     // int num_rings{40}; 
33 |     // int num_sectors{120};
34 |     int num_rings{4}; 
35 |     int num_sectors{12};
36 |     SC sc1(num_rings, num_sectors);
37 |     
38 |     disp(sc1.num_rings);    
39 |     disp(sc1.num_sectors);    
40 |     disp(sc1.max_len);    
41 |     lc;
42 | 
43 |     // @ Test 1
44 |     MatrixXd sc_const = MatrixXd::Constant(num_rings, num_sectors, 2.0);
45 |     {
46 |         MatrixXd sc_A0_eig = MatrixXd::Random(num_rings, num_sectors) + sc_const;
47 |         cout << sc_A0_eig << endl;
48 |         lc; 
49 | 
50 |         scancontext::Descriptor sc_A0 = eigen2desc(sc_A0_eig);
51 |         printDesc(sc_A0);
52 |         lc;
53 |     }
54 | 
55 |     // @ Test 2: distance
56 |     MatrixXd sc_A0_eig = MatrixXd::Random(num_rings, num_sectors) + sc_const;
57 |     scancontext::Descriptor sc_A0 = eigen2desc(sc_A0_eig);
58 | 
59 |     MatrixXd sc_B0_eig = MatrixXd::Random(num_rings, num_sectors) + sc_const;
60 |     scancontext::Descriptor sc_B0 = eigen2desc(sc_B0_eig);
61 | 
62 |     cout << "distance btn A0 and B0 is: " << scancontext::distance(sc_A0, sc_B0) << endl;
63 | 
64 | 
65 |     return 0;
66 | }
67 | 


--------------------------------------------------------------------------------
/backup/3 rotating and calc cossim disp ver/src/scancontext.cpp:
--------------------------------------------------------------------------------
  1 | #include "scancontext.h"
  2 | 
  3 | scancontext::Scancontext::Scancontext
  4 | ()
  5 | : num_rings(20), num_sectors(60), max_len(80.0)
  6 | {
  7 |     
  8 | }
  9 | 
 10 | scancontext::Scancontext::Scancontext
 11 | (int num_rings_, int num_sectors_, double max_len_)
 12 | : num_rings(num_rings_), num_sectors(num_sectors_), max_len(max_len_)
 13 | {
 14 |     
 15 | }
 16 | 
 17 | // TBD
 18 | void 
 19 | scancontext::Scancontext::ptcloud2desc (pointcloud::Pointcloud ptcloud) 
 20 | {
 21 |     // example case: read .bin to Descriptor
 22 |     // - later: from ROS Pointcloud2 topic 
 23 | 
 24 |     // NOTE: should add epsilon to the whole element to prevent being devided by zero at distance calc!
 25 |     // TBA
 26 | }
 27 | 
 28 | 
 29 | double norm (std::vector<double> &vec_)
 30 | {
 31 |     double accum = 0.;
 32 |     for(auto & v_: vec_) 
 33 |     {
 34 |         accum += v_ * v_;
 35 |     }
 36 | 
 37 |     double norm = sqrt(accum);
 38 | 
 39 |     return norm;
 40 | }
 41 | 
 42 | 
 43 | double cossim (std::vector<double> a, std::vector<double> b)
 44 | {
 45 |     printVector(a);
 46 |     printVector(b);
 47 |     cout << "DDF" <<endl;
 48 |     double inner_prod = std::inner_product(a.begin(), a.end(), b.begin(), 0.0); // last arg is init accumulation val.
 49 |     double cosine_sim = inner_prod / (norm(a) * norm(b));
 50 | 
 51 |     return cosine_sim;
 52 | }
 53 | 
 54 | double scancontext::distance(const Descriptor& sc1, const Descriptor& sc2)
 55 | {
 56 |     // copy once at first 
 57 |     Descriptor sc1_shift;
 58 |     sc1_shift.resize(sc1.size());
 59 |     std::copy( sc1.begin(), sc1.end(), sc1_shift.begin() );
 60 | 
 61 |     // info 
 62 |     int num_sectors = sc1.size();
 63 |     int num_rings = sc1.at(0).size();
 64 |     cout << "ring: " << num_rings << ", sector: " << num_sectors << endl;
 65 | 
 66 |     // cossim 
 67 |     // auto cossim = [](std::vector<double> a, std::vector<double> b) -> double
 68 |     // {
 69 |     //     return std::inner_product(a.begin(), a.end(), b.begin(), 0); // last arg is init accumulation val.
 70 |     // };
 71 | 
 72 |     // calc dist by aligning        
 73 |     double min_dist{1.0}; // init with max dist 
 74 |     int argmin_shift{0};
 75 |     for (int shift_idx = 0; shift_idx < num_sectors; ++shift_idx)
 76 |     {
 77 |         // @ calc current dist        
 78 |         std::vector<double> sectors_cossim;
 79 |         // sectors_cossim.reserve(num_sectors);         
 80 |         std::transform( sc1_shift.begin(), sc1_shift.end(), sc2.begin(), 
 81 |                         std::back_inserter(sectors_cossim), // save here 
 82 |                         cossim // two arguments function 
 83 |         );
 84 |         printVector(sectors_cossim);
 85 | 
 86 |         // @ check minimal 
 87 | 
 88 |         // @ shift sc1
 89 |         std::rotate(sc1_shift.rbegin(), sc1_shift.rbegin() + 1, sc1_shift.rend());
 90 | 
 91 |     }
 92 | 
 93 |     return 1.3;
 94 | }
 95 | 
 96 | 
 97 | vector<double> eigenvec2stdvec(const VectorXd& eigvec_)
 98 | {
 99 |     vector<double> stdvec;
100 |     stdvec.reserve(eigvec_.size());
101 |     for (int i = 0; i < eigvec_.size(); ++i)
102 |     {
103 |         stdvec.push_back(eigvec_[i]);
104 |     }
105 |     return stdvec;
106 | }
107 | 
108 | 
109 | scancontext::Descriptor eigen2desc(const MatrixXd& eigm_)
110 | {
111 |     int num_rings = eigm_.rows();
112 |     int num_sectors = eigm_.cols();
113 | 
114 |     scancontext::Descriptor desc;
115 |     desc.reserve(num_sectors);
116 | 
117 |     for (int sector_idx = 0; sector_idx < num_sectors; ++sector_idx)
118 |     {
119 |         scancontext::Sector sector = eigenvec2stdvec(eigm_.col(sector_idx));
120 |         desc.push_back(sector);
121 |     }
122 | 
123 |     return desc;
124 | }
125 | 
126 | void printDesc(const scancontext::Descriptor & desc_)
127 | {
128 |     int sector_idx{1};
129 |     for(auto sector_: desc_)
130 |     {
131 |         cout << "sector " << sector_idx << ": ";
132 |         printVector(sector_);
133 |         sector_idx = sector_idx + 1;
134 |     }
135 | }
136 | 


--------------------------------------------------------------------------------
/backup/3 rotating and calc cossim disp ver/src/utils.cpp:
--------------------------------------------------------------------------------
1 | #include "utils.h"
2 | 
3 | void utils::dispInt(int& msg_)
4 | {
5 |     cout << msg_ << endl;
6 | }
7 | 


--------------------------------------------------------------------------------
/do_build.sh:
--------------------------------------------------------------------------------
1 | mkdir build
2 | cd build
3 | rm rf *
4 | cmake ..
5 | make 
6 | ./main
7 | cd ..


--------------------------------------------------------------------------------
/include/pointcloud.h:
--------------------------------------------------------------------------------
 1 | #ifndef _POINTCLOUD_H_
 2 | #define _POINTCLOUD_H_
 3 | 
 4 | #include <iostream>
 5 | #include <vector>
 6 | #include <string>
 7 | using namespace std;
 8 | 
 9 | namespace pointcloud
10 | {
11 | 
12 | struct Point 
13 | {
14 |     float x;
15 |     float y;
16 |     float z;
17 | }; // Class Point
18 | 
19 | struct Pointcloud
20 | {
21 |     std::vector<Point> points;
22 | }; // Class Pointcloud
23 | 
24 | 
25 | 
26 | } // namespace pointcloud 
27 | #endif // _POINTCLOUD_H_
28 | 
29 | 


--------------------------------------------------------------------------------
/include/scancontext.h:
--------------------------------------------------------------------------------
 1 | #ifndef _SCANCONTEXT_H_
 2 | #define _SCANCONTEXT_H_
 3 | 
 4 | #include <iostream>
 5 | #include <vector>
 6 | #include <string>
 7 | #include <algorithm>
 8 | #include <numeric>
 9 | #include <cmath>
10 | // #include <complex>
11 | 
12 | #include <Eigen/Dense>
13 | 
14 | #include "pointcloud.h"
15 | #include "utils.h"
16 | 
17 | #define lc cout << endl
18 | 
19 | using namespace std;
20 | using namespace Eigen;
21 | 
22 | namespace scancontext
23 | {
24 | 
25 | using Ring = vector<double>;
26 | using Sector = vector<double>;
27 | 
28 | using Descriptor = vector<Sector>;
29 | using Ringkey = vector<double>;
30 | 
31 | class Scancontext
32 | {
33 | public:
34 |     Scancontext();
35 |     Scancontext(int, int, double = 80.0);
36 | 
37 |     void ptcloud2desc(pointcloud::Pointcloud ptcloud); // TBD
38 |     void desc2rk(void); // TBD
39 | 
40 | public:
41 |     // param default: original paper's setting
42 |     const int num_rings = 20;
43 |     const int num_sectors = 60;
44 |     const double max_len = 80; 
45 | 
46 |     Descriptor desc;
47 |     Ringkey ringkey;
48 | };
49 | 
50 | 
51 | double distance(const Descriptor& sc1, const Descriptor& sc2);
52 | 
53 | class ScancontextManager
54 | {
55 |     
56 | };
57 | 
58 | 
59 | } // namespace ScanContext 
60 | 
61 | // misc 
62 | scancontext::Descriptor eigen2desc(const MatrixXd& eigm);
63 | 
64 | template<class T>
65 | void printVector(const std::vector<T> & vec_)
66 | {
67 |     for(auto & v_: vec_)
68 |     {
69 |         cout << v_ << ", ";
70 |     }
71 |     cout << endl;
72 | }
73 | 
74 | void printDesc(const scancontext::Descriptor & desc_);
75 | 
76 | #endif // _SCANCONTEXT_H_
77 | 
78 | 


--------------------------------------------------------------------------------
/include/utils.h:
--------------------------------------------------------------------------------
 1 | #ifndef _UTILS_H_
 2 | #define _UTILS_H_
 3 | 
 4 | #include <iostream>
 5 | #include <vector>
 6 | #include <string>
 7 | 
 8 | #include "scancontext.h"
 9 | 
10 | using namespace std;
11 | 
12 | #define lc cout << endl
13 | #define disp(A) cout << A << endl
14 | 
15 | namespace utils
16 | {
17 | 
18 | 
19 | 
20 | 
21 | } // namespace utils 
22 | #endif // _UTILS_H_
23 | 
24 | 


--------------------------------------------------------------------------------
/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <iostream>
 2 | #include <string>
 3 | #include <memory>
 4 | #include <stdlib.h>     /* srand, rand */
 5 | #include <time.h>       /* time */
 6 | 
 7 | #include <Eigen/Dense>
 8 | 
 9 | #include "utils.h"
10 | #include "scancontext.h"
11 | #include "pointcloud.h"
12 | 
13 | using std::cout;
14 | using std::endl;
15 | using std::string;
16 | using namespace Eigen;
17 | 
18 | using namespace utils;
19 | using SC = scancontext::Scancontext;
20 | 
21 | auto main (void) -> int 
22 | {
23 |     // random seed change for tests
24 |     std::srand((unsigned int) time(0));
25 | 
26 |     // @ make
27 |     int num_rings{40}; 
28 |     int num_sectors{120};
29 |     // int num_rings{4}; 
30 |     // int num_sectors{12};
31 |     SC sc1(num_rings, num_sectors);
32 | 
33 |     disp(sc1.num_rings);    
34 |     disp(sc1.num_sectors);    
35 |     disp(sc1.max_len);    
36 |     lc;
37 | 
38 |     // @ Test 1
39 |     MatrixXd sc_const = MatrixXd::Constant(num_rings, num_sectors, 2.0);
40 |     {
41 |         MatrixXd sc_A0_eig = MatrixXd::Random(num_rings, num_sectors) + sc_const;
42 |         cout << sc_A0_eig << endl;
43 |         lc; 
44 | 
45 |         scancontext::Descriptor sc_A0 = eigen2desc(sc_A0_eig);
46 |         printDesc(sc_A0);
47 |         lc;
48 |     }
49 | 
50 |     // @ Test 2: distance
51 |     MatrixXd sc_A0_eig = MatrixXd::Random(num_rings, num_sectors) + sc_const;
52 |     scancontext::Descriptor sc_A0 = eigen2desc(sc_A0_eig);
53 | 
54 |     MatrixXd sc_B0_eig = MatrixXd::Random(num_rings, num_sectors) + sc_const;
55 |     scancontext::Descriptor sc_B0 = eigen2desc(sc_B0_eig);
56 | 
57 |     cout << "distance btn A0 and B0 is: " << scancontext::distance(sc_A0, sc_B0) << endl;
58 | 
59 | 
60 |     return 0;
61 | }
62 | 


--------------------------------------------------------------------------------
/src/scancontext.cpp:
--------------------------------------------------------------------------------
  1 | #include "scancontext.h"
  2 | 
  3 | scancontext::Scancontext::Scancontext
  4 | ()
  5 | : num_rings(20), num_sectors(60), max_len(80.0)
  6 | {
  7 |     
  8 | }
  9 | 
 10 | scancontext::Scancontext::Scancontext
 11 | (int num_rings_, int num_sectors_, double max_len_)
 12 | : num_rings(num_rings_), num_sectors(num_sectors_), max_len(max_len_)
 13 | {
 14 |     
 15 | }
 16 | 
 17 | // TBD
 18 | void 
 19 | scancontext::Scancontext::ptcloud2desc (pointcloud::Pointcloud ptcloud) 
 20 | {
 21 |     // example case: read .bin to Descriptor
 22 |     // - later: from ROS Pointcloud2 topic 
 23 | 
 24 |     // NOTE: should add epsilon to the whole element to prevent being devided by zero at distance calc!
 25 |     // TBA
 26 | }
 27 | 
 28 | 
 29 | double norm (std::vector<double> &vec_)
 30 | {
 31 |     double accum = 0.;
 32 |     for(auto & v_: vec_) 
 33 |         accum += v_ * v_;
 34 |     double norm = sqrt(accum);
 35 |     return norm;
 36 | }
 37 | 
 38 | double mean(std::vector<double> &vec_ )
 39 | {
 40 |     return accumulate( vec_.begin(), vec_.end(), 0.0) / vec_.size();
 41 | }
 42 | 
 43 | double cosine_similarity (std::vector<double> a, std::vector<double> b)
 44 | {
 45 |     double inner_prod = std::inner_product(a.begin(), a.end(), b.begin(), 0.0); // last arg is init accumulation val.
 46 |     double cosine_sim = inner_prod / (norm(a) * norm(b));
 47 |     return cosine_sim;
 48 | }
 49 | 
 50 | double scancontext::distance(const Descriptor& sc1, const Descriptor& sc2)
 51 | {
 52 |     // copy once at first 
 53 |     Descriptor sc1_shift;
 54 |     sc1_shift.resize(sc1.size());
 55 |     std::copy( sc1.begin(), sc1.end(), sc1_shift.begin() );
 56 | 
 57 |     // info 
 58 |     int num_sectors = sc1.size();
 59 |     int num_rings = sc1.at(0).size();
 60 |     cout << "ring: " << num_rings << ", sector: " << num_sectors << endl;
 61 | 
 62 |     // calc dist by aligning        
 63 |     double min_dist{1.0}; // init with max dist 
 64 |     int argmin_shift{0};
 65 |     for (int shift_idx = 0; shift_idx < num_sectors; ++shift_idx)
 66 |     {
 67 |         // @ calc current dist        
 68 |         std::vector<double> sectors_cossim;
 69 |         // sectors_cossim.reserve(num_sectors);         
 70 |         std::transform( sc1_shift.begin(), sc1_shift.end(), sc2.begin(), 
 71 |                         std::back_inserter(sectors_cossim), // save here 
 72 |                         cosine_similarity // two arguments function 
 73 |         );
 74 |         double cur_cossim = mean(sectors_cossim);              
 75 |         double cur_dist = 1.0 - cur_cossim;
 76 |         cout << cur_dist << endl;
 77 | 
 78 |         // @ check minimal 
 79 |         if(cur_dist < min_dist)
 80 |         {
 81 |             min_dist = cur_dist;
 82 |             argmin_shift = shift_idx;
 83 | 
 84 |             disp("found!");
 85 |             disp("cur min dist: " << min_dist);
 86 |             disp("shift idx: " << argmin_shift);
 87 |             lc;
 88 |         }
 89 | 
 90 |         // @ shift sc1
 91 |         std::rotate(sc1_shift.rbegin(), sc1_shift.rbegin() + 1, sc1_shift.rend());
 92 | 
 93 |     }
 94 | 
 95 |     return 1.3;
 96 | }
 97 | 
 98 | 
 99 | vector<double> eigenvec2stdvec(const VectorXd& eigvec_)
100 | {
101 |     vector<double> stdvec;
102 |     stdvec.reserve(eigvec_.size());
103 |     for (int i = 0; i < eigvec_.size(); ++i)
104 |     {
105 |         stdvec.push_back(eigvec_[i]);
106 |     }
107 |     return stdvec;
108 | }
109 | 
110 | 
111 | scancontext::Descriptor eigen2desc(const MatrixXd& eigm_)
112 | {
113 |     int num_rings = eigm_.rows();
114 |     int num_sectors = eigm_.cols();
115 | 
116 |     scancontext::Descriptor desc;
117 |     desc.reserve(num_sectors);
118 | 
119 |     for (int sector_idx = 0; sector_idx < num_sectors; ++sector_idx)
120 |     {
121 |         scancontext::Sector sector = eigenvec2stdvec(eigm_.col(sector_idx));
122 |         desc.push_back(sector);
123 |     }
124 | 
125 |     return desc;
126 | }
127 | 
128 | void printDesc(const scancontext::Descriptor & desc_)
129 | {
130 |     int sector_idx{1};
131 |     for(auto sector_: desc_)
132 |     {
133 |         cout << "sector " << sector_idx << ": ";
134 |         printVector(sector_);
135 |         sector_idx = sector_idx + 1;
136 |     }
137 | }
138 | 


--------------------------------------------------------------------------------
/src/utils.cpp:
--------------------------------------------------------------------------------
1 | #include "utils.h"
2 | 
3 | 


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