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

13
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) {
const double ax = axis.x(), ay = axis.y(), az = axis.z();
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)
: ExpmapFunctor(omega, nearZeroApprox), omega(omega) {
if (nearZero)
if (nearZero) {
dexp_ = I_3x3 - 0.5 * W;
else {
} else {
a = one_minus_cos / theta;
b = 1.0 - sin_theta / theta;
dexp_ = I_3x3 - a * K + b * KK;
@ -296,9 +297,9 @@ static Vector9 vec3(const Matrix3& R) {
}
// so<3> generators
static const std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))});
static std::vector<const Matrix3> G3({SO3::Hat(Vector3::Unit(0)),
SO3::Hat(Vector3::Unit(1)),
SO3::Hat(Vector3::Unit(2))});
// vectorized generators
static const Matrix93 P3 =

2
gtsam/geometry/SO4.cpp
View File

@ -138,7 +138,7 @@ static SO4::VectorN2 vec4(const Matrix4& Q) {
}
// 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(2)), SO4::Hat(Vector6::Unit(3)),
SO4::Hat(Vector6::Unit(4)), SO4::Hat(Vector6::Unit(5))});