Portal Example Source Files

Table of Contents

• Portal Example Source Files

  • main.cpp

  • Portal.h

  • Portal.cpp

  • PortalPair.h

  • PortalPair.cpp

main.cpp

/*
 * Copyright (c) 2014-2023 National Technology and Engineering
 * Solutions of Sandia, LLC. Under the terms of Contract DE-NA0003525
 * with National Technology and Engineering Solutions of Sandia, LLC,
 * the U.S. Government retains certain rights in this software.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "Portal.h"
#include "PortalPair.h"

#include <tracktable/CommandLineFactories/AssemblerFromCommandLine.h>
#include <tracktable/CommandLineFactories/PointReaderFromCommandLine.h>
#include <tracktable/Domain/Terrestrial.h>

#include <boost/timer/timer.hpp>

using TrajectoryT = tracktable::domain::terrestrial::trajectory_type;
using PointT = typename TrajectoryT::point_type;
using PointReaderT = tracktable::PointReader<PointT>;
using PointReaderIteratorT = typename PointReaderT::iterator;
using AssemblerT = tracktable::AssembleTrajectories<TrajectoryT, PointReaderIteratorT>;

static constexpr auto helpmsg = R"(
--------------------------------------------------------------------------------
The portal example takes trajectory data and attempts to find origin/destination
pairs. It breaks the USA into a grid, identifies what cells are populated by trajectories
and then refines the grid based on desired parameters. Each level of 'depth' is an
additional layer of refinement of the original grid. Each level is divided into
'bin-count' sections in both longitude and latitude. So each the number of cells:

cells = 12*5*bins^(2+depth)

empty cells are dropped but a cell is only empty if no trajectories pass through it

The portal example demonstrates:
    - Using command line factories to read points and assemble trajectories
    - Using boost program options to take parameters from command lines(in addition to the factories)
    - Use of boost::geometry::intersects to test where trajectories overlap regions

Typical use:
    ./portal-- input=/data/flights.tsv --depth=5 --min-value=12 --min-seperation=10 --bin-count=2

Defaults assume a tab separated file formatted as :

OBJECTID TIMESTAMP LON LAT
--------------------------------------------------------------------------------)";

int main(int _argc, char* _argv[]) {
    constexpr auto timerFormat = "\u001b[30;1m %w seconds\u001b[0m\n";
    // Set log level to reduce unecessary output
    tracktable::set_log_level(tracktable::log::info);
    // Create a basic command line option with boost
    boost::program_options::options_description commandLineOptions;
    commandLineOptions.add_options()("help", "Print help");
    // Create command line factories
    tracktable::PointReaderFromCommandLine<PointT> readerFactory;
    tracktable::AssemblerFromCommandLine<TrajectoryT> assemblerFactory;
    // Add options from the factories
    readerFactory.addOptions(commandLineOptions);
    assemblerFactory.addOptions(commandLineOptions);
    /** Boost program options using a variable map to tie everything together.
     * one parse will have a single variable map. We need to let the factories know
     * about this variable map so they can pull information out of it */
    auto vm = std::make_shared<boost::program_options::variables_map>();
    readerFactory.setVariables(vm);
    assemblerFactory.setVariables(vm);

    // Portal specific configuration
    auto seperationDistance = 10.0;
    auto depth = 5u;
    auto binSize = 2u;
    auto minValue = 16u;
    boost::program_options::options_description portalOptions("Portals");
    // clang-format off
    portalOptions.add_options()
        ("portal-sep", bpo::value(&seperationDistance)->default_value(10), "Set minimum portal separation distance (in lat-lon)")
        ("depth", bpo::value(&depth)->default_value(5), "Set depth for portal decomposition")
        ("bin-count", bpo::value(&binSize)->default_value(2), "Portal chopping factor (default is 2)")
        ("min-value", bpo::value(&minValue)->default_value(16), "Minumum number of portal pairs (default is 16)")
    ;
    // clang-format on

    // Parse the command lines, don't forget the 'notify' after
    try {
        // We use this try/catch to automatically display help when an unknown option is used
        boost::program_options::store(
            boost::program_options::command_line_parser(_argc, _argv).options(commandLineOptions).run(), *vm);
        boost::program_options::notify(*vm);
    } catch (boost::program_options::error e) {
        std::cerr << e.what();
        std::cerr << helpmsg << "\n\n";
        std::cerr << commandLineOptions << std::endl;
        return 1;
    }
    /** Parsing will give an error of an incorrect option is used, but it won't
     * display the help unless we tell it too */
    if (vm->count("help") != 0) {
        std::cerr << helpmsg << "\n\n";
        std::cerr << commandLineOptions << std::endl;
        return 1;
    }

    // Create Point Reader and assembler
    auto pointReader = readerFactory.createPointReader();
    auto assembler = assemblerFactory.createAssembler(pointReader);

    std::vector<std::shared_ptr<TrajectoryT>> trajectories = {};
    // This block exists for easy timing of trajectory assembling using the boost auto timer
    // Note that all feedback to the user is done on std::cerr, this allows us to only
    // put desired results into std::cout, this make processing output easier.
    {
        std::cerr << "Assemble Trajectories" << std::endl;
        boost::timer::auto_cpu_timer assembleTimer(std::cerr, timerFormat);
        auto count = 0u;
        std::cerr << std::right;
        for (auto tIter = assembler->begin(); tIter != assembler->end(); ++tIter) {
            if (tracktable::length(*tIter) < 100) {
                continue;
            }
            std::cerr << "\b\b\b\b\b\b\b\b\b\b" << std::setw(10)  // Using backspaces for in place counter
                      << count++;
            trajectories.push_back(std::make_shared<TrajectoryT>(*tIter));
        }
        std::cerr << std::left << "\nStarting with " << trajectories.size() << " trajectories" << std::endl;
    }

    // Create box for the USA
    PointT lowerLeft(-125.0, 25.0);  // lower left of USA
    PointT upperRight(-65.0, 50.0);  // upper right of USA
    auto USA = std::make_shared<Portal>(boost::geometry::model::box<PointT>(lowerLeft, upperRight));
    USA->level = 0;
    PairHeap pairs;
    pairs.minimumSeperation = seperationDistance;
    pairs.minimumValue = minValue;
    pairs.depth = depth;
    pairs.xDivisions = binSize;
    pairs.yDivisions = binSize;
    {
        std::cerr << "Initializing Pair Heap" << std::endl;
        boost::timer::auto_cpu_timer initializeTimer(std::cerr, timerFormat);
        pairs.initialize(trajectories, USA);
    }
    {
        std::cerr << "Finding Portals" << std::endl;
        boost::timer::auto_cpu_timer findTimer(std::cerr, timerFormat);
        pairs.find_portals();
    }
    return 0;
}

