void RadialTangentialDistortion::undistort(
    const Eigen::MatrixBase<DERIVED> & yconst) const {
  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
      Eigen::MatrixBase<DERIVED>, 2);

  Eigen::MatrixBase<DERIVED> & y =
      const_cast<Eigen::MatrixBase<DERIVED> &>(yconst);
  y.derived().resize(2);

  Eigen::Vector2d ybar = y;
  const int n = 5;
  Eigen::Matrix2d F;

  Eigen::Vector2d y_tmp;

  for (int i = 0; i < n; i++) {

    y_tmp = ybar;

    distort(y_tmp, F);

    Eigen::Vector2d e(y - y_tmp);
    Eigen::Vector2d du = (F.transpose() * F).inverse() * F.transpose() * e;

    ybar += du;

    if (e.dot(e) < 1e-15)
      break;

  }
  y = ybar;

}
  void
  LaserSensorModel2D::setInformationForVertexPoint(EdgeSE2PointXYCov*& edge, VertexPointXYCov*& point, LaserSensorParams& params)
  {
    double beamAngle = atan2(edge->measurement()(1), edge->measurement()(0));
    ///calculate incidence angle in point frame
    Eigen::Vector2d beam = edge->measurement();

    Eigen::Vector2d normal = point->normal();
    beam = beam.normalized();
    normal = normal.normalized();
    double incidenceAngle = 0.0;
    if(point->covariance() != Eigen::Matrix2d::Identity())
      incidenceAngle = acos(normal.dot(beam));

    double d = (tan(params.angularResolution * 0.5) * edge->measurement().norm());
    ///uncertainty of the surface measurement in direction of the beam
    double dk = fabs(params.scale * (d / cos(incidenceAngle)));
    edge->information().setIdentity();
    edge->information()(0,0) = 1.0 / ((dk + params.sensorPrecision) * (dk + params.sensorPrecision));
    double cError = 0.001 * edge->measurement().norm();
    edge->information()(1,1) = 1.0 / (cError * cError);

    Eigen::Rotation2Dd rot(beamAngle);
    Eigen::Matrix2d mrot = rot.toRotationMatrix();
    edge->information() = mrot * edge->information() * mrot.transpose();
  }
Esempio n. 3
0
    void EKF::correct(Eigen::Vector2d z_i)
    {
        // measurement matrix
        Eigen::Matrix2d I = Eigen::Matrix2d::Identity();
        Eigen::Matrix2d H = Eigen::Matrix2d::Identity();

        // innovation residual
        Eigen::Vector2d y = z_i - H * this->x;

        // innovation covariance
        Eigen::Matrix2d S = H * this->P * H.transpose() + this->R;

        // Kalman gain
        Eigen::Matrix2d K = this->P * H.transpose() * S.inverse();

        // updated state
        this->x = this->x + K * y;

        // updated covariance
        this->P = (I - K * H) * this->P;


    }
Esempio n. 4
0
void TimeSyncFilter::updateFilter(double device_time, double local_time) {
  if (!is_initialized_) {
    initialize(device_time, local_time);
    return;
  }

  double dt = device_time - last_update_device_time_;

  if (dt < kUpdateRate) return;

  dt_ = dt;
  Eigen::Matrix2d F;
  F << 1, dt_, 0, 1;

  // Prediction
  Eigen::Vector2d x_pred = F * x_;

  // check for outlier
  double measurement_residual = local_time - device_time - H_ * x_pred;

  if (measurement_residual > 2 * kSigmaMeasurementTimeOffset) {
    // std::cout << "Timesync outlier" << std::endl;
    return;
  }

  P_ = F * P_ * F.transpose() + dt_ * Q_;

  // Update
  double S = H_ * P_ * H_.transpose() + R_;

  Eigen::Vector2d K;
  K = P_ * H_.transpose() * (1 / S);

  x_ = x_pred + K * measurement_residual;
  P_ = (Eigen::Matrix2d::Identity() - K * H_) * P_;

  last_update_device_time_ = device_time;

  // print();
}
Esempio n. 5
0
    void EKF::predict(Eigen::Matrix3d Hom)
    {
        // homogeneous coordinates
        Eigen::Vector3d x_tilde;
        x_tilde  << this->x(0), this->x(1), 1.0;

        // intermediary values
        double gx   = Hom.row(0) * x_tilde;
        double gy   = Hom.row(1) * x_tilde;
        double h    = Hom.row(2) * x_tilde;

        // update transition matrix
        Eigen::Matrix2d F;
        F << 
            (Hom(0, 0) * h - Hom(2, 0) * gx) / (h*h),   (Hom(0, 1) * h - Hom(2, 1) * gx) / (h*h),
            (Hom(1, 0) * h - Hom(2, 0) * gy) / (h*h),   (Hom(1, 1) * h - Hom(2, 1) * gy) / (h*h);

        // propogate
        this->x = Eigen::Vector2d(gx / h, gy / h);
        this->P = F * this->P * F.transpose() + this->Q;
        
    }
LocalTime KalmanOwt::updateAndTranslateToLocalTimestamp(const RemoteTime remoteTimeTics, const LocalTime localTimeSecs) {
  if(!isInitialized_) {
    initialize(remoteTimeTics, localTimeSecs);
    return localTimeSecs;
  }

  const double dt = remoteTimeTics - lastUpdateDeviceTime_;

  if(dt >= config.updateRate){
    dt_=dt;
    Eigen::Matrix2d F;
    F << 1, dt_, 0, 1;

    // Prediction
    x_ = F * x_;
    P_ = F * P_ * F.transpose() + dt_ * Q_;
    lastUpdateDeviceTime_ = remoteTimeTics;

    // Update
    const double S = H_ * P_ * H_.transpose() + R_;

    Eigen::Vector2d K;
    K = P_ * H_.transpose() * (1 / S);

    const double measurementResidual = localTimeSecs - remoteTimeTics - H_ * x_;

    const double mahalDistance = sqrt(measurementResidual*measurementResidual*(1.0/S));

    if(config.outlierThreshold && mahalDistance > config.outlierThreshold){
      CUCKOO_TIME_TRANSLATOR_logWarn("KalmanOwt: local_time=%f, remote_time=%f -> measurement_residual=%g, mahal_distance=%g!", localTimeSecs, remoteTimeTics, measurementResidual, mahalDistance);
    } else {
      x_ = x_ + K * measurementResidual;
      P_ = (Eigen::Matrix2d::Identity() - K * H_) * P_;
    }
  }
  return translateToLocalTimestamp(remoteTimeTics);
}