12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Only keep essentials

release/4.3a0
Fan Jiang 2021-12-03 15:21:01 -05:00
parent 27579e4f34
commit 578b5e6ec5
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/geometry/Quaternion.h
View File

@ -82,7 +82,7 @@ struct traits<QUATERNION_TYPE> {
using std::sin; using std::sin;
if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>()); if (H) *H = SO3::ExpmapDerivative(omega.template cast<double>());
_Scalar theta2 = omega.dot(omega); _Scalar theta2 = omega.dot(omega);
if (theta2 > 1e-8) { if (theta2 > std::numeric_limits<_Scalar>::epsilon()) {
_Scalar theta = std::sqrt(theta2); _Scalar theta = std::sqrt(theta2);
_Scalar ha = _Scalar(0.5) * theta; _Scalar ha = _Scalar(0.5) * theta;
Vector3 vec = (sin(ha) / theta) * omega; Vector3 vec = (sin(ha) / theta) * omega;
@ -100,8 +100,8 @@ struct traits<QUATERNION_TYPE> {
using std::sqrt; using std::sqrt;
// define these compile time constants to avoid std::abs: // define these compile time constants to avoid std::abs:
static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-8, static const double twoPi = 2.0 * M_PI, NearlyOne = 1.0 - 1e-10,
NearlyNegativeOne = -1.0 + 1e-8; NearlyNegativeOne = -1.0 + 1e-10;
TangentVector omega; TangentVector omega;