C++ PiecewisePolynomial类代码示例

void testIntegralAndDerivative() {
    vector<Polynomial<CoefficientType>> polynomials;
    int num_coefficients = 5;
    int num_segments = 3;
    typedef typename Polynomial<CoefficientType>::CoefficientsType CoefficientsType;
    for (int i = 0; i < num_segments; ++i) {
        CoefficientsType coefficients = CoefficientsType::Random(num_coefficients);
        polynomials.push_back(Polynomial<CoefficientType>(coefficients));
    }

    // differentiate integral, get original back
    PiecewisePolynomial<CoefficientType> piecewise(polynomials, generateSegmentTimes(num_segments));
    PiecewisePolynomial<CoefficientType> piecewise_back = piecewise.integral().derivative();
    if (!piecewise.isApprox(piecewise_back, 1e-10))
        throw runtime_error("wrong");

    // check value at start time
    double value_at_t0 = uniform(generator);
    PiecewisePolynomial<CoefficientType> integral = piecewise.integral(value_at_t0);
    valuecheck(value_at_t0, integral.value(piecewise.getStartTime()), 1e-10);

    // check continuity at knot points
    for (int i = 0; i < piecewise.getNumberOfSegments() - 1; ++i) {
        valuecheck(integral.getPolynomial(i).value(integral.getDuration(i)), integral.getPolynomial(i + 1).value(0.0));
    }
}

PiecewisePolynomial<CoefficientType>
PiecewisePolynomial<CoefficientType>::integral(
    const typename PiecewisePolynomial<CoefficientType>::CoefficientMatrixRef&
    value_at_start_time) const {
    PiecewisePolynomial ret = *this;
    for (int segment_index = 0; segment_index < getNumberOfSegments();
            segment_index++) {
        PolynomialMatrix& matrix = ret.polynomials_[segment_index];
        for (Eigen::Index row = 0; row < rows(); row++) {
            for (Eigen::Index col = 0; col < cols(); col++) {
                if (segment_index == 0) {
                    matrix(row, col) =
                        matrix(row, col).integral(value_at_start_time(row, col));
                } else {
                    matrix(row, col) =
                        matrix(row, col).integral(
                            ret.segmentValueAtGlobalAbscissa(
                                segment_index - 1,
                                getStartTime(segment_index), row, col));
                }
            }
        }
    }
    return ret;
}

ExponentialPlusPiecewisePolynomial<CoefficientType>::ExponentialPlusPiecewisePolynomial(const PiecewisePolynomial<CoefficientType>& piecewise_polynomial_part) :
    PiecewiseFunction(piecewise_polynomial_part),
    K(Matrix<CoefficientType, Dynamic, Dynamic>::Zero(piecewise_polynomial_part.rows(), 1)),
    A(Matrix<CoefficientType, Dynamic, Dynamic>::Zero(1, 1)),
    alpha(Matrix<CoefficientType, Dynamic, Dynamic>::Zero(1, piecewise_polynomial_part.getNumberOfSegments())),
    piecewise_polynomial_part(piecewise_polynomial_part)
{
  assert(piecewise_polynomial_part.cols() == 1);
}

bool PiecewisePolynomial<CoefficientType>::isApprox(
    const PiecewisePolynomial<CoefficientType>& other, double tol) const {
    if (rows() != other.rows() || cols() != other.cols()) return false;

    if (!segmentTimesEqual(other, tol)) return false;

    for (int segment_index = 0; segment_index < getNumberOfSegments();
            segment_index++) {
        const PolynomialMatrix& matrix = polynomials_[segment_index];
        const PolynomialMatrix& other_matrix = other.polynomials_[segment_index];
        for (Eigen::Index row = 0; row < rows(); row++) {
            for (Eigen::Index col = 0; col < cols(); col++) {
                if (!matrix(row, col).isApprox(other_matrix(row, col), tol))
                    return false;
            }
        }
    }
    return true;
}

