Add VelociMeter to measure angle velocity and add DisplayInfo to get display information

Change-Id: I9aae596143754c2965995eda1adc0f728ca4a129
Signed-off-by: Tae-Young Chung <ty83.chung@samsung.com>

diff --git a/.gitignore b/.gitignore
index 63e67e0e25d43b3aa9f99d736cf591315386c787..d7510f56281b20860f81bd65264cc5b4cb1738cc 100644 (file)
--- a/.gitignore
+++ b/.gitignore
@@ -1 +1,2 @@
 install_rpm.sh
+test.py

diff --git a/common/include/SingleoCommonTypes.h b/common/include/SingleoCommonTypes.h
index 02480e69a5ef6dfb9f064ceda1242aab52064434..624fa117c72749aa195fedac2049611c940044a8 100644 (file)
--- a/common/include/SingleoCommonTypes.h
+++ b/common/include/SingleoCommonTypes.h
@@ -35,6 +35,12 @@ struct Point {
        unsigned int z {};
 };
 
+struct Pointi {
+       int x {};
+       int y {};
+       int z {};
+};
+
 using VecRect = std::vector<Rect>;
 
 enum class DataType { NONE, FILE, IMAGE, RAW };

diff --git a/services/smart_pointer/CMakeLists.txt b/services/smart_pointer/CMakeLists.txt
index 0a47b729f686f374ff3410103ea4a0cb838bcb58..582a5df91839832f8db765f50dccaa0bf2677959 100644 (file)
--- a/services/smart_pointer/CMakeLists.txt
+++ b/services/smart_pointer/CMakeLists.txt
@@ -9,6 +9,7 @@ SET(SINGLEO_SERVICE_SOURCE_FILES
     smart_pointer/src/PointerFactory.cpp
     smart_pointer/src/TizenPointer.cpp
     smart_pointer/src/DisplayInfo.cpp
+    smart_pointer/src/VelociMeter.cpp
 )
 
 LIST(APPEND SERVICE_LIBRARY_LIST singleo_inference)
\ No newline at end of file

diff --git a/services/smart_pointer/include/DisplayInfo.h b/services/smart_pointer/include/DisplayInfo.h
index eb95bb0dd96eb8abe2205bd1e0e4037d91d91c31..5d289dac7d51f5c6a7c745694113c794ef155830 100644 (file)
--- a/services/smart_pointer/include/DisplayInfo.h
+++ b/services/smart_pointer/include/DisplayInfo.h
@@ -29,15 +29,19 @@ namespace smartpointer
 class DisplayInfo
 {
 private:
-    int _physicalWidth;
-    int _physicalHeight;
+    float _physicalWidth;
+    float _physicalHeight;
     int _resolutionWidth;
     int _resolutionHeight;
 
+    float _pixelWPerInch;
+    float _pixelHPerInch;
+
 public:
     DisplayInfo();
-    std::tuple<int, int> getPhysicalSize();
+    std::tuple<float, float> getPhysicalSize();
     std::tuple<int, int> getResolutionSize();
+    std::tuple<float, float> getPixelPerInch();
 };
 } // smartpointer
 } // services

diff --git a/services/smart_pointer/include/SmartPointer.h b/services/smart_pointer/include/SmartPointer.h
index 264229edf74304153e011fa4a6e8097a738548c8..3c38ca486dc087ac92d09721a36fb687f5d37d38 100644 (file)
--- a/services/smart_pointer/include/SmartPointer.h
+++ b/services/smart_pointer/include/SmartPointer.h
@@ -28,6 +28,7 @@
 #include "SmartPointerDataType.h"
 #include "IPointer.h"
 #include "DisplayInfo.h"
+#include "VelociMeter.h"
 
 namespace singleo
 {
@@ -45,8 +46,10 @@ private:
     PoseVector _head_pose;
        PointerPosition _pointerPosition;
        std::unique_ptr<IPointer> _pointer;
-       Point _cursor;
+       Pointi _cursor;
+       Pointi _resetCursorPosition;
        DisplayInfo _displayInfo;
+       VelociMeter _velocimeter;
        std::map<std::string, PoseAngle> _result_keys = { { "PITCH", PoseAngle::PITCH },
                                                                                                                { "YAW", PoseAngle::YAW },
                                                                                                                { "ROLL", PoseAngle::ROLL } };

diff --git a/services/smart_pointer/include/VelociMeter.h b/services/smart_pointer/include/VelociMeter.h
new file mode 100644 (file)
index 0000000..50424c1
--- /dev/null
+++ b/services/smart_pointer/include/VelociMeter.h
@@ -0,0 +1,45 @@
+/**
+ * Copyright (c) 2024 Samsung Electronics Co., Ltd All Rights Reserved
+ *
+ * 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.
+*/
+
+#ifndef __SMART_POINTER_VELOCIMETER_H__
+#define __SMART_POINTER_VELOCIMETER_H__
+
+#include <queue>
+#include <tuple>
+
+namespace singleo
+{
+namespace services
+{
+namespace smartpointer
+{
+class VelociMeter
+{
+private:
+    float _radiusXmm;
+    float _radiusYmm;
+    int _maxCapacity;
+    std::queue<std::tuple<float, float>> _measuredValue;
+
+public:
+    VelociMeter();
+    ~VelociMeter() = default;
+    std::tuple<float, float> measure(float valueX, float valueY);
+};
+} // smartpointer
+} // services
+} // singleo
+#endif
\ No newline at end of file

diff --git a/services/smart_pointer/src/DisplayInfo.cpp b/services/smart_pointer/src/DisplayInfo.cpp
index 18e445a5d3721ff192c40d68bd2d065821be1979..831993649b251810cad0ffb2210be493b1a439dd 100644 (file)
--- a/services/smart_pointer/src/DisplayInfo.cpp
+++ b/services/smart_pointer/src/DisplayInfo.cpp
@@ -24,15 +24,17 @@ namespace services
 {
 namespace smartpointer
 {
-DisplayInfo::DisplayInfo() : _physicalWidth(510),
-                             _physicalHeight(290),
+DisplayInfo::DisplayInfo() : _physicalWidth(510.f),
+                             _physicalHeight(290.f),
                              _resolutionWidth(1920),
                              _resolutionHeight(1080)
 {
-
+    // TODO: read display information file
+    _pixelWPerInch = _resolutionWidth/ _physicalWidth;
+    _pixelHPerInch = _physicalHeight/ _physicalHeight;
 }
 
-tuple<int, int> DisplayInfo::getPhysicalSize()
+tuple<float, float> DisplayInfo::getPhysicalSize()
 {
     return {_physicalWidth, _physicalHeight};
 }
@@ -41,6 +43,11 @@ tuple<int, int> DisplayInfo::getResolutionSize()
 {
     return {_resolutionWidth, _resolutionHeight};
 }
+
+std::tuple<float, float> DisplayInfo::getPixelPerInch()
+{
+    return {_pixelWPerInch, _pixelHPerInch};
+}
 }
 }
 }
\ No newline at end of file

diff --git a/services/smart_pointer/src/HeadPoseEstimator.cpp b/services/smart_pointer/src/HeadPoseEstimator.cpp
index 59e4c604d3ba4aa4e49a8f060dcda25404955a01..53887b90c9167e2ccf7b4909e91f19920a004e7b 100644 (file)
--- a/services/smart_pointer/src/HeadPoseEstimator.cpp
+++ b/services/smart_pointer/src/HeadPoseEstimator.cpp
@@ -131,7 +131,7 @@ PoseVector HeadPoseEstimator::estimate(const vector<Point> &landmark2d)
     timer.reset();
     cv::solvePnPRansac(_landmarks_3d, _landmarks_2d,
                 _camera_matrix, _camera_dist_coeff,
-                _rotation_vector, _translation_vector, false, 100, 8.0F, 0.99, cv::noArray(), cv::SOLVEPNP_EPNP);
+                _rotation_vector, _translation_vector, false, 100, 3.0F, 0.99, cv::noArray(), cv::SOLVEPNP_EPNP);
     SINGLEO_LOGI("1st solvePnP takes %d ms", timer.check_ms());
 
     timer.reset();

diff --git a/services/smart_pointer/src/SmartPointer.cpp b/services/smart_pointer/src/SmartPointer.cpp
index 0a7f73a321bf475829568f1493c37ba78aabb07f..b27197508d7a1c978f97a78e05033a66c8cafafe 100644 (file)
--- a/services/smart_pointer/src/SmartPointer.cpp
+++ b/services/smart_pointer/src/SmartPointer.cpp
@@ -16,6 +16,7 @@
 
 #include <memory>
 #include <variant>
