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

Fixed issue with static vector being const

release/4.3a0
Frank Dellaert 2019-06-15 19:32:45 -04:00 committed by Fan Jiang
parent e758089f13
commit 40af21914b
2 changed files with 8 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/SO3.cpp
View File

@ -68,7 +68,8 @@ ExpmapFunctor::ExpmapFunctor(const Vector3& omega, bool nearZeroApprox)
} }
} }
ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle, bool nearZeroApprox) ExpmapFunctor::ExpmapFunctor(const Vector3& axis, double angle,
bool nearZeroApprox)
: theta2(angle * angle), theta(angle) { : theta2(angle * angle), theta(angle) {
const double ax = axis.x(), ay = axis.y(), az = axis.z(); const double ax = axis.x(), ay = axis.y(), az = axis.z();
K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0; K << 0.0, -az, +ay, +az, 0.0, -ax, -ay, +ax, 0.0;
@ -88,9 +89,9 @@ SO3 ExpmapFunctor::expmap() const {
DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox) DexpFunctor::DexpFunctor(const Vector3& omega, bool nearZeroApprox)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) { : ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero) if (nearZero) {
dexp_ = I_3x3 - 0.5 * W; dexp_ = I_3x3 - 0.5 * W;
else { } else {
a = one_minus_cos / theta; a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta; b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK; dexp_ = I_3x3 - a * K + b * KK;
@ -296,7 +297,7 @@ static Vector9 vec3(const Matrix3& R) {
} }
// so<3> generators // so<3> generators
static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)), static std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)), SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))}); SO3::Hat(Vector3::Unit(2))});

2
gtsam/geometry/SO4.cpp
View File

@ -138,7 +138,7 @@ static SO4::VectorN2 vec4(const Matrix4& Q) {
} }
// so<4> generators // so<4> generators
static const std::vector<const Matrix4> G4( static std::vector<const Matrix4> G4(
{SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)), {SO4::Hat(Vector6::Unit(0)), SO4::Hat(Vector6::Unit(1)),
SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)), SO4::Hat(Vector6::Unit(2)), SO4::Hat(Vector6::Unit(3)),
SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))}); SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});