double Polyline::project(const Vector2d &point) const
{
double bestS = 0.;
double minDistSq = (point - _pts[0]).squaredNorm();
for(int i = 0; i < _pts.endIdx(1); ++i)
{
double len = (_lengths[i + 1] - _lengths[i]);
double invLen = 1. / len;
Vector2d der = (_pts[i + 1] - _pts[i]) * invLen;
double dot = der.dot(point - _pts[i]);
if(dot < 0.)
dot = 0.;
if(dot > len)
dot = len;
Vector2d ptOnLine = _pts[i] + (_pts[i + 1] - _pts[i]) * (dot * invLen);
double distSq = (ptOnLine - point).squaredNorm();
if(distSq < minDistSq)
{
minDistSq = distSq;
bestS = _lengths[i] + dot;
}
}
return bestS;
}
double distance(Point2f aPoint, Vector2d aVector, Point2i startPoint)
{
Vector2d v = aVector;
Vector2d w(aPoint.x - startPoint.x, aPoint.y - startPoint.y);
double c1 = v.dot(w);
double c2 = v.dot(v);
if (c1 <= 0){
Point2d tmp;
tmp.x = startPoint.x;
tmp.y = startPoint.y;
return distance(aPoint, tmp);
}
if (c2 <= 0){
Point2d endPoint;
endPoint.x = startPoint.x + (float) aVector[0];
endPoint.y = startPoint.y + (float) aVector[1];
return distance(aPoint, endPoint);
}
double b = c1 / c2;
Point2d pb(startPoint.x + b * aVector[0], startPoint.y + b * aVector[1]);
return distance(aPoint, pb);
}
bool signed_distance_2d::onEdgeOfPoly(MatrixXd poly, Vector2d point) {
int i,j;
for (i = 0, j = poly.rows() - 1;i < poly.rows(); j=i++) {
Vector2d p1 = poly.row(i);
Vector2d p2 = poly.row(j);
Vector2d a = perpendicularAxis(p1 - p2); // Unit normal of the line
double d = a.dot(p1); // Constant, a'*x + d = 0
double eps = 1e-5;
if (abs(a.dot(point) - d) <= eps && (point(0) >= min(p1(0),p2(0))) && (point(0) <= max(p1(0),p2(0))) &&
(point(1) >= min(p1(1),p2(1))) && (point(1) <= max(p1(1),p2(1)))) {
return true;
}
}
return false;
}
long double angleBetween(const Vector2d &V1, const Vector2d &V2)
{
long double dotproduct = V1.dot(V2);
long double length = V1.length() * V2.length();
if (length==0) return 0;
long double quot = dotproduct / length;
if (quot > 1 && quot < 1.0001) quot = 1;
if (quot < -1 && quot > -1.0001) quot = -1;
long double result = acosl( quot ); // 0 .. pi
if (isleftof(Vector2d(0,0), V2, V1))
result = -result;
return result;
}
Vector2d signed_distance_2d::closestPointOnLineSegment(Vector2d point, Vector2d p1, Vector2d p2) {
// We are working with line representation L = {u*z + d | z is a real number, u is normalized}
// u and d are in R^2 (2D vectors)
// Get Unit vector in direction of edge
Vector2d u = p1 - p2;
if ((u.cwiseAbs()).sum() != 0) {
u.normalize(); // Assuming u has nonzero length
} else {
u << 0, 0;
}
Vector2d d = p1; // d can be any point on the line. p1 or p2 will do
double t = (point - d).dot(u);
// Find if point is on line segment or if it needs to be projected to an end of segment
// Do this in projected 1D space. Find t for both p1 and p2, and compare to t from the point.
double p1t = (p1 - d).dot(u);
double p2t = (p2 - d).dot(u);
if (t <= min(p1t, p2t)) {
t = min(p1t, p2t);
} else if (t >= max(p1t, p2t)) {
t = max(p1t, p2t);
}
Vector2d proj = u.array() * t;
proj += d;
// Sanity check
Vector2d a = perpendicularAxis(u);
double c = a.dot(p1); // Constant, a'*x + d = 0
double eps = 1e-5;
if (abs(a.dot(proj) - c) > eps) {
std::cout << "Warning: Projection on line is not on line with tolerance " << eps << std::endl;
}
return proj;
}
// sampling
void drwnGaussian::sample(VectorXd &x) const
{
DRWN_ASSERT(x.rows() == _n);
if (_mL == NULL) {
_mL = new MatrixXd(_mSigma.llt().matrixL());
}
// box-muller sampling algorithm
Vector2d uv;
for (int i = 0; i < x.size() - 1; i += 2) {
uv.setRandom();
double s = uv.dot(uv);
while (s >= 1.0 || s <= 0.0) {
uv.setRandom();
s = uv.dot(uv);
}
double scale = sqrt(-2.0 * log(s) / s);
x[i] = scale * uv[0];
x[i + 1] = scale * uv[1];
}
if (x.size() % 2 == 1) {
uv.setRandom();
double s = uv.dot(uv);
while (s >= 1.0 || s <= 0.0) {
uv.setRandom();
s = uv.dot(uv);
}
double scale = sqrt(-2.0 * log(s) / s);
x[x.size() - 1] = scale * uv[0];
}
// rescale unit gaussian
x = (*_mL) * x + _mu;
}
int intersect2D_Segments( const Vector2d &p1, const Vector2d &p2,
const Vector2d &p3, const Vector2d &p4,
Vector2d &I0, Vector2d &I1,
double maxerr)
{
Vector2d u = p2 - p1;
Vector2d v = p4 - p3;
Vector2d w = p1 - p3;
double D = perp(u,v);
double t0, t1; // Temp vars for parametric checks
// test if they are parallel (includes either being a point)
if (abs(D) < maxerr) { // S1 and S2 are parallel
if (perp(u,w) != 0 || perp(v,w) != 0) {
return 0; // they are NOT collinear
}
// they are collinear or degenerate
// check if they are degenerate points
double du = u.dot(u);
double dv = v.dot(v);
if (du==0 && dv==0) { // both segments are points
if (p1 != p3) // they are distinct points
return 0;
I0 = p1; // they are the same point
return 1;
}
if (du==0) { // S1 is a single point
if (!inSegment(p1, p3, p4)) // but is not in S2
return 0;
I0 = p1;
return 1;
}
if (dv==0) { // S2 a single point
if (!inSegment(p3, p1,p2)) // but is not in S1
return 0;
I0 = p3;
return 1;
}
// they are collinear segments - get overlap (or not)
Vector2d w2 = p2 - p3;
if (v.x() != 0) {
t0 = w.x() / v.x();
t1 = w2.x() / v.x();
}
else {
t0 = w.y() / v.y();
t1 = w2.y() / v.y();
}
if (t0 > t1) { // must have t0 smaller than t1
double t=t0; t0=t1; t1=t; // swap if not
}
if (t0 > 1 || t1 < 0) {
return 0; // NO overlap
}
t0 = t0<0? 0 : t0; // clip to min 0
t1 = t1>1? 1 : t1; // clip to max 1
if (t0 == t1) { // intersect is a point
I0 = p3 + v*t0;
return 1;
}
// they overlap in a valid subsegment
I0 = p3 + v*t0;
I1 = p3 + v*t1;
return 2;
} // end parallel
bool outside = false;
// the segments are skew and may intersect in a point
// get the intersect parameter for S1
t0 = perp(v,w) / D;
if (t0 < 0 || t0 > 1) // no intersect in S1
outside = true;
//return 0;
// get the intersect parameter for S2
t1 = perp(u,w) / D;
if (t1 < 0 || t1 > 1) // no intersect in S2
outside = true;
// return 0;
I0 = p1 + u * t0; // compute S1 intersect point
if (outside) return 3;
return 1;
}
// error function for distance cost
double Con2dP2::calcErrDist(const Node2d &nd0, const Node2d &nd1)
{
Vector2d derr = nd0.w2n * nd1.trans - tmean;
return derr.dot(derr);
}
bool Matcher::findEpipolarMatchDirect(
const Frame& ref_frame,
const Frame& cur_frame,
const Feature& ref_ftr,
const double d_estimate,
const double d_min,
const double d_max,
double& depth)
{
SE3 T_cur_ref = cur_frame.T_f_w_ * ref_frame.T_f_w_.inverse();
int zmssd_best = PatchScore::threshold();
Vector2d uv_best;
// Compute start and end of epipolar line in old_kf for match search, on unit plane!
Vector2d A = vk::project2d(T_cur_ref * (ref_ftr.f*d_min));
Vector2d B = vk::project2d(T_cur_ref * (ref_ftr.f*d_max));
epi_dir_ = A - B;
// Compute affine warp matrix
warp::getWarpMatrixAffine(
*ref_frame.cam_, *cur_frame.cam_, ref_ftr.px, ref_ftr.f,
d_estimate, T_cur_ref, ref_ftr.level, A_cur_ref_);
// feature pre-selection
reject_ = false;
if(ref_ftr.type == Feature::EDGELET && options_.epi_search_edgelet_filtering)
{
const Vector2d grad_cur = (A_cur_ref_ * ref_ftr.grad).normalized();
const double cosangle = fabs(grad_cur.dot(epi_dir_.normalized()));
if(cosangle < options_.epi_search_edgelet_max_angle) {
reject_ = true;
return false;
}
}
search_level_ = warp::getBestSearchLevel(A_cur_ref_, Config::nPyrLevels()-1);
// Find length of search range on epipolar line
Vector2d px_A(cur_frame.cam_->world2cam(A));
Vector2d px_B(cur_frame.cam_->world2cam(B));
epi_length_ = (px_A-px_B).norm() / (1<<search_level_);
// Warp reference patch at ref_level
warp::warpAffine(A_cur_ref_, ref_frame.img_pyr_[ref_ftr.level], ref_ftr.px,
ref_ftr.level, search_level_, halfpatch_size_+1, patch_with_border_);
createPatchFromPatchWithBorder();
if(epi_length_ < 2.0)
{
px_cur_ = (px_A+px_B)/2.0;
Vector2d px_scaled(px_cur_/(1<<search_level_));
bool res;
if(options_.align_1d)
res = feature_alignment::align1D(
cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
else
res = feature_alignment::align2D(
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
if(res)
{
px_cur_ = px_scaled*(1<<search_level_);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
return true;
}
return false;
}
size_t n_steps = epi_length_/0.7; // one step per pixel
Vector2d step = epi_dir_/n_steps;
if(n_steps > options_.max_epi_search_steps)
{
printf("WARNING: skip epipolar search: %zu evaluations, px_lenght=%f, d_min=%f, d_max=%f.\n",
n_steps, epi_length_, d_min, d_max);
return false;
}
// for matching, precompute sum and sum2 of warped reference patch
int pixel_sum = 0;
int pixel_sum_square = 0;
PatchScore patch_score(patch_);
// now we sample along the epipolar line
Vector2d uv = B-step;
Vector2i last_checked_pxi(0,0);
++n_steps;
for(size_t i=0; i<n_steps; ++i, uv+=step)
{
Vector2d px(cur_frame.cam_->world2cam(uv));
Vector2i pxi(px[0]/(1<<search_level_)+0.5,
px[1]/(1<<search_level_)+0.5); // +0.5 to round to closest int
if(pxi == last_checked_pxi)
continue;
last_checked_pxi = pxi;
// check if the patch is full within the new frame
if(!cur_frame.cam_->isInFrame(pxi, patch_size_, search_level_))
continue;
// TODO interpolation would probably be a good idea
uint8_t* cur_patch_ptr = cur_frame.img_pyr_[search_level_].data
+ (pxi[1]-halfpatch_size_)*cur_frame.img_pyr_[search_level_].cols
+ (pxi[0]-halfpatch_size_);
int zmssd = patch_score.computeScore(cur_patch_ptr, cur_frame.img_pyr_[search_level_].cols);
if(zmssd < zmssd_best) {
zmssd_best = zmssd;
uv_best = uv;
}
}
if(zmssd_best < PatchScore::threshold())
{
if(options_.subpix_refinement)
{
px_cur_ = cur_frame.cam_->world2cam(uv_best);
Vector2d px_scaled(px_cur_/(1<<search_level_));
bool res;
if(options_.align_1d)
res = feature_alignment::align1D(
cur_frame.img_pyr_[search_level_], (px_A-px_B).cast<float>().normalized(),
patch_with_border_, patch_, options_.align_max_iter, px_scaled, h_inv_);
else
res = feature_alignment::align2D(
cur_frame.img_pyr_[search_level_], patch_with_border_, patch_,
options_.align_max_iter, px_scaled);
if(res)
{
px_cur_ = px_scaled*(1<<search_level_);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, cur_frame.cam_->cam2world(px_cur_), depth))
return true;
}
return false;
}
px_cur_ = cur_frame.cam_->world2cam(uv_best);
if(depthFromTriangulation(T_cur_ref, ref_ftr.f, vk::unproject2d(uv_best).normalized(), depth))
return true;
}
return false;
}