+#include <algorithm>
 #include "ServiceFactory.h"
 #include "SingleoLog.h"
 #include "SingleoException.h"
@@ -53,8 +54,10 @@ SmartPointer::SmartPointer(InputConfigBase& config)
 
        _pointer = PointerFactory::createTizenPointer();
 
-       _cursor.x = get<0>(_displayInfo.getResolutionSize());
-       _cursor.y = get<1>(_displayInfo.getResolutionSize());
+       _resetCursorPosition.x = get<0>(_displayInfo.getResolutionSize()) >> 1;
+       _resetCursorPosition.y = get<1>(_displayInfo.getResolutionSize()) >> 1;
+       _cursor.x = _resetCursorPosition.x;
+       _cursor.y = _resetCursorPosition.y;
 }
 
 SmartPointer::~SmartPointer()
@@ -122,8 +125,22 @@ void SmartPointer::perform()
                // auto gestureResult = _gesture_recognizer->recognizeGesture(preprocessor.getData());
 
                updateResult(_head_pose);
-               _cursor.x = ++_cursor.x % get<0>(_displayInfo.getResolutionSize());
-               _cursor.y = ++_cursor.y % get<1>(_displayInfo.getResolutionSize());
+
+               auto velocity = _velocimeter.measure(_pointerPosition.pose._rot_vec.y,
+                                                                                               _pointerPosition.pose._rot_vec.x);
+
+               SINGLEO_LOGD("(CurX, CurY): (%4d, %4d) - Angular Velocity: %.4f, %.4f"
+                                                                                                       , _cursor.x, _cursor.y
+                                                                                                       , get<0>(velocity), get<1>(velocity));
+               auto deltaX = get<0>(velocity) * get<0>(_displayInfo.getPixelPerInch());
+               auto deltaY = get<1>(velocity) * get<1>(_displayInfo.getPixelPerInch());
+
+               _cursor.x = static_cast<int>(static_cast<float>(_cursor.x) + deltaX);
+               _cursor.x = min(max(_cursor.x, 0), get<0>(_displayInfo.getResolutionSize()));
+
+               _cursor.y = static_cast<int>(static_cast<float>(_cursor.y) + 3.f * deltaY);
+               _cursor.y = min(max(_cursor.y, 0), get<1>(_displayInfo.getResolutionSize()));
+               SINGLEO_LOGD("(CurX, CurY): (%4d, %4d)", _cursor.x, _cursor.y);
 
                _pointer->sendMouseEvent(_cursor.x, _cursor.y, ActionType::POINTER_MOVE);
 

diff --git a/services/smart_pointer/src/VelociMeter.cpp b/services/smart_pointer/src/VelociMeter.cpp
new file mode 100644 (file)
index 0000000..80d2627
--- /dev/null
+++ b/services/smart_pointer/src/VelociMeter.cpp
@@ -0,0 +1,37 @@
+
+#include "cmath"
+#include "VelociMeter.h"
+
+using namespace std;
+
+namespace singleo
+{
+namespace services
+{
+namespace smartpointer
+{
+VelociMeter::VelociMeter()
+{
+    _maxCapacity = 10;
+    _radiusXmm = 400.f;
+    _radiusYmm = 400.f;
+}
+
+tuple<float, float> VelociMeter::measure(float valueX, float valueY)
+{
+    _measuredValue.push( {valueX, valueY} );
+    if (_measuredValue.size() < _maxCapacity)
+        return { 0.0f, 0.0f };
+
+    auto diffX = fabs(get<0>(_measuredValue.back()) - get<0>(_measuredValue.front()));
+    auto diffY = fabs(get<1>(_measuredValue.back()) - get<1>(_measuredValue.front()));
+
+    auto velocityX = diffX < 0.02f ? 0.0f : (get<0>(_measuredValue.back()) - get<0>(_measuredValue.front())) / static_cast<float>(_maxCapacity);
+    auto velocityY = diffY < 0.02f ? 0.0f : (get<1>(_measuredValue.back()) - get<1>(_measuredValue.front())) / static_cast<float>(_maxCapacity);
+    _measuredValue.pop();
+
+    return { -velocityX * _radiusXmm, velocityY * _radiusYmm };
+}
+} // smartpointer
+} // services
+} // singleo
\ No newline at end of file