From f618466c25d43f3bae9e40920273bf77de1e1149 Mon Sep 17 00:00:00 2001 From: leochanj105 Date: Mon, 19 Oct 2020 23:09:30 -0400 Subject: initial sd-vbs initial sd-vbs add sd-vbs sd-vbs --- SD-VBS/benchmarks/localization/src/matlab/eul2quat.m | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 SD-VBS/benchmarks/localization/src/matlab/eul2quat.m (limited to 'SD-VBS/benchmarks/localization/src/matlab/eul2quat.m') diff --git a/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m new file mode 100644 index 0000000..57001a8 --- /dev/null +++ b/SD-VBS/benchmarks/localization/src/matlab/eul2quat.m @@ -0,0 +1,9 @@ +function retQuat=eul2quat(eulAngle) +x=eulAngle(:,1); +y=eulAngle(:,2); +z=eulAngle(:,3); +retQuat=[cos(x/2).*cos(y/2).*cos(z/2)+sin(x/2).*sin(y/2).*sin(z/2) ... + sin(x/2).*cos(y/2).*cos(z/2)-cos(x/2).*sin(y/2).*sin(z/2) ... + cos(x/2).*sin(y/2).*cos(z/2)+sin(x/2).*cos(y/2).*sin(z/2) ... + cos(x/2).*cos(y/2).*sin(z/2)-sin(x/2).*sin(y/2).*cos(z/2)]; + -- cgit v1.2.2