1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "calibration/diversity_checker/diversity_checker.h"
18
19 #include <errno.h>
20 #include <stdarg.h>
21 #include <stdio.h>
22 #include <string.h>
23
24 #include "common/math/vec.h"
25
26 // Struct initialization.
diversityCheckerInit(struct DiversityChecker * diverse_data,const struct DiversityCheckerParameters * parameters)27 void diversityCheckerInit(struct DiversityChecker* diverse_data,
28 const struct DiversityCheckerParameters* parameters) {
29 ASSERT_NOT_NULL(diverse_data);
30
31 // Initialize parameters.
32 diverse_data->threshold_tuning_param_sq =
33 (parameters->threshold_tuning_param * parameters->threshold_tuning_param);
34 diverse_data->max_distance_tuning_param_sq =
35 (parameters->max_distance_tuning_param *
36 parameters->max_distance_tuning_param);
37
38 // Updating the threshold and max_distance using assumed local field.
39 // Testing for zero and negative local_field.
40 const float local_field =
41 (parameters->local_field <= 0.0f) ? 1.0f : parameters->local_field;
42 diversityCheckerLocalFieldUpdate(diverse_data, local_field);
43 diverse_data->min_num_diverse_vectors = parameters->min_num_diverse_vectors;
44
45 // Checking for min_num_diverse_vectors = 0.
46 if (parameters->min_num_diverse_vectors < 1) {
47 diverse_data->min_num_diverse_vectors = 1;
48 }
49 diverse_data->max_num_max_distance = parameters->max_num_max_distance;
50 diverse_data->var_threshold = parameters->var_threshold;
51 diverse_data->max_min_threshold = parameters->max_min_threshold;
52
53 // Setting the rest to zero.
54 diversityCheckerReset(diverse_data);
55
56 // Debug Messages
57 #ifdef DIVERSE_DEBUG_ENABLE
58 memset(&diverse_data->diversity_dbg, 0, sizeof(diverse_data->diversity_dbg));
59 #endif
60 }
61
62 // Reset
diversityCheckerReset(struct DiversityChecker * diverse_data)63 void diversityCheckerReset(struct DiversityChecker* diverse_data) {
64 ASSERT_NOT_NULL(diverse_data);
65 // Clear data memory.
66 memset(&diverse_data->diverse_data, 0, sizeof(diverse_data->diverse_data));
67
68 // Resetting counters and data full bit.
69 diverse_data->num_points = 0;
70 diverse_data->num_max_dist_violations = 0;
71 diverse_data->data_full = false;
72 }
73
diversityCheckerFindNearestPoint(struct DiversityChecker * diverse_data,float x,float y,float z)74 bool diversityCheckerFindNearestPoint(struct DiversityChecker* diverse_data,
75 float x, float y, float z) {
76 // Converting three single inputs to a vector.
77 const float vec[THREE_AXIS_DATA_DIM] = {x, y, z};
78
79 // Result vector for vector difference.
80 float vec_diff[THREE_AXIS_DATA_DIM];
81
82 // normSquared result (k)
83 float norm_squared_result;
84
85 size_t i;
86
87 // Running over all existing data points
88 for (i = 0; i < diverse_data->num_points; ++i) {
89 // v = v1 - v2;
90 vecSub(vec_diff, &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec,
91 THREE_AXIS_DATA_DIM);
92
93 // k = |v|^2
94 norm_squared_result = vecNormSquared(vec_diff, THREE_AXIS_DATA_DIM);
95
96 // if k < Threshold then leave the function.
97 if (norm_squared_result < diverse_data->threshold) {
98 return false;
99 }
100
101 // if k > max_distance, count and leave the function.
102 if (norm_squared_result > diverse_data->max_distance) {
103 diverse_data->num_max_dist_violations++;
104 return false;
105 }
106 }
107 return true;
108 }
109
diversityCheckerUpdate(struct DiversityChecker * diverse_data,float x,float y,float z)110 void diversityCheckerUpdate(struct DiversityChecker* diverse_data, float x,
111 float y, float z) {
112 ASSERT_NOT_NULL(diverse_data);
113
114 // If memory is full, no need to run through the data.
115 if (!diverse_data->data_full) {
116 // diversityCheckerDataSet() returns true, if input data is diverse against
117 // the already stored.
118 if (diversityCheckerFindNearestPoint(diverse_data, x, y, z)) {
119 // Converting three single inputs to a vector.
120 const float vec[THREE_AXIS_DATA_DIM] = {x, y, z};
121
122 // Notice that the first data vector will be stored no matter what.
123 memcpy(
124 &diverse_data
125 ->diverse_data[diverse_data->num_points * THREE_AXIS_DATA_DIM],
126 vec, sizeof(float) * THREE_AXIS_DATA_DIM);
127
128 // Count new data point.
129 diverse_data->num_points++;
130
131 // Setting data_full to true, if memory is full.
132 if (diverse_data->num_points == NUM_DIVERSE_VECTORS) {
133 diverse_data->data_full = true;
134 }
135 }
136 }
137 }
138
diversityCheckerNormQuality(struct DiversityChecker * diverse_data,float x_bias,float y_bias,float z_bias)139 bool diversityCheckerNormQuality(struct DiversityChecker* diverse_data,
140 float x_bias, float y_bias, float z_bias) {
141 ASSERT_NOT_NULL(diverse_data);
142 // If not enough diverse data points or max distance violations return false.
143 if (diverse_data->num_points <= diverse_data->min_num_diverse_vectors ||
144 diverse_data->num_max_dist_violations >=
145 diverse_data->max_num_max_distance) {
146 return false;
147 }
148 float vec_bias[THREE_AXIS_DATA_DIM] = {x_bias, y_bias, z_bias};
149 float vec_bias_removed[THREE_AXIS_DATA_DIM];
150 float norm_results;
151 float acc_norm = 0.0f;
152 float acc_norm_square = 0.0f;
153 float max = 0.0f;
154 float min = 0.0f;
155 size_t i;
156 for (i = 0; i < diverse_data->num_points; ++i) {
157 // v = v1 - v_bias;
158 vecSub(vec_bias_removed,
159 &diverse_data->diverse_data[i * THREE_AXIS_DATA_DIM], vec_bias,
160 THREE_AXIS_DATA_DIM);
161
162 // norm = ||v||
163 norm_results = vecNorm(vec_bias_removed, THREE_AXIS_DATA_DIM);
164
165 // Accumulate for mean and VAR.
166 acc_norm += norm_results;
167 acc_norm_square += norm_results * norm_results;
168
169 if (i == 0) {
170 min = norm_results;
171 max = norm_results;
172 }
173 // Finding min
174 if (norm_results < min) {
175 min = norm_results;
176 }
177
178 // Finding max.
179 if (norm_results > max) {
180 max = norm_results;
181 }
182 // can leave the function if max-min is violated
183 // no need to continue.
184 if ((max - min) > diverse_data->max_min_threshold) {
185 return false;
186 }
187 }
188 float inv = 1.0f / diverse_data->num_points;
189 float var = (acc_norm_square - (acc_norm * acc_norm) * inv) * inv;
190
191 // Debug Message.
192 #ifdef DIVERSE_DEBUG_ENABLE
193 diverse_data->diversity_dbg.diversity_count++;
194 diverse_data->diversity_dbg.var_log = var;
195 diverse_data->diversity_dbg.mean_log = acc_norm * inv;
196 diverse_data->diversity_dbg.max_log = max;
197 diverse_data->diversity_dbg.min_log = min;
198 memcpy(&diverse_data->diversity_dbg.diverse_data_log,
199 &diverse_data->diverse_data,
200 sizeof(diverse_data->diversity_dbg.diverse_data_log));
201 #endif
202 return (var < diverse_data->var_threshold);
203 }
204
diversityCheckerLocalFieldUpdate(struct DiversityChecker * diverse_data,float local_field)205 void diversityCheckerLocalFieldUpdate(struct DiversityChecker* diverse_data,
206 float local_field) {
207 if (local_field > 0) {
208 // Updating threshold based on the local field information.
209 diverse_data->threshold =
210 diverse_data->threshold_tuning_param_sq * (local_field * local_field);
211
212 // Updating max distance based on the local field information.
213 diverse_data->max_distance = diverse_data->max_distance_tuning_param_sq *
214 (local_field * local_field);
215 }
216 }
217