47 BoundaryCheck(
bool checkLocal0,
bool checkLocal1,
double tolerance0 = 0,
48 double tolerance1 = 0);
56 operator bool()
const {
return (
m_type != Type::eNone); }
69 template <
typename Vector2DContainer>
96 template <
typename Vector2DContainer>
98 const Vector2DContainer& vertices)
const;
143 template <
typename Vector2DContainer>
145 const Vector2D&
point,
const Vector2DContainer& vertices)
const;
180 return m_weight.inverse();
186 m_type(check ?
Type::eAbsolute :
Type::eNone) {}
189 double tolerance0,
double tolerance1)
191 m_tolerance(checkLocal0 ? tolerance0 :
DBL_MAX,
192 checkLocal1 ? tolerance1 :
DBL_MAX),
193 m_type(
Type::eAbsolute) {}
197 : m_weight(localCovariance.inverse()),
198 m_tolerance(sigmaMax, 0),
199 m_type(
Type::eChi2) {}
204 if (m_type == Type::eAbsolute) {
207 bc.
m_tolerance = (jacobian * m_tolerance).cwiseAbs();
210 (jacobian * m_weight.inverse() * jacobian.transpose()).inverse();
215 template <
typename Vector2DContainer>
217 const Vector2D&
point,
const Vector2DContainer& vertices)
const {
218 if (m_type == Type::eNone) {
224 }
else if (m_tolerance ==
Vector2D(0., 0.)) {
239 auto closestPoint = computeClosestPointOnPolygon(point, vertices);
240 return isTolerated(closestPoint - point);
252 if (m_type == Type::eNone || m_type == Type::eAbsolute) {
255 computeEuclideanClosestPointOnRectangle(point, lowerLeft, upperRight);
259 Vector2D vertices[] = {{lowerLeft[0], lowerLeft[1]},
260 {upperRight[0], lowerLeft[1]},
261 {upperRight[0], upperRight[1]},
262 {lowerLeft[0], upperRight[1]}};
263 closestPoint = computeClosestPointOnPolygon(point, vertices);
266 return isTolerated(closestPoint - point);
270 template <
typename Vector2DContainer>
274 double d = squaredNorm(point - computeClosestPointOnPolygon(point, vertices));
282 if (m_type == Type::eNone || m_type == Type::eAbsolute) {
284 double d = (point - computeEuclideanClosestPointOnRectangle(
285 point, lowerLeft, upperRight))
293 Vector2D vertices[] = {{lowerLeft[0], lowerLeft[1]},
294 {upperRight[0], lowerLeft[1]},
295 {upperRight[0], upperRight[1]},
296 {lowerLeft[0], upperRight[1]}};
297 return distance(point, vertices);
302 if (m_type == Type::eNone) {
304 }
else if (m_type == Type::eAbsolute) {
305 return (
std::abs(delta[0]) <= m_tolerance[0]) &&
306 (
std::abs(delta[1]) <= m_tolerance[1]);
309 return (squaredNorm(delta) < (2 * m_tolerance[0]));
314 return (x.transpose() * m_weight *
x).
value();
317 template <
typename Vector2DContainer>
322 auto closestOnSegment = [&](
auto&& ll0,
auto&& ll1) {
325 auto weighted_n = m_weight *
n;
326 auto f = n.dot(weighted_n);
327 auto u = std::isnormal(
f)
328 ? (point - ll0).dot(weighted_n) /
f
334 auto iv = std::begin(vertices);
337 Vector2D closest = closestOnSegment(l0, l1);
338 auto closestDist = squaredNorm(closest - point);
340 for (++iv; iv != std::end(vertices); ++iv) {
343 Vector2D current = closestOnSegment(l0, l1);
344 auto currentDist = squaredNorm(current - point);
345 if (currentDist < closestDist) {
347 closestDist = currentDist;
351 Vector2D last = closestOnSegment(l1, *std::begin(vertices));
352 if (squaredNorm(last - point) < closestDist) {
380 double l0 = point[0], l1 = point[1];
381 double loc0Min = lowerLeft[0], loc0Max = upperRight[0];
382 double loc1Min = lowerLeft[1], loc1Max = upperRight[1];
385 if (loc0Min <= l0 && l0 < loc0Max && loc1Min <= l1 && l1 < loc1Max) {
387 double dist =
std::abs(loc0Max - l0);
404 return {l0, loc1Min};
411 return {loc0Max, loc1Max};
412 }
else if (l1 <= loc1Min) {
413 return {loc0Max, loc1Min};
415 return {loc0Max, l1};
417 }
else if (l0 < loc0Min) {
419 return {loc0Min, loc1Max};
420 }
else if (l1 <= loc1Min) {
421 return {loc0Min, loc1Min};
423 return {loc0Min, l1};
427 return {l0, loc1Max};
429 return {l0, loc1Min};