double randomSpeedTest(int ntests) {
    double ret = 0.0;
    int num_segments = 3;
    vector<double> segment_times = generateSegmentTimes(num_segments);
    for (int i = 0; i < ntests; i++) {

        double x0 = uniform(generator);
        double xd0 = uniform(generator);

        double xf = uniform(generator);
        double xdf = uniform(generator);

        double x1 = uniform(generator);
        double x2 = uniform(generator);

        PiecewisePolynomial<double> result = twoWaypointCubicSpline(segment_times, x0, xd0, xf, xdf, x1, x2);
        ret += result.value(0.0);
    }
    return ret;
}

void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
  string usage = "[coefs, objval] = nWaypointCubicSplinemex(ts, xs, xd0, xdf)";
  if (nrhs != 4)
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplinemex:WrongNumberOfInputs", usage.c_str());
  if (nlhs > 2)
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplinemex:WrongNumberOfOutputs", usage.c_str());

  const std::vector<double> segment_times = matlabToStdVector<double>(prhs[0]);
  MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[1]);
  auto xd0 = matlabToEigen<Dynamic, 1>(prhs[2]);
  auto xdf = matlabToEigen<Dynamic, 1>(prhs[3]);

  mwSize ndof = static_cast<mwSize>(xs.rows());
  mwSize num_segments = static_cast<mwSize>(xs.cols())-1;
  mwSize num_knots = num_segments - 1;
  mwSize num_coeffs_per_segment = 4;
  mwSize dims[] = {ndof, num_segments, num_coeffs_per_segment};
  plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);
  double objective_value = 0.0;
  for (mwSize dof = 0; dof < ndof; dof++) {
    VectorXd xi = xs.block(dof, 1, 1, num_knots).transpose();

    PiecewisePolynomial<double> spline = nWaypointCubicSpline(segment_times, xs(dof, 0), xd0[dof], xs(dof, num_segments), xdf[dof], xi);

    PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
    acceleration_squared *= acceleration_squared;
    PiecewisePolynomial<double> acceleration_squared_integral = acceleration_squared.integral();
    objective_value += acceleration_squared_integral.scalarValue(spline.getEndTime()) - acceleration_squared_integral.scalarValue(spline.getStartTime());

    for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments(); segment_index++) {
      for (mwSize coefficient_index = 0; coefficient_index < num_coeffs_per_segment; coefficient_index++) {
        mwSize sub[] = {dof, segment_index, num_coeffs_per_segment - coefficient_index - 1}; // Matlab's reverse coefficient indexing...
        *(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(static_cast<int>(segment_index)).getCoefficients()[coefficient_index];
      }
    }
  }

  if (nlhs > 1) {
    plhs[1] = mxCreateDoubleScalar(objective_value);
  }
}

void evaluateXYZExpmapCubicSpline(double t, const PiecewisePolynomial<double> &spline, Isometry3d &body_pose_des, Vector6d &xyzdot_angular_vel, Vector6d &xyzddot_angular_accel) {
  Vector6d xyzexp = spline.value(t);
  auto derivative = spline.derivative();
  Vector6d xyzexpdot = derivative.value(t);
  Vector6d xyzexpddot = derivative.derivative().value(t);
  xyzdot_angular_vel.head<3>() = xyzexpdot.head<3>();
  xyzddot_angular_accel.head<3>() = xyzexpddot.head<3>();
  Vector3d expmap = xyzexp.tail<3>();
  auto quat_grad = expmap2quat(expmap,2);
  Vector4d quat = quat_grad.value();
  body_pose_des.linear() = quat2rotmat(quat);
  body_pose_des.translation() = xyzexp.head<3>();
  Vector4d quat_dot = quat_grad.gradient().value() * xyzexpdot.tail<3>();
  quat_grad.gradient().gradient().value().resize(12,3);
  Matrix<double,12,3> dE = quat_grad.gradient().gradient().value();
  Vector3d expdot = xyzexpdot.tail<3>();
  Matrix<double,4,3> Edot = matGradMult(dE,expdot);
  Vector4d quat_ddot = quat_grad.gradient().value()*xyzexpddot.tail<3>() + Edot*expdot;
  Matrix<double,3,4> M;
  Matrix<double,12,4> dM;
  quatdot2angularvelMatrix(quat,M,&dM);
  xyzdot_angular_vel.tail<3>() = M*quat_dot;
  xyzddot_angular_accel.tail<3>() = M*quat_ddot + matGradMult(dM,quat_dot)*quat_dot;
}