---

Portal.h

/*
 * Copyright (c) 2014-2023 National Technology and Engineering
 * Solutions of Sandia, LLC. Under the terms of Contract DE-NA0003525
 * with National Technology and Engineering Solutions of Sandia, LLC,
 * the U.S. Government retains certain rights in this software.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#ifndef Portal_h
#define Portal_h

#include <tracktable/Domain/Terrestrial.h>

#include <boost/geometry/geometries/box.hpp>
#include <boost/geometry/geometries/register/box.hpp>

#include <list>
#include <memory>
#include <set>

using TrajectoryT = tracktable::domain::terrestrial::trajectory_type;
using PointT = typename TrajectoryT::point_type;
using BoxT = boost::geometry::model::box<PointT>;
using TrajectoryPtrT = std::shared_ptr<TrajectoryT>;

/**
 * @brief A portal is simply a box defined by two points
 * A portal has a set of trajectories that intersect it and the potentail to have a set of 'children' which
 * are fully contained subdivisions
 */
class Portal : public BoxT {
   public:
    Portal() = delete;
    Portal(const BoxT &b) : BoxT(b){};

    /// Sort Portals basic on how many trajectories they contain
    bool operator<(const Portal &p) const { return (trajectories.size() < p.trajectories.size()); }
    /** @brief Divides portal into x*y children portals and assign trajectories accordingly
     * @param _xDivision number of divisions in the x(longitude) direction
     * @param _yDivisions number of divisions in the y(latitude) direction */
    void divide(size_t _xDivision, size_t _yDivisions);
    /** @brief Adds a list of trajectories to the portal
     * @param _addList */
    void add_trajectories(std::vector<TrajectoryPtrT> &_addList);
    /** @brief Will assign the trajectory to this portal (does not check intersection) then adds the
     * trajectory to all children (based on intersection)
     * @note if the trajectory does not intersect the top level, it will still be added while not existing
     * in any of the children (impact unknown)
     * @param _t The trajectory to add */
    void add_trajectory(const TrajectoryPtrT &_t);
    /** @brief Removes a list of trajectories from the portal
     * @param _removeList */
    void remove_trajectories(const std::vector<TrajectoryPtrT> &_removeList);
    /** @brief Removes a specific trajectory from portal and all children
     * @param _t trajectory to remove */
    void remove_trajectory(const TrajectoryPtrT &_t);

