double signedDistanceInsideConvexHull(
const Ref<const Matrix<double, 2, Dynamic>> &pts,
const Ref<const Vector2d> &q) {
std::vector<Point> hull_pts = convexHull(eigenToPoints(pts));
double d_star = std::numeric_limits<double>::infinity();
Matrix2d R;
R << 0, 1, -1, 0;
for (int i = 0; i < (static_cast<int>(hull_pts.size()) - 1); ++i) {
Vector2d ai = R * Vector2d(hull_pts[i + 1].x - hull_pts[i].x,
hull_pts[i + 1].y - hull_pts[i].y);
double b = ai.transpose() * Vector2d(hull_pts[i].x, hull_pts[i].y);
double n = ai.norm();
if (std::isnormal(n)) {
ai = ai.array() / n;
b = b / n;
double d = b - ai.transpose() * q;
// std::cout << "pt0: " << hull_pts[i].x << " " << hull_pts[i].y <<
// std::endl;
// std::cout << "pt1: " << hull_pts[i+1].x << " " << hull_pts[i+1].y <<
// std::endl;
// std::cout << "ai: " << ai.transpose() << std::endl;
// std::cout << "b: " << b << std::endl;
// std::cout << "d: " << d << std::endl;
if (d < d_star) {
d_star = d;
}
}
}
return d_star;
}
void StringElement::buildParticlesAndSprings() {
Vector2d xAxis = getEndPosition() - getStartPosition();
double length = xAxis.norm();
int requiredMasses = static_cast<int>(ceil(length / stdMaxMassDistance) + 1.0);
double massDistance = length / requiredMasses;
xAxis.normalize();
for (int i = 0; i < requiredMasses; i++) {
Vector2d massPosition = getStartPosition() + (xAxis * (i * massDistance));
particles.push_back(createParticle(stdMassValue, stdMassRadius, massPosition));
}
for (int i = 0; i < requiredMasses - 1; i++) {
springs.push_back(new Spring(*particles[i], *particles[i + 1], stdK));
}
if (getFromConnectionPoint().isConnected()) {
ConnectionPoint::particle_list connectionParticles = getFromConnectionPoint().getConnectionParticles();
for (ConnectionPoint::particle_list::iterator it = connectionParticles.begin(); it != connectionParticles.end(); ++it) {
springs.push_back(new Spring(*particles[0], **it, stdK));
}
}
if (getToConnectionPoint().isConnected()) {
ConnectionPoint::particle_list connectionParticles = getToConnectionPoint().getConnectionParticles();
for (ConnectionPoint::particle_list::iterator it = connectionParticles.begin(); it != connectionParticles.end(); ++it) {
springs.push_back(new Spring(*particles[particles.size() - 1], **it, stdK));
}
}
}
Line::Line(const Point2d& p, const Point2d& q)
{
Vector2d t = q - p;
Real len = t.norm();
a = t.y / len;
b = - t.x / len;
c = -(a*p.x + b*p.y);
}
HexagonalNet::HexagonalNet(const Vector2d & a)
{
m_a = a.norm();
m_a1 = a;
m_a2 = a;
/*the second basis vector is rotated by 120 degrees*/
m_a2.Rotate(2 * M_PI / 3);
/*rho = 2/3 a1 + 1/3 a2*/
m_rho = m_a1 * 1.0 / 3 + m_a2 * 2.0 / 3;
}
bool StringElement::containsPoint(const Vector2d &point) const {
if (this->isErased()) return false;
Vector2d axis = getEndPosition() - getStartPosition();
Vector2d translation = -getStartPosition();
double angle = -(Vector2d(1, 0).angleTo(axis));
Vector2d translatedPoint = translation + point;
double transformedX = cos(angle) * translatedPoint.getX() - sin(angle) * translatedPoint.getY();
double transformedY = sin(angle) * translatedPoint.getX() + cos(angle) * translatedPoint.getY();
return transformedX >= 0 && transformedX <= axis.norm() && fabs(transformedY) <= (getStringWidth() / 2);
}
size_t Homography::
computeMatchesInliers()
{
inliers.clear(); inliers.resize(fts_c1.size());
size_t n_inliers = 0;
for(size_t i=0; i<fts_c1.size(); i++)
{
Vector2d projected = project2d(H_c2_from_c1 * unproject2d(fts_c1[i]));
Vector2d e = fts_c2[i] - projected;
double e_px = error_multiplier2 * e.norm();
inliers[i] = (e_px < thresh);
n_inliers += inliers[i];
}
return n_inliers;
}
Vector2d HexagonalNetRandomSources::displacement(const Vector2d& r)
{
static Vector2d u, dr, drdir;
static double drnorm;
/*sum over all sources of displacements*/
u = Vector2d(0, 0);
for(size_t i = 0; i < m_sources.size(); ++i)
{
dr = (r - m_sources[i].m_r0);
drnorm = dr.norm();
drdir = dr / drnorm;
/*every source creates the field of type alpha/r^k, where r - is a distance from the source measured in hexagon radii */
u += drdir / gsl_pow_uint(drnorm / m_a, m_k) * m_sources[i].m_amplitude;
}
return u;
}
void
Frame2d::set_angle( const Vector2d & vec )
{
double norm = vec.norm();
if ( norm == 0.0 )
{
//this is consistent with vec.arg() == 0.0 if vec.norm() == 0.0
n_x = 1.0 * scale;
n_y = 0.0;
return;
}
norm = scale / norm;
n_x = vec.x * norm;
n_y = vec.y * norm;
//f1.n_y= sqrt(1-f1.n_x*f1.n_x);
}
Circle2d::Circle(
double radius,
WindingDirection windingDirection,
const Point2d& firstPoint,
const Point2d& secondPoint
) : _radius(radius) {
if (radius <= Zero()) {
throw Error(new PlaceholderError());
}
Vector2d displacementVector = secondPoint - firstPoint;
double halfDistance = displacementVector.norm() / 2;
if (halfDistance == Zero()) {
// Points are coincident
throw Error(new PlaceholderError());
}
if (halfDistance - radius > Zero()) {
// Points are too far apart
throw Error(new PlaceholderError());
}
Vector2d sidewaysDirection = displacementVector.unitOrthogonal();
if (windingDirection == COUNTERCLOCKWISE) {
sidewaysDirection = -sidewaysDirection;
}
double sidewaysDistance = 0.0;
if (halfDistance - radius == Zero()) {
sidewaysDistance = radius;
} else {
sidewaysDistance = sqrt(halfDistance * halfDistance - radius * radius);
}
_centerPoint = firstPoint + displacementVector / 2 + sidewaysDistance * sidewaysDirection;
}
void Draw() {
if (leftPressed) for (Particle &p : w.particles) {
tmp = (Vector2d(mx, my) - p.pos);
if (tmp * tmp < 10000)
p.a += tmp.norm() * 100;
}
w.step(0.1);
Drawer::clear();
//Drawer::drawCircle(0, 0, i);
for (Particle &p : w.particles) {
Drawer::drawCircle(p.pos.x, p.pos.y, p.r);
}
for (Connection &c : w.connections) {
Drawer::drawLine(c.p1->pos.x, c.p1->pos.y, c.p2->pos.x, c.p2->pos.y);
}
for (Connection &c : w.lenConnections) {
if (!(c.broken))
Drawer::drawLine(c.p1->pos.x, c.p1->pos.y, c.p2->pos.x, c.p2->pos.y);
}
Drawer::update();
glutPostRedisplay();
}
void optimizeGaussNewton(
const double reproj_thresh,
const size_t n_iter,
const bool verbose,
FramePtr& frame,
double& estimated_scale,
double& error_init,
double& error_final,
size_t& num_obs)
{
// init
double chi2(0.0);
vector<double> chi2_vec_init, chi2_vec_final;
vk::robust_cost::TukeyWeightFunction weight_function;
SE3d T_old(frame->T_f_w_);
Matrix6d A;
Vector6d b;
// compute the scale of the error for robust estimation
std::vector<float> errors; errors.reserve(frame->fts_.size());
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Vector2d e = vk::project2d((*it)->f)
- vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
e *= 1.0 / (1<<(*it)->level);
errors.push_back(e.norm());
}
if(errors.empty())
return;
vk::robust_cost::MADScaleEstimator scale_estimator;
estimated_scale = scale_estimator.compute(errors);
num_obs = errors.size();
chi2_vec_init.reserve(num_obs);
chi2_vec_final.reserve(num_obs);
double scale = estimated_scale;
for(size_t iter=0; iter<n_iter; iter++)
{
// overwrite scale
if(iter == 5)
scale = 0.85/frame->cam_->errorMultiplier2();
b.setZero();
A.setZero();
double new_chi2(0.0);
// compute residual
for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Matrix26d J;
Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_);
Frame::jacobian_xyz2uv(xyz_f, J);
Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f);
double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
e *= sqrt_inv_cov;
if(iter == 0)
chi2_vec_init.push_back(e.squaredNorm()); // just for debug
J *= sqrt_inv_cov;
double weight = weight_function.value(e.norm()/scale);
A.noalias() += J.transpose()*J*weight;
b.noalias() -= J.transpose()*e*weight;
new_chi2 += e.squaredNorm()*weight;
}
// solve linear system
const Vector6d dT(A.ldlt().solve(b));
// check if error increased
if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0]))
{
if(verbose)
std::cout << "it " << iter
<< "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl;
frame->T_f_w_ = T_old; // roll-back
break;
}
// update the model
SE3d T_new = SE3d::exp(dT)*frame->T_f_w_;
T_old = frame->T_f_w_;
frame->T_f_w_ = T_new;
chi2 = new_chi2;
if(verbose)
std::cout << "it " << iter
<< "\t Success \t new_chi2 = " << new_chi2
<< "\t norm(dT) = " << vk::norm_max(dT) << std::endl;
// stop when converged
if(vk::norm_max(dT) <= EPS)
break;
}
// Set covariance as inverse information matrix. Optimistic estimator!
const double pixel_variance=1.0;
frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse();
// Remove Measurements with too large reprojection error
double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2();
size_t n_deleted_refs = 0;
for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
{
if((*it)->point == NULL)
continue;
Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_);
double sqrt_inv_cov = 1.0 / (1<<(*it)->level);
e *= sqrt_inv_cov;
chi2_vec_final.push_back(e.squaredNorm());
if(e.norm() > reproj_thresh_scaled)
{
// we don't need to delete a reference in the point since it was not created yet
(*it)->point = NULL;
++n_deleted_refs;
}
}
error_init=0.0;
error_final=0.0;
if(!chi2_vec_init.empty())
error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2();
if(!chi2_vec_final.empty())
error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2();
estimated_scale *= frame->cam_->errorMultiplier2();
if(verbose)
std::cout << "n deleted obs = " << n_deleted_refs
<< "\t scale = " << estimated_scale
<< "\t error init = " << error_init
<< "\t error end = " << error_final << std::endl;
num_obs -= n_deleted_refs;
}
void Odometry::Ransac()
{
assert(observationVec.size() == cloud.size());
int numPoints = observationVec.size();
inlierMask.resize(numPoints);
const int numIterMax = 25;
const Transformation<double> initialPose = TorigBase;
int bestInliers = 0;
//TODO add a termination criterion
for (unsigned int iteration = 0; iteration < numIterMax; iteration++)
{
Transformation<double> pose = initialPose;
int maxIdx = observationVec.size();
//choose three points at random
int idx1m = rand() % maxIdx;
int idx2m, idx3m;
do
{
idx2m = rand() % maxIdx;
} while (idx2m == idx1m);
do
{
idx3m = rand() % maxIdx;
} while (idx3m == idx1m or idx3m == idx2m);
//solve an optimization problem
Problem problem;
for (auto i : {idx1m, idx2m, idx3m})
{
CostFunction * costFunc = new OdometryError(cloud[i],
observationVec[i], TbaseCam, camera);
problem.AddResidualBlock(costFunc, NULL,
pose.transData(), pose.rotData());
}
Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
Solver::Summary summary;
options.max_num_iterations = 5;
Solve(options, &problem, &summary);
//count inliers
vector<Vector3d> XcamVec(numPoints);
Transformation<double> TorigCam = pose.compose(TbaseCam);
TorigCam.inverseTransform(cloud, XcamVec);
vector<Vector2d> projVec(numPoints);
camera.projectPointCloud(XcamVec, projVec);
vector<bool> currentInlierMask(numPoints, false);
int countInliers = 0;
for (unsigned int i = 0; i < numPoints; i++)
{
Vector2d err = observationVec[i] - projVec[i];
if (err.norm() < 2)
{
currentInlierMask[i] = true;
countInliers++;
}
}
//keep the best hypothesis
if (countInliers > bestInliers)
{
//TODO copy in a bettegit lor way
inlierMask = currentInlierMask;
bestInliers = countInliers;
TorigBase = pose;
}
}
}
