Debug MatrixLog3 and MatrixLog6 for MATLAB and Mathematica

packages/Mathematica/ModernRobotics.m

@@ -53,7 +53,7 @@ Example:
 		invR = RotInv[{{0,0,1},{1,0,0},{0,1,0}}]
 	Output: 
 		{{0,1,0},{0,0,1},{1,0,0}}"
-	
+
 
 VecToso3::usage=
 "VecToso3[omg]:
@@ -300,7 +300,7 @@ Returns the corresponding se(3) representation of exponential coordinates.
 
 Example:
 	Input: 
-		se3mat = MatrixLog6[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}]
+		se3mat = MatrixLog6[{{1,0,0,0},{0,0,-1,0},{0,1,0,3},{0,0,0,1}}]//N
 	Output: 
 		{{0,0,0,0},{0,0,-1.5708,2.35619},{0,1.5708,0,2.35619},{0,0,0,0}}"
 
@@ -1289,10 +1289,11 @@ MatrixExp3[so3mat_]:=Module[
 
 MatrixLog3[R_]:=Module[
 	{acosinput,theta,omgmat},
+	acosinput=(Tr[R]-1)/2;
 	Which[
-		NearZero[Norm[R-IdentityMatrix[3]]],
+		acosinput>=1,
 		Return[ConstantArray[0,{3,3}]],
-		NearZero[Tr[R]+1],
+		acosinput<=-1,
 		Which[
 			Not[NearZero[R[[3,3]]+1]],
 			Return[VecToso3[
@@ -1311,11 +1312,9 @@ MatrixLog3[R_]:=Module[
 			]]
 		],
 		True,
-		acosinput=(Tr[R]-1)/2;
-		Which[acosinput>1,acosinput=1,acosinput<-1,acosinput=-1];
 		theta=ArcCos[acosinput];
 		omgmat=(R-R\[Transpose])/(2*Sin[theta]);
-		Return[theta*omgmat]
+		Return[theta*omgmat]					
 	]
 ]
 
@@ -1408,20 +1407,18 @@ MatrixExp6[se3mat_]:=Module[
 MatrixLog6[T_]:=Module[
 	{R,p,acosinput,theta,omgmat},
 	{R,p}=TransToRp[T];
+	omgmat=MatrixLog3[R];
 	If[
-		NearZero[Norm[R-IdentityMatrix[3]]],
+		omgmat==ConstantArray[0,{3,3}],
 		ArrayFlatten[
 			{{ConstantArray[0,{3,3}],{T[[1;;3,4]]}\[Transpose]},{0,0}}
 		],
-		acosinput=(Tr[R]-1)/2;
+		theta=ArcCos[(Tr[R]-1)/2];			
-		Which[acosinput>1,acosinput=1,acosinput<-1,acosinput=-1];
-		theta=ArcCos[acosinput];
-		omgmat=MatrixLog3[R];
 		Return[ArrayFlatten[{
 			{omgmat,(IdentityMatrix[3]-omgmat/2+
 					 (1/theta-Cot[theta/2]/2)*omgmat.omgmat/theta).p},
 			{0,0}
-		}]]
+		}]]	
 	]
 ]
 

packages/Matlab/mr/MatrixLog3.m

@@ -15,25 +15,23 @@ function so3mat = MatrixLog3(R)
 %    1.2092         0   -1.2092
 %   -1.2092    1.2092         0
 
-if NearZero(norm(R - eye(3)))
+acosinput = (trace(R) - 1) / 2;
-	so3mat = zeros(3);
+if acosinput >= 1
-elseif NearZero(trace(R) + 1)
+    so3mat = zeros(3);
+elseif acosinput <= -1
     if ~NearZero(1 + R(3, 3))
-        omg = (1 / sqrt(2 * (1 + R(3, 3)))) * [R(1, 3); R(2, 3); 1 + R(3, 3)];
+        omg = (1 / sqrt(2 * (1 + R(3, 3)))) ...
+              * [R(1, 3); R(2, 3); 1 + R(3, 3)];
     elseif ~NearZero(1 + R(2, 2))
-        omg = (1 / sqrt(2 * (1 + R(2, 2)))) * [R(1, 2); 1 + R(2, 2); R(3, 2)];
+        omg = (1 / sqrt(2 * (1 + R(2, 2)))) ...
+              * [R(1, 2); 1 + R(2, 2); R(3, 2)];
     else
-        omg = (1 / sqrt(2 * (1 + R(1, 1)))) * [1 + R(1, 1); R(2, 1); R(3, 1)];
+        omg = (1 / sqrt(2 * (1 + R(1, 1)))) ...
+              * [1 + R(1, 1); R(2, 1); R(3, 1)];
     end
     so3mat = VecToso3(pi * omg);
 else
-    acosinput = (trace(R) - 1) / 2;
+	theta = acos(acosinput);
-    if acosinput > 1
-        acosinput = 1;
-    elseif acosinput < -1
-        acosinput = -1;
-    end
-    theta = acos(acosinput);
     so3mat = theta * (1 / (2 * sin(theta))) * (R - R');
 end
 end

packages/Matlab/mr/MatrixLog6.m

@@ -17,20 +17,14 @@ function expmat = MatrixLog6(T)
 %         0         0         0         0
 
 [R, p] = TransToRp(T);
-if NearZero(norm(R - eye(3)))
+omgmat = MatrixLog3(R);
+if isequal(omgmat, zeros(3))
     expmat = [zeros(3), T(1: 3, 4); 0, 0, 0, 0];
 else
-    acosinput = (trace(R) - 1) / 2;
+    theta = acos((trace(R) - 1) / 2);
-    if acosinput > 1
+    expmat = [omgmat, (eye(3) - omgmat / 2 ...
-        acosinput = 1;
+                      + (1 / theta - cot(theta / 2) / 2) ...
-    elseif acosinput < -1
+                        * omgmat * omgmat / theta) * p;
-        acosinput = -1;
+              0, 0, 0, 0];    
-    end
-    theta = acos(acosinput);
-    omgmat = MatrixLog3(R);
-    expmat = [ omgmat, (eye(3) - omgmat / 2 ...
-                        + (1 / theta - cot(theta / 2) / 2) ...
-                          * omgmat * omgmat / theta) * p;
-              0, 0, 0, 0];
 end
 end