void encodePiecewisePolynomial(const PiecewisePolynomial<double>& piecewise_polynomial, drake::lcmt_piecewise_polynomial& msg)
{
  msg.num_segments = piecewise_polynomial.getNumberOfSegments();
  msg.num_breaks = piecewise_polynomial.getNumberOfSegments() + 1;
  msg.breaks = piecewise_polynomial.getSegmentTimes();
  msg.polynomial_matrices.resize(piecewise_polynomial.getNumberOfSegments());
  for (int i = 0; i < piecewise_polynomial.getNumberOfSegments(); ++i) {
    encodePolynomialMatrix<Eigen::Dynamic,Eigen::Dynamic>(piecewise_polynomial.getPolynomialMatrix(i), msg.polynomial_matrices[i]);
  }
}

void evaluateXYZExpmapCubicSpline(double t,
                                  const PiecewisePolynomial<double> &spline,
                                  Isometry3d &body_pose_des,
                                  Vector6d &xyzdot_angular_vel,
                                  Vector6d &xyzddot_angular_accel) {
  Vector6d xyzexp = spline.value(t);
  auto derivative = spline.derivative();
  Vector6d xyzexpdot = derivative.value(t);
  Vector6d xyzexpddot = derivative.derivative().value(t);

  // translational part
  body_pose_des.translation() = xyzexp.head<3>();
  xyzdot_angular_vel.head<3>() = xyzexpdot.head<3>();
  xyzddot_angular_accel.head<3>() = xyzexpddot.head<3>();

  // rotational part
  auto expmap = xyzexp.tail<3>();
  auto expmap_dot = xyzexpdot.tail<3>();
  auto expmap_ddot = xyzexpddot.tail<3>();

  // construct autodiff version of expmap
  // autodiff derivatives represent first and second derivative w.r.t. time
  // TODO(tkoolen): should use 1 instead of dynamic, but causes issues
  // with eigen on MSVC 32 bit; should be fixed in 3.3
  typedef AutoDiffScalar<Matrix<double, Dynamic, 1>> ADScalar;
  // TODO(tkoolen): should use 1 instead of dynamic, but causes issues
  // with eigen on MSVC 32 bit; should be fixed in 3.3
  typedef AutoDiffScalar<Matrix<ADScalar, Dynamic, 1>> ADScalarSecondDeriv;
  Matrix<ADScalarSecondDeriv, 3, 1> expmap_autodiff;
  for (int i = 0; i < expmap_autodiff.size(); i++) {
    expmap_autodiff(i).value() = expmap(i);
    expmap_autodiff(i).derivatives().resize(1);
    expmap_autodiff(i).derivatives()(0) = expmap_dot(i);
    expmap_autodiff(i).derivatives()(0).derivatives().resize(1);
    expmap_autodiff(i).derivatives()(0).derivatives()(0) = expmap_ddot(i);
  }

  auto quat_autodiff = expmap2quat(expmap_autodiff);
  Vector4d quat = autoDiffToValueMatrix(autoDiffToValueMatrix(quat_autodiff));
  body_pose_des.linear() = quat2rotmat(quat);

  // angular velocity and acceleration are computed from quaternion derivative
  // meaning of derivative vectors remains the same: first and second
  // derivatives w.r.t. time
  decltype(quat_autodiff) quat_dot_autodiff;
  for (int i = 0; i < quat_dot_autodiff.size(); i++) {
    quat_dot_autodiff(i).value() = quat_autodiff(i).derivatives()(0).value();
    quat_dot_autodiff(i).derivatives().resize(1);
    quat_dot_autodiff(i).derivatives()(0).value() =
        quat_autodiff(i).derivatives()(0).derivatives()(0);
    quat_dot_autodiff(i).derivatives()(0).derivatives().resize(1);
    quat_dot_autodiff(i).derivatives()(0).derivatives()(0) =
        std::numeric_limits<double>::quiet_NaN();  // we're not interested in
                                                   // second deriv of angular
                                                   // velocity
  }

  auto omega_autodiff =
      (quatdot2angularvelMatrix(quat_autodiff) * quat_dot_autodiff).eval();
  auto omega = xyzdot_angular_vel.tail<3>();
  auto omega_dot = xyzddot_angular_accel.tail<3>();
  for (int i = 0; i < omega_autodiff.size(); i++) {
    omega(i) = omega_autodiff(i).value().value();
    omega_dot(i) = omega_autodiff(i).derivatives()(0).value();
  }
}