   public:
    size_t level;                                  ///< The level this portal is at
    std::set<TrajectoryPtrT> trajectories;         ///< Trajectories that intersect this portal
    std::list<std::shared_ptr<Portal> > children;  ///< Children portals (if any)
};

/// How we pass portal references around
using PortalPtrT = std::shared_ptr<Portal>;
/// Overide default sorting for PortalPtrT
bool operator<(const PortalPtrT &p1, const PortalPtrT &p2);

BOOST_GEOMETRY_REGISTER_BOX(Portal, PointT, min_corner(), max_corner());

#endif  // portal_h

---

Portal.cpp

#include "Portal.h"

#include <boost/geometry/arithmetic/arithmetic.hpp>

void Portal::divide(size_t _xDivisions, size_t _yDivisions) {
    /** notes about boost geomtry arithmetic;
     * the first point is piecwise modified by the second point
     * to preserve the original point it is necessary to make copies first
     */
    auto delta = this->max_corner();
    boost::geometry::subtract_point(delta, this->min_corner());
    boost::geometry::divide_point(delta,PointT(_xDivisions,_yDivisions));

    for (auto i = 0u; i < _xDivisions; ++i) {
        for (auto j = 0u; j < _yDivisions; ++j) {
            auto ll = this->min_corner();
            auto d = delta;
            boost::geometry::multiply_point(d,PointT(i,j));
            boost::geometry::add_point(ll,d);

            auto ur = ll;
            boost::geometry::add_point(ur,delta);

            auto p = std::make_shared<Portal>(BoxT(ll, ur));
            p->level = this->level + 1;
            // Now, go through all of the trajectories associated with the parent portal
            // and assign them to the child by testing intersection.
            for (auto &t : this->trajectories) {
                if (boost::geometry::intersects(*t, *p)) {
                    p->trajectories.insert(t);
                }
            }
            // only keep the child if it's non empty
            if (!p->trajectories.empty()) {
                this->children.push_back(p);
            }
        }
    }
}

void Portal::add_trajectories(std::vector<TrajectoryPtrT> &_addList) {
    for (auto &t : _addList) {
        add_trajectory(t);
    }
}
void Portal::add_trajectory(const TrajectoryPtrT &_t) {
    trajectories.insert(_t);
    for (auto &c : children) {
        if (boost::geometry::intersects(*_t, *c)) {
            c->add_trajectory(_t);
        }
    }
}

void Portal::remove_trajectories(const std::vector<TrajectoryPtrT> &_removeList) {
    for (auto &t : _removeList) {
        remove_trajectory( t);
    }
}

void Portal::remove_trajectory(const TrajectoryPtrT &_t) {
    if (0 == trajectories.erase(_t)) {
        return; //if it doesn't intersect the parent, it doesn't intersect any of the children
    }
    for (auto &c : children) {
        c->remove_trajectory(_t);
    }
}

