C++ (Cpp) Matrix6d::transpose Beispiele

Beispiel #1

Datei: auxiliar.cpp Projekt: rubengooj/StVO-PL

Matrix6d uncTinv_se3(Matrix4d T, Matrix6d covT ){
    Matrix6d covTinv = Matrix6d::Zero();
    Matrix6d adjTinv;
    adjTinv = adjoint_se3( inverse_se3(T) );
    covTinv = adjTinv * covT * adjTinv.transpose();
    return covTinv;
}

Beispiel #2

Datei: ekf.cpp Projekt: cwrucutter/snowmower_localization

// 3. System update
void Ekf::systemUpdate(double dt){
  // update state
  state_ = fSystem(state_,dt);
  // Then calculate F
  Matrix6d F;
  F = FSystem(state_,dt);
  // Then update covariance
  cov_ = F*cov_*F.transpose() + Q_;
  cov_ = zeroOutBiasXYThetaCov(cov_);
}

Beispiel #3

Datei: Gaussian.cpp Projekt: Cirromulus/slam-odometry

void GaussianSamplingPose3D::update( const Vector6d& mean, const Matrix6d& llt )
{
    poseMean = mean;
    poseCov = llt * llt.transpose();
    poseLT = llt;
}