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