bool operator<(const PortalPtrT &p1, const PortalPtrT &p2) {
    return p1->trajectories.size() < p2->trajectories.size();
}

---

PortalPair.h

/*
 * Copyright (c) 2014-2023 National Technology and Engineering
 * Solutions of Sandia, LLC. Under the terms of Contract DE-NA0003525
 * with National Technology and Engineering Solutions of Sandia, LLC,
 * the U.S. Government retains certain rights in this software.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#ifndef PortalPair_h
#define PortalPair_h

#include "Portal.h"

#include <algorithm>
#include <queue>

using TrajectoryIterT = TrajectoryT::iterator;

/** Used to trak and rank pairs of portal
 * The value of a pair represents how many trajectories travel between the two
 * portals */
class PortalPair {
   public:
    PortalPair(const PortalPtrT &_p1, const PortalPtrT &_p2) {
        if (_p1->trajectories.size() > _p2->trajectories.size()) {
            p1 = _p1;
            p2 = _p2;
        } else {
            p1 = _p2;
            p2 = _p1;
        }
        update_value();
        update_seperation();
    }

    /// Sort based on custom value
    bool operator<(const PortalPair &_other) const {
        return (value * seperation) < (_other.value * _other.seperation);
    }

    /** compute the value of this pair of portals
     * The value is equal to the number of trajectories that seem to start in one portal and end
     * in the other */
    void update_value();
    /** @brief updates the seperation between the two portals
     * No real reason this should ever change with normal use */
    void update_seperation();
    /** @brief Determines the 'innermost' points of a trajectory that connect the two portals
     *
     * @param trajectory Trajectory in question
     * @param first_pt determined first point
     * @param last_pt determined second point
     */
    void get_segment(const TrajectoryPtrT &trajectory, TrajectoryIterT &first_pt, TrajectoryIterT &last_pt) const;
    /** @brief determines if the innermost segment of a trajectory between the two portals constitutes the
     * bulk of the trajectory, this is the criteria for a trajectory being 'from/to' the pair of portals
     *
     * @param _trajectory The trajectory being tested
     * @param _ecc The deviation allowed
     * @return true The trajectory is 'from/to' the portals
     * @return false otherwise
     */
    bool is_within_portal_ellipse(const TrajectoryPtrT &_trajectory, const double &_ecc) const;

   public:
    PortalPtrT p1;                             ///< First Portal
    PortalPtrT p2;                             ///< Second Portal
    size_t value;                              ///< value of portal pair
    double seperation;                         ///< distance between portals
    std::vector<TrajectoryPtrT> contributors;  ///< trajectories that contribute to the value of the pair
};

/** Management object for a set of PortalPairs
 * Based on priority queue which keeps elements sorted
 * allows for management and refinement of pairs and their respective portals
 * @note Highly coupled to PortalPair and Portal
 * @sa PortalPair Portal */
class PairHeap : public std::priority_queue<PortalPair> {
   public:
    /** @brief If possible, subdivides one of the portal in the top ranked pair
     * Returning false implies a 'best' result has bubbled to the top
     *
     * @return true If refinement was done
     * @return false If refinement was unecessary or impossible*/
    bool refine_pairs();
    /** @brief takes a 'refinable' pair and subdivides one them
     * The pair is then destroyed and a new set of pairs between the undivided portal and the newly created
     * children of the divided portal. */
    void refine_top_pair();
    /** @brief MegaPop() removes the top pair from the list and removes any trajectories connecting that pair
     * finally the rest of the heap is rescored*/
    void remove_top_pair();
    /** @brief Does initial intersection of _trajectories and _top
     * Does initial subdivision of _top and assigns trajectories accordingly
     * Creates an initial pair list based on populated divisions
     *
     * @param _trajectories trajectories of interest
     * @param _startingPortal A presumably empty top level portal
     */
    void initialize(std::vector<TrajectoryPtrT> &_trajectories, PortalPtrT &_startingPortal);
    /** @brief goes through the pair heap and corresponding Portal;
     * recursively peeling off the top pair and refining it down to the depth specified
     * in the heap.
     *
     * Every time the top pair has a level equal to the desired depth, it is recorded
     * in a kml file and the corresponding trajectories are removed from all portals
     * in the heirachy
     */
    void find_portals();

