Commit 8ffb4e8c authored by Ryan Pavlik's avatar Ryan Pavlik

Update flexkalman

parent 637f1bb5
......@@ -94,7 +94,7 @@ namespace util {
Eigen::Quaternion<Scalar> ret;
ret.vec() = vecscale * vec;
ret.w() = std::cos(theta);
return ret.normalized();
return ret;
}
/*!
......@@ -107,14 +107,21 @@ namespace util {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
using Scalar = typename Derived::Scalar;
//! @todo get better way of determining "zero vec" for this purpose.
if (vec.isApproxToConstant(0, 1.e-4)) {
if (vec.derived().isApproxToConstant(0, 1.e-4)) {
return Eigen::Quaternion<Scalar>::Identity();
}
// For non-zero vectors, the vector scale sinc(theta) approximately
// equals 1, while the scale for w, cos(theta), is approximately 0.
// So, we treat this as a "pure" quaternion and normalize it.
return Eigen::Quaternion<Scalar>{0, vec.x(), vec.y(), vec.z()}
.normalized();
// For non-zero vectors, the vector scale
// sinc(theta)=sin(theta)/theta approximately equals 1, and w,
// cos(theta), is approximately 1 - theta^2/2.
// To ensure we're exactly normalized, we could treat the vec as the
// vector portion of a quaternion and compute the other part to make
// it exactly normalized:
// Scalar w = std::sqrt(1 - vec.derived().squaredNorm());
// Instead we'll do the small-angle approx to really skip the sqrt,
// and we'll be approximately normalized.
Scalar w = 1 - vec.derived().squaredNorm() / 2.;
return Eigen::Quaternion<Scalar>{
w, vec.derived().x(), vec.derived().y(), vec.derived().z()};
}
/*!
......@@ -150,7 +157,8 @@ namespace util {
// result (i.e., ln(qv, qw) = (phi/sin(phi)) * qv )
Scalar vecnorm = quat.vec().norm();
// "best for numerical stability" vs asin or acos
// "best for numerical stability" vs asin or acos.
// Approximately vecnorm near 0.
Scalar phi = std::atan2(vecnorm, quat.w());
// Here is where we compute the coefficient to scale the vector part
......
......@@ -100,9 +100,9 @@ inline types::SquareMatrix<getDimension<StateType>()>
predictErrorCovariance(StateType const &state, ProcessModelType &processModel,
double dt) {
using StateSquareMatrix = types::SquareMatrix<getDimension<StateType>()>;
StateSquareMatrix A = processModel.getStateTransitionMatrix(state, dt);
const auto A = processModel.getStateTransitionMatrix(state, dt);
// FLEXKALMAN_DEBUG_OUTPUT("State transition matrix", A);
StateSquareMatrix P = state.errorCovariance();
auto &&P = state.errorCovariance();
/*!
* @todo Determine if the fact that Q is (at least in one case)
* symmetrical implies anything else useful performance-wise here or
......
......@@ -158,7 +158,7 @@ namespace pose_exp_map {
}
void postCorrect() {
Eigen::Vector3d ori(orientation(m_state));
types::Vector<3> ori = orientation(m_state);
matrix_exponential_map::avoidSingularities(ori);
orientation(m_state) = ori;
m_cacheData.reset(ori);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment