-
Notifications
You must be signed in to change notification settings - Fork 150
/
Copy pathregion_modality.h
287 lines (247 loc) · 10.8 KB
/
region_modality.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
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
// SPDX-License-Identifier: MIT
// Copyright (c) 2021 Manuel Stoiber, German Aerospace Center (DLR)
#ifndef OBJECT_TRACKING_INCLUDE_SRT3D_REGION_MODALITY_H_
#define OBJECT_TRACKING_INCLUDE_SRT3D_REGION_MODALITY_H_
#include <srt3d/body.h>
#include <srt3d/camera.h>
#include <srt3d/common.h>
#include <srt3d/model.h>
#include <srt3d/occlusion_renderer.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <filesystem>
#include <iostream>
#include <memory>
#include <opencv2/opencv.hpp>
#include <string>
#include <unsupported/Eigen/MatrixFunctions>
#include <vector>
namespace srt3d {
// Class that implements all functionality for a region modality
// including correspondence search, calculation of the gradient vector and
// hessian matrix, pose optimization, calculation of color histograms, and
// visualization. Also the modality is able to consider occlusions using an
// occlusion mask.
class RegionModality {
private:
// Data for correspondence line calculated during CalculateCorrespondences
using DataLine = struct DataLine {
Eigen::Vector3f center_f_body;
Eigen::Vector3f center_f_camera;
float center_u = 0.0f;
float center_v = 0.0f;
float normal_u = 0.0f;
float normal_v = 0.0f;
float delta_r = 0.0f;
float normal_component_to_scale = 0.0f;
float continuous_distance = 0.0f;
std::vector<float> distribution;
float mean = 0.0f;
float standard_deviation = 0.0f;
float variance = 0.0f;
};
public:
// Constructors and setup methods
RegionModality(const std::string &name, std::shared_ptr<Body> body_ptr,
std::shared_ptr<Model> model_ptr,
std::shared_ptr<Camera> camera_ptr);
bool SetUp();
// Setters for general distribution
void set_n_lines(int n_lines);
void set_function_amplitude(float function_amplitude);
void set_function_slope(float function_slope);
void set_learning_rate(float learning_rate);
void set_function_length(int function_length);
void set_distribution_length(int distribution_length);
void set_scales(const std::vector<int> &scales);
void set_n_newton_iterations(int n_newton_iterations);
void set_min_continuous_distance(float min_continuous_distance);
// Setters for histogram calculation
bool set_n_histogram_bins(int n_histogram_bins);
void set_learning_rate_f(float learning_rate_f);
void set_learning_rate_b(float learning_rate_b);
void set_unconsidered_line_length(float unconsidered_line_length);
void set_considered_line_length(float considered_line_length);
// Setters for optimization
void set_tikhonov_parameter_rotation(float tikhonov_parameter_rotation);
void set_tikhonov_parameter_translation(float tikhonov_parameter_translation);
// Setters for occlusion handling
void UseOcclusionHandling(
std::shared_ptr<OcclusionRenderer> occlusion_renderer_ptr);
void DoNotUseOcclusionHandling();
// Setters for general visualization settings
void set_display_visualization(bool display_visualization);
void StartSavingVisualizations(const std::filesystem::path &save_directory);
void StopSavingVisualizations();
// Setters to turn on individual visualizations
void set_visualize_lines_correspondence(bool visualize_lines_correspondence);
void set_visualize_points_occlusion_mask_correspondence(
bool visualize_points_occlusion_mask_correspondence);
void set_visualize_points_pose_update(bool visualize_points_pose_update);
void set_visualize_points_histogram_image_pose_update(
bool visualize_points_histogram_image_pose_update);
void set_visualize_points_result(bool visualize_points_result);
void set_visualize_points_histogram_image_result(
bool visualize_points_result);
// Main methods
bool StartModality();
bool CalculateBeforeCameraUpdate();
bool CalculateCorrespondences(int corr_iteration);
bool VisualizeCorrespondences(int save_idx);
bool CalculatePoseUpdate(int corr_iteration, int update_iteration);
bool VisualizePoseUpdate(int save_idx);
bool VisualizeResults(int save_idx);
// Getters data
const std::string &name() const;
std::shared_ptr<Body> body_ptr() const;
std::shared_ptr<Model> model_ptr() const;
std::shared_ptr<Camera> camera_ptr() const;
std::shared_ptr<OcclusionRenderer> occlusion_renderer_ptr() const;
// Getters visualization and state
bool imshow_correspondence() const;
bool imshow_pose_update() const;
bool imshow_result() const;
bool set_up() const;
float probability_;
private:
// Helper methods for precalculation of internal data
void PrecalculateFunctionLookup();
void PrecalculateDistributionVariables();
void PrecalculateHistogramBinVariables();
void SetImshowVariables();
// Helper methods for precalculation of referenced data and changing data
void PrecalculateBodyVariables();
void PrecalculateCameraVariables();
void PrecalculatePoseVariables();
void PrecalculateScaleDependentVariables(int corr_iteration);
// Helper methods for histogram calculation
void AddLinePixelColorsToTempHistograms();
void AddPixelColorToHistogram(const cv::Vec3b &pixel_color,
std::vector<float> *enlarged_histogram) const;
bool CalculateHistogram(float learning_rate,
const std::vector<float> &temp_histogram,
std::vector<float> *histogram);
// Helper methods for CalculateCorrespondences
void CalculateBasicLineData(const Model::PointData &data_point,
DataLine *data_line) const;
bool IsLineValid(float u, float v, float continuous_distance) const;
bool CalculateSegmentProbabilities(
float center_u, float center_v, float normal_u, float normal_v,
std::vector<float> *segment_probabilities_f,
std::vector<float> *segment_probabilities_b,
float *normal_component_to_scale, float *delta_r) const;
void MultiplyPixelColorProbability(const cv::Vec3b &pixel_color,
float *probability_f,
float *probability_b) const;
void CalculateDistribution(const std::vector<float> &segment_probabilities_f,
const std::vector<float> &segment_probabilities_b,
std::vector<float> *distribution) const;
void CalculateDistributionMoments(const std::vector<float> &distribution,
float *mean, float *standard_deviation,
float *variance) const;
// Helper methods for visualization
void ShowAndSaveImage(const std::string &title, int save_index,
const cv::Mat &image) const;
void VisualizePointsCameraImage(const std::string &title,
int save_index) const;
void VisualizePointsHistogramImage(const std::string &title,
int save_index) const;
void VisualizePointsOcclusionMask(const std::string &title,
int save_index) const;
void VisualizeLines(const std::string &title, int save_index) const;
void DrawPoints(const cv::Vec3b &color_point, cv::Mat *image) const;
void DrawLines(const cv::Vec3b &color_line,
const cv::Vec3b &color_high_probability, cv::Mat *image) const;
void DrawProbabilityImage(const cv::Vec3b &color_b,
cv::Mat *probability_image) const;
void UpdateLineCentersWithCurrentPose();
// Other helper methods
static float MinAbsValueWithSignOfValue1(float value_1, float abs_value_2);
bool IsSetup() const;
// Internal data objects
std::string name_;
std::vector<float> temp_histogram_f_;
std::vector<float> temp_histogram_b_;
std::vector<float> histogram_f_;
std::vector<float> histogram_b_;
std::vector<DataLine> data_lines_;
// Pointers to referenced objects
std::shared_ptr<Body> body_ptr_ = nullptr;
std::shared_ptr<Model> model_ptr_ = nullptr;
std::shared_ptr<Camera> camera_ptr_ = nullptr;
std::shared_ptr<OcclusionRenderer> occlusion_renderer_ptr_ = nullptr;
// Parameters for general distribution
int n_lines_ = 200;
float function_amplitude_ = 0.36f;
float function_slope_ = 0.0f;
float learning_rate_ = 1.3f;
int function_length_ = 8;
int distribution_length_ = 12;
std::vector<int> scales_ = {5, 2, 2, 1};
int n_newton_iterations_ = 1;
float min_continuous_distance_ = 6.0f;
// Parameters for histogram calculation
int n_histogram_bins_ = 32;
int histogram_bitshift_ = 3;
float learning_rate_f_ = 0.2f;
float learning_rate_b_ = 0.2f;
float unconsidered_line_length_ = 1.0f;
float considered_line_length_ = 18.0f;
// Parameters for optimization
float tikhonov_parameter_rotation_ = 5000.0f;
float tikhonov_parameter_translation_ = 500000.0f;
Eigen::Matrix<float, 6, 6> tikhonov_matrix_;
// Parameters for occlusion handling
bool use_occlusion_handling_ = false;
// Parameters for general visualization settings
bool display_visualization_ = true;
bool save_visualizations_ = false;
std::filesystem::path save_directory_;
// Parameters to turn on individual visualizations
bool visualize_lines_correspondence_ = false;
bool visualize_points_occlusion_mask_correspondence_ = false;
bool visualize_points_pose_update_ = false;
bool visualize_points_histogram_image_pose_update_ = false;
bool visualize_points_result_ = false;
bool visualize_points_histogram_image_result_ = false;
// State variables (internal data)
bool imshow_correspondence_ = false;
bool imshow_pose_update_ = false;
bool imshow_result_ = false;
bool set_up_ = false;
// Precalculated variables for smoothed step function lookup (internal data)
std::vector<float> function_lookup_f_;
std::vector<float> function_lookup_b_;
// Precalculated variables for distributions (internal data)
int line_length_in_segments_{};
float distribution_length_minus_1_half_{};
float distribution_length_plus_1_half_{};
float min_variance_{};
// Precalculated variables for histogram calculation (internal data)
int n_histogram_bins_squared_{};
int n_histogram_bins_cubed_{};
// Precalculated variables for body (referenced data)
uchar encoded_occlusion_id_ = 0;
// Precalculated variables for camera (referenced data)
float fu_{};
float fv_{};
float ppu_{};
float ppv_{};
int image_width_minus_1_{};
int image_height_minus_1_{};
int image_width_minus_2_{};
int image_height_minus_2_{};
// Precalculated variables for poses (continuously changing)
Transform3fA body2camera_pose_;
Eigen::Matrix3f body2camera_rotation_;
Eigen::Matrix<float, 2, 3> body2camera_rotation_xy_;
// Precalculate variables depending on scale (continuously changing)
int scale_{};
float fscale_{};
int line_length_{};
int line_length_minus_1_{};
float line_length_minus_1_half_{};
float line_length_half_minus_1_{};
};
} // namespace srt3d
#endif // OBJECT_TRACKING_INCLUDE_SRT3D_REGION_MODALITY_H_