   public:
    double minimumSeperation = 10.0;  ///< Minimum Seperation for two portals to be considered a viable pair
    size_t minimumValue = 16u;        ///< Minimum value for a pair to be worth keeping
    size_t xDivisions = 2u;           ///< number of subdivisions to use in the x(longitude) direction
    size_t yDivisions = 2u;           ///< number of subdivisions to use in the y(latitude) direction
    size_t depth = 5u;                ///< maximum depth to subdivide to
    PortalPtrT topPortal;             ///< Pointer to top level portal
};

#endif

---

PortalPair.cpp

/*
 * Copyright (c) 2014-2023 National Technology and Engineering
 * Solutions of Sandia, LLC. Under the terms of Contract DE-NA0003525
 * with National Technology and Engineering Solutions of Sandia, LLC,
 * the U.S. Government retains certain rights in this software.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 * notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
#include "PortalPair.h"

#include <tracktable/RW/KmlOut.h>

#include <boost/geometry/algorithms/intersects.hpp>

void PortalPair::update_value() {
    value = 0;
    contributors.clear();
    std::set_intersection(p1->trajectories.begin(), p1->trajectories.end(), p2->trajectories.begin(),
                          p2->trajectories.end(), std::back_inserter(contributors));

    contributors.erase(
        std::remove_if(contributors.begin(), contributors.end(),
                       [&](const TrajectoryPtrT _t) {
                           if (is_within_portal_ellipse(_t, 1.01)) {
                               ++value;  // cheating by incrementing value from within the lambda
                               return true;
                           }
                           return false;
                       }),
        contributors.end());
}

void PortalPair::update_seperation() { seperation = boost::geometry::distance(*p1, *p2); }

bool PortalPair::is_within_portal_ellipse(const TrajectoryPtrT &_trajectory, const double &_ecc) const {
    TrajectoryT::iterator first_pt, last_pt;
    get_segment(_trajectory, first_pt, last_pt);
    TrajectoryT segment(first_pt, last_pt);

    return (!segment.empty() &&
            (tracktable::length(segment) < _ecc * tracktable::distance(segment.front(), segment.back())));
}

void PortalPair::get_segment(const TrajectoryPtrT &trajectory, TrajectoryIterT &first_pt,
                             TrajectoryIterT &last_pt) const {
    // We know that the trajectory intersects both portals because it is in both portals list and they got
    // added to the list because they intersect
    unsigned int prev = 0;  // Has intersected neither
    TrajectoryIterT cur_box1, cur_box2;

    if (trajectory->size() < 2)
        std::cout << "Warning, trajectory->size() = " << trajectory->size() << std::endl;
    // If the path zigzags in and out of portals, it will remember the two innermost intersections
    for (auto pt = trajectory->begin(); (pt + 1) != trajectory->end(); ++pt) {
        auto segment = boost::geometry::model::segment<PointT>(*pt, *(pt + 1));
        if (boost::geometry::intersects(segment, *p1)) {
            cur_box1 = pt;           // if we were leaving, this was the last point inside the box
            if (prev == 2) {         // means we are entering instead of leaving
                ++cur_box1;          // enter means the other point was the last one inside the box
                last_pt = cur_box1;  // entering the box means the segment is ending here
                first_pt = cur_box2;
            }
            prev = 1;
        }
        if (boost::geometry::intersects(segment, *p2)) {
            cur_box2 = pt;
            if (prev == 1) {  // last intersection was with portal 1
                ++cur_box2;
                first_pt = cur_box1;
                last_pt = cur_box2;
            }
            prev = 2;
        }
    }
    if (0 == prev) {
        std::cout << "Something is very wrong!" << std::endl;
    }
}

bool PairHeap::refine_pairs() {
    if (empty()) return false;
    if (((top().p1->level >= depth) && (top().p2->level >= depth))) {
        if (top().seperation > this->minimumSeperation) {
            return false;
        }
        remove_top_pair();  // Consider removing a pair that is too close as a 'refinement'
    } else {
        refine_top_pair();
    }
    return true;
}

void PairHeap::refine_top_pair() {
    // Decompose the first portal by default (it's the largest), or
    // do the second if the first is already at desired depth
    auto shrink = top().p1;
    auto keep = top().p2;
    if (shrink->level >= depth) {
        assert(top().p2->level < depth);  // check done before call should not allow
        std::swap(shrink, keep);
    }
    pop();

    // If we haven't already created the children in the decomposition, do so
    /* TODO: Is it possible to have it already divided? if it is, should the pairing below be done? should we
     * have popped? */
    if (shrink->children.empty()) {
        shrink->divide(xDivisions, yDivisions);
    }

    // Now reassign the pairs, we do not enforce minimum seperation at this time
    for (auto first = shrink->children.begin(); first != shrink->children.end(); ++first) {
        PortalPair p(*first, keep);
        if (p.value >= minimumValue) {
            push(p);
        }
        for (auto second = std::next(first); second != shrink->children.end(); ++second) {
            PortalPair p2(*first, *second);
            if (p2.value >= minimumValue) {
                push(p2);
            }
        }
    }
}

void PairHeap::remove_top_pair() {
    // remove the contributors to the top pairs value
    topPortal->remove_trajectories(top().contributors);
    // remove the top
    pop();

    // Rescore and filter the heap NOTE: 'c' comes from the base class
    c.erase(std::remove_if(c.begin(), c.end(),
                           [&](PortalPair &_p) {
                               _p.update_value();
                               return _p.value < minimumValue;
                           }),
            c.end());
    // resort the heap and store it like 'priority_queue' prefers.
    std::make_heap(c.begin(), c.end());
}

void PairHeap::initialize(std::vector<TrajectoryPtrT> &_trajectories, PortalPtrT &_startingPortal) {
    topPortal = _startingPortal;
    // Initialize the simulation by making one big portal out of the _startingPortal
    // and then decomposing it.

    for (auto &t : _trajectories) {
        if (boost::geometry::intersects(*t, *topPortal)) {
            topPortal->add_trajectory(t);
        }
    }

    // Note: we are assuming _startingPortal is the USA and has an aspect ratio
    // of 12 by 5.  Using a different aliquot than that in the command below
    // will result in non-square portals.  Not that there is anything wrong
    // with that.

    topPortal->divide(12, 5);

    // Now initialize pair list with all of the children...
    // We do not enforce a separation at this time as it potentials blocks future children with a valid
    // separation
    for (auto first = topPortal->children.begin(); first != topPortal->children.end(); ++first) {
        for (auto second = std::next(first); second != topPortal->children.end(); ++second) {
            PortalPair p(*first, *second);
            if (p.value > minimumValue /* && p.seperation > minimumSeperation*/) {
                push(p);
            }
        }
    }
}

void writeKmlPortalPair(const PortalPair &pp, const std::string &file_name);

void PairHeap::find_portals() {
    static int i = 0;
    while (!empty()) {
        while (refine_pairs())
            ;
        if (!empty()) {
            std::stringstream filename;
            filename << "flights" << i << ".kml";
            writeKmlPortalPair(top(), filename.str());
            remove_top_pair();
            ++i;
        }
    }
}

using tracktable::kml;
void writeKmlPortalPair(const PortalPair &pp, const std::string &file_name) {
    std::ofstream out(file_name.c_str());
    out.precision(15);
    out << kml::header;
    kml::width(3);
    kml::write(out, pp.contributors);

    out << kml::style("Portal", "FF0000FF", 1.0);
    out << kml::startpm();
    out << kml::startmulti();
    out << kml::box(pp.p1->min_corner(), pp.p1->max_corner());
    out << kml::box(pp.p2->min_corner(), pp.p2->max_corner());
    out << kml::stopmulti();
    out << kml::stoppm();
    out << kml::footer;
}