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. 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 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 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 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 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 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