Closed swiz23 closed 5 years ago
Hi Solomon,
I double checked all the versions and they are the same.
The function MatrixLog3
outputs the [w]*theta instead of [w]. You can check the function introduction and the last sentence above the algorithm (a)(b)(c) in Chapter 3, pg. 87 of Modern Robotics.
Oh, that makes sense! I didn't realize it was supposed to be outputting [w]*theta. I believe there might still be an issue though with the CPP version, but for a different reason. In the MatrixLog6 function, there is a line of code (line 256) that checks to see if omgmat is approximately equal to the zero matrix. The function used is 'isApprox'. Unfortunately, according the Eigen documentation here, this function will always return false if the matrix being compared to the zero matrix is not zero itself. My proposed fix would be just to do, 'if (NearZero(omgmat.norm()))' for that line. Otherwise, the program will jump to the 'else' statement (line 260). As a result, theta will equal '0', and the resulting matrix 'm_ret' will be full of 'nan'. I've tested this fix in my code, and it seems to work.
If there is an issue with the CPP version, it would be great to report that issue to the maintainers of that repo.
Issue is not with the Python version but seems to be with the cpp version. I will report this issue there.
In Chapter 3, pg. 88 of Modern Robotics, the formula (equation 3.61) given to calculate the skew-symmetric matrix [w] is (1/2sin(theta)) * (R - R^T). However, the equation coded for this function (line 177 in the Python version) is
theta / 2.0 / np.sin(theta) * (R - np.array(R).T).
Shouldn't this be (1.0/2.0np.sin(theta)) (R - np.array(R).T)?
I've noticed this issue also with the C++ and Julia version. However, the Matlab and Mathematica versions have this correct.