Method InitializeTrajectory

plugins/trajectory/filters/PulseCollection.cpp:262–304

}

void PulseCollection::InitializeTrajectory()
{
    if (pulses.empty())
        throw pdal_error("PulseCollection: no pulses for Solve");

    double tstart = std::floor((m_timeMin - m_timeOrigin) / m_args.tblock);
    int num = int(std::ceil((m_timeMax - m_timeOrigin) / m_args.tblock) - tstart) - 1;
    if (num < 1)
        throw pdal_error("PulseCollection: no time interval for Solve");
    tstart *= m_args.tblock;

    traj = SplineFit3(num, m_args.tblock, tstart);
    attitude = SplineFit2(num, m_args.tblock, tstart);

    for (int i = 0; i <= num; ++i)
    {
        traj.missing[i] = !EstimatedPositionVelocity(tstart + i * m_args.tblock,
            traj.r[i], traj.v[i]);
        traj.v[i] *= m_args.tblock;
    }

    if (!traj.fillmissing(true))
        throw pdal_error("PulseCollection: too few pulses for initial estimate of trajectory");

    for (int i = 0; i <= num; ++i)
    {
        int im = (std::max)(0, i - 1);
        int ip = (std::min)(num, i + 1);

        // atan(dx, dy) to give clockwise from north convention
        attitude.r[i] = Eigen::Vector2d(std::atan2(traj.r[ip](0) - traj.r[im](0),
            traj.r[ip](1) - traj.r[im](1)),
            std::isnan(m_args.fixedpitch) ? 0.0 : degreesToRadians(m_args.fixedpitch));
        attitude.v[i] = Eigen::Vector2d::Zero();
    }
    // Make sure heading doesn't jump around
    double ang0 = attitude.r[0](0);
    for (int i = 1; i <= num; ++i)
    {
        double ang1 = attitude.r[i](0);
        attitude.r[i](0) = ang0 + normalizeRadians(ang1 - ang0);
    }
}

void PulseCollection::Solve()
{