// uses simple explicit euler integration
void RigidBody::integrate(float delta_time) {
//printf("integrate %f\n", delta_time);
float inv_mass = 1.0f/mass;
position += delta_time * velocity;
#if 0
mat3 skew_mat = transpose(skewSymmetric(angular_velocity));
orientation = orientation + delta_time * (skew_mat*orientation);
orientation = orthoNormalize(orientation);
#endif
// integrate quaternion orientation
quat spin = 0.5f * quaternion(0.0f, angular_velocity.x, angular_velocity.y, angular_velocity.z) * q_orientation;
q_orientation += delta_time * spin;
q_orientation = normalize(q_orientation);
velocity += /*0.5f */ (delta_time*inv_mass) * force;
angular_momentum += /*0.5f */ delta_time * torque;
// compute auxiliaries
#if 1
orientation = m3(q_orientation);
inv_world_inertia_mat = orientation * inv_inertia_mat * transpose(orientation);
angular_velocity = inv_world_inertia_mat * angular_momentum;
#else
inv_world_inertia_mat = inv_inertia_mat;
#endif
angular_velocity = inv_world_inertia_mat * angular_momentum;
force = v3(0.0f);
torque = v3(0.0f);
}
//*************************************************************************
TEST (EssentialMatrixFactor, testData) {
CHECK(readOK);
// Check E matrix
Matrix expected(3, 3);
expected << 0, 0, 0, 0, 0, -0.1, 0.1, 0, 0;
Matrix aEb_matrix = skewSymmetric(c1Tc2.x(), c1Tc2.y(), c1Tc2.z())
* c1Rc2.matrix();
EXPECT(assert_equal(expected, aEb_matrix, 1e-8));
// Check some projections
EXPECT(assert_equal(Point2(0, 0), pA(0), 1e-8));
EXPECT(assert_equal(Point2(0, 0.1), pB(0), 1e-8));
EXPECT(assert_equal(Point2(0, -1), pA(4), 1e-8));
EXPECT(assert_equal(Point2(-1, 0.2), pB(4), 1e-8));
// Check homogeneous version
EXPECT(assert_equal(Vector3(-1, 0.2, 1), vB(4), 1e-8));
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, vA(i).transpose() * aEb_matrix * vB(i), 1e-8);
// Check epipolar constraint
for (size_t i = 0; i < 5; i++)
EXPECT_DOUBLES_EQUAL(0, trueE.error(vA(i), vB(i)), 1e-7);
}
/*
* @brief Convert a quaternion to the corresponding rotation matrix
* @note Pay attention to the convention used. The function follows the
* conversion in "Indirect Kalman Filter for 3D Attitude Estimation:
* A Tutorial for Quaternion Algebra", Equation (78).
*
* The input quaternion should be in the form
* [q1, q2, q3, q4(scalar)]^T
*/
inline Eigen::Matrix3d quaternionToRotation(
const Eigen::Vector4d& q) {
const Eigen::Vector3d& q_vec = q.block(0, 0, 3, 1);
const double& q4 = q(3);
Eigen::Matrix3d R =
(2*q4*q4-1)*Eigen::Matrix3d::Identity() -
2*q4*skewSymmetric(q_vec) +
2*q_vec*q_vec.transpose();
//TODO: Is it necessary to use the approximation equation
// (Equation (87)) when the rotation angle is small?
return R;
}
//------------------------------------------------------------------------------
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
// Correct for bias in the sensor frame
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
Vector3 j_correctedOmega = biasHat_.correctGyroscope(j_measuredOmega);
// Compensate for sensor-body displacement if needed: we express the quantities
// (originally in the IMU frame) into the body frame
// Equations below assume the "body" frame is the CG
if (p().body_P_sensor) {
// Correct omega to rotation rate vector in the body frame
const Matrix3 bRs = p().body_P_sensor->rotation().matrix();
j_correctedOmega = bRs * j_correctedOmega;
// Correct acceleration
j_correctedAcc = bRs * j_correctedAcc;
// Jacobians
if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero();
if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs;
// Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation().vector();
if (!b_arm.isZero()) {
// Subtract out the the centripetal acceleration from the measured one
// to get linear acceleration vector in the body frame:
const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
j_correctedAcc -= body_Omega_body * b_velocity_bs;
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (D_correctedAcc_measuredOmega) {
double wdp = j_correctedOmega.dot(b_arm);
*D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
+ j_correctedOmega * b_arm.transpose()) * bRs.matrix()
+ 2 * b_arm * j_measuredOmega.transpose();
}
}
}
// Do update in one fell swoop
return make_pair(j_correctedAcc, j_correctedOmega);
}
/* ************************************************************************* */
Matrix3 Unit3::skew() const {
return skewSymmetric(p_.x(), p_.y(), p_.z());
}
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
return Point3(rot_ * p.vector());
}