Reduce mallocs when constructing trajectories
We are mostly allocating because we are lazy. Allocate some of the data
up front and pass around pointers, and use spans to represent the rest
of the data and either point it at vectors, or at the underlying
flatbuffer.
Change-Id: I819dfaa85bc1ba9895eb92efc74a1540d93a6c38
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index ab38980..a1f19cd 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -9,16 +9,16 @@
namespace drivetrain {
::std::vector<float> DistanceSpline::BuildDistances(size_t num_alpha) {
- num_alpha = num_alpha == 0 ? 100 * splines_.size() : num_alpha;
+ num_alpha = num_alpha == 0 ? 100 * splines().size() : num_alpha;
::std::vector<float> distances;
distances.push_back(0.0);
- if (splines_.size() > 1) {
+ if (splines().size() > 1) {
// We've got a multispline to follow!
// Confirm that the ends line up to the correct number of derivatives.
- for (size_t i = 1; i < splines_.size(); ++i) {
- const Spline &spline0 = splines_[i - 1];
- const Spline &spline1 = splines_[i];
+ for (size_t i = 1; i < splines().size(); ++i) {
+ const Spline &spline0 = splines()[i - 1];
+ const Spline &spline1 = splines()[i];
const ::Eigen::Matrix<double, 2, 1> end0 = spline0.Point(1.0);
const ::Eigen::Matrix<double, 2, 1> start1 = spline1.Point(0.0);
@@ -57,7 +57,7 @@
}
const double dalpha =
- static_cast<double>(splines_.size()) / static_cast<double>(num_alpha - 1);
+ static_cast<double>(splines().size()) / static_cast<double>(num_alpha - 1);
double last_alpha = 0.0;
for (size_t i = 1; i < num_alpha; ++i) {
const double alpha = dalpha * i;
@@ -66,8 +66,8 @@
[this](double alpha) {
const size_t spline_index = ::std::min(
static_cast<size_t>(::std::floor(alpha)),
- splines_.size() - 1);
- return this->splines_[spline_index]
+ splines().size() - 1);
+ return this->splines()[spline_index]
.DPoint(alpha - spline_index)
.norm();
},
@@ -94,6 +94,24 @@
return splines;
}
+aos::SizedArray<Spline, FinishedDistanceSpline::kMaxSplines>
+SizedFlatbufferToSplines(const MultiSpline *fb) {
+ CHECK_NOTNULL(fb);
+ const size_t spline_count = fb->spline_count();
+ CHECK_EQ(fb->spline_x()->size(), static_cast<size_t>(spline_count * 5 + 1));
+ CHECK_EQ(fb->spline_y()->size(), static_cast<size_t>(spline_count * 5 + 1));
+ aos::SizedArray<Spline, FinishedDistanceSpline::kMaxSplines> splines;
+ for (size_t ii = 0; ii < spline_count; ++ii) {
+ Eigen::Matrix<double, 2, 6> points;
+ for (int jj = 0; jj < 6; ++jj) {
+ points(0, jj) = fb->spline_x()->Get(ii * 5 + jj);
+ points(1, jj) = fb->spline_y()->Get(ii * 5 + jj);
+ }
+ splines.emplace_back(Spline(points));
+ }
+ return splines;
+}
+
DistanceSpline::DistanceSpline(::std::vector<Spline> &&splines, int num_alpha)
: splines_(::std::move(splines)), distances_(BuildDistances(num_alpha)) {}
@@ -106,19 +124,18 @@
// TODO(james): Directly use the flatbuffer vector for accessing distances,
// rather than doing this redundant copy.
-DistanceSpline::DistanceSpline(const fb::DistanceSpline &fb)
- : splines_(FlatbufferToSplines(fb.spline())),
- distances_(CHECK_NOTNULL(fb.distances())->begin(),
- fb.distances()->end()) {}
+FinishedDistanceSpline::FinishedDistanceSpline(const fb::DistanceSpline &fb)
+ : splines_(SizedFlatbufferToSplines(fb.spline())),
+ distances_(fb.distances()->data(), fb.distances()->size()) {}
-flatbuffers::Offset<fb::DistanceSpline> DistanceSpline::Serialize(
+flatbuffers::Offset<fb::DistanceSpline> DistanceSplineBase::Serialize(
flatbuffers::FlatBufferBuilder *fbb,
flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<Constraint>>>
constraints) const {
- if (splines_.empty()) {
+ if (splines().empty()) {
return {};
}
- const size_t num_points = splines_.size() * 5 + 1;
+ const size_t num_points = splines().size() * 5 + 1;
float *spline_x_vector = nullptr;
float *spline_y_vector = nullptr;
const flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
@@ -127,38 +144,38 @@
fbb->CreateUninitializedVector(num_points, &spline_y_vector);
CHECK_NOTNULL(spline_x_vector);
CHECK_NOTNULL(spline_y_vector);
- spline_x_vector[0] = splines_[0].control_points()(0, 0);
- spline_y_vector[0] = splines_[0].control_points()(1, 0);
- for (size_t spline_index = 0; spline_index < splines_.size();
+ spline_x_vector[0] = splines()[0].control_points()(0, 0);
+ spline_y_vector[0] = splines()[0].control_points()(1, 0);
+ for (size_t spline_index = 0; spline_index < splines().size();
++spline_index) {
for (size_t point = 1; point < 6u; ++point) {
spline_x_vector[spline_index * 5 + point] =
- splines_[spline_index].control_points()(0, point);
+ splines()[spline_index].control_points()(0, point);
spline_y_vector[spline_index * 5 + point] =
- splines_[spline_index].control_points()(1, point);
+ splines()[spline_index].control_points()(1, point);
}
}
MultiSpline::Builder multi_spline_builder(*fbb);
- multi_spline_builder.add_spline_count(splines_.size());
+ multi_spline_builder.add_spline_count(splines().size());
multi_spline_builder.add_spline_x(spline_x_offset);
multi_spline_builder.add_spline_y(spline_y_offset);
multi_spline_builder.add_constraints(constraints);
const flatbuffers::Offset<MultiSpline> multi_spline_offset =
multi_spline_builder.Finish();
const flatbuffers::Offset<flatbuffers::Vector<float>> distances_offset =
- fbb->CreateVector(distances_);
+ fbb->CreateVector(distances().data(), distances().size());
fb::DistanceSpline::Builder spline_builder(*fbb);
spline_builder.add_spline(multi_spline_offset);
spline_builder.add_distances(distances_offset);
return spline_builder.Finish();
}
-::Eigen::Matrix<double, 2, 1> DistanceSpline::DDXY(double distance) const {
+::Eigen::Matrix<double, 2, 1> DistanceSplineBase::DDXY(double distance) const {
const AlphaAndIndex a = DistanceToAlpha(distance);
const ::Eigen::Matrix<double, 2, 1> dspline_point =
- splines_[a.index].DPoint(a.alpha);
+ splines()[a.index].DPoint(a.alpha);
const ::Eigen::Matrix<double, 2, 1> ddspline_point =
- splines_[a.index].DDPoint(a.alpha);
+ splines()[a.index].DDPoint(a.alpha);
const double squared_norm = dspline_point.squaredNorm();
@@ -169,17 +186,17 @@
::std::pow(squared_norm, 2);
}
-double DistanceSpline::DDTheta(double distance) const {
+double DistanceSplineBase::DDTheta(double distance) const {
const AlphaAndIndex a = DistanceToAlpha(distance);
// TODO(austin): We are re-computing DPoint here even worse
const ::Eigen::Matrix<double, 2, 1> dspline_point =
- splines_[a.index].DPoint(a.alpha);
+ splines()[a.index].DPoint(a.alpha);
const ::Eigen::Matrix<double, 2, 1> ddspline_point =
- splines_[a.index].DDPoint(a.alpha);
+ splines()[a.index].DDPoint(a.alpha);
- const double dtheta = splines_[a.index].DTheta(a.alpha);
- const double ddtheta = splines_[a.index].DDTheta(a.alpha);
+ const double dtheta = splines()[a.index].DTheta(a.alpha);
+ const double ddtheta = splines()[a.index].DDTheta(a.alpha);
const double squared_norm = dspline_point.squaredNorm();
@@ -189,25 +206,25 @@
::std::pow(squared_norm, 2);
}
-DistanceSpline::AlphaAndIndex DistanceSpline::DistanceToAlpha(
+DistanceSplineBase::AlphaAndIndex DistanceSplineBase::DistanceToAlpha(
double distance) const {
if (distance <= 0.0) {
return {0, 0.0};
}
if (distance >= length()) {
- return {splines_.size() - 1, 1.0};
+ return {splines().size() - 1, 1.0};
}
// Find the distance right below our number using a binary search.
size_t after = ::std::distance(
- distances_.begin(),
- ::std::lower_bound(distances_.begin(), distances_.end(), distance));
+ distances().begin(),
+ ::std::lower_bound(distances().begin(), distances().end(), distance));
size_t before = after - 1;
const double distance_step_size =
- (splines_.size() / static_cast<double>(distances_.size() - 1));
+ (splines().size() / static_cast<double>(distances().size() - 1));
- const double alpha = (distance - distances_[before]) /
- (distances_[after] - distances_[before]) *
+ const double alpha = (distance - distances()[before]) /
+ (distances()[after] - distances()[before]) *
distance_step_size +
static_cast<double>(before) * distance_step_size;
const size_t index = static_cast<size_t>(::std::floor(alpha));