const Eigen::Vector4d CMiniVisionToolbox::getPointHomogeneousStereoLinearTriangulationLU( const Eigen::Vector2d& p_vecPointLeft, const Eigen::Vector2d& p_vecPointRight, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLeft, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRight )
{
//ds A matrix for system: A*X=0
Eigen::Matrix< double, 4 , 4 > matA;
//ds fill the matrix
matA.row(0) = p_vecPointLeft(0)*p_matProjectionLeft.row(2)-p_matProjectionLeft.row(0);
matA.row(1) = p_vecPointLeft(1)*p_matProjectionLeft.row(2)-p_matProjectionLeft.row(1);
matA.row(2) = p_vecPointRight(0)*p_matProjectionRight.row(2)-p_matProjectionRight.row(0);
matA.row(3) = p_vecPointRight(1)*p_matProjectionRight.row(2)-p_matProjectionRight.row(1);
//ds homogeneous solution
return matA.fullPivLu( ).kernel( );
}
bool ErrorStateKF<Scalar>::update(const ErrorStateKF<Scalar>::quat &qm,
const ErrorStateKF<Scalar>::mat3 &varQ,
const ErrorStateKF<Scalar>::vec3 &pm,
const ErrorStateKF<Scalar>::mat3 &varP,
const ErrorStateKF<Scalar>::vec3 &vm,
const ErrorStateKF<Scalar>::mat3 &varV) {
// form the measurement jacobian
Eigen::Matrix<Scalar, 7, 15> H;
H.setZero();
// position and velocity
H.template block<3, 3>(0, 12).setIdentity();
H.template block<3, 3>(3, 6).setIdentity();
// orientation
// H.template block<3, 3>(6, 0).setIdentity();
H(6, 2) = 1;
// residual
Eigen::Matrix<Scalar,7,1> r;
// non-linear rotation residual on yaw axis
const quat dq = q_.conjugate() * qm;
const Eigen::AngleAxis<Scalar> aa(dq);
const vec3 rpy = kr::rotToEulerZYX(aa.matrix());
// r.template block<3, 1>(6, 0) = aa.angle()*aa.axis();
r(6, 0) = rpy[2];
// linear pos/velocity
r.template block<3, 1>(0, 0) = pm - p_;
r.template block<3, 1>(3, 0) = vm - v_;
// measurement covariance
Eigen::Matrix<Scalar,7,7> R;
R.setZero();
R.template block<3, 3>(0, 0) = varP;
R.template block<3, 3>(3, 3) = varV;
// R.template block<3, 3>(6, 6) = varQ;
R(6, 6) = varQ(2, 2);
// kalman update
Eigen::Matrix<Scalar,7,7> S = H * P_ * H.transpose() + R;
auto LU = S.fullPivLu();
if (!LU.isInvertible()) {
return false;
}
S = LU.inverse();
const Eigen::Matrix<Scalar,15,7> K = P_ * H.transpose() * S;
const Eigen::Matrix<Scalar,15,1> dx = K * r;
P_ = (Eigen::Matrix<Scalar, 15, 15>::Identity() - K * H) * P_;
// update state
q_ *= quat(1, dx[0] * 0.5, dx[1] * 0.5, dx[2] * 0.5);
q_.normalize();
bg_.noalias() += dx.template block<3, 1>(3, 0);
v_.noalias() += dx.template block<3, 1>(6, 0);
ba_.noalias() += dx.template block<3, 1>(9, 0);
p_.noalias() += dx.template block<3, 1>(12, 0);
return true;
}