FishPose: Add unit conversion

Src/Model/BioTrackerTrackingAlgorithm.cpp

@@ -66,7 +66,7 @@ void BioTrackerTrackingAlgorithm::request_shared_memory() {
 }
 
 void BioTrackerTrackingAlgorithm::receiveAreaDescriptorUpdate(IModelAreaDescriptor *areaDescr) {
-	_AreaInfo = areaDescr;
+	_areaInfo = areaDescr;
 }
 
 BioTrackerTrackingAlgorithm::~BioTrackerTrackingAlgorithm()
@@ -119,19 +119,21 @@ void BioTrackerTrackingAlgorithm::doTracking(std::shared_ptr<cv::Mat> p_image, u
     auto res = _sock.recv(_zmq_msg, zmq::recv_flags::none);
     auto msg = json::parse(_zmq_msg.to_string_view());
 
-    std::vector<FishPose> poses;
+    std::vector<std::tuple<int, FishPose>> poses;
     for (auto pos : msg["data"]) {
-        auto pose = FishPose(
-                cv::Point2f(1.0, 1.0), // FIXME: Calculate cm
+        auto pose = std::make_tuple(
+                pos["id"].get<int>(),
+                FishPose(
+                    _areaInfo,
                     cv::Point2f(pos["x"].get<int>(), pos["y"].get<int>()),
                     pos["orientation"].get<float>(),
-                pos["orientation"].get<float>() * (180.0 / CV_PI),
-                1.0, // FIXME: width
-                1.0, // FIXME: height
-                1.0 // FIXME: score
-		);
+                    1.0)); // FIXME: score
         poses.push_back(pose);
     }
+    std::sort(poses.begin(), poses.end(),
+            [](std::tuple<int, FishPose> a, std::tuple<int, FishPose> b) {
+                return std::get<0>(a) < std::get<0>(b);
+            });
 
 	//Insert new poses into data structure
 	int trajNumber = 0;
@@ -140,7 +142,7 @@ void BioTrackerTrackingAlgorithm::doTracking(std::shared_ptr<cv::Mat> p_image, u
 		if (t && t->getValid() && !t->getFixed()) {
 			BST::TrackedElement *e = new BST::TrackedElement(t, "n.a.", t->getId());
 
-			e->setFishPose(poses[trajNumber]);
+			e->setFishPose(std::get<1>(poses[trajNumber]));
 			e->setTime(start);
 			t->add(e, framenumber);
 			trajNumber++;

Src/Model/BioTrackerTrackingAlgorithm.h

@@ -58,6 +58,7 @@ private:
     zmq::socket_t _sock;
     zmq::message_t _zmq_msg;
     float *_shm_img;
+	IModelAreaDescriptor* _areaInfo;
 };
 
 #endif // BIOTRACKERTRACKINGALGORITHM_H

Src/Model/TrackedComponents/pose/FishPose.cpp

@@ -13,6 +13,18 @@ FishPose::FishPose(cv::Point2f pos_cm , cv::Point pos_px, float rad, float deg,
 	time(&_timev);
 }
 
+FishPose::FishPose(IModelAreaDescriptor *_areaInfo, cv::Point pos_px, float rad, float score) :
+	_position_px(pos_px),
+	_radAngle(rad),
+	_width(1.0),
+	_height(1.0),
+	_score(score)
+{
+	assert(_degAngle >= -360.0f && _degAngle <= 360.0f);
+	time(&_timev);
+    _position_cm = _areaInfo->pxToCm(_position_px);
+    _degAngle = _radAngle * (180.0 / CV_PI);
+}
 
 std::string FishPose::toString(bool rectified)
 {

Src/Model/TrackedComponents/pose/FishPose.h

@@ -2,6 +2,7 @@
 
 #include <time.h>
 #include <opencv2/opencv.hpp>
+#include "Interfaces/IModel/IModelAreaDescriptor.h"
 
 class FishPose
 {
@@ -11,6 +12,11 @@ public:
 	 */
 	FishPose(cv::Point2f pos_cm = cv::Point2f(-1,-1), cv::Point pos_px = cv::Point(-1,-1), float rad = 0, float deg = 0, float width = 0, float height = 0, float score = 0.0);
 
+	/**
+	 * Simplified constructor using unit conversion.
+	 */
+    FishPose(IModelAreaDescriptor *_areaInfo, cv::Point pos_px, float rad, float score);
+
 	/**
 	* Copy constructor.
 	*/
@@ -114,6 +120,8 @@ public:
 	std::string toString(bool rectified = false);
 
 private:
+	IModelAreaDescriptor* _areaDescr;
+
 	cv::Point2f _position_cm;
 	cv::Point _position_px;
 	float _radAngle;