Configurable trajectory

calico/bspline.h

@@ -16,13 +16,14 @@ namespace calico {
 template <int N, typename T = double>
 class BSpline {
  public:
-
   ~BSpline() = default;
 
   // Add this spline's control points to a ceres problem. Returns the number of
   // parameters added, which should be N * number of control points.
   int AddParametersToProblem(ceres::Problem& problem);
 
+  void EnableControlPointsEstimation(bool enable);
+
   // Fits an N-DOF uniform B-spline fitted to given timestamps
   // and N-dimensional data. User also specifies the spline order and the knot
   // frequency of the spline.
@@ -43,9 +44,20 @@ class BSpline {
 
   // Interpolate the spline at given times for the given derivative. If no
   // derivative is specified, it defaults to direct interpolation.
-  absl::StatusOr<std::vector<Eigen::Vector<T,N>>>
+  absl::StatusOr<std::vector<Eigen::Vector<T, N>>>
   Interpolate(const std::vector<T>& times, int derivative = 0) const;
 
+  // Interpolate the spline at single time for the given derivative.
+  absl::StatusOr<Eigen::Vector<T, N>>
+  Interpolate(T time, int derivative = 0) const {
+    const absl::StatusOr<std::vector<Eigen::Vector<T, N>>> statusor =
+        Interpolate(std::vector{time}, derivative);
+    if (!statusor.ok()) {
+      return statusor.status();
+    }
+    return statusor->front();
+  }
+
   // TODO(yangjames): Description
   Eigen::MatrixX<T>
   GetSplineBasis(int spline_idx, T stamp, int derivative) const;
@@ -91,6 +103,8 @@ class BSpline {
   std::vector<Eigen::MatrixXd> Mi_;
   std::vector<Eigen::Vector<T,N>> control_points_;
 
+  // Flag for estimating control points in optimization.
+  bool control_points_enabled_;
 
   // Convenience function for computing a knot vector.
   void ComputeKnotVector();

calico/bspline.hpp

@@ -1,3 +1,5 @@
+#ifndef CALICO_BSPLINE_HPP_
+#define CALICO_BSPLINE_HPP_
 #include <algorithm>
 
 #include "calico/statusor_macros.h"
@@ -13,9 +15,19 @@ int BSpline<N, T>::AddParametersToProblem(ceres::Problem& problem) {
     problem.AddParameterBlock(control_point.data(), control_point.size());
     num_parameters_added += control_point.size();
   }
+  if (!control_points_enabled_) {
+    for (Eigen::Vector<T, N>& control_point : control_points_) {
+        problem.SetParameterBlockConstant(control_point.data());
+    }
+  }
   return num_parameters_added;
 }
 
+template <int N, typename T>
+void BSpline<N, T>::EnableControlPointsEstimation(bool enable) {
+  control_points_enabled_ = enable;
+}
+
 template <int N, typename T>
 absl::Status BSpline<N, T>::FitToData(
     const std::vector<T>& time,
@@ -329,3 +341,4 @@ absl::Status BSpline<N, T>::CheckDataForSplineFit(
 }
 
 } // namespace calico
+#endif // CALICO_BSPLINE_HPP_

calico/calico.cpp

@@ -308,7 +308,9 @@ PYBIND11_MODULE(_calico, m) {
                  std::string("Error: ") + std::string(poses.status().message()));
            }
            return poses.value();
-         });
+         })
+    .def("EnablePositionEstimation", &Trajectory::EnablePositionEstimation)
+    .def("EnableRotationEstimation", &Trajectory::EnableRotationEstimation);
 
   // World model class.
   py::class_<Landmark, std::shared_ptr<Landmark>>(m, "Landmark")

calico/chart_detectors/aprilgrid_detector.cpp

@@ -17,6 +17,7 @@ AprilGridDetector::AprilGridDetector(const std::string& config_filename) {
       .tagRows = config_yaml["tagRows"].as<int>(),
       .tagSize = config_yaml["tagSize"].as<double>(),
       .tagSpacing = config_yaml["tagSpacing"].as<double>(),
+      .startId = config_yaml["startId"].as<int>(),
   };
   SetupDetector();
 }
@@ -38,7 +39,7 @@ void AprilGridDetector::SetupDetector() {
     for (int col = 0; col < config_.tagCols; ++col) {
       const double tag_origin_x = tag_width_with_spacing * col;
       const double tag_origin_y = tag_width_with_spacing * row;
-      const int tag_number = row * config_.tagCols + col;
+      const int tag_number = row * config_.tagCols + col + config_.startId;
       for (int k = 0; k < 4; ++k) {
         const double corner_x = tag_origin_x + tag_width * (k == 1 || k == 2);
         const double corner_y = tag_origin_y + tag_width * (k == 2 || k == 3);

calico/chart_detectors/aprilgrid_detector.h

@@ -19,6 +19,7 @@ struct AprilGridConfig {
   double tagSpacing;  // Ratio of space between tags to their size.
                       // For example:
                       //   space between tags=0.5m, tagSize=2m -> tagSpacing=0.25
+  int startId = 0;  // The id of the first tag in the grid. Zero by default.
 };
 
 class AprilGridDetector {

calico/sensors/accelerometer.cpp

@@ -77,29 +77,34 @@ absl::StatusOr<std::vector<AccelerometerMeasurement>> Accelerometer::Project(
     const std::vector<double>& interp_times,
     const Trajectory& sensorrig_trajectory,
     const WorldModel& world_model) const {
-  std::vector<Eigen::Vector<double, 6>> pose_vectors;
-  ASSIGN_OR_RETURN(pose_vectors, sensorrig_trajectory.spline().Interpolate(
+  ASSIGN_OR_RETURN(
+    std::vector<Eigen::Vector3d> ddt_world_sensorrig_vector,
+    sensorrig_trajectory.position_spline().Interpolate(
+      interp_times, /*derivative=*/2));
+  ASSIGN_OR_RETURN(std::vector<Eigen::Vector3d> phi_world_sensorrig_vector,
+    sensorrig_trajectory.rotation_spline().Interpolate(
       interp_times, /*derivative=*/0));
-  std::vector<Eigen::Vector<double, 6>> pose_dot_vectors;
-  ASSIGN_OR_RETURN(pose_dot_vectors, sensorrig_trajectory.spline().Interpolate(
+  ASSIGN_OR_RETURN(std::vector<Eigen::Vector3d> phi_dot_world_sensorrig_vector,
+    sensorrig_trajectory.rotation_spline().Interpolate(
       interp_times, /*derivative=*/1));
-  std::vector<Eigen::Vector<double, 6>> pose_ddot_vectors;
-  ASSIGN_OR_RETURN(pose_ddot_vectors, sensorrig_trajectory.spline().Interpolate(
+  ASSIGN_OR_RETURN(std::vector<Eigen::Vector3d> phi_ddot_world_sensorrig_vector,
+    sensorrig_trajectory.rotation_spline().Interpolate(
       interp_times, /*derivative=*/2));
+
   std::vector<AccelerometerMeasurement> measurements(interp_times.size());
   for (int i = 0; i < interp_times.size(); ++i) {
-    const Eigen::Vector3d phi_sensorrig_world = -pose_vectors.at(i).head(3);
+    const Eigen::Vector3d phi_sensorrig_world = -phi_world_sensorrig_vector[i];
     const Eigen::Vector3d phi_dot_sensorrig_world =
-        -pose_dot_vectors.at(i).head(3);
+        -phi_dot_world_sensorrig_vector[i];
     const Eigen::Vector3d phi_ddot_sensorrig_world =
-        -pose_ddot_vectors.at(i).head(3);
+        -phi_ddot_world_sensorrig_vector[i];
     double q_sensorrig_world_array[4];
     ceres::AngleAxisToQuaternion(phi_sensorrig_world.data(),
                                  q_sensorrig_world_array);
     const Eigen::Quaterniond q_sensorrig_world(
         q_sensorrig_world_array[0], q_sensorrig_world_array[1],
         q_sensorrig_world_array[2], q_sensorrig_world_array[3]);
-    const Eigen::Vector3d ddt_world_sensorrig = pose_ddot_vectors.at(i).tail(3);
+    const Eigen::Vector3d& ddt_world_sensorrig = ddt_world_sensorrig_vector[i];
     const Eigen::Matrix3d J = ExpSO3Jacobian(phi_sensorrig_world);
     const Eigen::Matrix3d Jdot = ExpSO3JacobianDot(phi_sensorrig_world,
                                                    phi_dot_sensorrig_world);

calico/sensors/accelerometer_cost_functor.cpp

@@ -10,8 +10,10 @@ AccelerometerCostFunctor::AccelerometerCostFunctor(
     double sigma, double stamp, const Trajectory& trajectory_world_sensorrig)
   : measurement_(measurement) {
   accelerometer_model_ = AccelerometerModel::Create(accelerometer_model);
-  trajectory_evaluation_params_
-      = trajectory_world_sensorrig.GetEvaluationParams(stamp);
+  position_trajectory_evaluation_params_
+      = trajectory_world_sensorrig.GetEvaluationParamsPosition(stamp);
+  rotation_trajectory_evaluation_params_
+      = trajectory_world_sensorrig.GetEvaluationParamsRotation(stamp);
   information_ = (sigma > 0.0) ? (1.0 / sigma) : 1.0;
 }
 
@@ -41,13 +43,21 @@ ceres::CostFunction* AccelerometerCostFunctor::CreateCostFunction(
   parameters.push_back(gravity.data());
   cost_function->AddParameterBlock(gravity.size());
   // trajectory spline control points.
-  const int idx = trajectory_world_sensorrig.spline().GetSplineIndex(stamp);
-  const int spline_order = trajectory_world_sensorrig.spline().GetSplineOrder();
-  for (int i = 0; i < spline_order; ++i) {
+  const int rotation_idx = trajectory_world_sensorrig.rotation_spline().GetSplineIndex(stamp);
+  const int rotation_spline_order = trajectory_world_sensorrig.rotation_spline().GetSplineOrder();
+  for (int i = 0; i < rotation_spline_order; ++i) {
     parameters.push_back(
-        trajectory_world_sensorrig.spline().control_points().at(idx + i).data());
+        trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).data());
     cost_function->AddParameterBlock(
-        trajectory_world_sensorrig.spline().control_points().at(idx + i).size());
+        trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).size());
+  }
+  const int position_idx = trajectory_world_sensorrig.position_spline().GetSplineIndex(stamp);
+  const int position_spline_order = trajectory_world_sensorrig.position_spline().GetSplineOrder();
+  for (int i = 0; i < position_spline_order; ++i) {
+    parameters.push_back(
+        trajectory_world_sensorrig.position_spline().control_points().at(position_idx + i).data());
+    cost_function->AddParameterBlock(
+        trajectory_world_sensorrig.position_spline().control_points().at(position_idx + i).size());
   }
   // Residual
   cost_function->SetNumResiduals(kAccelerometerResidualSize);

calico/sensors/accelerometer_cost_functor.h

@@ -84,40 +84,69 @@ class AccelerometerCostFunctor {
         &(parameters[static_cast<int>(
             AccelerometerParameterIndices::kGravityIndex)][0]));
     // Parse sensor rig spline resolved in the world frame.
-    const int& num_control_points =
-        trajectory_evaluation_params_.num_control_points;
-    Eigen::MatrixX<T> control_points(num_control_points, 6);
-    for (int i = 0; i < num_control_points; ++i) {
-      control_points.row(i) = Eigen::Map<const Eigen::Vector<T, 6>>(&(parameters[
-          static_cast<int>(
-              AccelerometerParameterIndices::kSensorRigPoseSplineControlPointsIndex
-          ) + i][0]));
+    const int num_rotation_control_points =
+        rotation_trajectory_evaluation_params_.num_control_points;
+    Eigen::MatrixX<T> rotation_control_points(num_rotation_control_points, 3);
+    for (int i = 0; i < num_rotation_control_points; ++i) {
+      rotation_control_points.row(i) = Eigen::Map<const Eigen::Vector3<T>>(
+          &(parameters[static_cast<int>(
+              AccelerometerParameterIndices::kSensorRigPoseSplineControlPointsIndex)
+                + i
+              ][0]));
     }
-    const Eigen::MatrixX<T> basis_matrix =
-        trajectory_evaluation_params_.basis_matrix.template cast<T>();
-    const T knot0 = static_cast<T>(trajectory_evaluation_params_.knot0);
-    const T knot1 = static_cast<T>(trajectory_evaluation_params_.knot1);
-    const T stamp =
-        static_cast<T>(trajectory_evaluation_params_.stamp) - latency;
+    const int num_position_control_points =
+        position_trajectory_evaluation_params_.num_control_points;
+    Eigen::MatrixX<T> position_control_points(num_position_control_points, 3);
+    for (int i = 0; i < num_position_control_points; ++i) {
+      position_control_points.row(i) = Eigen::Map<const Eigen::Vector3<T>>(
+          &(parameters[static_cast<int>(
+              AccelerometerParameterIndices::kSensorRigPoseSplineControlPointsIndex)
+                + i + num_rotation_control_points
+              ][0]));
+    }
+
+    const Eigen::MatrixX<T> rotation_basis_matrix =
+        rotation_trajectory_evaluation_params_.basis_matrix.template cast<T>();
+    const T rotation_knot0 = static_cast<T>(
+        rotation_trajectory_evaluation_params_.knot0);
+    const T rotation_knot1 = static_cast<T>(
+        rotation_trajectory_evaluation_params_.knot1);
+    const T rotation_stamp =
+        static_cast<T>(rotation_trajectory_evaluation_params_.stamp) - latency;
+
+    const Eigen::MatrixX<T> position_basis_matrix =
+        position_trajectory_evaluation_params_.basis_matrix.template cast<T>();
+    const T position_knot0 = static_cast<T>(
+        position_trajectory_evaluation_params_.knot0);
+    const T position_knot1 = static_cast<T>(
+        position_trajectory_evaluation_params_.knot1);
+    const T position_stamp =
+        static_cast<T>(position_trajectory_evaluation_params_.stamp) - latency;
+
     // Evaluate the pose, pose rate, and pose acceleration.
-    const Eigen::Vector<T, 6> pose_vector = BSpline<6, T>::Evaluate(
-        control_points, knot0, knot1, basis_matrix, stamp, /*derivative=*/0);
-    const Eigen::Vector<T, 6> pose_dot_vector = BSpline<6, T>::Evaluate(
-        control_points, knot0, knot1, basis_matrix, stamp, /*derivative=*/1);
-    const Eigen::Vector<T, 6> pose_ddot_vector = BSpline<6, T>::Evaluate(
-        control_points, knot0, knot1, basis_matrix, stamp, /*derivative=*/2);
-    // Compute the kinematics of the accelerometer.
-    const Eigen::Vector3<T> phi_sensorrig_world = -pose_vector.head(3);
-    const Eigen::Vector3<T> phi_dot_sensorrig_world = -pose_dot_vector.head(3);
-    const Eigen::Vector3<T> phi_ddot_sensorrig_world = -pose_ddot_vector.head(3);
-    const Eigen::Vector3<T> ddt_world_sensorrig = pose_ddot_vector.tail(3);
+    const Eigen::Vector3<T> phi_sensorrig_world = -BSpline<3, T>::Evaluate(
+        rotation_control_points, rotation_knot0, rotation_knot1,
+        rotation_basis_matrix, rotation_stamp, 0);
     T q_sensorrig_world_array[4];
-    ceres::AngleAxisToQuaternion(phi_sensorrig_world.data(),
-                                 q_sensorrig_world_array);
+    ceres::AngleAxisToQuaternion(
+        phi_sensorrig_world.data(), q_sensorrig_world_array);
     const Eigen::Quaternion<T> q_sensorrig_world(
         q_sensorrig_world_array[0], q_sensorrig_world_array[1],
         q_sensorrig_world_array[2], q_sensorrig_world_array[3]);
+    const Eigen::Vector3<T> t_world_sensorrig = BSpline<3, T>::Evaluate(
+        position_control_points, position_knot0, position_knot1,
+        position_basis_matrix, position_stamp, 0);
+    const Eigen::Vector3<T> phi_dot_sensorrig_world = -BSpline<3, T>::Evaluate(
+        rotation_control_points, rotation_knot0, rotation_knot1,
+        rotation_basis_matrix, rotation_stamp, 1);
+    const Eigen::Vector3<T> phi_ddot_sensorrig_world = -BSpline<3, T>::Evaluate(
+        rotation_control_points, rotation_knot0, rotation_knot1,
+        rotation_basis_matrix, rotation_stamp, 2);
+    const Eigen::Vector3<T> ddt_world_sensorrig = BSpline<3, T>::Evaluate(
+        position_control_points, position_knot0, position_knot1,
+        position_basis_matrix, position_stamp, 2);
 
+    // Compute the kinematics of the accelerometer.
     const Eigen::Matrix3<T> J_sensorrig_world =
         ExpSO3Jacobian(phi_sensorrig_world);
     const Eigen::Matrix3<T> J_dot_sensorrig_world =
@@ -150,7 +179,8 @@ class AccelerometerCostFunctor {
   Eigen::Vector3d measurement_;
   double information_;
   std::unique_ptr<AccelerometerModel> accelerometer_model_;
-  TrajectoryEvaluationParams trajectory_evaluation_params_;
+  TrajectoryEvaluationParams position_trajectory_evaluation_params_;
+  TrajectoryEvaluationParams rotation_trajectory_evaluation_params_;
 };
 } // namespace calico::sensors
 #endif // CALICO_SENSORS_ACCELEROMETER_COST_FUNCTOR_H_

calico/sensors/camera_cost_functor.cpp

@@ -10,8 +10,10 @@ CameraCostFunctor::CameraCostFunctor(
     double sigma, double stamp, const Trajectory& trajectory_world_sensorrig)
   : pixel_(pixel) {
   camera_model_ = CameraModel::Create(camera_model);
-  trajectory_evaluation_params_
-      = trajectory_world_sensorrig.GetEvaluationParams(stamp);
+  position_trajectory_evaluation_params_
+      = trajectory_world_sensorrig.GetEvaluationParamsPosition(stamp);
+  rotation_trajectory_evaluation_params_
+      = trajectory_world_sensorrig.GetEvaluationParamsRotation(stamp);
   information_ = (sigma > 0.0) ? (1.0 / sigma) : 1.0;
 }
 
@@ -49,13 +51,21 @@ ceres::CostFunction* CameraCostFunctor::CreateCostFunction(
   parameters.push_back(t_world_model.data());
   cost_function->AddParameterBlock(t_world_model.size());
   // trajectory spline control points.
-  const int idx = trajectory_world_sensorrig.spline().GetSplineIndex(stamp);
-  const int spline_order = trajectory_world_sensorrig.spline().GetSplineOrder();
-  for (int i = 0; i < spline_order; ++i) {
+  const int rotation_idx = trajectory_world_sensorrig.rotation_spline().GetSplineIndex(stamp);
+  const int rotation_spline_order = trajectory_world_sensorrig.rotation_spline().GetSplineOrder();
+  for (int i = 0; i < rotation_spline_order; ++i) {
     parameters.push_back(
-        trajectory_world_sensorrig.spline().control_points().at(idx + i).data());
+        trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).data());
     cost_function->AddParameterBlock(
-        trajectory_world_sensorrig.spline().control_points().at(idx + i).size());
+        trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).size());
+  }
+  const int position_idx = trajectory_world_sensorrig.position_spline().GetSplineIndex(stamp);
+  const int position_spline_order = trajectory_world_sensorrig.position_spline().GetSplineOrder();
+  for (int i = 0; i < position_spline_order; ++i) {
+    parameters.push_back(
+        trajectory_world_sensorrig.position_spline().control_points().at(position_idx + i).data());
+    cost_function->AddParameterBlock(
+        trajectory_world_sensorrig.position_spline().control_points().at(position_idx + i).size());
   }
   // Residual
   cost_function->SetNumResiduals(kCameraResidualSize);