forked from isl-org/FastGlobalRegistration
-
Notifications
You must be signed in to change notification settings - Fork 0
/
app.h
89 lines (79 loc) · 3.45 KB
/
app.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
// ----------------------------------------------------------------------------
// - Fast Global Registration -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) Intel Corporation 2016
// Qianyi Zhou <[email protected]>
// Jaesik Park <[email protected]>
// Vladlen Koltun <[email protected]>
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------
#include <vector>
#include <flann/flann.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#define DIV_FACTOR 1.4 // Division factor used for graduated non-convexity
#define USE_ABSOLUTE_SCALE 0 // Measure distance in absolute scale (1) or in scale relative to the diameter of the model (0)
#define MAX_CORR_DIST 0.025 // Maximum correspondence distance (also see comment of USE_ABSOLUTE_SCALE)
#define ITERATION_NUMBER 64 // Maximum number of iteration
#define TUPLE_SCALE 0.95 // Similarity measure used for tuples of feature points.
#define TUPLE_MAX_CNT 1000 // Maximum tuple numbers.
using namespace Eigen;
using namespace std;
typedef vector<Vector3f> Points;
typedef vector<VectorXf> Feature;
typedef flann::Index<flann::L2<float>> KDTree;
typedef std::vector<std::pair<int, int>> Correspondences;
class CApp{
public:
void LoadFeature(const Points& pts, const Feature& feat);
void ReadFeature(const char* filepath);
void NormalizePoints();
void AdvancedMatching();
Eigen::Matrix4f ReadTrans(const char* filepath);
void WriteTrans(const char* filepath);
Matrix4f GetOutputTrans();
double OptimizePairwise(bool decrease_mu_, int numIter_);
void Evaluation(const char* gth, const char* estimation, const char *output);
private:
// containers
vector<Points> pointcloud_;
vector<Feature> features_;
Matrix4f TransOutput_;
vector<pair<int, int>> corres_;
// for normalization
Points Means;
float GlobalScale = 1.0f;
float StartScale = 1.0f;
// some internal functions
void ReadFeature(const char* filepath, Points& pts, Feature& feat);
void TransformPoints(Points& points, const Eigen::Matrix4f& Trans);
void BuildDenseCorrespondence(const Eigen::Matrix4f& gth,
Correspondences& corres);
template <typename T>
void BuildKDTree(const vector<T>& data, KDTree* tree);
template <typename T>
void SearchKDTree(KDTree* tree,
const T& input,
std::vector<int>& indices,
std::vector<float>& dists,
int nn);
};