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