//! Return the range of numbers that might be used with this format to
//! represent a number within `x`.
Interval FloatFormat::convert (const Interval& x) const
{
Interval ret;
Interval tmp = x;
if (x.hasNaN())
{
// If NaN might be supported, NaN is a legal return value
if (m_hasNaN != NO)
ret |= TCU_NAN;
// If NaN might not be supported, any (non-NaN) value is legal,
// _subject_ to clamping. Hence we modify tmp, not ret.
if (m_hasNaN != YES)
tmp = Interval::unbounded();
}
// Round both bounds _inwards_ to closest representable values.
if (!tmp.empty())
ret |= clampValue(round(tmp.lo(), true)) | clampValue(round(tmp.hi(), false));
// If this format's precision is not exact, the (possibly out-of-bounds)
// original value is also a possible result.
if (!m_exactPrecision)
ret |= x;
return ret;
}
/**
Calculates the intersection of this interval with another.
@param rhs The other interval
@return The intersection of the two intervals
*/
Interval Interval::intersect(const Interval& rhs) const
{
if(empty() || rhs.empty()) return Interval();
double largerLow = std::max(m_low, rhs.m_low);
double smallerHigh = std::min(m_high, rhs.m_high);
return Interval(largerLow, smallerHigh);
}
//! Round output of an operation.
//! \param roundUnderOverflow Can +/-inf rounded to min/max representable;
//! should be false if any of operands was inf, true otherwise.
Interval FloatFormat::roundOut (const Interval& x, bool roundUnderOverflow) const
{
Interval ret = x.nan();
if (!x.empty())
ret |= Interval(roundOut(x.lo(), false, roundUnderOverflow),
roundOut(x.hi(), true, roundUnderOverflow));
return ret;
}
std::string FloatFormat::intervalToHex (const Interval& interval) const
{
if (interval.empty())
return interval.hasNaN() ? "{ NaN }" : "{}";
else if (interval.lo() == interval.hi())
return (std::string(interval.hasNaN() ? "{ NaN, " : "{ ") +
floatToHex(interval.lo()) + " }");
else if (interval == Interval::unbounded(true))
return "<any>";
return (std::string(interval.hasNaN() ? "{ NaN } | " : "") +
"[" + floatToHex(interval.lo()) + ", " + floatToHex(interval.hi()) + "]");
}
void ManhattanGroundTruth::ProcessWalls(const proto::FloorPlan& fp,
const PosedCamera& pc) {
// Initialize
nocclusions = 0;
nwalls = 0;
// First strip out the NaN vertices
vector<Vec2> verts;
for (int i = 0; i < fp.vertices_size(); i++) {
Vec2 v = asToon(fp.vertices(i));
if (!isnan(v[0]) && !isnan(v[1])) {
verts.push_back(v);
}
}
// Compute rectifier in retina coords
Mat3 ret_vrect = GetVerticalRectifierInRetina(pc);
Mat3 ret_vrect_inv = LU<3>(ret_vrect).get_inverse();
Bounds2D<> ret_vrect_bounds = Bounds2D<>::ComputeBoundingBox
(pc.retina_bounds().GetPolygon().Transform(ret_vrect));
Interval xbounds(ret_vrect_bounds.left(), ret_vrect_bounds.right());
// Note that the floorplan vertices do _not_ wrap around
Vec3 focal_line = makeVector(0, -1, 1e-6);
vector<Vec3> lines;
vector<Interval> intervals;
for (int i = 0; i < verts.size()-1; i++) {
Vec3 a_cam = ret_vrect * (pc.pose() * concat(verts[i], 0.0));
Vec3 b_cam = ret_vrect * (pc.pose() * concat(verts[i+1], 0.0));
Vec3 a_flat = makeVector(a_cam[0], a_cam[2], 1.0); // y no longer matters, and now homog
Vec3 b_flat = makeVector(b_cam[0], b_cam[2], 1.0); // y no longer matters, and now homog
bool nonempty = ClipAgainstLine(a_flat, b_flat, focal_line, -1);
if (nonempty) {
// project() must come after clipping
Vec2 a0 = project(a_flat);
Vec2 b0 = project(b_flat);
intervals.push_back(Interval(a0[0]/a0[1], b0[0]/b0[1]));
lines.push_back(a_flat ^ b_flat);
}
}
// Compare polygons
MatI interval_comp(verts.size()-1, verts.size()-1);
for (int i = 0; i < intervals.size(); i++) {
for (int j = 0; j < intervals.size(); j++) {
if (i==j) continue;
Interval isct = Intersect(intervals[i], intervals[j]);
if (isct.empty()) {
interval_comp[i][j] = i>j ? 1 : -1;
} else {
double m = (isct.hi + isct.lo) / 2.;
double zi = -lines[i][2] / (lines[i][0]*m + lines[i][1]);
double zj = -lines[j][2] / (lines[j][0]*m + lines[j][1]);
interval_comp[i][j] = zi > zj ? 1 : -1;
}
}
}
// Generate tokens
vector<Token> tokens;
for (int i = 0; i < intervals.size(); i++) {
tokens.push_back(Token(intervals[i].lo, i, true));
tokens.push_back(Token(intervals[i].hi, i, false));
}
// Add tokens for left and right image bounds
tokens.push_back(Token(xbounds.lo, -1, true));
tokens.push_back(Token(xbounds.hi, -1, false));
sort_all(tokens);
// Walk from left to right
vector<Vec2> xs;
int prev = -1;
bool inbounds = false;
bool done = false;
VecI active(intervals.size(), 0);
TableComp comp(interval_comp);
priority_queue<int,vector<int>,TableComp> heap(comp);
for (int i = 0; !done; i++) {
// Process next token
bool bound_token = tokens[i].index == -1;
if (bound_token && tokens[i].activate) {
inbounds = true;
} else if (bound_token && !tokens[i].activate) {
done = true;
} else if (tokens[i].activate) {
active[tokens[i].index] = true;
heap.push(tokens[i].index);
} else {
active[tokens[i].index] = false;
while (!heap.empty() && !active[heap.top()]) { // okay for heap to be temporarily empty
heap.pop();
}
}
double xcur = tokens[i].x;
if ((tokens[i+1].x-xcur) > 1e-8) {
int cur = heap.empty() ? -1 : heap.top();
// Count walls
if (inbounds && (cur != prev || bound_token)) {
if (!done) {
nwalls++;
if (!bound_token && RingDist<int>(prev, cur, intervals.size()) > 1) {
nocclusions++;
}
}
// Reconstruct vertices
vector<int> to_add;
if (!xs.empty()) {
to_add.push_back(prev);
}
if (!done) {
to_add.push_back(cur);
}
// Compute vertices
BOOST_FOREACH(int j, to_add) {
const Vec3& line = lines[j];
const Interval& intv = intervals[j];
Vec2 pflat = project(line ^ makeVector(-1, xcur, 0));
Vec3 pcam = makeVector(pflat[0], 0, pflat[1]);
Vec3 pworld = pc.pose_inverse() * (ret_vrect_inv * pcam);
xs.push_back(pworld.slice<0,2>());
}
}
prev = cur;
}
}
void NavMeshGenerator::determine_links()
{
for(EdgePlaneTable::const_iterator it=m_edgePlaneTable.begin(), iend=m_edgePlaneTable.end(); it!=iend; ++it)
{
// Generate (n,u,v) coordinate system for plane, where v = (0,0,1).
const Plane& plane = it->first;
OrthonormalCoordSystem2D coordSystem(plane);
// Check pairs of different-facing edges to see whether we need to create any links.
const EdgeReferences& sameFacingEdgeRefs = it->second.sameFacing;
const EdgeReferences& oppFacingEdgeRefs = it->second.oppFacing;
int sameFacingEdgeRefCount = static_cast<int>(sameFacingEdgeRefs.size());
int oppFacingEdgeRefCount = static_cast<int>(oppFacingEdgeRefs.size());
for(int j=0; j<sameFacingEdgeRefCount; ++j)
{
const EdgeReference& edgeJ = sameFacingEdgeRefs[j];
NavPolygon& navPolyJ = *m_walkablePolygons[edgeJ.navPolyIndex];
const CollisionPolygon& colPolyJ = *m_polygons[navPolyJ.collision_poly_index()];
int mapIndexJ = colPolyJ.auxiliary_data().map_index();
// Calculate the 2D coordinates of edgeJ in the plane.
const Vector3d& p1J = colPolyJ.vertex(edgeJ.startVertex);
const Vector3d& p2J = colPolyJ.vertex((edgeJ.startVertex+1) % colPolyJ.vertex_count());
Vector2d q1J = coordSystem.from_canonical(p1J), q2J = coordSystem.from_canonical(p2J);
Interval xIntervalJ(std::min(q1J.x,q2J.x), std::max(q1J.x,q2J.x));
for(int k=0; k<oppFacingEdgeRefCount; ++k)
{
const EdgeReference& edgeK = oppFacingEdgeRefs[k];
NavPolygon& navPolyK = *m_walkablePolygons[edgeK.navPolyIndex];
const CollisionPolygon& colPolyK = *m_polygons[navPolyK.collision_poly_index()];
int mapIndexK = colPolyK.auxiliary_data().map_index();
// We only want to create links between polygons in the same map.
if(mapIndexJ != mapIndexK) continue;
// Calculate the 2D coordinates of edgeK in the plane.
const Vector3d& p1K = colPolyK.vertex(edgeK.startVertex);
const Vector3d& p2K = colPolyK.vertex((edgeK.startVertex+1) % colPolyK.vertex_count());
Vector2d q1K = coordSystem.from_canonical(p1K), q2K = coordSystem.from_canonical(p2K);
// Calculate the x overlap between the 2D edges. If there's no overlap,
// then we don't need to carry on looking for a link.
Interval xIntervalK(std::min(q1K.x,q2K.x), std::max(q1K.x,q2K.x));
Interval xOverlap = xIntervalJ.intersect(xIntervalK);
if(xOverlap.empty()) continue;
// Calculate the segments for the various types of link.
LinkSegments linkSegments = calculate_link_segments(q1J, q2J, q1K, q2K, xOverlap);
// Add the appropriate links.
if(linkSegments.stepDownSourceToDestSegment)
{
assert(linkSegments.stepUpDestToSourceSegment != NULL);
// Add a step down link from j -> k, and a step up one from k -> j.
Vector3d j1 = coordSystem.to_canonical(linkSegments.stepDownSourceToDestSegment->e1);
Vector3d j2 = coordSystem.to_canonical(linkSegments.stepDownSourceToDestSegment->e2);
Vector3d k1 = coordSystem.to_canonical(linkSegments.stepUpDestToSourceSegment->e1);
Vector3d k2 = coordSystem.to_canonical(linkSegments.stepUpDestToSourceSegment->e2);
add_nav_link(NavLink_Ptr(new StepDownLink(edgeJ.navPolyIndex, edgeK.navPolyIndex, j1, j2, k1, k2)));
add_nav_link(NavLink_Ptr(new StepUpLink(edgeK.navPolyIndex, edgeJ.navPolyIndex, k1, k2, j1, j2)));
}
if(linkSegments.stepUpSourceToDestSegment)
{
assert(linkSegments.stepDownDestToSourceSegment != NULL);
// Add a step up link from j -> k, and a step down one from k -> j.
Vector3d j1 = coordSystem.to_canonical(linkSegments.stepUpSourceToDestSegment->e1);
Vector3d j2 = coordSystem.to_canonical(linkSegments.stepUpSourceToDestSegment->e2);
Vector3d k1 = coordSystem.to_canonical(linkSegments.stepDownDestToSourceSegment->e1);
Vector3d k2 = coordSystem.to_canonical(linkSegments.stepDownDestToSourceSegment->e2);
add_nav_link(NavLink_Ptr(new StepUpLink(edgeJ.navPolyIndex, edgeK.navPolyIndex, j1, j2, k1, k2)));
add_nav_link(NavLink_Ptr(new StepDownLink(edgeK.navPolyIndex, edgeJ.navPolyIndex, k1, k2, j1, j2)));
}
if(linkSegments.walkSegment)
{
// Add a walk link from j -> k, and one from k -> j.
Vector3d e1 = coordSystem.to_canonical(linkSegments.walkSegment->e1);
Vector3d e2 = coordSystem.to_canonical(linkSegments.walkSegment->e2);
add_nav_link(NavLink_Ptr(new WalkLink(edgeJ.navPolyIndex, edgeK.navPolyIndex, e1, e2)));
add_nav_link(NavLink_Ptr(new WalkLink(edgeK.navPolyIndex, edgeJ.navPolyIndex, e1, e2)));
}
}
}
}
}