diff --git a/EdgeImpulse.EI-SDK.pdsc b/EdgeImpulse.EI-SDK.pdsc
index b022e56..709676a 100644
--- a/EdgeImpulse.EI-SDK.pdsc
+++ b/EdgeImpulse.EI-SDK.pdsc
@@ -5,13 +5,16 @@
EI-SDK
LICENSE-apache-2.0.txt
Edge Impulse SDK
- https://github.com/edgeimpulse/edge-impulse-sdk-pack/releases/download/v1.60.13/
+ https://github.com/edgeimpulse/edge-impulse-sdk-pack/releases/download/v1.61.7/
hello@edgeimpulse.com
https://github.com/edgeimpulse/edge-impulse-sdk-pack.git
-
+
EI-SDK
+
+ EI-SDK
+
EI-SDK
@@ -98,9 +101,6 @@
EI-SDK
-
-
- EI-SDK
@@ -146,7 +146,7 @@
-
+
Edge Impulse SDK
@@ -403,9 +403,11 @@
-
+
+
+
@@ -656,6 +658,9 @@
+
+
+
diff --git a/EdgeImpulse.pidx b/EdgeImpulse.pidx
index f57e09e..3824330 100644
--- a/EdgeImpulse.pidx
+++ b/EdgeImpulse.pidx
@@ -2,8 +2,8 @@
EdgeImpulse
https://raw.githubusercontent.com/edgeimpulse/edge-impulse-sdk-pack/main/
- 2024-10-29 12:24:25
+ 2024-11-06 09:42:54
-
+
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/ei_fill_result_struct.h b/edgeimpulse/edge-impulse-sdk/classifier/ei_fill_result_struct.h
index 5c19b37..346f3ef 100644
--- a/edgeimpulse/edge-impulse-sdk/classifier/ei_fill_result_struct.h
+++ b/edgeimpulse/edge-impulse-sdk/classifier/ei_fill_result_struct.h
@@ -147,6 +147,7 @@ __attribute__((unused)) static void fill_result_struct_from_cubes(ei_impulse_res
static std::vector results;
int added_boxes_count = 0;
results.clear();
+
for (auto sc : *cubes) {
bool has_overlapping = false;
@@ -199,7 +200,7 @@ __attribute__((unused)) static void fill_result_struct_from_cubes(ei_impulse_res
}
result->bounding_boxes = results.data();
- result->bounding_boxes_count = results.size();
+ result->bounding_boxes_count = added_boxes_count;
}
#endif
@@ -283,8 +284,10 @@ __attribute__((unused)) static EI_IMPULSE_ERROR fill_result_struct_f32_object_de
bool debug) {
#ifdef EI_HAS_SSD
static std::vector results;
+ int added_boxes_count = 0;
results.clear();
results.resize(impulse->object_detection_count);
+
for (size_t ix = 0; ix < impulse->object_detection_count; ix++) {
float score = scores[ix];
@@ -329,13 +332,15 @@ __attribute__((unused)) static EI_IMPULSE_ERROR fill_result_struct_f32_object_de
results[ix].width = static_cast((xend - xstart) * static_cast(impulse->input_width));
results[ix].height = static_cast((yend - ystart) * static_cast(impulse->input_height));
results[ix].value = score;
+
+ added_boxes_count++;
}
else {
results[ix].value = 0.0f;
}
}
result->bounding_boxes = results.data();
- result->bounding_boxes_count = results.size();
+ result->bounding_boxes_count = added_boxes_count;
return EI_IMPULSE_OK;
#else
@@ -547,7 +552,7 @@ __attribute__((unused)) static EI_IMPULSE_ERROR fill_result_struct_f32_yolov5(co
}
result->bounding_boxes = results.data();
- result->bounding_boxes_count = results.size();
+ result->bounding_boxes_count = added_boxes_count;
return EI_IMPULSE_OK;
#else
@@ -647,7 +652,7 @@ __attribute__((unused)) static EI_IMPULSE_ERROR fill_result_struct_quantized_yol
}
result->bounding_boxes = results.data();
- result->bounding_boxes_count = results.size();
+ result->bounding_boxes_count = added_boxes_count;
return EI_IMPULSE_OK;
#else
@@ -852,7 +857,7 @@ __attribute__((unused)) static EI_IMPULSE_ERROR fill_result_struct_f32_yolox(con
}
result->bounding_boxes = results.data();
- result->bounding_boxes_count = results.size();
+ result->bounding_boxes_count = added_boxes_count;
return EI_IMPULSE_OK;
#else
@@ -1711,7 +1716,7 @@ __attribute__((unused)) static EI_IMPULSE_ERROR fill_result_struct_f32_yolov2(co
}
result->bounding_boxes = results.data();
- result->bounding_boxes_count = results.size();
+ result->bounding_boxes_count = added_boxes_count;
return EI_IMPULSE_OK;
#else
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/ei_model_types.h b/edgeimpulse/edge-impulse-sdk/classifier/ei_model_types.h
index 93b5685..9be4bc5 100644
--- a/edgeimpulse/edge-impulse-sdk/classifier/ei_model_types.h
+++ b/edgeimpulse/edge-impulse-sdk/classifier/ei_model_types.h
@@ -100,7 +100,19 @@ typedef struct {
float detection_threshold;
uint32_t suppression_ms;
uint32_t suppression_flags;
-} ei_model_performance_calibration_t;
+} ei_performance_calibration_config_t;
+
+typedef struct {
+ uint16_t implementation_version;
+ uint32_t keep_grace;
+ uint16_t max_observations;
+ float iou_threshold;
+} ei_object_tracking_config_t;
+
+typedef struct {
+ uint16_t implementation_version;
+ std::vector> segments;
+} ei_object_counting_config_t;
typedef int (*extract_fn_t)(ei::signal_t *signal, ei::matrix_t *output_matrix, void *config, float frequency);
@@ -134,9 +146,9 @@ typedef struct {
typedef struct {
uint32_t block_id;
- EI_IMPULSE_ERROR (*init_fn)(ei_impulse_handle_t *handle, void *config);
- EI_IMPULSE_ERROR (*deinit_fn)(ei_impulse_handle_t *handle, void *config);
- EI_IMPULSE_ERROR (*postprocess_fn)(ei_impulse_handle_t *handle, ei_impulse_result_t *result, void *config, bool debug);
+ EI_IMPULSE_ERROR (*init_fn)(ei_impulse_handle_t *handle, void **state, void *config);
+ EI_IMPULSE_ERROR (*deinit_fn)(void *state, void *config);
+ EI_IMPULSE_ERROR (*postprocess_fn)(ei_impulse_handle_t *handle, ei_impulse_result_t *result, void *config, void* state);
void *config;
} ei_postprocessing_block_t;
@@ -340,10 +352,10 @@ typedef DspHandle* _dsp_handle_ptr_t;
class ei_impulse_handle_t {
public:
ei_impulse_handle_t(const ei_impulse_t *impulse)
- : state(impulse), impulse(impulse) {};
- void* post_processing_state;
+ : state(impulse), impulse(impulse), post_processing_state(nullptr) {};
ei_impulse_state_t state;
const ei_impulse_t *impulse;
+ void** post_processing_state;
};
typedef struct {
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/ei_run_classifier.h b/edgeimpulse/edge-impulse-sdk/classifier/ei_run_classifier.h
index 67652aa..14c864d 100644
--- a/edgeimpulse/edge-impulse-sdk/classifier/ei_run_classifier.h
+++ b/edgeimpulse/edge-impulse-sdk/classifier/ei_run_classifier.h
@@ -233,7 +233,12 @@ extern "C" EI_IMPULSE_ERROR process_impulse(ei_impulse_handle_t *handle,
// Shortcut for quantized image models
ei_learning_block_t block = handle->impulse->learning_blocks[0];
if (can_run_classifier_image_quantized(handle->impulse, block) == EI_IMPULSE_OK) {
- return run_classifier_image_quantized(handle->impulse, signal, result, debug);
+ EI_IMPULSE_ERROR res = run_classifier_image_quantized(handle->impulse, signal, result, debug);
+ if (res != EI_IMPULSE_OK) {
+ return res;
+ }
+ res = run_postprocessing(handle, result, debug);
+ return res;
}
#endif
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/alignment/ei_alignment.hpp b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/alignment/ei_alignment.hpp
new file mode 100644
index 0000000..f6ad775
--- /dev/null
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/alignment/ei_alignment.hpp
@@ -0,0 +1,138 @@
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include "rectangular_lsap.hpp"
+
+#if !defined(STANDALONE)
+#include "edge-impulse-sdk/classifier/ei_classifier_types.h"
+#endif
+
+__attribute__((unused)) static bool compare_tuples(std::tuple a, std::tuple b) {
+ return std::get<2>(a) < std::get<2>(b);
+}
+
+float intersection_over_union(const ei_impulse_result_bounding_box_t bbox1, const ei_impulse_result_bounding_box_t bbox2) {
+ uint32_t x_left = std::max(bbox1.x, bbox2.x);
+ uint32_t y_top = std::max(bbox1.y, bbox2.y);
+ uint32_t x_right = std::min(bbox1.x + bbox1.width, bbox2.x + bbox2.width);
+ uint32_t y_bottom = std::min(bbox1.y + bbox1.height, bbox2.y + bbox2.height);
+
+ if (x_right < x_left || y_bottom < y_top) {
+ return 0.0;
+ }
+
+ uint32_t intersection_area = (x_right - x_left) * (y_bottom - y_top);
+ uint32_t bbox1_area = bbox1.width * bbox1.height;
+ uint32_t bbox2_area = bbox2.width * bbox2.height;
+
+ return static_cast(intersection_area) / static_cast(bbox1_area + bbox2_area - intersection_area);
+}
+
+class JonkerVolgenantAlignment {
+public:
+ JonkerVolgenantAlignment(float iou_threshold) : iou_threshold(iou_threshold) {
+ }
+
+ std::vector> align(const std::vector traces,
+ const std::vector detections) {
+
+ if (traces.empty() || detections.empty()) {
+ return {};
+ }
+
+ std::vector cost_mtx(traces.size() * detections.size());
+ for (size_t trace_idx = 0; trace_idx < traces.size(); ++trace_idx) {
+ for (size_t detection_idx = 0; detection_idx < detections.size(); ++detection_idx) {
+ float iou = intersection_over_union(traces[trace_idx], detections[detection_idx]);
+ float cost = 1 - iou;
+ EI_LOGD("t_idx=%zu d_idx=%zu cost=%.6f\n", trace_idx, detection_idx, cost);
+ cost_mtx[trace_idx * detections.size() + detection_idx] = cost;
+ }
+ }
+
+ int64_t *alignments_a = new int64_t[traces.size()];
+ int64_t *alignments_b = new int64_t[detections.size()];
+
+ solve(traces.size(), detections.size(), cost_mtx.data(), false, alignments_a, alignments_b);
+ EI_LOGD("detections size %zu\n", detections.size());
+ EI_LOGD("traces size %zu\n", traces.size());
+
+ for (size_t i = 0; i < traces.size(); i++) {
+ EI_LOGD("alignments_a[%zu] %lld\n", i, alignments_a[i]);
+ }
+
+ for (size_t i = 0; i < detections.size(); i++) {
+ EI_LOGD("alignments_b[%zu] %lld\n", i, alignments_b[i]);
+ }
+
+ std::vector> matches;
+
+ for (size_t i = 0; i < traces.size(); i++) {
+ size_t trace_idx = i;
+ size_t detection_idx = alignments_b[alignments_a[i]];
+ float iou = 1 - cost_mtx[trace_idx * detections.size() + detection_idx];
+ if (iou > iou_threshold) {
+ matches.emplace_back(trace_idx, detection_idx, iou);
+ }
+ }
+ delete[] alignments_a;
+ delete[] alignments_b;
+ return matches;
+ }
+
+ float iou_threshold;
+};
+
+class GreedyAlignment {
+public:
+ GreedyAlignment(float iou_threshold) : iou_threshold(iou_threshold) {
+ }
+ std::vector> align(const std::vector traces,
+ const std::vector detections) {
+
+ if (traces.empty() || detections.empty()) {
+ return {};
+ }
+
+ std::vector> alignments;
+ for (size_t trace_idx = 0; trace_idx < traces.size(); ++trace_idx) {
+ for (size_t detection_idx = 0; detection_idx < detections.size(); ++detection_idx) {
+ float iou = intersection_over_union(traces[trace_idx], detections[detection_idx]);
+ float cost = 1 - iou;
+ EI_LOGD("t_idx=%zu d_idx=%zu cost=%.6f\n", trace_idx, detection_idx, cost);
+ if (iou > iou_threshold) {
+ alignments.emplace_back(trace_idx, detection_idx, cost);
+ }
+ }
+ }
+
+ std::sort(alignments.begin(), alignments.end(), compare_tuples);
+ EI_LOGD("alignments.size() %zu\n", alignments.size());
+ std::vector> matches;
+ std::set trace_idxs_matched;
+ std::set detection_idxs_matched;
+
+ for (size_t i = 0; i < alignments.size(); i++) {
+ uint32_t trace_idx = std::get<0>(alignments[i]);
+ uint32_t detection_idx = std::get<1>(alignments[i]);
+ float cost = std::get<2>(alignments[i]);
+
+ if (trace_idxs_matched.find(trace_idx) == trace_idxs_matched.end() && detection_idxs_matched.find(detection_idx) == detection_idxs_matched.end()) {
+ // (1 - cost) to get iou
+ matches.emplace_back(trace_idx, detection_idx, 1 - cost);
+ trace_idxs_matched.insert(trace_idx);
+ if (trace_idxs_matched.size() == traces.size()) return matches;
+ detection_idxs_matched.insert(detection_idx);
+ if (detection_idxs_matched.size() == detections.size()) return matches;
+ }
+ }
+
+ return matches;
+ }
+
+ float iou_threshold;
+};
\ No newline at end of file
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/alignment/rectangular_lsap.hpp b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/alignment/rectangular_lsap.hpp
new file mode 100644
index 0000000..16ff32b
--- /dev/null
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/alignment/rectangular_lsap.hpp
@@ -0,0 +1,255 @@
+/*
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+
+1. Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above
+ copyright notice, this list of conditions and the following
+ disclaimer in the documentation and/or other materials provided
+ with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+This code implements the shortest augmenting path algorithm for the
+rectangular assignment problem. This implementation is based on the
+pseudocode described in pages 1685-1686 of:
+
+ DF Crouse. On implementing 2D rectangular assignment algorithms.
+ IEEE Transactions on Aerospace and Electronic Systems
+ 52(4):1679-1696, August 2016
+ doi: 10.1109/TAES.2016.140952
+
+Author: PM Larsen
+*/
+
+#include
+#include
+#include
+#include
+
+#define RECTANGULAR_LSAP_INFEASIBLE -1
+#define RECTANGULAR_LSAP_INVALID -2
+
+template std::vector argsort_iter(const std::vector &v)
+{
+ std::vector index(v.size());
+ std::iota(index.begin(), index.end(), 0);
+ std::sort(index.begin(), index.end(), [&v](intptr_t i, intptr_t j)
+ {return v[i] < v[j];});
+ return index;
+}
+
+static intptr_t
+augmenting_path(intptr_t nc, double *cost, std::vector& u,
+ std::vector& v, std::vector& path,
+ std::vector& row4col,
+ std::vector& shortestPathCosts, intptr_t i,
+ std::vector& SR, std::vector& SC,
+ std::vector& remaining, double* p_minVal)
+{
+ double minVal = 0;
+
+ // Crouse's pseudocode uses set complements to keep track of remaining
+ // nodes. Here we use a vector, as it is more efficient in C++.
+ intptr_t num_remaining = nc;
+ for (intptr_t it = 0; it < nc; it++) {
+ // Filling this up in reverse order ensures that the solution of a
+ // constant cost matrix is the identity matrix (c.f. #11602).
+ remaining[it] = nc - it - 1;
+ }
+
+ std::fill(SR.begin(), SR.end(), false);
+ std::fill(SC.begin(), SC.end(), false);
+ std::fill(shortestPathCosts.begin(), shortestPathCosts.end(), INFINITY);
+
+ // find shortest augmenting path
+ intptr_t sink = -1;
+ while (sink == -1) {
+
+ intptr_t index = -1;
+ double lowest = INFINITY;
+ SR[i] = true;
+
+ for (intptr_t it = 0; it < num_remaining; it++) {
+ intptr_t j = remaining[it];
+
+ double r = minVal + cost[i * nc + j] - u[i] - v[j];
+ if (r < shortestPathCosts[j]) {
+ path[j] = i;
+ shortestPathCosts[j] = r;
+ }
+
+ // When multiple nodes have the minimum cost, we select one which
+ // gives us a new sink node. This is particularly important for
+ // integer cost matrices with small co-efficients.
+ if (shortestPathCosts[j] < lowest ||
+ (shortestPathCosts[j] == lowest && row4col[j] == -1)) {
+ lowest = shortestPathCosts[j];
+ index = it;
+ }
+ }
+
+ minVal = lowest;
+ if (minVal == INFINITY) { // infeasible cost matrix
+ return -1;
+ }
+
+ intptr_t j = remaining[index];
+ if (row4col[j] == -1) {
+ sink = j;
+ } else {
+ i = row4col[j];
+ }
+
+ SC[j] = true;
+ remaining[index] = remaining[--num_remaining];
+ }
+
+ *p_minVal = minVal;
+ return sink;
+}
+
+static int solve(intptr_t nr, intptr_t nc, double* cost, bool maximize,
+ int64_t* a, int64_t* b) {
+ // handle trivial inputs
+ if (nr == 0 || nc == 0) {
+ return 0;
+ }
+
+ // tall rectangular cost matrix must be transposed
+ bool transpose = nc < nr;
+
+ // make a copy of the cost matrix if we need to modify it
+ std::vector temp;
+ if (transpose || maximize) {
+ temp.resize(nr * nc);
+
+ if (transpose) {
+ for (intptr_t i = 0; i < nr; i++) {
+ for (intptr_t j = 0; j < nc; j++) {
+ temp[j * nr + i] = cost[i * nc + j];
+ }
+ }
+
+ std::swap(nr, nc);
+ }
+ else {
+ std::copy(cost, cost + nr * nc, temp.begin());
+ }
+
+ // negate cost matrix for maximization
+ if (maximize) {
+ for (intptr_t i = 0; i < nr * nc; i++) {
+ temp[i] = -temp[i];
+ }
+ }
+
+ cost = temp.data();
+ }
+
+ // test for NaN and -inf entries
+ for (intptr_t i = 0; i < nr * nc; i++) {
+ if (cost[i] != cost[i] || cost[i] == -INFINITY) {
+ return RECTANGULAR_LSAP_INVALID;
+ }
+ }
+
+ // initialize variables
+ std::vector u(nr, 0);
+ std::vector v(nc, 0);
+ std::vector shortestPathCosts(nc);
+ std::vector path(nc, -1);
+ std::vector col4row(nr, -1);
+ std::vector row4col(nc, -1);
+ std::vector SR(nr);
+ std::vector SC(nc);
+ std::vector remaining(nc);
+
+ // iteratively build the solution
+ for (intptr_t curRow = 0; curRow < nr; curRow++) {
+
+ double minVal;
+ intptr_t sink = augmenting_path(nc, cost, u, v, path, row4col,
+ shortestPathCosts, curRow, SR, SC,
+ remaining, &minVal);
+ if (sink < 0) {
+ return RECTANGULAR_LSAP_INFEASIBLE;
+ }
+
+ // update dual variables
+ u[curRow] += minVal;
+ for (intptr_t i = 0; i < nr; i++) {
+ if (SR[i] && i != curRow) {
+ u[i] += minVal - shortestPathCosts[col4row[i]];
+ }
+ }
+
+ for (intptr_t j = 0; j < nc; j++) {
+ if (SC[j]) {
+ v[j] -= minVal - shortestPathCosts[j];
+ }
+ }
+
+ // augment previous solution
+ intptr_t j = sink;
+ while (1) {
+ intptr_t i = path[j];
+ row4col[j] = i;
+ std::swap(col4row[i], j);
+ if (i == curRow) {
+ break;
+ }
+ }
+ }
+
+ if (transpose) {
+ intptr_t i = 0;
+ for (auto v: argsort_iter(col4row)) {
+ a[i] = col4row[v];
+ b[i] = v;
+ i++;
+ }
+ }
+ else {
+ for (intptr_t i = 0; i < nr; i++) {
+ a[i] = i;
+ b[i] = col4row[i];
+ }
+ }
+
+ return 0;
+}
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int
+solve_rectangular_linear_sum_assignment(intptr_t nr, intptr_t nc,
+ double* input_cost, bool maximize,
+ int64_t* a, int64_t* b)
+{
+ return solve(nr, nc, input_cost, maximize, a, b);
+}
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_object_counting.h b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_object_counting.h
index ec6cc70..15bcc5b 100644
--- a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_object_counting.h
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_object_counting.h
@@ -1,5 +1,5 @@
/*
- * Copyright (c) 2022 EdgeImpulse Inc.
+ * Copyright (c) 2024 EdgeImpulse Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
@@ -18,110 +18,142 @@
#ifndef EI_OBJECT_COUNTING_H
#define EI_OBJECT_COUNTING_H
-#if EI_CLASSIFIER_OBJECT_COUNTING_ENABLED
-
/* Includes ---------------------------------------------------------------- */
#include "edge-impulse-sdk/dsp/numpy_types.h"
#include "edge-impulse-sdk/dsp/returntypes.hpp"
#include "edge-impulse-sdk/classifier/ei_model_types.h"
-
-/* Private const types ----------------------------------------------------- */
-#define MEM_ERROR "ERR: Failed to allocate memory for performance calibration\r\n"
-
-#define EI_PC_RET_NO_EVENT_DETECTED -1
-#define EI_PC_RET_MEMORY_ERROR -2
+#include "edge-impulse-sdk/porting/ei_logging.h"
extern ei_impulse_handle_t & ei_default_impulse;
-class ObjectCounter {
+#if EI_CLASSIFIER_OBJECT_COUNTING_ENABLED == 1
+
+class CrossingCounter {
public:
- ObjectCounter() {
- this->_count = 0;
+ CrossingCounter(std::vector> segments) : segments(segments) {
+ counts.resize(segments.size(), 0);
}
- void countObjectDetected() {
- this->_count++;
+ void update(std::tuple other_segment) {
+ for (size_t segment_idx = 0; segment_idx < segments.size(); segment_idx++) {
+ if (_line_intersects(segments[segment_idx], other_segment)) {
+ counts[segment_idx] += 1;
+ }
+ }
}
- int getCount() {
- return this->_count;
+ std::vector counts;
+ std::vector> segments;
+private:
+ bool ccw(int A[2], int B[2], int C[2]) {
+ return (C[1] - A[1]) * (B[0] - A[0]) > (B[1] - A[1]) * (C[0] - A[0]);
}
-private:
- int _count;
+ bool _line_intersects(std::tuple L1, std::tuple L2) {
+ int A[2] = { std::get<0>(L1), std::get<1>(L1) };
+ int B[2] = { std::get<2>(L1), std::get<3>(L1) };
+ int C[2] = { std::get<0>(L2), std::get<1>(L2) };
+ int D[2] = { std::get<2>(L2), std::get<3>(L2) };
+// verbose debug
+#if EI_LOG_LEVEL == 5
+ ei_printf("A[0] = %d A[1] = %d\n", A[0], A[1]);
+ ei_printf("B[0] = %d B[1] = %d\n", B[0], B[1]);
+ ei_printf("C[0] = %d C[1] = %d\n", C[0], C[1]);
+ ei_printf("D[0] = %d D[1] = %d\n", D[0], D[1]);
+#endif
+ return ccw(A, C, D) != ccw(B, C, D) && ccw(A, B, C) != ccw(A, B, D);
+ }
};
-EI_IMPULSE_ERROR init_objcnt(ei_impulse_handle_t *handle, void *config)
+EI_IMPULSE_ERROR init_object_counting(ei_impulse_handle_t *handle, void **state, void *config)
{
- const ei_impulse_t *impulse = handle->impulse;
- const ei_model_performance_calibration_t *calibration = (ei_model_performance_calibration_t*)config;
+ // const ei_impulse_t *impulse = handle->impulse;
+ const ei_object_counting_config_t *object_counting_config = (ei_object_counting_config_t*)config;
// Allocate the object counter
- ObjectCounter *objcnt = new ObjectCounter();
- if (!objcnt) {
- return EI_IMPULSE_OUT_OF_MEMORY;
+ CrossingCounter *object_counter = new CrossingCounter(object_counting_config->segments);
+
+ if (!object_counter) {
+ return EI_IMPULSE_POSTPROCESSING_ERROR;
}
// Store the object counter in the handle
- handle->post_processing_state = (void *)objcnt;
+ *state = (void *)object_counter;
return EI_IMPULSE_OK;
}
-EI_IMPULSE_ERROR deinit_objcnt(ei_impulse_handle_t *handle, void *config)
+EI_IMPULSE_ERROR deinit_object_counting(void *state, void *config)
{
- ObjectCounter *objcnt = (ObjectCounter *)handle->post_processing_state;
- if (objcnt) {
- delete objcnt;
+ CrossingCounter *object_counter = (CrossingCounter*)state;
+
+ if (object_counter) {
+ delete object_counter;
}
return EI_IMPULSE_OK;
}
-EI_IMPULSE_ERROR process_objcnt(ei_impulse_handle_t *handle,
- ei_impulse_result_t *result,
- void *config,
- bool debug) {
-
+EI_IMPULSE_ERROR process_object_counting(ei_impulse_handle_t *handle,
+ ei_impulse_result_t *result,
+ void *config,
+ void *state)
+{
const ei_impulse_t *impulse = handle->impulse;
- ObjectCounter *objcnt = (ObjectCounter *)handle->post_processing_state;
+ CrossingCounter *object_counter = (CrossingCounter*)state;
if (impulse->sensor == EI_CLASSIFIER_SENSOR_CAMERA) {
- if((void *)objcnt != NULL) {
- objcnt->countObjectDetected();
+ if((void *)object_counter != NULL) {
+ for (size_t i = 0; i < result->postprocessed_output.object_tracking_output.open_traces_count; i++) {
+ ei_object_tracking_trace_t trace = result->postprocessed_output.object_tracking_output.open_traces[i];
+ object_counter->update(trace.last_centroid_segment);
+ }
+
+ result->postprocessed_output.object_counting_output.counts = object_counter->counts.data();
+ result->postprocessed_output.object_counting_output.counter_num = object_counter->counts.size();
}
}
return EI_IMPULSE_OK;
}
-typedef struct {
- float detection_threshold;
-} ei_obj_cnt_params_t;
+EI_IMPULSE_ERROR set_post_process_params(ei_impulse_handle_t* handle, ei_object_counting_config_t* params) {
+ int16_t block_number = get_block_number(handle, (void*)init_object_counting);
+ if (block_number == -1) {
+ return EI_IMPULSE_POSTPROCESSING_ERROR;
+ }
+ CrossingCounter *object_counter = (CrossingCounter*)handle->post_processing_state[block_number];
-EI_IMPULSE_ERROR set_post_process_params(ei_impulse_handle_t* handle, ei_obj_cnt_params_t* params) {
+ object_counter->segments = params->segments;
return EI_IMPULSE_OK;
}
-EI_IMPULSE_ERROR get_post_process_params(ei_impulse_handle_t* handle, ei_obj_cnt_params_t* params) {
+EI_IMPULSE_ERROR get_post_process_params(ei_impulse_handle_t* handle, ei_object_counting_config_t* params) {
+ int16_t block_number = get_block_number(handle, (void*)init_object_counting);
+ if (block_number == -1) {
+ return EI_IMPULSE_POSTPROCESSING_ERROR;
+ }
+ CrossingCounter *object_counter = (CrossingCounter*)handle->post_processing_state[block_number];
+
+ params->segments = object_counter->segments;
return EI_IMPULSE_OK;
}
// versions that operate on the default impulse
-EI_IMPULSE_ERROR set_post_process_params(ei_obj_cnt_params_t *params) {
+EI_IMPULSE_ERROR set_post_process_params(ei_object_counting_config_t *params) {
ei_impulse_handle_t* handle = &ei_default_impulse;
if(handle->post_processing_state != NULL) {
- // set params
+ set_post_process_params(handle, params);
}
return EI_IMPULSE_OK;
}
-EI_IMPULSE_ERROR get_post_process_params(ei_obj_cnt_params_t *params) {
+EI_IMPULSE_ERROR get_post_process_params(ei_object_counting_config_t *params) {
ei_impulse_handle_t* handle = &ei_default_impulse;
if(handle->post_processing_state != NULL) {
- // get params
+ get_post_process_params(handle, params);
}
return EI_IMPULSE_OK;
}
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_object_tracking.h b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_object_tracking.h
new file mode 100644
index 0000000..7fb0dc8
--- /dev/null
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_object_tracking.h
@@ -0,0 +1,474 @@
+/*
+ * Copyright (c) 2024 EdgeImpulse Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an "AS
+ * IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
+ * express or implied. See the License for the specific language
+ * governing permissions and limitations under the License.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#ifndef EI_OBJECT_TRACKING_H
+#define EI_OBJECT_TRACKING_H
+
+#include "edge-impulse-sdk/dsp/numpy_types.h"
+#include "edge-impulse-sdk/dsp/returntypes.hpp"
+#include "edge-impulse-sdk/classifier/ei_model_types.h"
+#include "edge-impulse-sdk/porting/ei_logging.h"
+#include "edge-impulse-sdk/classifier/postprocessing/ei_postprocessing_common.h"
+#include "model-parameters/model_metadata.h"
+
+extern ei_impulse_handle_t & ei_default_impulse;
+
+#include
+#include "tinyEKF/tinyekf.hpp"
+#include "alignment/ei_alignment.hpp"
+
+float clip(float num, float min_val = -3.4028235e+38, float max_val = 3.4028235e+38) {
+ return std::fmax(min_val, std::fmin(num, max_val));
+}
+
+#if EI_CLASSIFIER_OBJECT_TRACKING_ENABLED == 1
+
+typedef struct {
+ float keep_grace;
+} ei_obj_tracking_params_t;
+
+class ExponentialMovingAverage {
+public:
+ ExponentialMovingAverage(int n, float gain = 2) : gain(gain / (n + 1)), ema_value(-255.0) {
+ }
+
+ void update(float value) {
+ if (ema_value == -255.0) {
+ ema_value = value;
+ } else {
+ ema_value = (value * gain) + (ema_value * (1 - gain));
+ }
+ }
+
+ float smoothed_value() {
+ return ema_value;
+ }
+
+private:
+ float gain;
+ float ema_value;
+};
+
+class Trace {
+public:
+ Trace(int id, int t, const ei_impulse_result_bounding_box_t& initial_bbox, uint32_t max_observations = 5)
+ : id(id), last_ground_truth_update_t(t), last_prediction(initial_bbox), max_observations(max_observations) {
+ if (max_observations < 2) {
+ EI_LOGE("%s", "max_observations needs to be at least 2 for counting");
+ }
+
+ trace_label = initial_bbox.label;
+ observations.push_back(initial_bbox);
+ float initial_centroid[2] = { initial_bbox.x + static_cast(initial_bbox.width) / 2,
+ initial_bbox.y + static_cast(initial_bbox.height) / 2 };
+
+ float initial_width_height[2] = { static_cast(initial_bbox.width),
+ static_cast(initial_bbox.height) };
+
+ centroid_filter = new TinyEKF(initial_centroid, 8, 2);
+ width_height_filter = new TinyEKF(initial_width_height, 8, 2);
+
+ xyxy_emas[0] = new ExponentialMovingAverage(this->max_observations);
+ xyxy_emas[1] = new ExponentialMovingAverage(this->max_observations);
+ xyxy_emas[2] = new ExponentialMovingAverage(this->max_observations);
+ xyxy_emas[3] = new ExponentialMovingAverage(this->max_observations);
+ }
+
+ ~Trace() {
+ delete centroid_filter;
+ delete width_height_filter;
+ delete xyxy_emas[0];
+ delete xyxy_emas[1];
+ delete xyxy_emas[2];
+ delete xyxy_emas[3];
+ }
+
+ ei_impulse_result_bounding_box_t predict() {
+
+ fx_centroid[0] = centroid_filter->x[0];
+ fx_centroid[1] = centroid_filter->x[1];
+ fx_width_height[0] = width_height_filter->x[0];
+ fx_width_height[1] = width_height_filter->x[1];
+
+ centroid_filter->predict(fx_centroid);
+ width_height_filter->predict(fx_width_height);
+
+ ei_impulse_result_bounding_box_t p_bbox = {"", 0, 0, 0, 0, 0.0};
+ p_bbox.label = trace_label;
+ p_bbox.x = clip((centroid_filter->x[0] - width_height_filter->x[0] / 2), 0);
+ p_bbox.y = clip(centroid_filter->x[1] - width_height_filter->x[1] / 2, 0);
+ p_bbox.width = clip(width_height_filter->x[0], 0);
+ p_bbox.height = clip(width_height_filter->x[1], 0);
+ p_bbox.value = 0.0;
+ last_prediction = p_bbox;
+ EI_LOGD("predict %d %d %d %d %f\n", last_prediction.x, last_prediction.y, last_prediction.width, last_prediction.height, last_prediction.value);
+ return last_prediction;
+ }
+
+ void update(int t, const ei_impulse_result_bounding_box_t* bbox) {
+ if (bbox == nullptr) {
+ EI_LOGD("update (last prediction) %d %d %d %d %f\n", last_prediction.x, last_prediction.y, last_prediction.width, last_prediction.height, last_prediction.value);
+ bbox = &last_prediction;
+ } else {
+ EI_LOGD("update (ground truth prediction) %d %d %d %d %f\n", bbox->x, bbox->y, bbox->width, bbox->height, bbox->value);
+ last_ground_truth_update_t = t;
+ }
+
+ hx_centroid[0] = centroid_filter->x[0];
+ hx_centroid[1] = centroid_filter->x[1];
+ hx_width_height[0] = width_height_filter->x[0];
+ hx_width_height[1] = width_height_filter->x[1];
+
+ float centroid[2] = { bbox->x + static_cast(bbox->width) / 2,
+ bbox->y + static_cast(bbox->height) / 2 };
+ centroid_filter->update(centroid , hx_centroid);
+
+ float width_height[2] = { static_cast(bbox->width),
+ static_cast(bbox->height) };
+ width_height_filter->update(width_height, hx_width_height);
+
+ observations.push_back(*bbox);
+ while (observations.size() > max_observations) {
+ observations.erase(observations.begin());
+ }
+
+ xyxy_emas[0]->update(bbox->x);
+ xyxy_emas[1]->update(bbox->y);
+ xyxy_emas[2]->update(bbox->width);
+ xyxy_emas[3]->update(bbox->height);
+
+ }
+
+ std::tuple last_centroid_segment() const {
+ if (observations.size() < 2) {
+ return {};
+ }
+ auto obs_t_minus1 = observations[observations.size() - 2];
+ auto obs_t_0 = observations.back();
+
+ return {obs_t_minus1.x + static_cast(obs_t_minus1.width) / 2,
+ obs_t_minus1.y + static_cast(obs_t_minus1.height) / 2,
+ obs_t_0.x + static_cast(obs_t_0.width) / 2,
+ obs_t_0.y + static_cast(obs_t_0.height) / 2};
+ }
+
+ const ei_impulse_result_bounding_box_t* last_observation() const {
+ if (observations.empty()) {
+ return nullptr;
+ }
+ return &observations.back();
+ }
+
+ ei_impulse_result_bounding_box_t smoothed_last_observation() const {
+ ei_impulse_result_bounding_box_t bbox = {"", 0, 0, 0, 0, 0.0};
+ if (observations.empty()) {
+ return bbox;
+ }
+
+ bbox.x = round(xyxy_emas[0]->smoothed_value());
+ bbox.y = round(xyxy_emas[1]->smoothed_value());
+ bbox.width = round(xyxy_emas[2]->smoothed_value());
+ bbox.height = round(xyxy_emas[3]->smoothed_value());
+
+ return bbox;
+ }
+
+ void debug_output() const {
+#if EI_LOG_LEVEL == EI_LOG_LEVEL_DEBUG
+ // output debug info, C-style
+ ei_printf("Trace %d:\n", id);
+ ei_printf(" Last ground truth update: %d\n", last_ground_truth_update_t);
+ ei_printf(" Last prediction: %d %d %d %d %f\n", last_prediction.x, last_prediction.y, last_prediction.width, last_prediction.height, last_prediction.value);
+ ei_printf(" Observations:\n");
+ for (const auto& obs : observations) {
+ ei_printf("%d %d %d %d %f\n", obs.x, obs.y, obs.width, obs.height, obs.value);
+ }
+#endif
+ }
+
+ uint32_t id;
+ uint32_t last_ground_truth_update_t;
+ ei_impulse_result_bounding_box_t last_prediction;
+
+private:
+ std::vector observations;
+ TinyEKF* centroid_filter;
+ TinyEKF* width_height_filter;
+ uint32_t max_observations;
+ float fx_centroid[2];
+ float fx_width_height[2];
+ float hx_centroid[2];
+ float hx_width_height[2];
+ const char* trace_label;
+ ExponentialMovingAverage *xyxy_emas[4];
+};
+
+class Tracker {
+public:
+ Tracker (uint32_t keep_grace = 5, uint16_t max_observations = 5, float iou_threshold = 0.5)
+ : keep_grace(keep_grace),
+ max_observations(max_observations),
+ alignment(iou_threshold) {
+ trace_seq_id = 0;
+ t = 0;
+ }
+
+ ~Tracker() {
+ for (auto trace : open_traces) {
+ delete trace;
+ }
+ for (auto trace : closed_traces) {
+ delete trace;
+ }
+ }
+
+ std::vectoropen_traces;
+ std::vectorclosed_traces;
+ std::vector object_tracking_output;
+
+ void process_new_detections(std::vector detections) {
+ // firstly try an alignment with last observations...
+ std::vector last_obs_bboxes;
+ for (auto trace : open_traces) {
+ last_obs_bboxes.push_back(*trace->last_observation());
+ }
+
+ std::vector> last_obs_matches = alignment.align(last_obs_bboxes, detections);
+
+ float last_obs_cost = 0;
+ for (auto last_obs_match : last_obs_matches) {
+ EI_LOGD("last_obs_match %d %d %f\n", std::get<0>(last_obs_match), std::get<1>(last_obs_match), std::get<2>(last_obs_match));
+ last_obs_cost += std::get<2>(last_obs_match);
+ }
+ EI_LOGD("last_obs_cost %f\n", last_obs_cost);
+
+ // ... then with the kalman filter predictions
+ std::vector predicted_bboxes;
+ for (auto trace : open_traces) {
+ predicted_bboxes.push_back(trace->predict());
+ EI_LOGD("predicted %d %d %d %d %f\n", trace->last_prediction.x, trace->last_prediction.y, trace->last_prediction.width, trace->last_prediction.height, trace->last_prediction.value);
+ }
+
+ std::vector> predicted_matches = alignment.align(predicted_bboxes, detections);
+ float predicted_cost = 0;
+ for (auto predicted_match : predicted_matches) {
+ EI_LOGD("predicted_match %d %d %f\n", std::get<0>(predicted_match), std::get<1>(predicted_match), std::get<2>(predicted_match));
+ predicted_cost += std::get<2>(predicted_match);
+ }
+ EI_LOGD("predicted_cost %f\n", predicted_cost);
+
+ // and use whichever matching set is better
+ std::vector> matches;
+
+ if (last_obs_cost > predicted_cost) {
+ EI_LOGD("using last_obs_matches matches\n");
+ matches = last_obs_matches;
+ }
+ else {
+ EI_LOGD("using predicted_matches matches\n");
+ matches = predicted_matches;
+ }
+
+ // assume all detections are unassigned and will becomes new tracks
+ // until we see otherwise ( i.e. they match an existing track )∂ //
+ std::setunassigned_detection_idxs;
+ for (size_t i = 0; i < detections.size(); i++) {
+ unassigned_detection_idxs.insert(i);
+ }
+
+ // keep track of open traces idxs that haven't been updated
+ std::setopen_traces_idxs_to_be_updated;
+ for (size_t i = 0; i < open_traces.size(); i++) {
+ open_traces_idxs_to_be_updated.insert(i);
+ }
+
+ // update existing traces with any matches
+ for (size_t i = 0; i < matches.size(); i++) {
+ uint32_t trace_idx = std::get<0>(matches[i]);
+ uint32_t detection_idx = std::get<1>(matches[i]);
+ EI_LOGD("t_idx=%u d_idx=%u iou=%.6f\n", trace_idx, detection_idx, std::get<2>(matches[i]));
+
+ Trace *trace = open_traces[trace_idx];
+ open_traces_idxs_to_be_updated.erase(trace_idx);
+ trace->update(t, &detections[detection_idx]);
+ unassigned_detection_idxs.erase(detection_idx);
+ }
+
+ for (auto detection_idx : unassigned_detection_idxs ) {
+ EI_LOGD("unassigned detection %d %d %d %d %d %f => starting new trace\n", detection_idx, detections[detection_idx].x, detections[detection_idx].y, detections[detection_idx].width, detections[detection_idx].height, detections[detection_idx].value);
+ open_traces.push_back(new Trace(trace_seq_id, t, detections[detection_idx], max_observations));
+ trace_seq_id += 1;
+ }
+
+ std::vectortraces_tmp;
+
+ for (auto trace : open_traces) {
+ EI_LOGD("grace checking trace %d at t=%d (trace.last_ground_truth_update_t=%d)\n", trace->id, t, trace->last_ground_truth_update_t);
+ uint32_t time_since_last_update = t - trace->last_ground_truth_update_t;
+ if (time_since_last_update > keep_grace) {
+ // been too long since last update, close it
+ EI_LOGD("closing trace %d\n", trace->id);
+ closed_traces.push_back(trace);
+ }
+ else {
+ if (trace->last_ground_truth_update_t != t) {
+ // wasn't match this step, so do rollout of filters
+ EI_LOGD("self rollout of trace %d\n", trace->id);
+ trace->update(t, nullptr);
+ }
+ EI_LOGD("trace %d still alive\n", trace->id);
+ traces_tmp.push_back(trace);
+ }
+ }
+
+ open_traces = traces_tmp;
+ object_tracking_output.clear();
+
+ for (auto trace : open_traces) {
+ ei_object_tracking_trace_t trace_result = { 0 };
+ trace_result.id = trace->id;
+ trace_result.last_ground_truth_update_t = trace->last_ground_truth_update_t;
+ trace_result.label = trace->last_prediction.label;
+ trace_result.x = trace->last_prediction.x;
+ trace_result.y = trace->last_prediction.y;
+ trace_result.width = trace->last_prediction.width;
+ trace_result.height = trace->last_prediction.height;
+ trace_result.last_centroid_segment = trace->last_centroid_segment();
+
+ object_tracking_output.push_back(trace_result);
+ }
+ t += 1;
+ }
+
+ bool set_iou_threshold(float iou_threshold) {
+ alignment.iou_threshold = iou_threshold;
+ return true;
+ }
+
+ float get_iou_threshold() {
+ return alignment.iou_threshold;
+ }
+
+ uint32_t keep_grace;
+ uint16_t max_observations;
+private:
+ uint32_t trace_seq_id;
+ uint32_t t;
+ GreedyAlignment alignment;
+};
+
+EI_IMPULSE_ERROR init_object_tracking(ei_impulse_handle_t *handle, void** state, void *config)
+{
+ //const ei_impulse_t *impulse = handle->impulse;
+ const ei_object_tracking_config_t *ei_object_tracking_config = (ei_object_tracking_config_t*)config;
+
+ // Allocate the object counter
+ Tracker *object_tracker = new Tracker(ei_object_tracking_config->keep_grace,
+ ei_object_tracking_config->max_observations,
+ ei_object_tracking_config->iou_threshold);
+ if (!object_tracker) {
+ return EI_IMPULSE_OUT_OF_MEMORY;
+ }
+
+ // Store the object counter state
+ *state = (void*)object_tracker;
+
+ return EI_IMPULSE_OK;
+}
+
+EI_IMPULSE_ERROR deinit_object_tracking(void* state, void *config)
+{
+ Tracker *object_tracker = (Tracker *)state;
+
+ if (object_tracker) {
+ delete object_tracker;
+ }
+
+ return EI_IMPULSE_OK;
+}
+
+EI_IMPULSE_ERROR process_object_tracking(ei_impulse_handle_t *handle,
+ ei_impulse_result_t *result,
+ void *config,
+ void *state)
+{
+ const ei_impulse_t *impulse = handle->impulse;
+ Tracker *object_tracker = (Tracker *)state;
+
+ if (impulse->sensor == EI_CLASSIFIER_SENSOR_CAMERA) {
+ if((void *)object_tracker != NULL) {
+ ei_impulse_result_bounding_box_t *bbs = result->bounding_boxes;
+ uint32_t bbs_num = result->bounding_boxes_count;
+ std::vector detections(bbs, bbs + bbs_num);
+
+ object_tracker->process_new_detections(detections);
+
+ result->postprocessed_output.object_tracking_output.open_traces = object_tracker->object_tracking_output.data();
+ result->postprocessed_output.object_tracking_output.open_traces_count = object_tracker->object_tracking_output.size();
+ }
+ }
+
+ return EI_IMPULSE_OK;
+}
+
+EI_IMPULSE_ERROR set_post_process_params(ei_impulse_handle_t* handle, ei_object_tracking_config_t* params) {
+ int16_t block_number = get_block_number(handle, (void*)init_object_tracking);
+ if (block_number == -1) {
+ return EI_IMPULSE_POSTPROCESSING_ERROR;
+ }
+ Tracker *object_tracker = (Tracker*)handle->post_processing_state[block_number];
+
+ object_tracker->keep_grace = params->keep_grace;
+ object_tracker->max_observations = params->max_observations;
+ object_tracker->set_iou_threshold(params->iou_threshold);
+ return EI_IMPULSE_OK;
+}
+
+EI_IMPULSE_ERROR get_post_process_params(ei_impulse_handle_t* handle, ei_object_tracking_config_t* params) {
+ int16_t block_number = get_block_number(handle, (void*)init_object_tracking);
+ if (block_number == -1) {
+ return EI_IMPULSE_POSTPROCESSING_ERROR;
+ }
+ Tracker *object_tracker = (Tracker*)handle->post_processing_state[block_number];
+
+ params->keep_grace = object_tracker->keep_grace;
+ params->max_observations = object_tracker->max_observations;
+ params->iou_threshold = object_tracker->get_iou_threshold();
+ return EI_IMPULSE_OK;
+}
+
+// versions that operate on the default impulse
+EI_IMPULSE_ERROR set_post_process_params(ei_object_tracking_config_t *params) {
+ ei_impulse_handle_t* handle = &ei_default_impulse;
+
+ if(handle->post_processing_state != NULL) {
+ set_post_process_params(handle, params);
+ }
+ return EI_IMPULSE_OK;
+}
+
+EI_IMPULSE_ERROR get_post_process_params(ei_object_tracking_config_t *params) {
+ ei_impulse_handle_t* handle = &ei_default_impulse;
+
+ if(handle->post_processing_state != NULL) {
+ get_post_process_params(handle, params);
+ }
+ return EI_IMPULSE_OK;
+}
+
+#endif // EI_CLASSIFIER_OBJECT_TRACKING_ENABLED
+#endif // EI_OBJECT_TRACKING_H
\ No newline at end of file
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_performance_calibration.h b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_performance_calibration.h
index 644552d..dab87b4 100644
--- a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_performance_calibration.h
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_performance_calibration.h
@@ -25,6 +25,8 @@
#include "edge-impulse-sdk/dsp/returntypes.hpp"
#include "edge-impulse-sdk/classifier/ei_model_types.h"
#include "model-parameters/model_metadata.h"
+#include "edge-impulse-sdk/classifier/postprocessing/ei_postprocessing_common.h"
+#include "edge-impulse-sdk/porting/ei_logging.h"
/* Private const types ----------------------------------------------------- */
#define MEM_ERROR "ERR: Failed to allocate memory for performance calibration\r\n"
@@ -39,10 +41,9 @@ typedef struct {
} ei_perf_cal_params_t;
class PerfCal {
-
public:
PerfCal(
- const ei_model_performance_calibration_t *config,
+ const ei_performance_calibration_config_t *config,
uint32_t n_labels,
uint32_t sample_length,
float sample_interval_ms)
@@ -70,7 +71,7 @@ class PerfCal {
/* Detection threshold should be high enough to only classify 1 possible output */
if (this->_detection_threshold <= (1.f / this->_n_labels)) {
- ei_printf("ERR: Classifier detection threshold too low\r\n");
+ EI_LOGE("Classifier detection threshold too low\r\n");
return;
}
@@ -215,39 +216,39 @@ class PerfCal {
uint32_t _n_scores_in_array;
};
-EI_IMPULSE_ERROR init_perfcal(ei_impulse_handle_t *handle, void *config)
+EI_IMPULSE_ERROR init_perfcal(ei_impulse_handle_t *handle, void **state, void *config)
{
const ei_impulse_t *impulse = handle->impulse;
- const ei_model_performance_calibration_t *calibration = (ei_model_performance_calibration_t*)config;
+ const ei_performance_calibration_config_t *calibration = (ei_performance_calibration_config_t*)config;
if(calibration != NULL) {
PerfCal *perf_cal = new PerfCal(calibration, impulse->label_count, impulse->slice_size,
impulse->interval_ms);
- handle->post_processing_state = (void *)perf_cal;
+ *state = (void *)perf_cal;
}
return EI_IMPULSE_OK;
}
-EI_IMPULSE_ERROR deinit_perfcal(ei_impulse_handle_t *handle, void *config)
+EI_IMPULSE_ERROR deinit_perfcal(void *state, void *config)
{
- PerfCal *perf_cal = (PerfCal*)handle->post_processing_state;
+ PerfCal *perf_cal = (PerfCal*)state;
if((void *)perf_cal != NULL) {
delete perf_cal;
}
- handle->post_processing_state = NULL;
+ state = NULL;
return EI_IMPULSE_OK;
}
EI_IMPULSE_ERROR process_perfcal(ei_impulse_handle_t *handle,
ei_impulse_result_t *result,
void *config,
- bool debug) {
-
+ void *state)
+{
const ei_impulse_t *impulse = handle->impulse;
- PerfCal *perf_cal = (PerfCal*)handle->post_processing_state;
+ PerfCal *perf_cal = (PerfCal*)state;
if (impulse->sensor == EI_CLASSIFIER_SENSOR_MICROPHONE) {
if((void *)perf_cal != NULL) {
@@ -280,13 +281,23 @@ EI_IMPULSE_ERROR process_perfcal(ei_impulse_handle_t *handle,
}
EI_IMPULSE_ERROR set_post_process_params(ei_impulse_handle_t* handle, ei_perf_cal_params_t* params) {
- PerfCal *perf_cal = (PerfCal*)handle->post_processing_state;
+ int16_t block_number = get_block_number(handle, (void*)init_perfcal);
+ if (block_number == -1) {
+ return EI_IMPULSE_POSTPROCESSING_ERROR;
+ }
+ PerfCal *perf_cal = (PerfCal*)handle->post_processing_state[block_number];
+
perf_cal->set_detection_threshold(params->detection_threshold);
return EI_IMPULSE_OK;
}
EI_IMPULSE_ERROR get_post_process_params(ei_impulse_handle_t* handle, ei_perf_cal_params_t* params) {
- PerfCal *perf_cal = (PerfCal*)handle->post_processing_state;
+ int16_t block_number = get_block_number(handle, (void*)init_perfcal);
+ if (block_number == -1) {
+ return EI_IMPULSE_POSTPROCESSING_ERROR;
+ }
+ PerfCal *perf_cal = (PerfCal*)handle->post_processing_state[block_number];
+
params->detection_threshold = perf_cal->get_detection_threshold();
return EI_IMPULSE_OK;
}
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing.h b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing.h
index 0d4dfa8..4faf5df 100644
--- a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing.h
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing.h
@@ -24,7 +24,12 @@
#include "edge-impulse-sdk/classifier/postprocessing/ei_performance_calibration.h"
#endif
+#if EI_CLASSIFIER_OBJECT_TRACKING_ENABLED
+#include "edge-impulse-sdk/classifier/postprocessing/ei_object_tracking.h"
+#endif
+
#if EI_CLASSIFIER_OBJECT_COUNTING_ENABLED
+#include "edge-impulse-sdk/classifier/postprocessing/ei_object_tracking.h"
#include "edge-impulse-sdk/classifier/postprocessing/ei_object_counting.h"
#endif
@@ -33,10 +38,11 @@ extern "C" EI_IMPULSE_ERROR init_postprocessing(ei_impulse_handle_t *handle) {
return EI_IMPULSE_OUT_OF_MEMORY;
}
auto impulse = handle->impulse;
+ handle->post_processing_state = (void **)ei_malloc(impulse->postprocessing_blocks_size * sizeof(void *));
for (size_t i = 0; i < impulse->postprocessing_blocks_size; i++) {
- EI_IMPULSE_ERROR res = impulse->postprocessing_blocks[i].init_fn(handle, impulse->postprocessing_blocks[i].config);
+ EI_IMPULSE_ERROR res = impulse->postprocessing_blocks[i].init_fn(handle, &handle->post_processing_state[i], impulse->postprocessing_blocks[i].config);
if (res != EI_IMPULSE_OK) {
return res;
}
@@ -52,12 +58,18 @@ extern "C" EI_IMPULSE_ERROR deinit_postprocessing(ei_impulse_handle_t *handle) {
auto impulse = handle->impulse;
for (size_t i = 0; i < impulse->postprocessing_blocks_size; i++) {
+ void* state = NULL;
+ if (handle->post_processing_state != NULL) {
+ state = handle->post_processing_state[i];
+ }
- EI_IMPULSE_ERROR res = impulse->postprocessing_blocks[i].deinit_fn(handle, impulse->postprocessing_blocks[i].config);
+ EI_IMPULSE_ERROR res = impulse->postprocessing_blocks[i].deinit_fn(state, impulse->postprocessing_blocks[i].config);
if (res != EI_IMPULSE_OK) {
return res;
}
}
+ ei_free(handle->post_processing_state);
+ handle->post_processing_state = NULL;
return EI_IMPULSE_OK;
}
@@ -71,8 +83,15 @@ extern "C" EI_IMPULSE_ERROR run_postprocessing(ei_impulse_handle_t *handle,
auto impulse = handle->impulse;
for (size_t i = 0; i < impulse->postprocessing_blocks_size; i++) {
+ void* state = NULL;
+ if (handle->post_processing_state != NULL) {
+ state = handle->post_processing_state[i];
+ }
- EI_IMPULSE_ERROR res = impulse->postprocessing_blocks[i].postprocess_fn(handle, result, impulse->postprocessing_blocks[i].config, debug);
+ EI_IMPULSE_ERROR res = impulse->postprocessing_blocks[i].postprocess_fn(handle,
+ result,
+ impulse->postprocessing_blocks[i].config,
+ state);
if (res != EI_IMPULSE_OK) {
return res;
}
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing_types.h b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing_common.h
similarity index 59%
rename from edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing_types.h
rename to edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing_common.h
index 7486d2a..16a1b62 100644
--- a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing_types.h
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/ei_postprocessing_common.h
@@ -15,16 +15,19 @@
* SPDX-License-Identifier: Apache-2.0
*/
-#ifndef EI_POSTPROCESSING_TYPES_H
-#define EI_POSTPROCESSING_TYPES_H
+#ifndef EI_POSTPROCESSING_COMMON_H
+#define EI_POSTPROCESSING_COMMON_H
#include "edge-impulse-sdk/classifier/ei_model_types.h"
-EI_IMPULSE_ERROR init_perfcal(ei_impulse_handle_t *handle, void *config);
-EI_IMPULSE_ERROR deinit_perfcal(ei_impulse_handle_t *handle, void *config);
-EI_IMPULSE_ERROR process_perfcal(ei_impulse_handle_t *handle,
- ei_impulse_result_t *result,
- void *config,
- bool debug);
+int16_t get_block_number(ei_impulse_handle_t *handle, void *init_func)
+{
+ for (size_t i = 0; i < handle->impulse->postprocessing_blocks_size; i++) {
+ if (handle->impulse->postprocessing_blocks[i].init_fn == init_func) {
+ return i;
+ }
+ }
+ return -1;
+}
-#endif // EI_POSTPROCESSING_TYPES_H
\ No newline at end of file
+#endif // EI_POSTPROCESSING_COMMON_H
\ No newline at end of file
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/LICENSE.md b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/LICENSE.md
new file mode 100644
index 0000000..af37c3d
--- /dev/null
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/LICENSE.md
@@ -0,0 +1,22 @@
+TinyEKF
+
+Copyright (c) Simon D. Levy
+
+All rights reserved.
+
+MIT License
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the ""Software""), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do
+so, subject to the following conditions: The above copyright notice and this
+permission notice shall be included in all copies or substantial portions of
+the Software. THE SOFTWARE IS PROVIDED *AS IS*, WITHOUT WARRANTY OF ANY KIND,
+EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO
+EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES
+OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/tinyekf.hpp b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/tinyekf.hpp
new file mode 100644
index 0000000..cec8447
--- /dev/null
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/tinyekf.hpp
@@ -0,0 +1,496 @@
+/*
+ * Extended Kalman Filter for embedded processors
+ *
+ * Copyright (C) 2024 Simon D. Levy
+ *
+ * MIT License
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+
+template
+void print_arr(T *arr, int m, int n, const char *name = "arr") {
+// verbose debug
+#if EI_LOG_LEVEL == 5
+ ei_printf("%s:\n", name);
+ for (int i = 0; i < m; i++) {
+ for (int j = 0; j < n; j++) {
+ ei_printf("[%d][%d] = %g ", i, j, arr[i * n + j]);
+ }
+ ei_printf("\n");
+ }
+ ei_printf("\n");
+#endif
+}
+
+class TinyEKF {
+public:
+ TinyEKF(const float* x0, uint32_t EKF_N, uint32_t EKF_M,
+ float dt = 0.1,
+ float *u = nullptr,
+ float process_noise_scale = 0.1,
+ float observation_noise_scale=0.1)
+{
+ // set private variables
+ this->EKF_N = EKF_N;
+ this->EKF_M = EKF_M;
+ this->dt = dt;
+
+ x = new float[this->EKF_N];
+ memset(x, 0, sizeof(float) * this->EKF_N);
+ // x is the state
+ x[0] = x0[0];
+ x[1] = x0[1];
+ x[2] = x0[0];
+ x[3] = x0[1];
+
+ // print init x
+ print_arr(x, 1, this->EKF_N, "init x");
+
+ // F is the state transition model
+ // self.F = np.array(
+ // [[1, 0, self.dt, 0],
+ // [0, 1, 0, self.dt],
+ // [0, 0, 1, 0],
+ // [0, 0, 0, 1]]
+ // )
+
+ F = new float[16];
+ memset(F, 0, sizeof(float) * 16);
+ for (int i = 0; i < 4; ++i) {
+ for (int j = 0; j < 4; ++j) {
+ F[i * 4 + j] = (i == j) ? 1 : 0;
+ }
+ }
+ F[2] = F[7] = this->dt;
+
+ // print F
+ print_arr(F, 4, 4, "init F");
+
+ // H is the observation model
+ H = new float[8];
+ memset(H, 0, sizeof(float) * 8);
+
+ H[0] = H[5] = 1;
+
+ // print H
+ print_arr(H, 2, 4, "init H");
+
+ // Q is the covariance of the process noise
+ Q = new float[16];
+ memset(Q, 0, sizeof(float) * 16);
+
+ // self.Q = (
+ // np.array(
+ // [
+ // [(self.dt**4) / 4, 0, (self.dt**3) / 2, 0],
+ // [0, (self.dt**4) / 4, 0, (self.dt**3) / 2],
+ // [(self.dt**3) / 2, 0, self.dt**2, 0],
+ // [0, (self.dt**3) / 2, 0, self.dt**2],
+ // ]
+ // )
+ // * process_noise_scale**2
+ // )
+
+ Q[0] = Q[5] = pow(dt, 4) / 4;
+ Q[2] = Q[7] = Q[8] = Q[13] = pow(dt, 3) / 2;
+ Q[10] = Q[15] = pow(dt, 2);
+
+ for (int i = 0; i < 4; ++i) {
+ for (int j = 0; j < 4; ++j) {
+ Q[i * 4 + j] = Q[i * 4 + j] * pow(process_noise_scale, 2);
+ }
+ }
+
+ // print Q
+ print_arr(Q, 4, 4, "init Q");
+
+ // R is the covariance of the observation noise
+ R = new float[4];
+ memset(R, 0, sizeof(float) * 4);
+
+ for (int i = 0; i < 2; ++i) {
+ for (int j = 0; j < 2; ++j) {
+ R[i * 2 + j] = (i == j) ? (pow(observation_noise_scale, 2)) : 0;
+ }
+ }
+ // print R
+ print_arr(R, 2, 2, "init R");
+
+ // control-input mode
+ // self.B = np.array(
+ // [[(self.dt**2) / 2, 0],
+ // [0, (self.dt**2) / 2],
+ // [self.dt, 0],
+ // [0, self.dt]]
+ // )
+
+ B = new float[this->EKF_N * this->EKF_N * 2];
+ memset(B, 0, sizeof(float) * this->EKF_N * this->EKF_N * 2);
+ B[0] = B[3] = (dt * dt) / 2;
+ B[4] = B[7] = dt;
+
+ if (u == nullptr) {
+ u = new float[2];
+ u[0] = u[1] = 0.1;
+ }
+ this->u = u;
+
+ // P is the predict / update transition
+ P = new float[16];
+ memset(P, 0, sizeof(float) * 16);
+
+ for (int i = 0; i < 4; ++i) {
+ for (int j = 0; j < 4; ++j) {
+ P[i * 4 + j] = (i == j) ? 1 : 0;
+ }
+ }
+
+ // print P
+ print_arr(P, 4, 4, "init P");
+ }
+
+ ~TinyEKF() {
+ delete[] x;
+ delete[] P;
+ delete[] Q;
+ delete[] F;
+ delete[] H;
+ delete[] R;
+ delete[] B;
+ delete[] u;
+ }
+
+ void predict(const float *fx);
+ bool update(const float *z, const float *hx);
+ float *x;
+private:
+ uint32_t EKF_N;
+ uint32_t EKF_M;
+
+ float *P;
+ float *Q;
+ float *F;
+ float *H;
+ float *R;
+
+ float *B;
+ float *u;
+ float dt;
+
+ void update_step3(float *GH);
+
+ /// @private
+ static void _mulmat(
+ const float * a,
+ const float * b,
+ float * c,
+ const int arows,
+ const int acols,
+ const int bcols)
+ {
+ for (int i=0; i= 0; k--) {
+ sum -= a[i*n+k] * a[j*n+k];
+ }
+ if (i == j) {
+ if (sum <= 0) {
+ return 1; /* error */
+ }
+ p[i] = sqrt(sum);
+ }
+ else {
+ a[j*n+i] = sum / p[i];
+ }
+ }
+ }
+
+ return 0; // success:w
+ }
+
+ /// @private
+ static int _choldcsl(const float * A, float * a, float * p, const int n)
+ {
+ for (int i = 0; i < n; i++) {
+ for (int j = 0; j < n; j++) {
+ a[i*n+j] = A[i*n+j];
+ }
+ }
+ if (_choldc1(a, p, n)) {
+ return 1;
+ }
+ for (int i = 0; i < n; i++) {
+ a[i*n+i] = 1 / p[i];
+ for (int j = i + 1; j < n; j++) {
+ float sum = 0;
+ for (int k = i; k < j; k++) {
+ sum -= a[j*n+k] * a[k*n+i];
+ }
+ a[j*n+i] = sum / p[j];
+ }
+ }
+
+ return 0; // success
+ }
+
+ /// @private
+ static int _cholsl(const float * A, float * a, float * p, const int n)
+ {
+ if (_choldcsl(A,a,p,n)) {
+ return 1;
+ }
+
+ for (int i = 0; i < n; i++) {
+ for (int j = i + 1; j < n; j++) {
+ a[i*n+j] = 0.0;
+ }
+ }
+ for (int i = 0; i < n; i++) {
+ a[i*n+i] *= a[i*n+i];
+ for (int k = i + 1; k < n; k++) {
+ a[i*n+i] += a[k*n+i] * a[k*n+i];
+ }
+ for (int j = i + 1; j < n; j++) {
+ for (int k = j; k < n; k++) {
+ a[i*n+j] += a[k*n+i] * a[k*n+j];
+ }
+ }
+ }
+ for (int i = 0; i < n; i++) {
+ for (int j = 0; j < i; j++) {
+ a[i*n+j] = a[j*n+i];
+ }
+ }
+
+ return 0; // success
+ }
+
+ /// @private
+ static void _addvec(
+ const float * a, const float * b, float * c, const int n)
+ {
+ for (int j=0; jB, this->u, Bu, 4, 2, 1);
+
+ // print Bu
+ print_arr(Bu, 4, 1, "Bu");
+
+ // print x before
+ print_arr(this->x, 1, 4, "x before");
+
+ float Fx[8];
+ _mulmat(this->F, this->x, Fx, 4, 4, 2);
+
+ // print Fx
+ print_arr(Fx, 4, 2, "Fx");
+ // print x after
+ print_arr(this->x, 1, 4, "x after");
+
+ //_addmat(Fx, Bu, this->x, 4, 1);
+ this->x[0] = Fx[0] + Bu[0];
+ this->x[1] = Fx[1] + Bu[1];
+ this->x[2] = Fx[2] + Bu[0];
+ this->x[3] = Fx[3] + Bu[1];
+ this->x[4] = Fx[4] + Bu[2];
+ this->x[5] = Fx[5] + Bu[3];
+ this->x[6] = Fx[6] + Bu[2];
+ this->x[7] = Fx[7] + Bu[3];
+
+ // this is the formula for the next part
+ // self.P_pre = np.dot(F, self.P_post).dot(F.T) + Q
+
+ // np.dot(F, self.P_post)
+ float FP[16];
+ _mulmat(F, P, FP, 4, 4, 4);
+ // print FP
+ print_arr(FP, 4, 4, "FP");
+
+ // F.T
+ float Ft[16];
+ _transpose(F, Ft, 4, 4);
+
+ // .dot(F.T)
+ float FPFt[16];
+ _mulmat(FP, Ft, FPFt, 4, 4, 4);
+
+ // + Q
+ _addmat(FPFt, Q, P, 4, 4);
+
+ // print P
+ print_arr(P, 4, 4, "P");
+}
+
+bool TinyEKF::update(const float *z, const float *hx) {
+
+ float Ht[8];
+ _transpose(H, Ht, 2, 4);
+
+ // print Ht
+ print_arr(Ht, 4, 2, "Ht");
+
+ float PHt[8];
+ _mulmat(P, Ht, PHt, 4, 4, 2);
+
+ float HP[8];
+ _mulmat(H, P, HP, 2, 4, 4);
+
+ float HpHt[4];
+ _mulmat(HP, Ht, HpHt, 2, 4, 2);
+
+ float HpHtR[4];
+ _addmat(HpHt, R, HpHtR, 2, 2);
+
+ float HPHtRinv[4];
+ if (!invert(HpHtR, HPHtRinv, 2)) {
+ return false;
+ }
+
+ float G[8];
+ _mulmat(PHt, HPHtRinv, G, 4, 2, 2);
+
+ // print G
+ print_arr(G, 4, 2, "G");
+
+ // print x
+ print_arr(this->x, 1, 4, "x in update");
+
+ // we get hx as an argument to function
+ float z_hx[4];
+ //_sub(z, Hx, z_hx, 2);
+ z_hx[0] = z[0] - hx[0];
+ z_hx[1] = z[1] - hx[1];
+ z_hx[2] = z[0] - hx[0];
+ z_hx[3] = z[1] - hx[1];
+
+ // print z_hx
+ print_arr(z_hx, 2, 2, "z_hx");
+
+ float Gz_hx[8];
+ _mulmat(G, z_hx, Gz_hx, 4, 2, 2);
+
+ // // print Gz_hx
+ print_arr(Gz_hx, 4, 2, "Gz_hx");
+
+ _addvec(this->x, Gz_hx, this->x, 8);
+
+ float GH[16];
+ _mulmat(G, H, GH, 4, 2, 4);
+ update_step3(GH);
+ return true;
+}
+
+/// @private
+void TinyEKF::update_step3(float *GH)
+{
+ _negate(GH, 4, 4);
+ _addeye(GH, 4);
+
+ float GHP[16];
+ _mulmat(GH, P, GHP, 4, 4, 4);
+ memcpy(P, GHP, 16 * sizeof(float));
+}
\ No newline at end of file
diff --git a/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/tinyekf_custom.h b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/tinyekf_custom.h
new file mode 100644
index 0000000..5e8aa98
--- /dev/null
+++ b/edgeimpulse/edge-impulse-sdk/classifier/postprocessing/tinyEKF/tinyekf_custom.h
@@ -0,0 +1,119 @@
+/**
+ * Custom EKF methods
+ *
+ * Copyright (C) 2024 Simon D. Levy
+ *
+ * MIT License
+ */
+
+/// @private
+static void outer(
+ const _float_t x[EKF_N],
+ const _float_t y[EKF_N],
+ _float_t a[EKF_N*EKF_N])
+{
+ for (int i=0; iP, AP, EKF_N, EKF_N, EKF_N);
+
+ _float_t At[EKF_N*EKF_N] = {};
+ _transpose(A, At, EKF_N, EKF_N);
+
+ _mulmat(AP, At, ekf->P, EKF_N, EKF_N, EKF_N);
+}
+
+/**
+ * Enforces symmetry of the covariance matrix and ensures that the its values stay bounded
+ * @param ekf pointer to an ekf_t structure
+ * @param minval minimum covariance bound
+ * @param maxval maximum covariance bound
+ *
+ */
+static void ekf_custom_cleanup_covariance(
+ ekf_t * ekf, const float minval, const float maxval)
+{
+
+ for (int i=0; iP[i*EKF_N+j] + ekf->P[EKF_N*j+i]) / 2;
+
+ ekf->P[i*EKF_N+j] = ekf->P[j*EKF_N+i] =
+ pval > maxval ? maxval :
+ (i==j && pval < minval) ? minval :
+ pval;
+ }
+ }
+}
+
+/**
+ * Updates the EKF with a single scalar observation
+ * @param ekf pointer to an ekf_t structure
+ * @param z the observation
+ * @param hx the predicted value
+ * @param h one column of the sensor-function Jacobian matrix H
+ * @param r one entry in the measurement-noise matrix R
+ *
+ */
+static void ekf_custom_scalar_update(
+ ekf_t * ekf,
+ const _float_t z,
+ const _float_t hx,
+ const _float_t h[EKF_N],
+ const _float_t r)
+{
+ (void)ekf_update;
+
+ // G_k = P_k H^T_k (H_k P_k H^T_k + R)^{-1}
+ _float_t ph[EKF_N] = {};
+ _mulvec(ekf->P, h, ph, EKF_N, EKF_N);
+ const _float_t hphtr_inv = 1 / (r + dot(h, ph));
+ _float_t g[EKF_N] = {};
+ for (int i=0; ix[i] += g[i] * (z - hx);
+ }
+
+ // P_k = (I - G_k H_k) P_k$
+ _float_t GH[EKF_N*EKF_N];
+ outer(g, h, GH);
+ ekf_update_step3(ekf, GH);
+
+ // Does this belong here, or in caller?
+ for (int i=0; iP[i*EKF_N+j] += r * g[i] * g[j];
+ }
+ }
+}
diff --git a/edgeimpulse/edge-impulse-sdk/dsp/returntypes.h b/edgeimpulse/edge-impulse-sdk/dsp/returntypes.h
index d7906ac..2ef7644 100644
--- a/edgeimpulse/edge-impulse-sdk/dsp/returntypes.h
+++ b/edgeimpulse/edge-impulse-sdk/dsp/returntypes.h
@@ -5,11 +5,11 @@
/**
* @defgroup ei_returntypes Return codes
- *
+ *
* Return codes for Edge Impulse functions.
- *
+ *
* **Source**: [dsp/returntypes.h](https://github.com/edgeimpulse/inferencing-sdk-cpp/blob/master/dsp/returntypes.h)
- *
+ *
* @addtogroup ei_returntypes
* @{
*/
@@ -43,6 +43,7 @@ typedef enum {
EI_IMPULSE_MEMRYX_ERROR = -26, /**< Error in Memryx inference engine */
EI_IMPULSE_DEVICE_INIT_ERROR = -27, /**< Device initialization (usually NPU accelerator) failed */
EI_IMPULSE_LAST_LAYER_NOT_SUPPORTED = -28, /**< The last layer is not supported by inferencing engine. */
+ EI_IMPULSE_POSTPROCESSING_ERROR = -29, /**< Error in post-processing portion of impulse */
} EI_IMPULSE_ERROR;
#endif // _EIDSP_RETURN_TYPES_H_
diff --git a/edgeimpulse/edge-impulse-sdk/porting/ambiq/ei_classifier_porting.cpp b/edgeimpulse/edge-impulse-sdk/porting/ambiq/ei_classifier_porting.cpp
index 025cf8e..8e67fe4 100644
--- a/edgeimpulse/edge-impulse-sdk/porting/ambiq/ei_classifier_porting.cpp
+++ b/edgeimpulse/edge-impulse-sdk/porting/ambiq/ei_classifier_porting.cpp
@@ -94,6 +94,19 @@ void ei_putchar(char c)
ei_printf("%c", c);
}
+char ei_getchar(void)
+{
+ char c = 0xFF;
+
+ c = ei_get_serial_byte(false);
+
+ if (c == 0xFF ) {
+ return 0; //weird ei convention
+ }
+
+ return c;
+}
+
#if defined(__cplusplus) && EI_C_LINKAGE == 1
extern "C"
#endif