diff --git a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp
index 41a3b869..1c203584 100644
--- a/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp
+++ b/include/trajopt/constraint/AngularVelocityMaxMagnitudeConstraint.hpp
@@ -48,8 +48,7 @@ class TRAJOPT_DLLEXPORT AngularVelocityMaxMagnitudeConstraint {
     }
   }
 
- private:
-  double m_maxMagnitude;
+  const double m_maxMagnitude;
 };
 
 }  // namespace trajopt
diff --git a/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp
index 8ce14bc5..78795d65 100644
--- a/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp
+++ b/include/trajopt/constraint/LinearVelocityMaxMagnitudeConstraint.hpp
@@ -49,8 +49,7 @@ class TRAJOPT_DLLEXPORT LinearVelocityMaxMagnitudeConstraint {
     }
   }
 
- private:
-  double m_maxMagnitude;
+  const double m_maxMagnitude;
 };
 
 }  // namespace trajopt
diff --git a/src/spline/SplineUtil.cpp b/src/spline/SplineUtil.cpp
index a522d30d..df1f7ca1 100644
--- a/src/spline/SplineUtil.cpp
+++ b/src/spline/SplineUtil.cpp
@@ -14,7 +14,7 @@ namespace trajopt {
 
 std::vector<CubicHermitePoseSplineHolonomic>
 CubicPoseControlVectorsFromWaypoints(
-    const std::vector<std::vector<InitialGuessPoint>> initialGuessPoints) {
+    const std::vector<std::vector<Pose2d>> initialGuessPoints) {
   size_t totalGuessPoints = 0;
   for (const auto& points : initialGuessPoints) {
     totalGuessPoints += points.size();
@@ -27,9 +27,8 @@ CubicPoseControlVectorsFromWaypoints(
   // populate translation and heading vectors
   for (const auto& guessPoints : initialGuessPoints) {
     for (const auto& guessPoint : guessPoints) {
-      flatTranslationPoints.emplace_back(units::meter_t(guessPoint.x),
-                                         units::meter_t(guessPoint.y));
-      flatHeadings.emplace_back(units::radian_t(guessPoint.heading));
+      flatTranslationPoints.emplace_back(guessPoint.Translation());
+      flatHeadings.emplace_back(guessPoint.Rotation());
     }
   }
 
diff --git a/src/spline/SplineUtil.hpp b/src/spline/SplineUtil.hpp
index 29c1a2d6..5e875e2f 100644
--- a/src/spline/SplineUtil.hpp
+++ b/src/spline/SplineUtil.hpp
@@ -5,12 +5,12 @@
 #include <vector>
 
 #include "spline/CubicHermitePoseSplineHolonomic.hpp"
-#include "trajopt/path/InitialGuessPoint.hpp"
+#include "trajopt/geometry/Pose2.hpp"
 
 namespace trajopt {
 
 std::vector<CubicHermitePoseSplineHolonomic>
 CubicPoseControlVectorsFromWaypoints(
-    const std::vector<std::vector<InitialGuessPoint>> initialGuessPoints);
+    const std::vector<std::vector<Pose2d>> initialGuessPoints);
 
 }  // namespace trajopt