coef.push_back(3.0); coef.push_back(1.0);
coef.push_back(0.0); coef.push_back(0.0);
coefs.push_back(coef);
coef.clear();

coef.push_back(0.0); coef.push_back(0.0);
coef.push_back(0.0); coef.push_back(3.0);
coefs.push_back(coef);
coef.clear();

intervals.push_back(Interval(0.0, 1.0));
intervals.push_back(Interval(1.0, 2.0));
intervals.push_back(Interval(2.0, 3.0));

PiecewisePolynomial poly(4, intervals, coefs);

RadialDistributionFunction* RDFp;

CHECK(RadialDistributionFunction::RadialDistributionFunction())
	RDFp = new RadialDistributionFunction();
	TEST_NOT_EQUAL(RDFp, 0)
RESULT


CHECK(RadialDistributionFunction::~RadialDistributionFunction())
  delete RDFp;
RESULT


CHECK(RadialDistributionFunction::RadialDistributionFunction(const RadialDistributionFunction& rdf))

int main(int argc, char **argv) {
    int num_segments = 3;

    vector<double> segment_times = generateSegmentTimes(num_segments);

    double x0 = uniform(generator);
    double xd0 = uniform(generator);

    double xf = uniform(generator);
    double xdf = uniform(generator);

    double x1 = uniform(generator);
    double x2 = uniform(generator);


    PiecewisePolynomial<double> result = twoWaypointCubicSpline(segment_times, x0, xd0, xf, xdf, x1, x2);

    for (int i = 0; i < num_segments; i++) {
        valuecheck(segment_times[i], result.getStartTime(i));
    }
    valuecheck(segment_times[num_segments], result.getEndTime(num_segments - 1));

    // check value constraints
    double tol = 1e-10;
    PiecewisePolynomial<double> derivative = result.derivative();
    PiecewisePolynomial<double> second_derivative = derivative.derivative();


    valuecheck(result.value(result.getStartTime(0)), x0, tol);
    valuecheck(derivative.value(result.getStartTime(0)), xd0, tol);


    valuecheck(result.value(result.getEndTime(num_segments - 1)), xf, tol);
    valuecheck(derivative.value(result.getEndTime(num_segments - 1)), xdf, tol);

    valuecheck(result.value(result.getStartTime(1)), x1, tol);
    valuecheck(result.value(result.getStartTime(2)), x2, tol);

    // check continuity constraints
    double eps = 1e-10;
    int num_knots = num_segments - 1;
    for (int i = 0; i < num_knots; i++) {
        double t_knot = result.getEndTime(i);
        valuecheck(result.value(t_knot - eps), result.value(t_knot + eps), 1e-8);
        valuecheck(derivative.value(t_knot - eps), derivative.value(t_knot + eps), 1e-8);
        valuecheck(second_derivative.value(t_knot - eps), second_derivative.value(t_knot + eps), 1e-8);
    }

#if !defined(WIN32) && !defined(WIN64)
    int ntests = 1000;
    cout << "time: " << measure<chrono::microseconds>::execution(randomSpeedTest, ntests) / (double) ntests << " microseconds." << endl;
#endif

    cout << "test passed" << endl;

    return 0;
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
  string usage =
      "[coefs, ts, objective_value] = "
      "nWaypointCubicSplineFreeKnotTimesmex.cpp(t0, tf, xs, xd0, xdf)";
  if (nrhs != 5)
    mexErrMsgIdAndTxt(
        "Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfInputs",
        usage.c_str());
  if (nlhs < 2 || nlhs > 3)
    mexErrMsgIdAndTxt(
        "Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfOutputs",
        usage.c_str());

  double t0 = mxGetPrSafe(prhs[0])[0];
  double tf = mxGetPrSafe(prhs[1])[0];
  MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[2]);
  auto xd0 = matlabToEigen<Dynamic, 1>(prhs[3]);
  auto xdf = matlabToEigen<Dynamic, 1>(prhs[4]);

  mwSize ndof = static_cast<mwSize>(xs.rows());
  mwSize num_segments = static_cast<mwSize>(xs.cols()) - 1;
  mwSize num_knots = num_segments - 1;
  if (num_knots >= 3)
    mexWarnMsgTxt(
        "More knots than two is likely to be super slow in a grid search!\n");
  if (num_knots <= 0)
    mexErrMsgIdAndTxt(
        "Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:"
        "NotEnoughKnotsToJustifyThisFunction",
        usage.c_str());
  mwSize num_coeffs_per_segment = 4;
  mwSize dims[] = {ndof, num_segments, num_coeffs_per_segment};
  plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);

  std::vector<double> segment_times;
  segment_times.resize(static_cast<size_t>(num_segments) + 1);
  segment_times[0] = t0;
  segment_times[static_cast<size_t>(num_segments)] = tf;
  std::vector<double> best_segment_times = segment_times;
  double t_step = (tf - t0) / GRID_STEPS;
  double min_objective_value = numeric_limits<double>::infinity();

  // assemble the knot point locations for input to nWaypointCubicSpline
  MatrixXd xi = xs.block(0, 1, ndof, num_knots);

  if (GRID_STEPS <= num_knots) {
    // If we have have too few grid steps, then by pigeonhole it's
    // impossible to give each a unique time in our grid search.
    mexErrMsgIdAndTxt(
        "Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:"
        "TooManyKnotsForNumGridSteps",
        usage.c_str());
  }
  std::vector<int> t_indices;
  t_indices.reserve(num_knots);
  for (mwSize i = 0; i < num_knots; i++) {
    t_indices.push_back(i + 1);  // assume knot point won't be the same time as
                                 // the initial state, or previous knot point
  }

  while (t_indices[0] < (GRID_STEPS - static_cast<int>(num_knots) + 1)) {
    for (mwSize i = 0; i < num_knots; i++)
      segment_times[i + 1] = t0 + t_indices[i] * t_step;

    bool valid_solution = true;
    double objective_value = 0.0;
    for (mwSize dof = 0; dof < ndof && valid_solution; dof++) {
      try {
        PiecewisePolynomial<double> spline = nWaypointCubicSpline(
            segment_times, xs(dof, 0), xd0[dof], xs(dof, num_segments),
            xdf[dof], xi.row(dof).transpose());
        PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
        acceleration_squared *= acceleration_squared;
        PiecewisePolynomial<double> acceleration_squared_integral =
            acceleration_squared.integral();
        objective_value +=
            acceleration_squared_integral.scalarValue(spline.getEndTime()) -
            acceleration_squared_integral.scalarValue(spline.getStartTime());
      } catch (ConstraintMatrixSingularError &) {
        valid_solution = false;
      }
    }

    if (valid_solution && objective_value < min_objective_value) {
      best_segment_times = segment_times;
      min_objective_value = objective_value;
    }

    // Advance grid search counter or terminate, counting from
    // the latest t_index, and on overflow carrying to the
    // next lowest t_index and resetting to the new value of that
    // next lowest t_index. (since times must always be in order!)
    t_indices[num_knots - 1]++;
    // carry, except for the lowest place, which we
    // use to detect doneness.
    for (size_t i = num_knots - 1; i > 0; i--) {
      if ((i == num_knots - 1 && t_indices[i] >= GRID_STEPS) ||
          (i < num_knots - 1 && t_indices[i] >= t_indices[i + 1])) {
        t_indices[i - 1]++;
        t_indices[i] = t_indices[i - 1] + 1;
//.........这里部分代码省略.........

ExponentialPlusPiecewisePolynomial<double> s1Trajectory(const TVLQRData &sys, const PiecewisePolynomial<double> &zmp_trajectory,const Ref<const MatrixXd> &S) {
  size_t n = static_cast<size_t>(zmp_trajectory.getNumberOfSegments());
  int d = zmp_trajectory.getSegmentPolynomialDegree(0);
  int k = d + 1;

  for (size_t i = 1; i < n; i++) {
    assert(zmp_trajectory.getSegmentPolynomialDegree(i) == d);
  }

  VectorXd dt(n);
  std::vector<double> breaks = zmp_trajectory.getSegmentTimes();

  for (size_t i = 0; i < n; i++) {
    dt(i) = breaks[i + 1] - breaks[i];
  }  
  
  MatrixXd zmp_tf = zmp_trajectory.value(zmp_trajectory.getEndTime());
  PiecewisePolynomial<double> zbar_pp = zmp_trajectory - zmp_tf;

  Matrix2d R1i = sys.R1.inverse();
  MatrixXd NB = sys.N.transpose() + sys.B.transpose() * S; //2 x 4
  Matrix4d A2 = NB.transpose() * R1i * sys.B.transpose() - sys.A.transpose();
  MatrixXd B2 = 2 * (sys.C.transpose() - NB.transpose() * R1i * sys.D) * sys.Qy; //4 x 2
  Matrix4d A2i = A2.inverse();

  MatrixXd alpha = MatrixXd::Zero(4, n);

  vector<MatrixXd> beta;
  VectorXd s1dt;
  
  for (size_t i = 0; i < n ; i++) {
    beta.push_back(MatrixXd::Zero(4, k));
  }

  for (int j = static_cast<int>(n) - 1; j >= 0; j--) { 

    auto poly_mat = zbar_pp.getPolynomialMatrix(j);
    size_t nq = poly_mat.rows();
    MatrixXd poly_coeffs = MatrixXd::Zero(nq, k);

    for (size_t x = 0; x < nq; x++) {
      poly_coeffs.row(x) = poly_mat(x).getCoefficients().transpose();
    }    
    
    beta[j].col(k - 1) = -A2i * B2 * poly_coeffs.col(k - 1);
    
    for (int i = k - 2; i >= 0; i--) {
      beta[j].col(i) = A2i * ((i+1) * beta[j].col(i + 1) - B2 * poly_coeffs.col(i));
    }
    
    if (j == n - 1) {
      s1dt = VectorXd::Zero(4);
    } else {
      s1dt = alpha.col(j+1) + beta[j + 1].col(0);
    }

    VectorXd dtpow(k);
    for (size_t p = 0; p < k; p++) { 
      dtpow(p) = pow(dt(j), static_cast<int>(p));
    }
    
    alpha.col(j) = (A2*dt(j)).eval().exp().inverse() * (s1dt - beta[j]*dtpow);
  }
  
  vector<PiecewisePolynomial<double>::PolynomialMatrix> polynomial_matrices;
  for (int segment = 0; segment < n ; segment++) {
    PiecewisePolynomial<double>::PolynomialMatrix polynomial_matrix(4, 1);
    for(int row = 0; row < 4; row++) {
      polynomial_matrix(row) = Polynomial<double>(beta[segment].row(row));
    }
    polynomial_matrices.push_back(polynomial_matrix);
  }

  PiecewisePolynomial<double> pp_part = PiecewisePolynomial<double>(polynomial_matrices, breaks);
  auto s1traj = ExponentialPlusPiecewisePolynomial<double>(Matrix4d::Identity(), A2, alpha, pp_part);
  return s1traj;
}

void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[]) {
  string usage = "[coefs, ts, objective_value] = twoWaypointCubicSplineFreeKnotTimesmex.cpp(t0, tf, xs, xd0, xdf)";
  if (nrhs != 5)
    mexErrMsgIdAndTxt("Drake:twoWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfInputs", usage.c_str());
  if (nlhs < 2 || nlhs > 3)
    mexErrMsgIdAndTxt("Drake:twoWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfOutputs", usage.c_str());

  double t0 = mxGetPrSafe(prhs[0])[0];
  double tf = mxGetPrSafe(prhs[1])[0];
  MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[2]);
  auto xd0 = matlabToEigen<Dynamic, 1>(prhs[3]);
  auto xdf = matlabToEigen<Dynamic, 1>(prhs[4]);

  mwSize ndof = static_cast<mwSize>(xs.rows());
  mwSize num_segments = 3;
  mwSize num_coeffs_per_segment = 4;
  mwSize dims[] = {ndof, num_segments, num_coeffs_per_segment};
  plhs[0] = mxCreateNumericArray(num_segments, dims, mxDOUBLE_CLASS, mxREAL);

  std::vector<double> segment_times;
  segment_times.resize(static_cast<size_t>(num_segments) + 1);
  segment_times[0] = t0;
  segment_times[static_cast<size_t>(num_segments)] = tf;
  std::vector<double> best_segment_times = segment_times;
  double t_step = (tf - t0) / GRID_STEPS;
  double min_objective_value = numeric_limits<double>::infinity();

  for (int t1_index = 0; t1_index < GRID_STEPS; t1_index++) {
    segment_times[1] = t0 + t1_index * t_step;
    for (int t2_index = t1_index; t2_index < GRID_STEPS; t2_index++) {
      segment_times[2] = t0 + t2_index * t_step;

      bool valid_solution = true;
      double objective_value = 0.0;
      for (int dof = 0; dof < ndof && valid_solution; dof++) {
        try {
          PiecewisePolynomial<double> spline = twoWaypointCubicSpline(segment_times, xs(dof, 0), xd0[dof], xs(dof, 3), xdf[dof], xs(dof, 1), xs(dof, 2));
          PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
          acceleration_squared *= acceleration_squared;
          PiecewisePolynomial<double> acceleration_squared_integral = acceleration_squared.integral();
          objective_value += acceleration_squared_integral.value(spline.getEndTime()) - acceleration_squared_integral.value(spline.getStartTime());
        }
        catch (ConstraintMatrixSingularError&) {
          valid_solution = false;
        }
      }

      if (valid_solution && objective_value < min_objective_value) {
        best_segment_times[1] = segment_times[1];
        best_segment_times[2] = segment_times[2];
        min_objective_value = objective_value;
      }
    }
  }

  for (mwSize dof = 0; dof < ndof; dof++) {
    PiecewisePolynomial<double> spline = twoWaypointCubicSpline(best_segment_times, xs(dof, 0), xd0[dof], xs(dof, 3), xdf[dof], xs(dof, 1), xs(dof, 2));
    for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments(); segment_index++) {
      for (mwSize coefficient_index = 0; coefficient_index < num_coeffs_per_segment; coefficient_index++) {
        mwSize sub[] = {dof, segment_index, num_coeffs_per_segment - coefficient_index - 1}; // Matlab's reverse coefficient indexing...
        *(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(segment_index).getCoefficients()[coefficient_index];
      }
    }
  }
  plhs[1] = stdVectorToMatlab(best_segment_times);

  if (nlhs > 2)
    plhs[2] = mxCreateDoubleScalar(min_objective_value);
}