forked from JdeRobot/slam-SDVL
-
Notifications
You must be signed in to change notification settings - Fork 0
/
feature_align.h
120 lines (94 loc) · 4.92 KB
/
feature_align.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
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
/*
* Copyright (C) 1997-2017 JdeRobot Developers Team
*
* This program is free software; you can redistribute it and/or modifdisty
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Library General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* Authors : Eduardo Perdices <[email protected]>
*
*/
#ifndef SDVL_FEATURE_ALIGN_H_
#define SDVL_FEATURE_ALIGN_H_
#include <iostream>
#include <vector>
#include <list>
#include <utility>
#include <memory>
#include "./frame.h"
#include "./map.h"
namespace sdvl {
class Map;
class Point;
// Store info for each grid cell
typedef std::pair<std::shared_ptr<Point>, Eigen::Vector2d> PointInfo;
typedef std::list<PointInfo> GridCell;
class FeatureAlign {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
FeatureAlign(Map *map, Camera *camera, int max_matches);
~FeatureAlign();
// Reproject all points in current frame found in other frames
void Reproject(const std::shared_ptr<Frame> &frame, const std::shared_ptr<Frame> &last_frame,
const std::shared_ptr<Frame> &last_kf, bool reloc = false);
// Minimize reprojection error of a single frame.
bool OptimizePose(const std::shared_ptr<Frame> &frame);
inline int GetMatches() { return matches_; }
inline int GetAttempts() { return num_attempts_; }
private:
// Select points to reproject. Return selected points.
void SelectPoints(const std::shared_ptr<Frame> &frame, const std::shared_ptr<Frame> &last_frame,
const std::shared_ptr<Frame> &last_kf, std::vector<std::shared_ptr<Feature>> *fs_found);
// Rule out outiliers using RANSAC
void SelectInliers(const std::shared_ptr<Frame> &frame, std::vector<std::shared_ptr<Feature>> &fs_found,
std::vector<std::shared_ptr<Feature>> *inliers, std::vector<std::shared_ptr<Feature>> *outliers);
// Minimize reprojection error. Return observations
void OptimizePose(const std::shared_ptr<Frame> &frame, std::vector<std::shared_ptr<Feature>> *features,
std::vector<std::shared_ptr<Feature>> *outliers);
// Rescue high innovation outliers
bool RescueOutliers(const std::shared_ptr<Frame> &frame, std::vector<std::shared_ptr<Feature>> *inliers,
std::vector<std::shared_ptr<Feature>> *outliers);
// Remove outliers
void RemoveOutliers(const std::shared_ptr<Frame> &frame, std::vector<std::shared_ptr<Feature>> *outliers);
// Check reprojection error. Return number of inliers
int CheckReprojectionError(const std::vector<std::shared_ptr<Feature>> features, const SE3 &se3, double threshold,
std::vector<std::shared_ptr<Feature>> *inliers = NULL,
std::vector<std::shared_ptr<Feature>> *outliers = NULL);
// Reset grid
void ResetGrid();
// Project points into frame
void ProjectPoints(const std::shared_ptr<Frame> &frame, const std::shared_ptr<Frame> &last_frame);
bool ProjectPoint(const std::shared_ptr<Frame> &frame, const std::shared_ptr<Point> &point);
// Converge frame pose to minimize reprojection error
bool ConvergePose(const std::shared_ptr<Frame> &frame, const std::vector<std::shared_ptr<Feature>> &features, SE3 *se3);
// Tukey's hard re-descending function
double GetTukeyValue(double x);
Map * map_; // Stored map
int cell_size_; // Grid cell size
int max_matches_; // Max matches detected
int grid_width_; // Grid width
int grid_height_; // Grid height
std::vector<GridCell*> grid_; // Image grid
Eigen::MatrixXi grid_used_; // True if grid is occupied
std::vector<int> cell_order_; // Randomly access to each grid cell
std::vector<std::shared_ptr<Frame>> fov_kfs_; // Store all keyframes from current frame of view
int matches_; // Number of matches found
int num_attempts_; // Number of attempted matches
bool relocalizing_; // True if algorithm is relocalizing
std::vector<std::shared_ptr<Feature>> inliers_; // Store inliers
std::vector<std::shared_ptr<Feature>> outliers_; // Store outliers
static constexpr double KMADNorm = 1.4826; // See: https://en.wikipedia.org/wiki/Median_absolute_deviation
static constexpr double KTukeyC = 4.6851*4.6851; // See: http://en.wikipedia.org/wiki/Redescending_M-estimator
};
} // namespace sdvl
#endif // SDVL_FEATURE_ALIGN_H_