//------------------------------------------------------------------------------
void ManifoldPreintegration::update(const Vector3& measuredAcc,
const Vector3& measuredOmega, const double dt, Matrix9* A, Matrix93* B,
Matrix93* C) {
// Correct for bias in the sensor frame
Vector3 acc = biasHat_.correctAccelerometer(measuredAcc);
Vector3 omega = biasHat_.correctGyroscope(measuredOmega);
// Possibly correct for sensor pose
Matrix3 D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega;
if (p().body_P_sensor)
boost::tie(acc, omega) = correctMeasurementsBySensorPose(acc, omega,
D_correctedAcc_acc, D_correctedAcc_omega, D_correctedOmega_omega);
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();
// Do update
deltaTij_ += dt;
deltaXij_ = deltaXij_.update(acc, omega, dt, A, B, C); // functional
if (p().body_P_sensor) {
// More complicated derivatives in case of non-trivial sensor pose
*C *= D_correctedOmega_omega;
if (!p().body_P_sensor->translation().isZero())
*C += *B * D_correctedAcc_omega;
*B *= D_correctedAcc_acc; // NOTE(frank): needs to be last
}
// Update Jacobians
// TODO(frank): Try same simplification as in new approach
Matrix3 D_acc_R;
oldRij.rotate(acc, D_acc_R);
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
const Vector3 integratedOmega = omega * dt;
Matrix3 D_incrR_integratedOmega;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * dt;
double dt22 = 0.5 * dt * dt;
const Matrix3 dRij = oldRij.matrix(); // expensive
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
delVdelBiasAcc_ += -dRij * dt;
delVdelBiasOmega_ += D_acc_biasOmega * dt;
}
//------------------------------------------------------------------------------
void PreintegrationBase::update(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt,
Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
// Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude();
// Do update
deltaTij_ += dt;
deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
// Update Jacobians
// TODO(frank): we are repeating some computation here: accessible in F ?
Vector3 j_correctedAcc, j_correctedOmega;
boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega);
Matrix3 D_acc_R;
oldRij.rotate(j_correctedAcc, D_acc_R);
const Matrix3 D_acc_biasOmega = D_acc_R * delRdelBiasOmega_;
const Vector3 integratedOmega = j_correctedOmega * dt;
const Rot3 incrR = Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !!
const Matrix3 incrRt = incrR.transpose();
delRdelBiasOmega_ = incrRt * delRdelBiasOmega_
- *D_incrR_integratedOmega * dt;
double dt22 = 0.5 * dt * dt;
const Matrix3 dRij = oldRij.matrix(); // expensive
delPdelBiasAcc_ += delVdelBiasAcc_ * dt - dt22 * dRij;
delPdelBiasOmega_ += dt * delVdelBiasOmega_ + dt22 * D_acc_biasOmega;
delVdelBiasAcc_ += -dRij * dt;
delVdelBiasOmega_ += D_acc_biasOmega * dt;
}
Vec3F toLocal(Vec3F const & a, Rot3<float> const & r) {
return r.rotate(a);
}
void go() {
typedef Vec4<T> Vec;
typedef Vec2<T> Vec2D;
std::cout << std::endl;
std::cout << sizeof(Vec) << std::endl;
std::vector<Vec> vec1; vec1.reserve(50);
std::vector<T> vect(23);
std::vector<Vec> vec2(53);
std::vector<Vec> vec3; vec3.reserve(50234);
Vec x(2.0,4.0,5.0);
Vec y(-3.0,2.0,-5.0);
std::cout << x << std::endl;
std::cout << Vec4<float>(x) << std::endl;
std::cout << Vec4<double>(x) << std::endl;
std::cout << -x << std::endl;
std::cout << x.template get1<2>() << std::endl;
std::cout << y << std::endl;
std::cout << T(3.)*x << std::endl;
std::cout << y*T(0.1) << std::endl;
std::cout << (Vec(1) - y*T(0.1)) << std::endl;
std::cout << mathSSE::sqrt(x) << std::endl;
std::cout << dot(x,y) << std::endl;
std::cout << dotSimple(x,y) << std::endl;
std::cout << "equal" << (x==x ? " " : " not ") << "ok" << std::endl;
std::cout << "not equal" << (x==y ? " not " : " ") << "ok" << std::endl;
Vec z = cross(x,y);
std::cout << z << std::endl;
std::cout << "rotations" << std::endl;
T a = 0.01;
T ca = std::cos(a);
T sa = std::sin(a);
Rot3<T> r1( ca, sa, 0,
-sa, ca, 0,
0, 0, 1);
Rot2<T> r21( ca, sa,
-sa, ca);
Rot3<T> r2(Vec( 0, 1 ,0), Vec( 0, 0, 1), Vec( 1, 0, 0));
Rot2<T> r22(Vec2D( 0, 1), Vec2D( 1, 0));
{
std::cout << "\n3D rot" << std::endl;
Vec xr = r1.rotate(x);
std::cout << x << std::endl;
std::cout << xr << std::endl;
std::cout << r1.rotateBack(xr) << std::endl;
Rot3<T> rt = r1.transpose();
Vec xt = rt.rotate(xr);
std::cout << x << std::endl;
std::cout << xt << std::endl;
std::cout << rt.rotateBack(xt) << std::endl;
std::cout << r1 << std::endl;
std::cout << rt << std::endl;
std::cout << r1*rt << std::endl;
std::cout << r2 << std::endl;
std::cout << r1*r2 << std::endl;
std::cout << r2*r1 << std::endl;
std::cout << r1*r2.transpose() << std::endl;
std::cout << r1.transpose()*r2 << std::endl;
}
{
std::cout << "\n2D rot" << std::endl;
Vec2D xr = r21.rotate(x.xy());
std::cout << x.xy() << std::endl;
std::cout << xr << std::endl;
std::cout << r21.rotateBack(xr) << std::endl;
Rot2<T> rt = r21.transpose();
Vec2D xt = rt.rotate(xr);
std::cout << x.xy() << std::endl;
std::cout << xt << std::endl;
std::cout << rt.rotateBack(xt) << std::endl;
std::cout << r21 << std::endl;
std::cout << rt << std::endl;
std::cout << r21*rt << std::endl;
std::cout << r22 << std::endl;
std::cout << r21*r22 << std::endl;
std::cout << r22*r21 << std::endl;
std::cout << r21*r22.transpose() << std::endl;
std::cout << r21.transpose()*r22 << std::endl;
}
}
