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(); }
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; }
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(); }
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); }