//------------------------------------------------------------------------------ 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; } }