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( );
}
Example #2
0
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;
}