Merge pull request #157 from AIRLegend/dev

v0.6.6

.gitignore

@@ -350,7 +350,9 @@ Client/models
 prefs.ini
 log.txt
 
+
 # CMake
 CMakeCache.txt
 CMakeFiles/
-
+/cmake-build-debug/
+cmake_install.cmake

AITracker/AITracker.vcxproj

@@ -189,7 +189,7 @@
       <ConformanceMode>true</ConformanceMode>
       <AdditionalIncludeDirectories>$(SolutionDir)Dependencies\OpenCV\include\;$(SolutionDir)Dependencies\onnxruntime\include\;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories>
       <RuntimeLibrary>MultiThreadedDLL</RuntimeLibrary>
-      <Optimization>Disabled</Optimization>
+      <Optimization>MaxSpeed</Optimization>
     </ClCompile>
     <Link>
       <SubSystem>Console</SubSystem>

AITracker/src/PositionSolver.cpp

@@ -1,5 +1,6 @@
 #include "PositionSolver.h"
 
+#define USE_FOV  // Use FOV correction for the camera matrix.
 
 PositionSolver::PositionSolver(
     int width, 
@@ -20,16 +21,16 @@ PositionSolver::PositionSolver(
 {
     this->prior_pitch = -1.57;
     this->prior_yaw = -1.57;
-    this->prior_distance = prior_distance * -2.;
+    this->prior_distance = prior_distance * -2.0;
 
     this->rv[0] = this->prior_pitch;
     this->rv[1] = this->prior_yaw;
     this->rv[2] = -1.57;
     this->tv[2] = this->prior_distance;
 
     head3dScale = (cv::Mat_<double>(3, 3) <<
-        x_scale, 0.0, 0,
-        0.0, y_scale, 0,
+        y_scale, 0.0, 0,        // pitch is rv[0], pitch involves y-axis
+        0.0, x_scale, 0,        // yaw is rv[1], yaw involves x-axis
         0.0, 0.0, z_scale
     );
 
@@ -63,9 +64,9 @@ PositionSolver::PositionSolver(
     {
         contour_indices = { 0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,27,28,29,30,31,32,33,34,35,36,39,42,45 };
 
-        landmark_points_buffer = cv::Mat(contour_indices.size(), 1, CV_32FC2);
+        landmark_points_buffer = cv::Mat((int)contour_indices.size(), 1, CV_32FC2);
 
-        mat3dcontour = (cv::Mat_<double>(contour_indices.size(), 3) <<
+        mat3dcontour = (cv::Mat_<double>((int)contour_indices.size(), 3) <<
             0.45517698, -0.30089578, 0.76442945,
             0.44899884, -0.16699584, 0.76514298,
             0.43743154, -0.02265548, 0.73926717,
@@ -102,15 +103,44 @@ PositionSolver::PositionSolver(
     // https://github.com/opentrack/opentrack/blob/3cc3ef246ad71c463c8952bcc96984b25d85b516/tracker-aruco/ftnoir_tracker_aruco.cpp#L193
     // Taking into account the camera FOV instead of assuming raw image dims is more clever and
     // will make the solver more camera-agnostic.
-    float diag_fov = fov * TO_RAD;
+    float diag_fov = (float)(fov * TO_RAD);
 
     // Get expressed in sensor-size units
 
+ #ifdef USE_FOV
+    // field of view is a rectangular viewport with corners on a circular lens
+    // the diagonal of the rectangle can be expressed as the angular field of view or pixels
+    // the width of the rectangle can be expressed as either the field of view width or width in pixels
+    // the height of the rectangle can be expressed as either the field of view height or height in pixes
+    double width_squared    = (double)width * width;
+    double height_squared   = (double)height * height;
+    double diagonal_squared = width_squared + height_squared;
+    double diagonal         = sqrt(diagonal_squared); // hypotenuse of triangle
+
+    // set default focalLength for width and heigh if field of view is not set
+    double focalLength_width = width;
+    double focalLength_height = height;
+    if (fov != 0.0)
+    {
+        double fov_w = (double)diag_fov * width / diagonal;
+        double fov_h = (double)diag_fov * height / diagonal;
+
+        focalLength_width = 0.5 * width / tan(0.5 * fov_w);
+        focalLength_height = 0.5 * height / tan(0.5 * fov_h);
+    }
+
+    camera_matrix = (cv::Mat_<double>(3, 3) <<
+        focalLength_height, 0, height / 2,
+        0, focalLength_width, width / 2,
+        0, 0, 1
+        );
+
+ #else
     double fov_w = 2. * atan(tan(diag_fov / 2.) / sqrt(1. + height / (double)width * height / (double)width));
     double fov_h = 2. * atan(tan(diag_fov / 2.) / sqrt(1. + width / (double)height * width / (double)height));
 
-    float i_height = .5 * height / (tan(.5*fov_w));
-    float i_width = .5* width / (tan(.5*fov_h));
+    float i_height = (float)(0.5f * height / (tan(0.5 * fov_w)));
+    float i_width = (float)(0.5f * width / (tan(0.5 * fov_h)));
 
     /*camera_matrix = (cv::Mat_<double>(3, 3) <<
         height, 0, height / 2,
@@ -122,7 +152,10 @@ PositionSolver::PositionSolver(
         i_width, 0, height / 2,
         0, i_height, width / 2,
         0, 0, 1
-     );
+    );
+
+
+ #endif
 
     camera_distortion = (cv::Mat_<double>(4, 1) << 0, 0, 0, 0);
 
@@ -131,6 +164,7 @@ PositionSolver::PositionSolver(
     if(complex) std::cout << "Using complex solver" << std::endl;
 }
 
+
 void PositionSolver::solve_rotation(FaceData* face_data)
 {
     int contour_idx = 0;
@@ -139,8 +173,7 @@ void PositionSolver::solve_rotation(FaceData* face_data)
         for (int i = 0; i < contour_indices.size(); i++)
         {
             contour_idx = contour_indices[i];
-            landmark_points_buffer.at<float>(i, j) = (int)face_data->landmark_coords[2 * contour_idx + j];
-
+            landmark_points_buffer.at<float>(i, j) = (float)(int)face_data->landmark_coords[2 * contour_idx + j]; // fix complation warnings.
         }
     }
 
@@ -164,15 +197,18 @@ void PositionSolver::solve_rotation(FaceData* face_data)
     for (int i = 0; i < 3; i++)
     {
         face_data->rotation[i] = rvec.at<double>(i, 0);
-        face_data->translation[i] = tvec.at<double>(i, 0) * 10;
+        face_data->translation[i] = tvec.at<double>(i, 0) * 10; // scale solvePnP coordinates to opentrack units in centimeters
     }
 
-    // We dont want the Z axis oversaturated.
+    // We dont want the Z axis oversaturated since opentrack has +/-600 centimeter range
     face_data->translation[2] /= 100;
 
-    std::cout << face_data->to_string() << std::endl;
+#ifdef _DEBUG
+    std::cout << face_data->to_string() << std::endl; // disable copy constructor and output to std::cout
+#endif
 
     correct_rotation(*face_data);
+    clip_rotations(*face_data);
 
 }
 
@@ -218,14 +254,38 @@ void PositionSolver::get_euler(cv::Mat& rvec, cv::Mat& tvec)
 
 void PositionSolver::correct_rotation(FaceData& face_data)
 {
-    float distance = -(face_data.translation[2]);
-    float lateral_offset = face_data.translation[1];
-    float verical_offset = face_data.translation[0];
+    float distance = (float) -(face_data.translation[2]);
+    float lateral_offset = (float)face_data.translation[1];
+    float verical_offset = (float)face_data.translation[0];
 
-    float correction_yaw = std::atan(std::tan(lateral_offset / distance)) * TO_DEG;
-    float correction_pitch = std::atan(std::tan(verical_offset / distance)) * TO_DEG;
+    float correction_yaw = (float)(std::atan(lateral_offset / distance) * TO_DEG); // (lateral_offset / distance) is already tangent, so only need atan to obtain radians
+    float correction_pitch = (float)(std::atan(verical_offset / distance) * TO_DEG); // (verical_offset / distance) is already tangent, so only need atan to obtain radians
 
     face_data.rotation[1] += correction_yaw;
     face_data.rotation[0] += correction_pitch;
+
+    // Note: We could saturate pitch here, but its better to let the user do it via Opentrack.
+    // The coefficient could be problematic for some users.
+    //face_data.rotation[0] = face_data.rotation[0] * 1.5;
+}
+
+
+void PositionSolver::clip_rotations(FaceData& face_data)
+{
+    // Limit yaw between -90.0 and +90.0 degrees
+    if (face_data.rotation[1] >= 90.0)
+        face_data.rotation[1] = 90.0;
+    else if (face_data.rotation[1] <= -90.0)
+        face_data.rotation[1] = -90.0;
+    // Limit pitch between -90.0 and +90.0 
+    if (face_data.rotation[0] >= 90.0)
+        face_data.rotation[0] = 90.0;
+    else if (face_data.rotation[0] <= -90.0)
+        face_data.rotation[0] = -90.0;
+    // Limit roll between -90.0 and +90.0 
+    if (face_data.rotation[2] >= 90.0)
+        face_data.rotation[2] = 90.0;
+    else if (face_data.rotation[2] <= -90.0)
+        face_data.rotation[2] = -90.0;
 }
 

AITracker/src/PositionSolver.h

@@ -1,7 +1,7 @@
 #pragma once
 
 #include "opencv2/core/matx.hpp"
-#include "opencv.hpp"
+#include "opencv2/opencv.hpp"
 #include "data.h"
 
 /**
@@ -68,5 +68,12 @@ class PositionSolver
 	* This method corrects them.
 	*/
 	void correct_rotation(FaceData& face_data);
+
+
+	/**
+	* Ensures all rotations are in -90/90 range. 
+	* (No human head can rotate more supposing camera view is frontal).
+	*/
+	void clip_rotations(FaceData& face_data);
 };
 

AITracker/src/data.cpp

@@ -9,7 +9,6 @@ FaceData::FaceData():
 	face_detected = false;
 }
 
-
 std::string FaceData::to_string()
 {
 	std::string datastring =
@@ -21,7 +20,7 @@ std::string FaceData::to_string()
 		std::string("   X: ") +
 		std::to_string(this->translation[0]) + ", Y: " +
 		std::to_string(this->translation[1]) + ", Z: " +
-		std::to_string(this->translation[2]) ;
+		std::to_string(this->translation[2]);
 
 	return datastring;
 }

AITracker/src/filters.cpp

@@ -1,37 +1,57 @@
 #include "filters.h"
-
 #include <cstring>
+#include <cmath>
+
 
 MAFilter::MAFilter(int steps, int array_size)
 {
 	this->n_steps = steps;
 	this->array_size = array_size;
-	this->idx = 0;
+	this->idx = 0; // this->idx < this->n_steps
 
-	this->circular_buffer = new float[steps * array_size];
+	this->circular_buffer = (float *)new float[steps * array_size]; // float[steps][array_size]
+	this->sum = (float *)new float[array_size]; // use this array to cache the sum
+	for (int i = 0; i < array_size; i++)
+		this->sum[i] = nanf("");
 }
 
 MAFilter::~MAFilter()
 {
 	delete[] this->circular_buffer;
+	delete[] this->sum;
 }
 
 void MAFilter::filter(float* in_array, float* out_array)
 {
 	int offset = this->idx * this->array_size;
+	// equivalent to:
+	// typedef float (*CIRCULAR_BUFFER_IDX)[this->array_size];
+	// typedef float (*CIRCULAR_BUFFER)[this->n_steps][this->array_size];
+	float *circular_buffer_idx = &this->circular_buffer[offset]; // CIRCULAR_BUFFER_IDX circular_buffer_idx = &((CIRCULAR_BUFFER)this->circular_buffer)[this->idx][0];
+
 	for (int i = 0; i < this->array_size; i++)
 	{
-		// Insert current position
-		this->circular_buffer[offset + i] = in_array[i];
-		out_array[i] = 0;
-
-		// get mean of all steps for this position
-		for (int j = 0; j < this->n_steps; j++)
+		if (isnan(this->sum[i]))
 		{
-			out_array[i] += this->circular_buffer[j * this->array_size + i];
+			// initialize sum
+			this->sum[i] = in_array[i] * this->n_steps;
+			// calculate average
+			out_array[i] = this->sum[i] / this->n_steps;
+			// initialize empty circular_buffer with new value
+			for (int j = 0; j < this->n_steps; j++)
+			{
+				this->circular_buffer[j * this->array_size + i] = in_array[i];
+			}
+		}
+		else
+		{
+			// Recalculate sum by subtracting old value and adding new value
+			this->sum[i] = this->sum[i] - circular_buffer_idx[i] + in_array[i];
+			// calculate average
+			out_array[i] = this->sum[i] / this->n_steps;
+			// Insert current position
+			circular_buffer_idx[i] = in_array[i];
 		}
-
-		out_array[i] /= this->n_steps;
 	}
 
 	this->idx = (this->idx + 1) % this->n_steps;
@@ -56,7 +76,7 @@ void EAFilter::filter(float* in_array, float* out_array)
 {
 	for (int i = 0; i < array_size; i++)
 	{
-		out_array[i] = 0.6 * in_array[i] + 0.4 * this->last_value[i];
+		out_array[i] = (0.6f * in_array[i] + 0.4f * this->last_value[i]);
 		this->last_value[i] = in_array[i];
 	}
 

AITracker/src/filters.h

@@ -16,8 +16,7 @@ class MAFilter : public IFilter
 	int n_steps;
 
 	float *circular_buffer;
-
-
+	float *sum;
 
 public:
 	MAFilter(int steps, int array_size);
@@ -34,8 +33,6 @@ class EAFilter : public IFilter
 	int array_size;
 	float* last_value;
 
-
-
 public:
 	EAFilter(int array_size);
 	~EAFilter();