Home | History | Annotate | Download | only in diversity_checker 
      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