Replace 1-cos(t) by 2*sin^2(t/2), which si more numerically stable for t ~ 0
parent
eb99d4c974
commit
3dbb69dcbd
|
@ -47,7 +47,7 @@ SO3 SO3::AxisAngle(const Vector3& axis, double theta) {
|
|||
// get components of axis \omega, where is a unit vector
|
||||
const double& wx = axis.x(), wy = axis.y(), wz = axis.z();
|
||||
|
||||
const double costheta = cos(theta), sintheta = sin(theta), c_1 = 1 - costheta;
|
||||
const double costheta = cos(theta), sintheta = sin(theta), s2 = sin(theta/2.0), c_1 = 2.0*s2*s2;
|
||||
const double wx_sintheta = wx * sintheta, wy_sintheta = wy * sintheta,
|
||||
wz_sintheta = wz * sintheta;
|
||||
|
||||
|
@ -130,7 +130,6 @@ Vector3 SO3::Logmap(const SO3& R, ChartJacobian H) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
||||
using std::cos;
|
||||
using std::sin;
|
||||
|
||||
double theta2 = omega.dot(omega);
|
||||
|
@ -155,8 +154,9 @@ Matrix3 SO3::ExpmapDerivative(const Vector3& omega) {
|
|||
*/
|
||||
// element of Lie algebra so(3): X = omega^, normalized by normx
|
||||
const Matrix3 Y = skewSymmetric(omega) / theta;
|
||||
return I_3x3 - ((1 - cos(theta)) / (theta)) * Y
|
||||
+ (1 - sin(theta) / theta) * Y * Y; // right Jacobian
|
||||
const double s2 = sin(theta / 2.0);
|
||||
return I_3x3 - (2.0 * s2 * s2 / theta) * Y +
|
||||
(1.0 - sin(theta) / theta) * Y * Y; // right Jacobian
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue