Merge pull request #17 from stereolabs/r4.0.4

push to sdk 4.0.4

README.md

@@ -17,7 +17,7 @@ All the information is available [here](zed-livelink-fusion/README.md)
 
 This unreal project shows you how to use the body tracking data sent through live link to animate a 3D model.
 
-You can find more information on the Stereolabs [documentation](https://www.stereolabs.com/docs/livelink/)
+You can find more information on the Stereolabs [documentation](https://www.stereolabs.com/docs/livelink/livelink-ue5/)
 
 
 ## Connect to a livelink Source

UnrealProject/Source/ZEDUnrealLiveLink/LiveLinkOrientationsRemapAsset.cpp

@@ -6,7 +6,7 @@
 
 void ULiveLinkOrientationsRemapAsset::SetHeightOffset(float Offset)
 {
-	HeightOffset = Offset;
+	ManualHeightOffset = Offset;
 }
 
 void ULiveLinkOrientationsRemapAsset::EnableBoneScaling(bool BoneScaling)
@@ -19,16 +19,56 @@ void ULiveLinkOrientationsRemapAsset::EnableStickAvatarOnFloor(bool bEnableStick
 	bStickAvatarOnFloor = bEnableStickAvatarOnFloor;
 }
 
-FCompactPoseBoneIndex ULiveLinkOrientationsRemapAsset::GetCPIndex(int32 idx, FCompactPose& OutPose, TArray<FName, TMemStackAllocator<>> TransformedBoneNames){
-    FName BoneName = TransformedBoneNames[idx];
-    const int32 MeshIndex = OutPose.GetBoneContainer().GetPoseBoneIndexForBoneName(BoneName);
-    if (MeshIndex != INDEX_NONE)
-    {
-        FCompactPoseBoneIndex CPIndex = OutPose.GetBoneContainer().MakeCompactPoseIndex(
-            FMeshPoseBoneIndex(MeshIndex));
-        return CPIndex;
-    }
-    return (FCompactPoseBoneIndex) INDEX_NONE;
+FCompactPoseBoneIndex ULiveLinkOrientationsRemapAsset::GetCPIndex(int32 idx, FCompactPose& OutPose, TArray<FName, TMemStackAllocator<>> TransformedBoneNames) {
+	FName BoneName = TransformedBoneNames[idx];
+	const int32 MeshIndex = OutPose.GetBoneContainer().GetPoseBoneIndexForBoneName(BoneName);
+	if (MeshIndex != INDEX_NONE)
+	{
+		FCompactPoseBoneIndex CPIndex = OutPose.GetBoneContainer().MakeCompactPoseIndex(
+			FMeshPoseBoneIndex(MeshIndex));
+		return CPIndex;
+	}
+	return (FCompactPoseBoneIndex)INDEX_NONE;
+}
+
+float ULiveLinkOrientationsRemapAsset::ComputeRootTranslationFactor(FCompactPose& OutPose, TArray<FName, TMemStackAllocator<>> TransformedBoneNames, const FLiveLinkAnimationFrameData* InFrameData) 
+{
+	float avatarTotalTranslation = 0.f;
+	float SDKTotalTranslation = 0.f;
+
+	if (NbKeypoints <= 34)
+	{
+		for (int32 i = 22; i < 24; i++)
+		{
+			FTransform BoneTransform = InFrameData->Transforms[i];
+			FCompactPoseBoneIndex CPIndex = GetCPIndex(i, OutPose, TransformedBoneNames);
+			if (CPIndex != INDEX_NONE)
+			{
+				avatarTotalTranslation += OutPose[CPIndex].GetTranslation().Size();
+				SDKTotalTranslation += BoneTransform.GetTranslation().Size();
+			}
+		}
+	}
+	else
+	{
+		for (int32 i = 19; i < 23; i++)
+		{
+			FTransform BoneTransform = InFrameData->Transforms[i];
+			FCompactPoseBoneIndex CPIndex = GetCPIndex(i, OutPose, TransformedBoneNames);
+			if (CPIndex != INDEX_NONE)
+			{
+				avatarTotalTranslation += OutPose[CPIndex].GetTranslation().Size();
+				SDKTotalTranslation += BoneTransform.GetTranslation().Size();
+			}
+		}
+	}
+
+	float factor = avatarTotalTranslation / SDKTotalTranslation;
+	float scale = 1.f;
+	FCompactPoseBoneIndex CPIndexRoot = GetCPIndex(0, OutPose, TransformedBoneNames);
+	if (CPIndexRoot != INDEX_NONE)
+		scale = OutPose[CPIndexRoot].GetScale3D().Z;
+	return FMath::Abs(scale * factor);
 }
 
 void ULiveLinkOrientationsRemapAsset::putInRefPose(FCompactPose& OutPose, TArray<FName, TMemStackAllocator<>> TransformedBoneNames){
@@ -102,6 +142,12 @@ void ULiveLinkOrientationsRemapAsset::BuildPoseFromZEDAnimationData(float DeltaT
 			Keypoints = Keypoints34;
 			ParentsIdx = parents34Idx;
 		}
+		else
+		{
+			NbKeypoints = 38;
+			Keypoints = Keypoints38;
+			ParentsIdx = parents38Idx;
+		}
 	}
 
     // Find remapped bone names and cache them for fast subsequent retrieval.
@@ -158,47 +204,38 @@ void ULiveLinkOrientationsRemapAsset::BuildPoseFromZEDAnimationData(float DeltaT
 			// Compute the distance between one foot and the ground (the first static object found by the ray cast).
 			if (RaycastLeftFoot)
 			{
-				LeftFootFloorDistance = (LeftFootPosition + FVector(0, 0, FeetOffset) - HitLeftFoot.ImpactPoint).Z;
+				LeftFootFloorDistance = (LeftFootPosition + FVector(0, 0, AutomaticHeightOffset) - HitLeftFoot.ImpactPoint).Z;
 			}
 
 			if (RaycastRightFoot)
 			{
-				RightFootFloorDistance = (RightFootPosition + FVector(0, 0, FeetOffset) - HitRightFoot.ImpactPoint).Z;
+				RightFootFloorDistance = (RightFootPosition + FVector(0, 0, AutomaticHeightOffset) - HitRightFoot.ImpactPoint).Z;
 			}
 
-			float MinFootFloorDistance = 0;
-
-			// If both feet are under the ground, use the max value instead of the min value.
-			if (RightFootFloorDistance < 0 && LeftFootFloorDistance < 0) {
-
-				MinFootFloorDistance = -1.0f * fmax(abs(RightFootFloorDistance), abs(LeftFootFloorDistance));
-				FeetOffset = FeetOffsetAlpha* MinFootFloorDistance + (1 - FeetOffsetAlpha) * FeetOffset;
-
+			if (abs(fminf(LeftFootFloorDistance, RightFootFloorDistance)) <= DistanceToFloorThreshold)
+			{
+				// Reset counter 
+				DurationOffsetError = 0;
 			}
-			else if (RightFootFloorDistance > 0 && LeftFootFloorDistance > 0)
+			else
 			{
-				MinFootFloorDistance = fmin(abs(RightFootFloorDistance), abs(LeftFootFloorDistance));
+				auto NowTS_ms = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+				DurationOffsetError += (NowTS_ms - PreviousTS_ms) / 1000.0f;
+				PreviousTS_ms = NowTS_ms;
 
-				// The feet offset is added in the buffer of size "FeetOffsetBufferSize". If the buffer is already full, remove the oldest value (the first of the deque)
-				if (FeetOffsetBuffer.size() == FeetOffsetBufferSize)
+				if (DurationOffsetError > DurationOffsetErrorThreshold)
 				{
-					FeetOffsetBuffer.pop_front();
-				}
-				FeetOffsetBuffer.push_back(MinFootFloorDistance);
+					AutomaticHeightOffset = fmin(LeftFootFloorDistance, RightFootFloorDistance);
+					DurationOffsetError = 0;
 
-				// The feet offset is the min element of this deque (of size FeetOffsetBufferSize).
-				FeetOffset = *std::min_element(FeetOffsetBuffer.begin(), FeetOffsetBuffer.end());
-			}
-			else
-			{
-				MinFootFloorDistance = fmin(RightFootFloorDistance, LeftFootFloorDistance);
-				FeetOffset = FeetOffsetAlpha * MinFootFloorDistance + (1 - FeetOffsetAlpha) * FeetOffset;
+					//UE_LOG(LogTemp, Warning, TEXT("Recomputing offset ... %f"), AutomaticHeightOffset);
+				}
 			}
 		}
 	}
 	else
 	{
-		FeetOffset = 0;
+		AutomaticHeightOffset = 0;
 	}
 
 	TArray<FName> TargetBoneNames;
@@ -260,13 +297,16 @@ void ULiveLinkOrientationsRemapAsset::BuildPoseFromZEDAnimationData(float DeltaT
                 // Only use position + rotation data for root. For all other bones, set rotation only.
                 if (BoneName == BoneNameMap[GetTargetRootName()])
                 {
+					float rootScaleFactor = ComputeRootTranslationFactor(OutPose, TransformedBoneNames, InFrameData);
+
                     FVector RootPosition = BoneTransform.GetTranslation();
                     FCompactPoseBoneIndex leftUpLegIndex = GetCPIndex(*Keypoints.FindKey(FName("LEFT_HIP")), OutPose, TransformedBoneNames);
                     float HipOffset = FMath::Abs(OutPose[leftUpLegIndex].GetTranslation().Z) * OutPose[CPIndexRoot].GetScale3D().Z;
 
                     RootPosition.Z += HipOffset; // The position of the root in UE and in the SDK are slightly different. This offset compensates it.
-					RootPosition.Z += HeightOffset;
-					RootPosition.Z -= FeetOffset;
+					RootPosition.Z += ManualHeightOffset;
+					RootPosition.Z -= AutomaticHeightOffset;
+					//RootPosition.Z *= rootScaleFactor;
 
 					Translation = RootPosition;
 

UnrealProject/Source/ZEDUnrealLiveLink/LiveLinkOrientationsRemapAsset.h

@@ -6,6 +6,7 @@
 #include "Containers/CircularQueue.h"
 #include <deque>
 #include <algorithm>
+#include <chrono> 
 
 #include "LiveLinkOrientationsRemapAsset.generated.h"
 
@@ -20,7 +21,7 @@ class ZEDUNREALLIVELINK_API ULiveLinkOrientationsRemapAsset : public ULiveLinkRe
     void propagateRestPoseRotations(int32 parentIdx, FCompactPose& OutPose, TArray<FName, TMemStackAllocator<>> TransformedBoneNames, TArray<int32> SourceBoneParents, FQuat restPoseRot, bool inverse);
     void putInRefPose(FCompactPose& OutPose, TArray<FName, TMemStackAllocator<>> TransformedBoneNames);
     FCompactPoseBoneIndex GetCPIndex(int32 idx, FCompactPose& OutPose, TArray<FName, TMemStackAllocator<>> TransformedBoneNames);
-
+	float ComputeRootTranslationFactor(FCompactPose& OutPose, TArray<FName, TMemStackAllocator<>> TransformedBoneNames, const FLiveLinkAnimationFrameData* InFrameData);
 public:
     void BuildPoseFromZEDAnimationData(float DeltaTime, const FLiveLinkSkeletonStaticData* InSkeletonData,
         const FLiveLinkAnimationFrameData* InFrameData,
@@ -50,12 +51,17 @@ class ZEDUNREALLIVELINK_API ULiveLinkOrientationsRemapAsset : public ULiveLinkRe
         // factor used to computer foot offset over time.
         float BoneScaleAlpha = 0.2f;
 
-		int FeetOffsetBufferSize = 120;
-		std::deque<float> FeetOffsetBuffer;
-		float FeetOffsetAlpha = 1.0f;
+		float DurationOffsetErrorThreshold = 3.0f;
+		float DurationOffsetError = 0.0f;
+		long long PreviousTS_ms = 0;
+
+		float DistanceToFloorThreshold = 3.f;
+
+		float AutomaticHeightOffset = 0;
+		float ManualHeightOffset = 0;
 
-		float FeetOffset = 0;
-		float HeightOffset = 0;
+		float LeftAnkleToHeelOffset = 0;
+		float RightAnkleToHeelOffset = 0;
 
 		bool bStickAvatarOnFloor = true;
 

zed-livelink-fusion/Release/linux/ZEDFusionLiveLinkConfig.json

@@ -10,7 +10,7 @@
       "detection_model": "HUMAN_BODY_ACCURATE",
       "body_format": "BODY_38",
       "body_selection": "FULL",
-      "confidence": 40,
+      "confidence": 50,
       "max_range": -1,
       "minimum_keypoints_threshold" : -1
     },

zed-livelink-fusion/Release/win64/ZEDFusionLiveLinkConfig.json

@@ -1,7 +1,7 @@
 {
     "InitParameters":
     {
-      "resolution": "HD1080",
+      "resolution": "HD720",
       "fps": 30,
       "depth_mode": "ULTRA"
     },
@@ -10,7 +10,7 @@
       "detection_model": "HUMAN_BODY_ACCURATE",
       "body_format": "BODY_38",
       "body_selection": "FULL",
-      "confidence": 40,
+      "confidence": 50,
       "max_range": -1,
       "minimum_keypoints_threshold" : -1
     },

zed-livelink-fusion/Source/Private/SenderRunner.cpp

@@ -90,6 +90,8 @@ void SenderRunner::work() {
 	SL_BodyTrackingRuntimeParameters bt_rt_params;
 	bt_rt_params.detection_confidence_threshold = 40;
 	bt_rt_params.detection_confidence_threshold = confidence_threshold;
+	bt_rt_params.skeleton_smoothing = 0.0f;
+
     while (running) {
 		auto err = zed.Grab(rt_params);
         if (err == sl::ERROR_CODE::SUCCESS) {

zed-livelink-fusion/Source/Private/main.cpp

@@ -202,7 +202,7 @@ int main(int argc, char **argv)
 	SL_Bodies bodies;
 	SL_BodyTrackingFusionRuntimeParameters BodyTrackingRuntimeParams;
 	BodyTrackingRuntimeParams.skeleton_minimum_allowed_keypoints = zed_fusion_config.skeleton_minimm_allowed_keypoints;
-	BodyTrackingRuntimeParams.skeleton_smoothing = zed_fusion_config.skeleton_smoothing;
+	BodyTrackingRuntimeParams.skeleton_smoothing = zed_fusion_config.fusion_skeleton_smoothing;
 	BodyTrackingRuntimeParams.skeleton_minimum_allowed_camera = zed_fusion_config.skeleton_minimum_allowed_camera;
 
 	SetCtrlHandler();

zed-livelink-fusion/Source/Public/Utils/Util.h

@@ -149,7 +149,7 @@ struct ZEDFusionConfig {
 	bool verbose = false;
 	int skeleton_minimm_allowed_keypoints = -1;
 	int skeleton_minimum_allowed_camera = -1;
-	float skeleton_smoothing = 0;
+	float fusion_skeleton_smoothing = 0;
 
 	sl::RESOLUTION sender_resolution;
 	int sender_fps;
@@ -206,8 +206,8 @@ struct ZEDFusionConfig {
 		skeleton_minimum_allowed_camera = injson["BodyTrackingFusionParameters"]["skeleton_minimum_allowed_camera"];
 		std::cout << "skeleton_minimum_allowed_camera : " << skeleton_minimum_allowed_camera << std::endl;
 
-		skeleton_smoothing = injson["BodyTrackingFusionParameters"]["skeleton_smoothing"];
-		std::cout << "skeleton_smoothing : " << skeleton_smoothing << std::endl;
+		fusion_skeleton_smoothing = injson["BodyTrackingFusionParameters"]["skeleton_smoothing"];
+		std::cout << "skeleton_smoothing : " << fusion_skeleton_smoothing << std::endl;
 	}
 };
 

zed-livelink-fusion/Source/Public/ZEDStructs.h

@@ -373,6 +373,7 @@ struct SL_BodyTrackingRuntimeParameters
 {
 	float detection_confidence_threshold = 20;
 	int minimum_keypoints_threshold = -1;
+	float skeleton_smoothing = 0.2f;
 };
 
 struct SL_Pose {
@@ -403,6 +404,7 @@ struct SL_PositionalTrackingParameters
 	float depth_min_range = -1.0f;
 	bool set_gravity_as_origin = true;
 	sl::String area_file_path = "";
+	sl::POSITIONAL_TRACKING_MODE mode = sl::POSITIONAL_TRACKING_MODE::STANDARD;
 };
 
 /*

zed-livelink-fusion/ZEDFusionLiveLinkConfig.json

@@ -10,7 +10,7 @@
       "detection_model": "HUMAN_BODY_ACCURATE",
       "body_format": "BODY_38",
       "body_selection": "FULL",
-      "confidence": 40,
+      "confidence": 50,
       "max_range": -1,
       "minimum_keypoints_threshold" : -1
     },

zed-livelink-mono/Releases/linux/ZEDLiveLinkConfig.json

@@ -3,8 +3,8 @@
   {
     "input": "USB_ID",
     "input_path": 0,
-    "resolution": "HD1080",
-    "fps": 30,
+    "resolution": "HD720",
+    "fps": 60,
     "depth_mode": "ULTRA"
   },
   "PositionalTrackingParameters":
@@ -18,8 +18,9 @@
     "detection_model": "HUMAN_BODY_ACCURATE",
     "body_format": "BODY_38",
     "body_selection": "FULL",
-    "confidence": 40,
+    "confidence": 50,
     "max_range": -1,
-    "minimum_keypoints_threshold" : -1
+    "minimum_keypoints_threshold" : -1,
+	"skeleton_smoothing": 0.2
   }
 }

zed-livelink-mono/Releases/win64/ZEDLiveLinkConfig.json

@@ -3,8 +3,8 @@
   {
     "input": "USB_ID",
     "input_path": 0,
-    "resolution": "HD1080",
-    "fps": 30,
+    "resolution": "HD720",
+    "fps": 60,
     "depth_mode": "ULTRA"
   },
   "PositionalTrackingParameters":
@@ -18,8 +18,9 @@
     "detection_model": "HUMAN_BODY_ACCURATE",
     "body_format": "BODY_38",
     "body_selection": "FULL",
-    "confidence": 40,
+    "confidence": 50,
     "max_range": -1,
-    "minimum_keypoints_threshold" : -1
+    "minimum_keypoints_threshold" : -1,
+	"skeleton_smoothing": 0.2
   }
 }

zed-livelink-mono/Source/Private/main.cpp

@@ -68,7 +68,7 @@ void UpdateAnimationFrameData(StreamedSkeletonData StreamedSkeleton);
 ERROR_CODE PopulateSkeletonsData(ZEDCamera* cam, ZEDConfig& config);
 ERROR_CODE InitCamera(int argc, char **argv, ZEDConfig& config);
 FTransform BuildUETransformFromZEDTransform(SL_PoseData& pose);
-StreamedSkeletonData BuildSkeletonsTransformFromZEDObjects(SL_ObjectData& objectData, double timestamp);
+StreamedSkeletonData BuildSkeletonsTransformFromZEDObjects(SL_BodyData& objectData, double timestamp);
 
 bool IsConnected = false;
 bool enableBodyTracking = false;
@@ -290,7 +290,7 @@ StreamedSkeletonData BuildSkeletonsTransformFromZEDObjects(SL_BodyData& bodyData
 	}
 
 	FVector position = FVector(bodyPosition.x, bodyPosition.y, bodyPosition.z);
-	FQuat global_rotation = FQuat(bodyRotation.x, bodyRotation.y, bodyRotation.z, bodyRotation.w); // 180deg rotation on Z Axis.
+	FQuat global_rotation = FQuat(bodyRotation.x, bodyRotation.y, bodyRotation.z, bodyRotation.w);
 
 	if (position.ContainsNaN())
 	{
@@ -348,6 +348,7 @@ ERROR_CODE PopulateSkeletonsData(ZEDCamera* zed, ZEDConfig& config)
 	SL_BodyTrackingRuntimeParameters body_tracking_rt_params;
 	body_tracking_rt_params.detection_confidence_threshold = config.detection_confidence;
 	body_tracking_rt_params.minimum_keypoints_threshold = config.minimum_keypoints_threshold;
+	body_tracking_rt_params.skeleton_smoothing = config.skeleton_smoothing;
 	SL_Bodies bodies;
 	e = zed->RetrieveBodies(body_tracking_rt_params, bodies, 0);
 	if (e != ERROR_CODE::SUCCESS)