From e0cddf058709015803b10257e1d4579daed96ded Mon Sep 17 00:00:00 2001 From: James Yang Date: Sat, 14 Jun 2025 09:59:25 -0700 Subject: [PATCH 1/8] Updated gyroscope and camera --- CMakeLists.txt | 542 +++++++++++----------- calico/sensors/camera_cost_functor.cpp | 24 +- calico/sensors/camera_cost_functor.h | 67 ++- calico/sensors/gyroscope.cpp | 15 +- calico/sensors/gyroscope_cost_functor.cpp | 24 +- calico/sensors/gyroscope_cost_functor.h | 78 +++- calico/trajectory.cpp | 57 ++- calico/trajectory.h | 20 +- 8 files changed, 472 insertions(+), 355 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1632c21..66e23d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,84 +85,84 @@ target_link_libraries( world_model ) -# Accelerometer library and tests. -add_library( - accelerometer_models - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_models.cpp -) -target_link_libraries( - accelerometer_models - absl::statusor - Eigen3::Eigen -) +# # Accelerometer library and tests. +# add_library( +# accelerometer_models +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_models.cpp +# ) +# target_link_libraries( +# accelerometer_models +# absl::statusor +# Eigen3::Eigen +# ) -add_library( - accelerometer_cost_functor - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_cost_functor.cpp -) -target_link_libraries( - accelerometer_cost_functor - accelerometer_models - trajectory -) +# add_library( +# accelerometer_cost_functor +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_cost_functor.cpp +# ) +# target_link_libraries( +# accelerometer_cost_functor +# accelerometer_models +# trajectory +# ) -add_library( - accelerometer - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer.cpp -) -target_link_libraries( - accelerometer - accelerometer_cost_functor - accelerometer_models - trajectory - world_model -) +# add_library( +# accelerometer +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer.cpp +# ) +# target_link_libraries( +# accelerometer +# accelerometer_cost_functor +# accelerometer_models +# trajectory +# world_model +# ) -# Camera library. -add_library( - camera_models - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_models.cpp -) -target_link_libraries( - camera_models - absl::statusor - Eigen3::Eigen -) +# # Camera library. +# add_library( +# camera_models +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_models.cpp +# ) +# target_link_libraries( +# camera_models +# absl::statusor +# Eigen3::Eigen +# ) -add_library( - camera_cost_functor - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_cost_functor.cpp -) -target_link_libraries( - camera_cost_functor - camera_models - trajectory -) +# add_library( +# camera_cost_functor +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_cost_functor.cpp +# ) +# target_link_libraries( +# camera_cost_functor +# camera_models +# trajectory +# ) -add_library( - camera - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera.cpp -) -target_link_libraries( - camera - camera_models - camera_cost_functor - trajectory - world_model -) +# add_library( +# camera +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera.cpp +# ) +# target_link_libraries( +# camera +# camera_models +# camera_cost_functor +# trajectory +# world_model +# ) -# Batch optimizer. -add_library( - batch_optimizer - ${CMAKE_CURRENT_SOURCE_DIR}/calico/batch_optimizer.cpp -) -target_link_libraries( - batch_optimizer - accelerometer - camera - gyroscope - world_model -) +# # Batch optimizer. +# add_library( +# batch_optimizer +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/batch_optimizer.cpp +# ) +# target_link_libraries( +# batch_optimizer +# accelerometer +# camera +# gyroscope +# world_model +# ) # AprilTags c++ library. include_directories( @@ -204,104 +204,104 @@ target_link_libraries( ${YAML_CPP_LIBRARIES} ) -# Python bindings. -pybind11_add_module( - _calico - ${CMAKE_CURRENT_SOURCE_DIR}/calico/calico.cpp -) -target_link_libraries( - _calico PUBLIC - accelerometer - aprilgrid_detector - batch_optimizer - camera - gyroscope - trajectory - world_model - Eigen3::Eigen -) -set_target_properties( - _calico PROPERTIES - LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/calico -) -file( - COPY - ${CMAKE_CURRENT_SOURCE_DIR}/calico/__init__.py - ${CMAKE_CURRENT_SOURCE_DIR}/calico/utils.py - DESTINATION ${PROJECT_BINARY_DIR}/calico -) +# # Python bindings. +# pybind11_add_module( +# _calico +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/calico.cpp +# ) +# target_link_libraries( +# _calico PUBLIC +# accelerometer +# aprilgrid_detector +# batch_optimizer +# camera +# gyroscope +# trajectory +# world_model +# Eigen3::Eigen +# ) +# set_target_properties( +# _calico PROPERTIES +# LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/calico +# ) +# file( +# COPY +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/__init__.py +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/utils.py +# DESTINATION ${PROJECT_BINARY_DIR}/calico +# ) -# Install cpp library. -install( - TARGETS - accelerometer - accelerometer_cost_functor - accelerometer_models - apriltags - batch_optimizer - camera - camera_cost_functor - camera_models - gyroscope - gyroscope_cost_functor - gyroscope_models - trajectory - world_model - aprilgrid_detector - EXPORT - CalicoConfig - DESTINATION lib/calico -) -install( - EXPORT - CalicoConfig - DESTINATION lib/cmake/calico -) -set(top_level_headers - ${CMAKE_CURRENT_SOURCE_DIR}/calico/profiler.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/trajectory.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/bspline.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/test_utils.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/optimization_utils.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/batch_optimizer.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/typedefs.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/matchers.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/status_builder.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/geometry.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/world_model.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/statusor_macros.h -) -install( - FILES ${top_level_headers} - DESTINATION include/calico -) -set(sensor_headers - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope_models.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_cost_functor.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/sensor_base.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope_cost_functor.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_cost_functor.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_models.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_models.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope.h - ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer.h -) -install( - FILES ${sensor_headers} - DESTINATION include/calico/sensors/ -) -# Install python library. -install( - TARGETS _calico - DESTINATION ${Python_SITELIB}/calico -) -install( - FILES - ${CMAKE_CURRENT_SOURCE_DIR}/calico/utils.py - ${CMAKE_CURRENT_SOURCE_DIR}/calico/__init__.py - DESTINATION ${Python_SITELIB}/calico -) +# # Install cpp library. +# install( +# TARGETS +# accelerometer +# accelerometer_cost_functor +# accelerometer_models +# apriltags +# batch_optimizer +# camera +# camera_cost_functor +# camera_models +# gyroscope +# gyroscope_cost_functor +# gyroscope_models +# trajectory +# world_model +# aprilgrid_detector +# EXPORT +# CalicoConfig +# DESTINATION lib/calico +# ) +# install( +# EXPORT +# CalicoConfig +# DESTINATION lib/cmake/calico +# ) +# set(top_level_headers +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/profiler.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/trajectory.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/bspline.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/test_utils.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/optimization_utils.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/batch_optimizer.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/typedefs.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/matchers.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/status_builder.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/geometry.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/world_model.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/statusor_macros.h +# ) +# install( +# FILES ${top_level_headers} +# DESTINATION include/calico +# ) +# set(sensor_headers +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope_models.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_cost_functor.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/sensor_base.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope_cost_functor.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_cost_functor.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_models.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_models.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope.h +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer.h +# ) +# install( +# FILES ${sensor_headers} +# DESTINATION include/calico/sensors/ +# ) +# # Install python library. +# install( +# TARGETS _calico +# DESTINATION ${Python_SITELIB}/calico +# ) +# install( +# FILES +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/utils.py +# ${CMAKE_CURRENT_SOURCE_DIR}/calico/__init__.py +# DESTINATION ${Python_SITELIB}/calico +# ) if (BUILD_TESTING) # Typedefs test. @@ -403,106 +403,106 @@ if (BUILD_TESTING) gmock ) add_test(GyroscopeLibTest gyroscope_test) - # Accelerometer models test. - add_executable( - accelerometer_models_test - ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_models_test.cpp - ) - target_link_libraries( - accelerometer_models_test - accelerometer_models - GTest::Main - gmock - ) - add_test(AccelerometerModelsTest accelerometer_models_test) - # Accelerometer cost functor test. - add_executable( - accelerometer_cost_functor_test - ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_cost_functor_test.cpp - ) - target_link_libraries( - accelerometer_cost_functor_test - accelerometer_cost_functor - GTest::Main - gmock - ) - add_test(AccelerometerCostFunctorTest accelerometer_cost_functor_test) - # Accelerometer test. - add_executable( - accelerometer_test - ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_test.cpp - ) - target_link_libraries( - accelerometer_test - accelerometer - GTest::Main - gmock - ) - add_test(AccelerometerLibTest accelerometer_test) - # Camera models test. - add_executable( - camera_models_test - ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_models_test.cpp - ) - target_link_libraries( - camera_models_test - camera_models - GTest::Main - gmock - ) - add_test(CameraModelsTest camera_models_test) - # Camera cost functor test. - add_executable( - camera_cost_functor_test - ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_cost_functor_test.cpp - ) - target_link_libraries( - camera_cost_functor_test - camera_cost_functor - GTest::Main - gmock - ) - add_test(CameraCostFunctorTest camera_cost_functor_test) - # Camera test. - add_executable( - camera_test - ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_test.cpp - ) - target_link_libraries( - camera_test - camera - GTest::Main - gmock - ) - add_test(CameraLibTest camera_test) - # Batch optimizer test. - add_executable( - batch_optimizer_test - ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/batch_optimizer_test.cpp - ) - target_link_libraries( - batch_optimizer_test - camera - world_model - batch_optimizer - GTest::Main - gmock - ) - add_test(BatchOptimizerTest batch_optimizer_test) - # Python bindings test. - add_test( - NAME python_bindings_test - COMMAND python3 -m pytest ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/python_bindings_test.py - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/calico/test - ) - # Python utilities test. - add_test( - NAME python_utils_test - COMMAND python3 -m pytest ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/python_utils_test.py - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/calico/test - ) - set_tests_properties( - python_bindings_test python_utils_test - PROPERTIES ENVIRONMENT "PYTHONPATH=${PROJECT_BINARY_DIR}:$ENV{PYTHONPATH}" - ) + # # Accelerometer models test. + # add_executable( + # accelerometer_models_test + # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_models_test.cpp + # ) + # target_link_libraries( + # accelerometer_models_test + # accelerometer_models + # GTest::Main + # gmock + # ) + # add_test(AccelerometerModelsTest accelerometer_models_test) + # # Accelerometer cost functor test. + # add_executable( + # accelerometer_cost_functor_test + # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_cost_functor_test.cpp + # ) + # target_link_libraries( + # accelerometer_cost_functor_test + # accelerometer_cost_functor + # GTest::Main + # gmock + # ) + # add_test(AccelerometerCostFunctorTest accelerometer_cost_functor_test) + # # Accelerometer test. + # add_executable( + # accelerometer_test + # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_test.cpp + # ) + # target_link_libraries( + # accelerometer_test + # accelerometer + # GTest::Main + # gmock + # ) + # add_test(AccelerometerLibTest accelerometer_test) + # # Camera models test. + # add_executable( + # camera_models_test + # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_models_test.cpp + # ) + # target_link_libraries( + # camera_models_test + # camera_models + # GTest::Main + # gmock + # ) + # add_test(CameraModelsTest camera_models_test) + # # Camera cost functor test. + # add_executable( + # camera_cost_functor_test + # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_cost_functor_test.cpp + # ) + # target_link_libraries( + # camera_cost_functor_test + # camera_cost_functor + # GTest::Main + # gmock + # ) + # add_test(CameraCostFunctorTest camera_cost_functor_test) + # # Camera test. + # add_executable( + # camera_test + # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_test.cpp + # ) + # target_link_libraries( + # camera_test + # camera + # GTest::Main + # gmock + # ) + # add_test(CameraLibTest camera_test) + # # Batch optimizer test. + # add_executable( + # batch_optimizer_test + # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/batch_optimizer_test.cpp + # ) + # target_link_libraries( + # batch_optimizer_test + # camera + # world_model + # batch_optimizer + # GTest::Main + # gmock + # ) + # add_test(BatchOptimizerTest batch_optimizer_test) + # # Python bindings test. + # add_test( + # NAME python_bindings_test + # COMMAND python3 -m pytest ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/python_bindings_test.py + # WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/calico/test + # ) + # # Python utilities test. + # add_test( + # NAME python_utils_test + # COMMAND python3 -m pytest ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/python_utils_test.py + # WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/calico/test + # ) + # set_tests_properties( + # python_bindings_test python_utils_test + # PROPERTIES ENVIRONMENT "PYTHONPATH=${PROJECT_BINARY_DIR}:$ENV{PYTHONPATH}" + # ) endif() diff --git a/calico/sensors/camera_cost_functor.cpp b/calico/sensors/camera_cost_functor.cpp index 4661b31..1ca0a26 100644 --- a/calico/sensors/camera_cost_functor.cpp +++ b/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 position_idx = trajectory_world_sensorrig.position_spline().GetSplineIndex(stamp); + const int rotation_idx = trajectory_world_sensorrig.rotation_spline().GetSplineIndex(stamp); + const int position_spline_order = trajectory_world_sensorrig.position_spline().GetSplineOrder(); + const int rotation_spline_order = trajectory_world_sensorrig.rotation_spline().GetSplineOrder(); + for (int i = 0; i < position_spline_order; ++i) { parameters.push_back( - trajectory_world_sensorrig.spline().control_points().at(idx + i).data()); + trajectory_world_sensorrig.position_spline().control_points().at(position_idx + i).data()); cost_function->AddParameterBlock( - trajectory_world_sensorrig.spline().control_points().at(idx + i).size()); + trajectory_world_sensorrig.position_spline().control_points().at(position_idx + i).size()); + } + for (int i = 0; i < rotation_spline_order; ++i) { + parameters.push_back( + trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).data()); + cost_function->AddParameterBlock( + trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).size()); } // Residual cost_function->SetNumResiduals(kCameraResidualSize); diff --git a/calico/sensors/camera_cost_functor.h b/calico/sensors/camera_cost_functor.h index 4d3ce24..8bdb435 100644 --- a/calico/sensors/camera_cost_functor.h +++ b/calico/sensors/camera_cost_functor.h @@ -26,8 +26,8 @@ enum class CameraParameterIndices : int { kModelPointIndex = 4, kModelRotationIndex = 5, kModelTranslationIndex = 6, - // Rotation and position control points of the associated spline segment as an - // Nx6 matrix where N is the spline order. + // Rotation and position control points of the associated spline segment as + // two Nx3 matrices (rotation then position) where N is the spline order. kSensorRigPoseSplineControlPointsIndex = 7, }; @@ -98,32 +98,58 @@ class CameraCostFunctor { &(parameters[static_cast( CameraParameterIndices::kModelTranslationIndex)][0])); // Parse sensor rig spline resolved in the world frame. - const int num_control_points = - trajectory_evaluation_params_.num_control_points; - Eigen::MatrixX control_points(num_control_points, 6); - for (int i = 0; i < num_control_points; ++i) { - control_points.row(i) = Eigen::Map>( + const int num_rotation_control_points = + rotation_trajectory_evaluation_params_.num_control_points; + Eigen::MatrixX rotation_control_points(num_rotation_control_points, 6); + for (int i = 0; i < num_rotation_control_points; ++i) { + rotation_control_points.row(i) = Eigen::Map>( &(parameters[static_cast( CameraParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i ][0])); } - const Eigen::MatrixX basis_matrix = - trajectory_evaluation_params_.basis_matrix.template cast(); - const T knot0 = static_cast(trajectory_evaluation_params_.knot0); - const T knot1 = static_cast(trajectory_evaluation_params_.knot1); - const T stamp = - static_cast(trajectory_evaluation_params_.stamp) - latency; + const int num_position_control_points = + position_trajectory_evaluation_params_.num_control_points; + Eigen::MatrixX position_control_points(num_position_control_points, 6); + for (int i = num_rotation_control_points; + i < num_rotation_control_points + num_position_control_points; + ++i) { + position_control_points.row(i) = Eigen::Map>( + &(parameters[static_cast( + CameraParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i + ][0])); + } + + const Eigen::MatrixX rotation_basis_matrix = + rotation_trajectory_evaluation_params_.basis_matrix.template cast(); + const T rotation_knot0 = static_cast( + rotation_trajectory_evaluation_params_.knot0); + const T rotation_knot1 = static_cast( + rotation_trajectory_evaluation_params_.knot1); + const T rotation_stamp = + static_cast(rotation_trajectory_evaluation_params_.stamp) - latency; + + const Eigen::MatrixX position_basis_matrix = + position_trajectory_evaluation_params_.basis_matrix.template cast(); + const T position_knot0 = static_cast( + position_trajectory_evaluation_params_.knot0); + const T position_knot1 = static_cast( + position_trajectory_evaluation_params_.knot1); + const T position_stamp = + static_cast(position_trajectory_evaluation_params_.stamp) - latency; // Evaluate the pose. - const Eigen::Vector pose_vector = - BSpline<6, T>::Evaluate( - control_points, knot0, knot1, basis_matrix, stamp, 0); - const Eigen::Vector3 phi_sensorrig_world = -pose_vector.head(3); + const Eigen::Vector3 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 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_world_sensorrig(pose_vector.data() + 3); + const Eigen::Vector3 t_world_sensorrig = BSpline<3, T>::Evaluate( + position_control_points, position_knot0, position_knot1, + position_basis_matrix, position_stamp, 0); + // Resolve the model point in the camera frame. const Eigen::Quaternion q_camera_model = q_sensorrig_camera.inverse() * q_sensorrig_world * q_world_model; @@ -150,7 +176,8 @@ class CameraCostFunctor { Eigen::Vector2d pixel_; double information_; std::unique_ptr camera_model_; - TrajectoryEvaluationParams trajectory_evaluation_params_; + TrajectoryEvaluationParams position_trajectory_evaluation_params_; + TrajectoryEvaluationParams rotation_trajectory_evaluation_params_; }; } // namespace calico::sensors diff --git a/calico/sensors/gyroscope.cpp b/calico/sensors/gyroscope.cpp index 933cd08..21cee62 100644 --- a/calico/sensors/gyroscope.cpp +++ b/calico/sensors/gyroscope.cpp @@ -57,17 +57,18 @@ absl::StatusOr> Gyroscope::Project( const std::vector& interp_times, const Trajectory& sensorrig_trajectory, const WorldModel& world_model) const { - std::vector> pose_vectors; - ASSIGN_OR_RETURN(pose_vectors, sensorrig_trajectory.spline().Interpolate( + ASSIGN_OR_RETURN(std::vector phi_world_sensorrig_vector, + sensorrig_trajectory.rotation_spline().Interpolate( interp_times, /*derivative=*/0)); - std::vector> pose_dot_vectors; - ASSIGN_OR_RETURN(pose_dot_vectors, sensorrig_trajectory.spline().Interpolate( + ASSIGN_OR_RETURN(std::vector phi_dot_world_sensorrig_vector, + sensorrig_trajectory.rotation_spline().Interpolate( interp_times, /*derivative=*/1)); std::vector 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_dot_sensorrig_world = - -pose_dot_vectors.at(i).head(3); + const Eigen::Vector3d& phi_sensorrig_world = + -phi_world_sensorrig_vector.at(i); + const Eigen::Vector3d& phi_dot_sensorrig_world = + -phi_dot_world_sensorrig_vector.at(i); const Eigen::Matrix3d J = ExpSO3Jacobian(phi_sensorrig_world); const Eigen::Vector3d omega_sensorrig_world = J * phi_dot_sensorrig_world; const Eigen::Vector3d omega_gyroscope_world = diff --git a/calico/sensors/gyroscope_cost_functor.cpp b/calico/sensors/gyroscope_cost_functor.cpp index f082d20..3c1ae16 100644 --- a/calico/sensors/gyroscope_cost_functor.cpp +++ b/calico/sensors/gyroscope_cost_functor.cpp @@ -10,8 +10,10 @@ GyroscopeCostFunctor::GyroscopeCostFunctor( double sigma, double stamp, const Trajectory& trajectory_world_sensorrig) : measurement_(measurement) { gyroscope_model_ = GyroscopeModel::Create(gyroscope_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; } @@ -37,13 +39,21 @@ ceres::CostFunction* GyroscopeCostFunctor::CreateCostFunction( parameters.push_back(&latency); cost_function->AddParameterBlock(1); // 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 position_idx = trajectory_world_sensorrig.position_spline().GetSplineIndex(stamp); + const int rotation_idx = trajectory_world_sensorrig.rotation_spline().GetSplineIndex(stamp); + const int position_spline_order = trajectory_world_sensorrig.position_spline().GetSplineOrder(); + const int rotation_spline_order = trajectory_world_sensorrig.rotation_spline().GetSplineOrder(); + for (int i = 0; i < position_spline_order; ++i) { parameters.push_back( - trajectory_world_sensorrig.spline().control_points().at(idx + i).data()); + trajectory_world_sensorrig.position_spline().control_points().at(position_idx + i).data()); cost_function->AddParameterBlock( - trajectory_world_sensorrig.spline().control_points().at(idx + i).size()); + trajectory_world_sensorrig.position_spline().control_points().at(position_idx + i).size()); + } + for (int i = 0; i < rotation_spline_order; ++i) { + parameters.push_back( + trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).data()); + cost_function->AddParameterBlock( + trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).size()); } // Residual cost_function->SetNumResiduals(kGyroscopeResidualSize); diff --git a/calico/sensors/gyroscope_cost_functor.h b/calico/sensors/gyroscope_cost_functor.h index 994e35b..f0326e3 100644 --- a/calico/sensors/gyroscope_cost_functor.h +++ b/calico/sensors/gyroscope_cost_functor.h @@ -76,31 +76,62 @@ class GyroscopeCostFunctor { parameters[static_cast( GyroscopeParameterIndices::kLatencyIndex)][0]; // Parse sensor rig spline resolved in the world frame. - const int& num_control_points = - trajectory_evaluation_params_.num_control_points; - Eigen::MatrixX control_points(num_control_points, 6); - for (int i = 0; i < num_control_points; ++i) { - control_points.row(i) = Eigen::Map>( + const int num_rotation_control_points = + rotation_trajectory_evaluation_params_.num_control_points; + Eigen::MatrixX rotation_control_points(num_rotation_control_points, 6); + for (int i = 0; i < num_rotation_control_points; ++i) { + rotation_control_points.row(i) = Eigen::Map>( &(parameters[static_cast( - GyroscopeParameterIndices::kSensorRigPoseSplineControlPointsIndex - ) + i][0])); + GyroscopeParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i + ][0])); } - const Eigen::MatrixX basis_matrix = - trajectory_evaluation_params_.basis_matrix.template cast(); - const T knot0 = static_cast(trajectory_evaluation_params_.knot0); - const T knot1 = static_cast(trajectory_evaluation_params_.knot1); - const T stamp = - static_cast(trajectory_evaluation_params_.stamp) - latency; + const int num_position_control_points = + position_trajectory_evaluation_params_.num_control_points; + Eigen::MatrixX position_control_points(num_position_control_points, 6); + for (int i = num_rotation_control_points; + i < num_rotation_control_points + num_position_control_points; + ++i) { + position_control_points.row(i) = Eigen::Map>( + &(parameters[static_cast( + GyroscopeParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i + ][0])); + } + + const Eigen::MatrixX rotation_basis_matrix = + rotation_trajectory_evaluation_params_.basis_matrix.template cast(); + const T rotation_knot0 = static_cast( + rotation_trajectory_evaluation_params_.knot0); + const T rotation_knot1 = static_cast( + rotation_trajectory_evaluation_params_.knot1); + const T rotation_stamp = + static_cast(rotation_trajectory_evaluation_params_.stamp) - latency; + + const Eigen::MatrixX position_basis_matrix = + position_trajectory_evaluation_params_.basis_matrix.template cast(); + const T position_knot0 = static_cast( + position_trajectory_evaluation_params_.knot0); + const T position_knot1 = static_cast( + position_trajectory_evaluation_params_.knot1); + const T position_stamp = + static_cast(position_trajectory_evaluation_params_.stamp) - latency; + // Evaluate the pose and pose rate. - const Eigen::Vector pose_vector = BSpline<6, T>::Evaluate( - control_points, knot0, knot1, basis_matrix, stamp, /*derivative=*/0); - const Eigen::Vector pose_dot_vector = BSpline<6, T>::Evaluate( - control_points, knot0, knot1, basis_matrix, stamp, /*derivative=*/1); - // Compute the angular velocity of the gyroscope. - // TODO(yangjames): Also evaluate acceleration for g-sensitivity - // calculations. - const Eigen::Vector3 phi_sensorrig_world = -pose_vector.head(3); - const Eigen::Vector3 phi_dot_sensorrig_world = -pose_dot_vector.head(3); + const Eigen::Vector3 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); + const Eigen::Quaternion 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_world_sensorrig = BSpline<3, T>::Evaluate( + position_control_points, position_knot0, position_knot1, + position_basis_matrix, position_stamp, /*derivative=*/0); + const Eigen::Vector3 phi_dot_sensorrig_world = -BSpline<3, T>::Evaluate( + rotation_control_points, rotation_knot0, rotation_knot1, + rotation_basis_matrix, rotation_stamp, 1); + const Eigen::Matrix3 J = ExpSO3Jacobian(phi_sensorrig_world); const Eigen::Vector3 omega_sensorrig_world = J * phi_dot_sensorrig_world; const Eigen::Vector3 omega_gyroscope_world = @@ -121,7 +152,8 @@ class GyroscopeCostFunctor { Eigen::Vector3d measurement_; double information_; std::unique_ptr gyroscope_model_; - TrajectoryEvaluationParams trajectory_evaluation_params_; + TrajectoryEvaluationParams position_trajectory_evaluation_params_; + TrajectoryEvaluationParams rotation_trajectory_evaluation_params_; }; } // namespace calico::sensors #endif // CALICO_SENSORS_GYROSCOPE_COST_FUNCTOR_H_ diff --git a/calico/trajectory.cpp b/calico/trajectory.cpp index 106f805..24b5c23 100644 --- a/calico/trajectory.cpp +++ b/calico/trajectory.cpp @@ -43,13 +43,18 @@ absl::Status Trajectory::FitSpline( pose_world_body[i].tail(3) = t_world_body[i]; } - RETURN_IF_ERROR(spline_pose_world_body_.FitToData( - stamps, pose_world_body, spline_order, knot_frequency)); + RETURN_IF_ERROR(spline_rotation_world_from_body_.FitToData( + stamps, phi_world_body, spline_order, knot_frequency)); + RETURN_IF_ERROR(spline_position_world_to_body_.FitToData( + stamps, t_world_body, spline_order, knot_frequency)); return absl::OkStatus(); } int Trajectory::AddParametersToProblem(ceres::Problem& problem) { - return spline_pose_world_body_.AddParametersToProblem(problem); + int num_parameters; + num_parameters += spline_position_world_to_body_.AddParametersToProblem(problem); + num_parameters += spline_rotation_world_from_body_.AddParametersToProblem(problem); + return num_parameters; } const absl::flat_hash_map& Trajectory::trajectory() const { @@ -60,21 +65,39 @@ absl::flat_hash_map& Trajectory::trajectory() { return pose_id_to_pose_world_body_; } -TrajectoryEvaluationParams Trajectory::GetEvaluationParams(double stamp) const { +TrajectoryEvaluationParams Trajectory::GetEvaluationParamsPosition(double stamp) const { const int control_point_idx = - spline_pose_world_body_.GetSplineIndex(stamp); + spline_position_world_to_body_.GetSplineIndex(stamp); const int knot_idx = - spline_pose_world_body_.GetKnotIndexFromSplineIndex( + spline_position_world_to_body_.GetKnotIndexFromSplineIndex( control_point_idx); - const int num_control_points = spline_pose_world_body_.GetSplineOrder(); + const int num_control_points = spline_position_world_to_body_.GetSplineOrder(); return TrajectoryEvaluationParams { .spline_index = control_point_idx, - .knot0 = spline_pose_world_body_.knots().at(knot_idx), - .knot1 = spline_pose_world_body_.knots().at(knot_idx + 1), + .knot0 = spline_position_world_to_body_.knots().at(knot_idx), + .knot1 = spline_position_world_to_body_.knots().at(knot_idx + 1), .stamp = stamp, .num_control_points = num_control_points, .basis_matrix = - spline_pose_world_body_.basis_matrices().at(control_point_idx), + spline_position_world_to_body_.basis_matrices().at(control_point_idx), + }; +} + +TrajectoryEvaluationParams Trajectory::GetEvaluationParamsRotation(double stamp) const { + const int control_point_idx = + spline_rotation_world_from_body_.GetSplineIndex(stamp); + const int knot_idx = + spline_rotation_world_from_body_.GetKnotIndexFromSplineIndex( + control_point_idx); + const int num_control_points = spline_rotation_world_from_body_.GetSplineOrder(); + return TrajectoryEvaluationParams { + .spline_index = control_point_idx, + .knot0 = spline_rotation_world_from_body_.knots().at(knot_idx), + .knot1 = spline_rotation_world_from_body_.knots().at(knot_idx + 1), + .stamp = stamp, + .num_control_points = num_control_points, + .basis_matrix = + spline_rotation_world_from_body_.basis_matrices().at(control_point_idx), }; } @@ -94,12 +117,18 @@ void Trajectory::UnwrapPhaseLogMap(std::vector& phi) { absl::StatusOr> Trajectory::Interpolate(const std::vector& interp_times) const { - std::vector> pose_vectors_interp; - ASSIGN_OR_RETURN(pose_vectors_interp, - spline_pose_world_body_.Interpolate(interp_times)); + std::vector position_vectors_interp; + std::vector rotation_vectors_interp; + ASSIGN_OR_RETURN(position_vectors_interp, + spline_position_world_to_body_.Interpolate(interp_times)); + ASSIGN_OR_RETURN(rotation_vectors_interp, + spline_rotation_world_from_body_.Interpolate(interp_times)); std::vector interpolated_poses(interp_times.size()); for (int i = 0; i < interp_times.size(); ++i) { - interpolated_poses[i] = VectorToPose3(pose_vectors_interp[i]); + Eigen::Vector pose_vector; + pose_vector.head<3>() = rotation_vectors_interp[i]; + pose_vector.tail<3>() = position_vectors_interp[i]; + interpolated_poses[i] = VectorToPose3(pose_vector); } return interpolated_poses; } diff --git a/calico/trajectory.h b/calico/trajectory.h index 965061d..56b3d49 100644 --- a/calico/trajectory.h +++ b/calico/trajectory.h @@ -63,9 +63,13 @@ class Trajectory { /// parameters added to the problem. int AddParametersToProblem(ceres::Problem& problem); - /// Accessors for the spline. - const BSpline<6>& spline() const { return spline_pose_world_body_; } - BSpline<6>& spline() { return spline_pose_world_body_; } + /// Accessors for the rotation spline. + const BSpline<3>& rotation_spline() const { return spline_rotation_world_from_body_; } + BSpline<3>& rotation_spline() { return spline_rotation_world_from_body_; } + + /// Accesors for the position spline. + const BSpline<3>& position_spline() const { return spline_position_world_to_body_; } + BSpline<3>& position_spline() { return spline_position_world_to_body_; } /// Interpolate the trajectory at given timestamps. Returns a vector of /// world-from-sensorrig poses evaluated in the order of the timestamps. @@ -104,12 +108,16 @@ class Trajectory { const absl::flat_hash_map& trajectory() const; absl::flat_hash_map& trajectory(); - /// Get the parameters needed to evaluate the spline for a given timestamp. - TrajectoryEvaluationParams GetEvaluationParams(double stamp) const; + /// Get the parameters needed to evaluate the position spline for a given timestamp. + TrajectoryEvaluationParams GetEvaluationParamsPosition(double stamp) const; + + /// Get the parameters needed to evaluate the rotation spline for a given timestamp. + TrajectoryEvaluationParams GetEvaluationParamsRotation(double stamp) const; private: absl::flat_hash_map pose_id_to_pose_world_body_; - BSpline<6> spline_pose_world_body_; + BSpline<3> spline_rotation_world_from_body_; + BSpline<3> spline_position_world_to_body_; // Convenience function for unwrapping discrete axis-angle vectors in order to // get a more continuous signal. From 371e30e4cdd9352282d4e4a2dd76e049d77974d5 Mon Sep 17 00:00:00 2001 From: James Yang Date: Sat, 14 Jun 2025 10:43:46 -0700 Subject: [PATCH 2/8] Fixed a bunch of tests for gyroscope, gyroscope cost functor, and geometry --- calico/bspline.h | 13 +++++++- calico/sensors/gyroscope_cost_functor.cpp | 24 ++++++++------ calico/sensors/gyroscope_cost_functor.h | 15 +++++---- calico/test/geometry_test.cpp | 35 +++++++++++---------- calico/test/gyroscope_cost_functor_test.cpp | 2 +- calico/test/gyroscope_test.cpp | 25 ++++++++------- 6 files changed, 66 insertions(+), 48 deletions(-) diff --git a/calico/bspline.h b/calico/bspline.h index 36fb1f8..48d0ad1 100644 --- a/calico/bspline.h +++ b/calico/bspline.h @@ -43,9 +43,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>> + absl::StatusOr>> Interpolate(const std::vector& times, int derivative = 0) const; + // Interpolate the spline at single time for the given derivative. + absl::StatusOr> + Interpolate(T time, int derivative = 0) const { + const absl::StatusOr>> statusor = + Interpolate(std::vector{time}, derivative); + if (!statusor.ok()) { + return statusor.status(); + } + return statusor->front(); + } + // TODO(yangjames): Description Eigen::MatrixX GetSplineBasis(int spline_idx, T stamp, int derivative) const; diff --git a/calico/sensors/gyroscope_cost_functor.cpp b/calico/sensors/gyroscope_cost_functor.cpp index 3c1ae16..937e533 100644 --- a/calico/sensors/gyroscope_cost_functor.cpp +++ b/calico/sensors/gyroscope_cost_functor.cpp @@ -39,22 +39,26 @@ ceres::CostFunction* GyroscopeCostFunctor::CreateCostFunction( parameters.push_back(&latency); cost_function->AddParameterBlock(1); // trajectory spline control points. - const int position_idx = trajectory_world_sensorrig.position_spline().GetSplineIndex(stamp); - const int rotation_idx = trajectory_world_sensorrig.rotation_spline().GetSplineIndex(stamp); - const int position_spline_order = trajectory_world_sensorrig.position_spline().GetSplineOrder(); - const int rotation_spline_order = trajectory_world_sensorrig.rotation_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()); - } + const int rotation_spline_order = + trajectory_world_sensorrig.rotation_spline().GetSplineOrder(); + const int rotation_idx = + trajectory_world_sensorrig.rotation_spline().GetSplineIndex(stamp); for (int i = 0; i < rotation_spline_order; ++i) { parameters.push_back( trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).data()); cost_function->AddParameterBlock( 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(kGyroscopeResidualSize); return cost_function; diff --git a/calico/sensors/gyroscope_cost_functor.h b/calico/sensors/gyroscope_cost_functor.h index f0326e3..d756903 100644 --- a/calico/sensors/gyroscope_cost_functor.h +++ b/calico/sensors/gyroscope_cost_functor.h @@ -78,22 +78,21 @@ class GyroscopeCostFunctor { // Parse sensor rig spline resolved in the world frame. const int num_rotation_control_points = rotation_trajectory_evaluation_params_.num_control_points; - Eigen::MatrixX rotation_control_points(num_rotation_control_points, 6); + Eigen::MatrixX 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>( + rotation_control_points.row(i) = Eigen::Map>( &(parameters[static_cast( GyroscopeParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i ][0])); } const int num_position_control_points = position_trajectory_evaluation_params_.num_control_points; - Eigen::MatrixX position_control_points(num_position_control_points, 6); - for (int i = num_rotation_control_points; - i < num_rotation_control_points + num_position_control_points; - ++i) { - position_control_points.row(i) = Eigen::Map>( + Eigen::MatrixX 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>( &(parameters[static_cast( - GyroscopeParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i + GyroscopeParameterIndices::kSensorRigPoseSplineControlPointsIndex) + + i + num_rotation_control_points ][0])); } diff --git a/calico/test/geometry_test.cpp b/calico/test/geometry_test.cpp index 2339b47..5e9a4b7 100644 --- a/calico/test/geometry_test.cpp +++ b/calico/test/geometry_test.cpp @@ -55,17 +55,18 @@ TEST(GeometryTest, TestComputeRodriguesFormulaJacobians) { const double stamp_i = timestamps.at(i); const double stamp_ii = stamp_i + dt; const double stamp_iii = stamp_ii + dt; - const Eigen::Vector pose_vector_i = - trajectory.spline().Interpolate({stamp_i}, /*derivative=*/0).value()[0]; - const Eigen::Vector pose_vector_ii = - trajectory.spline().Interpolate( - {stamp_ii}, /*derivative=*/0).value()[0]; - const Eigen::Vector pose_vector_iii = - trajectory.spline().Interpolate( - {stamp_iii}, /*derivative=*/0).value()[0]; - const Eigen::Vector3d phi_world_body_i = pose_vector_i.head(3); - const Eigen::Vector3d phi_world_body_ii = pose_vector_ii.head(3); - const Eigen::Vector3d phi_world_body_iii = pose_vector_iii.head(3); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_world_body_i, + trajectory.rotation_spline().Interpolate(stamp_i, + /*derivative=*/0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_world_body_ii, + trajectory.rotation_spline().Interpolate(stamp_ii, + /*derivative=*/0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_world_body_iii, + trajectory.rotation_spline().Interpolate(stamp_iii, + /*derivative=*/0)); const Eigen::Vector3d phi_body_world_i = -phi_world_body_i; const Eigen::Vector3d phi_body_world_ii = -phi_world_body_ii; const Eigen::Vector3d phi_body_world_iii = -phi_world_body_iii; @@ -116,13 +117,13 @@ TEST(GeometryTest, TestComputeRodriguesFormulaJacobians) { const Eigen::Vector3d expected_alpha_body_world = dt_inv * (expected_omega_body_world_ii - expected_omega_body_world_i); // Compute analytical first and second time derivatives. - const Eigen::Vector pose_dot_vector_i = - trajectory.spline().Interpolate({stamp_i}, /*derivative=*/1).value()[0]; - const Eigen::Vector pose_ddot_vector_i = - trajectory.spline().Interpolate({stamp_i}, /*derivative=*/2).value()[0]; - const Eigen::Vector3d phi_dot_world_body_i = pose_dot_vector_i.head(3); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_dot_world_body_i, + trajectory.rotation_spline().Interpolate(stamp_i, /*derivative=*/1)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_ddot_world_body_i, + trajectory.rotation_spline().Interpolate(stamp_i, /*derivative=*/2)); const Eigen::Vector3d phi_dot_body_world_i = -phi_dot_world_body_i; - const Eigen::Vector3d phi_ddot_world_body_i = pose_ddot_vector_i.head(3); const Eigen::Vector3d phi_ddot_body_world_i = -phi_ddot_world_body_i; // Compute angular velocity. const Eigen::Matrix3d J_world_body = ExpSO3Jacobian(phi_world_body_i); diff --git a/calico/test/gyroscope_cost_functor_test.cpp b/calico/test/gyroscope_cost_functor_test.cpp index 9856206..c2f56f2 100644 --- a/calico/test/gyroscope_cost_functor_test.cpp +++ b/calico/test/gyroscope_cost_functor_test.cpp @@ -39,7 +39,7 @@ TEST_P(GyroscopeCostFunctionCreationTest, Instantiation) { std::vector parameters; for (const auto& stamp : timestamps) { TrajectoryEvaluationParams segment = - trajectory_world_sensorrig.GetEvaluationParams(stamp); + trajectory_world_sensorrig.GetEvaluationParamsRotation(stamp); auto* cost_function = GyroscopeCostFunctor::CreateCostFunction( test_case.measurement, test_case.sigma, test_case.gyroscope_model, diff --git a/calico/test/gyroscope_test.cpp b/calico/test/gyroscope_test.cpp index f0b4db0..d7cf1bc 100644 --- a/calico/test/gyroscope_test.cpp +++ b/calico/test/gyroscope_test.cpp @@ -128,17 +128,20 @@ TEST_P(GyroscopeTest, AnalyticallyVsNumericallyDiffedKinematicsMatch) { for (int i = 0; i < stamps.size() - 2; ++i) { const double stamp_i = stamps.at(i); const double stamp_ii = stamp_i + dt; - const Eigen::Vector pose_vector_i = - trajectory_world_sensorrig.spline().Interpolate( - {stamp_i}, /*derivative=*/0).value()[0]; - const Eigen::Vector pose_vector_ii = - trajectory_world_sensorrig.spline().Interpolate( - {stamp_ii}, /*derivative=*/0).value()[0]; - const Eigen::Vector pose_dot_vector_i = - trajectory_world_sensorrig.spline().Interpolate( - {stamp_i}, /*derivative=*/1).value()[0]; - const Eigen::Vector3d phi_sensorrig_world_i = -pose_vector_i.head(3); - const Eigen::Vector3d phi_sensorrig_world_ii = -pose_vector_ii.head(3); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_world_sensorrig_i, + trajectory_world_sensorrig.rotation_spline().Interpolate( + stamp_i, /*derivative=*/0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_world_sensorrig_ii, + trajectory_world_sensorrig.rotation_spline().Interpolate( + stamp_ii, /*derivative=*/0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_dot_world_sensorrig_i, + trajectory_world_sensorrig.rotation_spline().Interpolate( + stamp_i, /*derivative=*/1)); + const Eigen::Vector3d phi_sensorrig_world_i = -phi_world_sensorrig_i; + const Eigen::Vector3d phi_sensorrig_world_ii = -phi_world_sensorrig_ii; const Eigen::Matrix3d R_sensorrig_world_i = ExpSO3(phi_sensorrig_world_i); const Eigen::Matrix3d R_sensorrig_world_ii = ExpSO3(phi_sensorrig_world_ii); From 492d1a1e7fae9d55c7f9dce652bc383127840eb7 Mon Sep 17 00:00:00 2001 From: James Yang Date: Sun, 15 Jun 2025 07:13:41 -0700 Subject: [PATCH 3/8] Fixed all tests --- CMakeLists.txt | 512 +++++++++--------- calico/sensors/accelerometer.cpp | 25 +- calico/sensors/accelerometer_cost_functor.cpp | 24 +- calico/sensors/accelerometer_cost_functor.h | 86 ++- calico/sensors/camera_cost_functor.cpp | 16 +- calico/sensors/camera_cost_functor.h | 18 +- calico/sensors/gyroscope_cost_functor.h | 3 +- .../test/accelerometer_cost_functor_test.cpp | 2 - calico/test/accelerometer_test.cpp | 33 +- calico/test/batch_optimizer_test.cpp | 5 +- calico/test/camera_cost_functor_test.cpp | 2 - calico/test/gyroscope_cost_functor_test.cpp | 2 - 12 files changed, 387 insertions(+), 341 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 66e23d1..0cc13d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,84 +85,84 @@ target_link_libraries( world_model ) -# # Accelerometer library and tests. -# add_library( -# accelerometer_models -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_models.cpp -# ) -# target_link_libraries( -# accelerometer_models -# absl::statusor -# Eigen3::Eigen -# ) +# Accelerometer library and tests. +add_library( + accelerometer_models + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_models.cpp +) +target_link_libraries( + accelerometer_models + absl::statusor + Eigen3::Eigen +) -# add_library( -# accelerometer_cost_functor -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_cost_functor.cpp -# ) -# target_link_libraries( -# accelerometer_cost_functor -# accelerometer_models -# trajectory -# ) +add_library( + accelerometer_cost_functor + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_cost_functor.cpp +) +target_link_libraries( + accelerometer_cost_functor + accelerometer_models + trajectory +) -# add_library( -# accelerometer -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer.cpp -# ) -# target_link_libraries( -# accelerometer -# accelerometer_cost_functor -# accelerometer_models -# trajectory -# world_model -# ) +add_library( + accelerometer + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer.cpp +) +target_link_libraries( + accelerometer + accelerometer_cost_functor + accelerometer_models + trajectory + world_model +) -# # Camera library. -# add_library( -# camera_models -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_models.cpp -# ) -# target_link_libraries( -# camera_models -# absl::statusor -# Eigen3::Eigen -# ) +# Camera library. +add_library( + camera_models + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_models.cpp +) +target_link_libraries( + camera_models + absl::statusor + Eigen3::Eigen +) -# add_library( -# camera_cost_functor -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_cost_functor.cpp -# ) -# target_link_libraries( -# camera_cost_functor -# camera_models -# trajectory -# ) +add_library( + camera_cost_functor + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_cost_functor.cpp +) +target_link_libraries( + camera_cost_functor + camera_models + trajectory +) -# add_library( -# camera -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera.cpp -# ) -# target_link_libraries( -# camera -# camera_models -# camera_cost_functor -# trajectory -# world_model -# ) +add_library( + camera + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera.cpp +) +target_link_libraries( + camera + camera_models + camera_cost_functor + trajectory + world_model +) -# # Batch optimizer. -# add_library( -# batch_optimizer -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/batch_optimizer.cpp -# ) -# target_link_libraries( -# batch_optimizer -# accelerometer -# camera -# gyroscope -# world_model -# ) +# Batch optimizer. +add_library( + batch_optimizer + ${CMAKE_CURRENT_SOURCE_DIR}/calico/batch_optimizer.cpp +) +target_link_libraries( + batch_optimizer + accelerometer + camera + gyroscope + world_model +) # AprilTags c++ library. include_directories( @@ -204,104 +204,104 @@ target_link_libraries( ${YAML_CPP_LIBRARIES} ) -# # Python bindings. -# pybind11_add_module( -# _calico -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/calico.cpp -# ) -# target_link_libraries( -# _calico PUBLIC -# accelerometer -# aprilgrid_detector -# batch_optimizer -# camera -# gyroscope -# trajectory -# world_model -# Eigen3::Eigen -# ) -# set_target_properties( -# _calico PROPERTIES -# LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/calico -# ) -# file( -# COPY -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/__init__.py -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/utils.py -# DESTINATION ${PROJECT_BINARY_DIR}/calico -# ) +# Python bindings. +pybind11_add_module( + _calico + ${CMAKE_CURRENT_SOURCE_DIR}/calico/calico.cpp +) +target_link_libraries( + _calico PUBLIC + accelerometer + aprilgrid_detector + batch_optimizer + camera + gyroscope + trajectory + world_model + Eigen3::Eigen +) +set_target_properties( + _calico PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR}/calico +) +file( + COPY + ${CMAKE_CURRENT_SOURCE_DIR}/calico/__init__.py + ${CMAKE_CURRENT_SOURCE_DIR}/calico/utils.py + DESTINATION ${PROJECT_BINARY_DIR}/calico +) -# # Install cpp library. -# install( -# TARGETS -# accelerometer -# accelerometer_cost_functor -# accelerometer_models -# apriltags -# batch_optimizer -# camera -# camera_cost_functor -# camera_models -# gyroscope -# gyroscope_cost_functor -# gyroscope_models -# trajectory -# world_model -# aprilgrid_detector -# EXPORT -# CalicoConfig -# DESTINATION lib/calico -# ) -# install( -# EXPORT -# CalicoConfig -# DESTINATION lib/cmake/calico -# ) -# set(top_level_headers -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/profiler.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/trajectory.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/bspline.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/test_utils.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/optimization_utils.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/batch_optimizer.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/typedefs.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/matchers.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/status_builder.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/geometry.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/world_model.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/statusor_macros.h -# ) -# install( -# FILES ${top_level_headers} -# DESTINATION include/calico -# ) -# set(sensor_headers -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope_models.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_cost_functor.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/sensor_base.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope_cost_functor.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_cost_functor.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_models.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_models.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope.h -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer.h -# ) -# install( -# FILES ${sensor_headers} -# DESTINATION include/calico/sensors/ -# ) -# # Install python library. -# install( -# TARGETS _calico -# DESTINATION ${Python_SITELIB}/calico -# ) -# install( -# FILES -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/utils.py -# ${CMAKE_CURRENT_SOURCE_DIR}/calico/__init__.py -# DESTINATION ${Python_SITELIB}/calico -# ) +# Install cpp library. +install( + TARGETS + accelerometer + accelerometer_cost_functor + accelerometer_models + apriltags + batch_optimizer + camera + camera_cost_functor + camera_models + gyroscope + gyroscope_cost_functor + gyroscope_models + trajectory + world_model + aprilgrid_detector + EXPORT + CalicoConfig + DESTINATION lib/calico +) +install( + EXPORT + CalicoConfig + DESTINATION lib/cmake/calico +) +set(top_level_headers + ${CMAKE_CURRENT_SOURCE_DIR}/calico/profiler.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/trajectory.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/bspline.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/test_utils.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/optimization_utils.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/batch_optimizer.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/typedefs.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/matchers.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/status_builder.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/geometry.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/world_model.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/statusor_macros.h +) +install( + FILES ${top_level_headers} + DESTINATION include/calico +) +set(sensor_headers + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope_models.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_cost_functor.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/sensor_base.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope_cost_functor.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_cost_functor.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/camera_models.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer_models.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/gyroscope.h + ${CMAKE_CURRENT_SOURCE_DIR}/calico/sensors/accelerometer.h +) +install( + FILES ${sensor_headers} + DESTINATION include/calico/sensors/ +) +# Install python library. +install( + TARGETS _calico + DESTINATION ${Python_SITELIB}/calico +) +install( + FILES + ${CMAKE_CURRENT_SOURCE_DIR}/calico/utils.py + ${CMAKE_CURRENT_SOURCE_DIR}/calico/__init__.py + DESTINATION ${Python_SITELIB}/calico +) if (BUILD_TESTING) # Typedefs test. @@ -403,92 +403,92 @@ if (BUILD_TESTING) gmock ) add_test(GyroscopeLibTest gyroscope_test) - # # Accelerometer models test. - # add_executable( - # accelerometer_models_test - # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_models_test.cpp - # ) - # target_link_libraries( - # accelerometer_models_test - # accelerometer_models - # GTest::Main - # gmock - # ) - # add_test(AccelerometerModelsTest accelerometer_models_test) - # # Accelerometer cost functor test. - # add_executable( - # accelerometer_cost_functor_test - # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_cost_functor_test.cpp - # ) - # target_link_libraries( - # accelerometer_cost_functor_test - # accelerometer_cost_functor - # GTest::Main - # gmock - # ) - # add_test(AccelerometerCostFunctorTest accelerometer_cost_functor_test) - # # Accelerometer test. - # add_executable( - # accelerometer_test - # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_test.cpp - # ) - # target_link_libraries( - # accelerometer_test - # accelerometer - # GTest::Main - # gmock - # ) - # add_test(AccelerometerLibTest accelerometer_test) - # # Camera models test. - # add_executable( - # camera_models_test - # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_models_test.cpp - # ) - # target_link_libraries( - # camera_models_test - # camera_models - # GTest::Main - # gmock - # ) - # add_test(CameraModelsTest camera_models_test) - # # Camera cost functor test. - # add_executable( - # camera_cost_functor_test - # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_cost_functor_test.cpp - # ) - # target_link_libraries( - # camera_cost_functor_test - # camera_cost_functor - # GTest::Main - # gmock - # ) - # add_test(CameraCostFunctorTest camera_cost_functor_test) - # # Camera test. - # add_executable( - # camera_test - # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_test.cpp - # ) - # target_link_libraries( - # camera_test - # camera - # GTest::Main - # gmock - # ) - # add_test(CameraLibTest camera_test) - # # Batch optimizer test. - # add_executable( - # batch_optimizer_test - # ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/batch_optimizer_test.cpp - # ) - # target_link_libraries( - # batch_optimizer_test - # camera - # world_model - # batch_optimizer - # GTest::Main - # gmock - # ) - # add_test(BatchOptimizerTest batch_optimizer_test) + # Accelerometer models test. + add_executable( + accelerometer_models_test + ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_models_test.cpp + ) + target_link_libraries( + accelerometer_models_test + accelerometer_models + GTest::Main + gmock + ) + add_test(AccelerometerModelsTest accelerometer_models_test) + # Accelerometer cost functor test. + add_executable( + accelerometer_cost_functor_test + ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_cost_functor_test.cpp + ) + target_link_libraries( + accelerometer_cost_functor_test + accelerometer_cost_functor + GTest::Main + gmock + ) + add_test(AccelerometerCostFunctorTest accelerometer_cost_functor_test) + # Accelerometer test. + add_executable( + accelerometer_test + ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/accelerometer_test.cpp + ) + target_link_libraries( + accelerometer_test + accelerometer + GTest::Main + gmock + ) + add_test(AccelerometerLibTest accelerometer_test) + # Camera models test. + add_executable( + camera_models_test + ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_models_test.cpp + ) + target_link_libraries( + camera_models_test + camera_models + GTest::Main + gmock + ) + add_test(CameraModelsTest camera_models_test) + # Camera cost functor test. + add_executable( + camera_cost_functor_test + ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_cost_functor_test.cpp + ) + target_link_libraries( + camera_cost_functor_test + camera_cost_functor + GTest::Main + gmock + ) + add_test(CameraCostFunctorTest camera_cost_functor_test) + # Camera test. + add_executable( + camera_test + ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/camera_test.cpp + ) + target_link_libraries( + camera_test + camera + GTest::Main + gmock + ) + add_test(CameraLibTest camera_test) + # Batch optimizer test. + add_executable( + batch_optimizer_test + ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/batch_optimizer_test.cpp + ) + target_link_libraries( + batch_optimizer_test + camera + world_model + batch_optimizer + GTest::Main + gmock + ) + add_test(BatchOptimizerTest batch_optimizer_test) # # Python bindings test. # add_test( # NAME python_bindings_test @@ -504,5 +504,5 @@ if (BUILD_TESTING) # set_tests_properties( # python_bindings_test python_utils_test # PROPERTIES ENVIRONMENT "PYTHONPATH=${PROJECT_BINARY_DIR}:$ENV{PYTHONPATH}" - # ) + # ) endif() diff --git a/calico/sensors/accelerometer.cpp b/calico/sensors/accelerometer.cpp index adcc91e..ff8b890 100644 --- a/calico/sensors/accelerometer.cpp +++ b/calico/sensors/accelerometer.cpp @@ -77,29 +77,34 @@ absl::StatusOr> Accelerometer::Project( const std::vector& interp_times, const Trajectory& sensorrig_trajectory, const WorldModel& world_model) const { - std::vector> pose_vectors; - ASSIGN_OR_RETURN(pose_vectors, sensorrig_trajectory.spline().Interpolate( + ASSIGN_OR_RETURN( + std::vector ddt_world_sensorrig_vector, + sensorrig_trajectory.position_spline().Interpolate( + interp_times, /*derivative=*/2)); + ASSIGN_OR_RETURN(std::vector phi_world_sensorrig_vector, + sensorrig_trajectory.rotation_spline().Interpolate( interp_times, /*derivative=*/0)); - std::vector> pose_dot_vectors; - ASSIGN_OR_RETURN(pose_dot_vectors, sensorrig_trajectory.spline().Interpolate( + ASSIGN_OR_RETURN(std::vector phi_dot_world_sensorrig_vector, + sensorrig_trajectory.rotation_spline().Interpolate( interp_times, /*derivative=*/1)); - std::vector> pose_ddot_vectors; - ASSIGN_OR_RETURN(pose_ddot_vectors, sensorrig_trajectory.spline().Interpolate( + ASSIGN_OR_RETURN(std::vector phi_ddot_world_sensorrig_vector, + sensorrig_trajectory.rotation_spline().Interpolate( interp_times, /*derivative=*/2)); + std::vector 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); diff --git a/calico/sensors/accelerometer_cost_functor.cpp b/calico/sensors/accelerometer_cost_functor.cpp index f714d26..53d86a5 100644 --- a/calico/sensors/accelerometer_cost_functor.cpp +++ b/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); diff --git a/calico/sensors/accelerometer_cost_functor.h b/calico/sensors/accelerometer_cost_functor.h index 1fe5c55..e051f34 100644 --- a/calico/sensors/accelerometer_cost_functor.h +++ b/calico/sensors/accelerometer_cost_functor.h @@ -84,40 +84,69 @@ class AccelerometerCostFunctor { &(parameters[static_cast( 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 control_points(num_control_points, 6); - for (int i = 0; i < num_control_points; ++i) { - control_points.row(i) = Eigen::Map>(&(parameters[ - static_cast( - AccelerometerParameterIndices::kSensorRigPoseSplineControlPointsIndex - ) + i][0])); + const int num_rotation_control_points = + rotation_trajectory_evaluation_params_.num_control_points; + Eigen::MatrixX 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>( + &(parameters[static_cast( + AccelerometerParameterIndices::kSensorRigPoseSplineControlPointsIndex) + + i + ][0])); } - const Eigen::MatrixX basis_matrix = - trajectory_evaluation_params_.basis_matrix.template cast(); - const T knot0 = static_cast(trajectory_evaluation_params_.knot0); - const T knot1 = static_cast(trajectory_evaluation_params_.knot1); - const T stamp = - static_cast(trajectory_evaluation_params_.stamp) - latency; + const int num_position_control_points = + position_trajectory_evaluation_params_.num_control_points; + Eigen::MatrixX 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>( + &(parameters[static_cast( + AccelerometerParameterIndices::kSensorRigPoseSplineControlPointsIndex) + + i + num_rotation_control_points + ][0])); + } + + const Eigen::MatrixX rotation_basis_matrix = + rotation_trajectory_evaluation_params_.basis_matrix.template cast(); + const T rotation_knot0 = static_cast( + rotation_trajectory_evaluation_params_.knot0); + const T rotation_knot1 = static_cast( + rotation_trajectory_evaluation_params_.knot1); + const T rotation_stamp = + static_cast(rotation_trajectory_evaluation_params_.stamp) - latency; + + const Eigen::MatrixX position_basis_matrix = + position_trajectory_evaluation_params_.basis_matrix.template cast(); + const T position_knot0 = static_cast( + position_trajectory_evaluation_params_.knot0); + const T position_knot1 = static_cast( + position_trajectory_evaluation_params_.knot1); + const T position_stamp = + static_cast(position_trajectory_evaluation_params_.stamp) - latency; + // Evaluate the pose, pose rate, and pose acceleration. - const Eigen::Vector pose_vector = BSpline<6, T>::Evaluate( - control_points, knot0, knot1, basis_matrix, stamp, /*derivative=*/0); - const Eigen::Vector pose_dot_vector = BSpline<6, T>::Evaluate( - control_points, knot0, knot1, basis_matrix, stamp, /*derivative=*/1); - const Eigen::Vector 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 phi_sensorrig_world = -pose_vector.head(3); - const Eigen::Vector3 phi_dot_sensorrig_world = -pose_dot_vector.head(3); - const Eigen::Vector3 phi_ddot_sensorrig_world = -pose_ddot_vector.head(3); - const Eigen::Vector3 ddt_world_sensorrig = pose_ddot_vector.tail(3); + const Eigen::Vector3 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 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_world_sensorrig = BSpline<3, T>::Evaluate( + position_control_points, position_knot0, position_knot1, + position_basis_matrix, position_stamp, 0); + const Eigen::Vector3 phi_dot_sensorrig_world = -BSpline<3, T>::Evaluate( + rotation_control_points, rotation_knot0, rotation_knot1, + rotation_basis_matrix, rotation_stamp, 1); + const Eigen::Vector3 phi_ddot_sensorrig_world = -BSpline<3, T>::Evaluate( + rotation_control_points, rotation_knot0, rotation_knot1, + rotation_basis_matrix, rotation_stamp, 2); + const Eigen::Vector3 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 J_sensorrig_world = ExpSO3Jacobian(phi_sensorrig_world); const Eigen::Matrix3 J_dot_sensorrig_world = @@ -150,7 +179,8 @@ class AccelerometerCostFunctor { Eigen::Vector3d measurement_; double information_; std::unique_ptr 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_ diff --git a/calico/sensors/camera_cost_functor.cpp b/calico/sensors/camera_cost_functor.cpp index 1ca0a26..4ffffd4 100644 --- a/calico/sensors/camera_cost_functor.cpp +++ b/calico/sensors/camera_cost_functor.cpp @@ -51,22 +51,22 @@ ceres::CostFunction* CameraCostFunctor::CreateCostFunction( parameters.push_back(t_world_model.data()); cost_function->AddParameterBlock(t_world_model.size()); // trajectory spline control points. - const int position_idx = trajectory_world_sensorrig.position_spline().GetSplineIndex(stamp); const int rotation_idx = trajectory_world_sensorrig.rotation_spline().GetSplineIndex(stamp); - const int position_spline_order = trajectory_world_sensorrig.position_spline().GetSplineOrder(); const int rotation_spline_order = trajectory_world_sensorrig.rotation_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()); - } for (int i = 0; i < rotation_spline_order; ++i) { parameters.push_back( trajectory_world_sensorrig.rotation_spline().control_points().at(rotation_idx + i).data()); cost_function->AddParameterBlock( 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); return cost_function; diff --git a/calico/sensors/camera_cost_functor.h b/calico/sensors/camera_cost_functor.h index 8bdb435..5084f77 100644 --- a/calico/sensors/camera_cost_functor.h +++ b/calico/sensors/camera_cost_functor.h @@ -100,22 +100,22 @@ class CameraCostFunctor { // Parse sensor rig spline resolved in the world frame. const int num_rotation_control_points = rotation_trajectory_evaluation_params_.num_control_points; - Eigen::MatrixX rotation_control_points(num_rotation_control_points, 6); + Eigen::MatrixX 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>( + rotation_control_points.row(i) = Eigen::Map>( &(parameters[static_cast( - CameraParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i + CameraParameterIndices::kSensorRigPoseSplineControlPointsIndex) + + i ][0])); } const int num_position_control_points = position_trajectory_evaluation_params_.num_control_points; - Eigen::MatrixX position_control_points(num_position_control_points, 6); - for (int i = num_rotation_control_points; - i < num_rotation_control_points + num_position_control_points; - ++i) { - position_control_points.row(i) = Eigen::Map>( + Eigen::MatrixX 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>( &(parameters[static_cast( - CameraParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i + CameraParameterIndices::kSensorRigPoseSplineControlPointsIndex) + + i + num_rotation_control_points ][0])); } diff --git a/calico/sensors/gyroscope_cost_functor.h b/calico/sensors/gyroscope_cost_functor.h index d756903..4874f3c 100644 --- a/calico/sensors/gyroscope_cost_functor.h +++ b/calico/sensors/gyroscope_cost_functor.h @@ -82,7 +82,8 @@ class GyroscopeCostFunctor { for (int i = 0; i < num_rotation_control_points; ++i) { rotation_control_points.row(i) = Eigen::Map>( &(parameters[static_cast( - GyroscopeParameterIndices::kSensorRigPoseSplineControlPointsIndex) + i + GyroscopeParameterIndices::kSensorRigPoseSplineControlPointsIndex) + + i ][0])); } const int num_position_control_points = diff --git a/calico/test/accelerometer_cost_functor_test.cpp b/calico/test/accelerometer_cost_functor_test.cpp index 96de66c..369fc89 100644 --- a/calico/test/accelerometer_cost_functor_test.cpp +++ b/calico/test/accelerometer_cost_functor_test.cpp @@ -39,8 +39,6 @@ TEST_P(AccelerometerCostFunctionCreationTest, Instantiation) { AccelerometerCostFunctionCreationTestCase test_case = GetParam(); std::vector parameters; for (const auto& stamp : timestamps) { - TrajectoryEvaluationParams segment = - trajectory_world_sensorrig.GetEvaluationParams(stamp); auto* cost_function = AccelerometerCostFunctor::CreateCostFunction( test_case.measurement, test_case.sigma, test_case.accelerometer_model, diff --git a/calico/test/accelerometer_test.cpp b/calico/test/accelerometer_test.cpp index 850e2f1..81047ce 100644 --- a/calico/test/accelerometer_test.cpp +++ b/calico/test/accelerometer_test.cpp @@ -131,21 +131,24 @@ TEST_P(AccelerometerTest, AnalyticallyVsNumericallyDiffedKinematicsMatch) { const double stamp_i = stamps.at(i); const double stamp_ii = stamp_i + dt; const double stamp_iii = stamp_ii + dt; - const Eigen::Vector pose_vector_i = - trajectory_world_sensorrig.spline().Interpolate( - {stamp_i}, /*derivative=*/0).value()[0]; - const Eigen::Vector pose_vector_ii = - trajectory_world_sensorrig.spline().Interpolate( - {stamp_ii}, /*derivative=*/0).value()[0]; - const Eigen::Vector pose_vector_iii = - trajectory_world_sensorrig.spline().Interpolate( - {stamp_iii}, /*derivative=*/0).value()[0]; - const Eigen::Vector3d phi_world_sensorrig_i = pose_vector_i.head(3); - const Eigen::Vector3d phi_world_sensorrig_ii = pose_vector_ii.head(3); - const Eigen::Vector3d phi_world_sensorrig_iii = pose_vector_iii.head(3); - const Eigen::Vector3d t_world_sensorrig_i = pose_vector_i.tail(3); - const Eigen::Vector3d t_world_sensorrig_ii = pose_vector_ii.tail(3); - const Eigen::Vector3d t_world_sensorrig_iii = pose_vector_iii.tail(3); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_world_sensorrig_i, + trajectory_world_sensorrig.rotation_spline().Interpolate(stamp_i, 0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_world_sensorrig_ii, + trajectory_world_sensorrig.rotation_spline().Interpolate(stamp_ii, 0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d phi_world_sensorrig_iii, + trajectory_world_sensorrig.rotation_spline().Interpolate(stamp_iii, 0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d t_world_sensorrig_i, + trajectory_world_sensorrig.position_spline().Interpolate(stamp_i, 0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d t_world_sensorrig_ii, + trajectory_world_sensorrig.position_spline().Interpolate(stamp_ii, 0)); + ASSERT_OK_AND_ASSIGN( + Eigen::Vector3d t_world_sensorrig_iii, + trajectory_world_sensorrig.position_spline().Interpolate(stamp_iii, 0)); const Eigen::Matrix3d R_world_sensorrig_i = ExpSO3(phi_world_sensorrig_i); const Eigen::Matrix3d R_world_sensorrig_ii = ExpSO3(phi_world_sensorrig_ii); diff --git a/calico/test/batch_optimizer_test.cpp b/calico/test/batch_optimizer_test.cpp index f6ff3c8..ab965fe 100644 --- a/calico/test/batch_optimizer_test.cpp +++ b/calico/test/batch_optimizer_test.cpp @@ -179,7 +179,10 @@ TEST_F(BatchOptimizerTest, ToyStereoCameraAndImuCalibration) { optimizer.AddSensor(accelerometer); optimizer.AddWorldModel(world_model); optimizer.AddTrajectory(trajectory_world_sensorrig); - ASSERT_OK_AND_ASSIGN(auto summary, optimizer.Optimize()); + const auto statusor_summary = optimizer.Optimize(); + EXPECT_OK(statusor_summary.status()) + << statusor_summary.status().ToString(); + const auto summary = statusor_summary.value(); // Expect near perfect calibration results due to perfect data. constexpr double kSmallNumber = 1e-7; diff --git a/calico/test/camera_cost_functor_test.cpp b/calico/test/camera_cost_functor_test.cpp index 8216672..90a94f6 100644 --- a/calico/test/camera_cost_functor_test.cpp +++ b/calico/test/camera_cost_functor_test.cpp @@ -40,8 +40,6 @@ TEST_P(CameraCostFunctionCreationTest, Instantiation) { CameraCostFunctionCreationTestCase test_case = GetParam(); std::vector parameters; for (const auto& stamp : timestamps) { - TrajectoryEvaluationParams segment = - trajectory_world_sensorrig.GetEvaluationParams(stamp); auto* cost_function = CameraCostFunctor::CreateCostFunction( test_case.pixel, test_case.sigma, test_case.camera_model, diff --git a/calico/test/gyroscope_cost_functor_test.cpp b/calico/test/gyroscope_cost_functor_test.cpp index c2f56f2..70c100a 100644 --- a/calico/test/gyroscope_cost_functor_test.cpp +++ b/calico/test/gyroscope_cost_functor_test.cpp @@ -38,8 +38,6 @@ TEST_P(GyroscopeCostFunctionCreationTest, Instantiation) { GyroscopeCostFunctionCreationTestCase test_case = GetParam(); std::vector parameters; for (const auto& stamp : timestamps) { - TrajectoryEvaluationParams segment = - trajectory_world_sensorrig.GetEvaluationParamsRotation(stamp); auto* cost_function = GyroscopeCostFunctor::CreateCostFunction( test_case.measurement, test_case.sigma, test_case.gyroscope_model, From 1587587b89b42754669f4985afd4a0683864143c Mon Sep 17 00:00:00 2001 From: James Yang Date: Sun, 15 Jun 2025 07:38:01 -0700 Subject: [PATCH 4/8] Made trajectory configurable --- calico/bspline.h | 5 ++++- calico/bspline.hpp | 13 +++++++++++++ calico/trajectory.cpp | 12 +++++++++++- calico/trajectory.h | 5 +++++ 4 files changed, 33 insertions(+), 2 deletions(-) diff --git a/calico/bspline.h b/calico/bspline.h index 48d0ad1..9ad52ed 100644 --- a/calico/bspline.h +++ b/calico/bspline.h @@ -16,13 +16,14 @@ namespace calico { template 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. @@ -102,6 +103,8 @@ class BSpline { std::vector Mi_; std::vector> control_points_; + // Flag for estimating control points in optimization. + bool control_points_enabled_; // Convenience function for computing a knot vector. void ComputeKnotVector(); diff --git a/calico/bspline.hpp b/calico/bspline.hpp index 31331fc..4068581 100644 --- a/calico/bspline.hpp +++ b/calico/bspline.hpp @@ -1,3 +1,5 @@ +#ifndef CALICO_BSPLINE_HPP_ +#define CALICO_BSPLINE_HPP_ #include #include "calico/statusor_macros.h" @@ -13,9 +15,19 @@ int BSpline::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& control_point : control_points_) { + problem.SetParameterBlockConstant(control_point.data()); + } + } return num_parameters_added; } +template +void BSpline::EnableControlPointsEstimation(bool enable) { + control_points_enabled_ = enable; +} + template absl::Status BSpline::FitToData( const std::vector& time, @@ -329,3 +341,4 @@ absl::Status BSpline::CheckDataForSplineFit( } } // namespace calico +#endif // CALICO_BSPLINE_HPP_ \ No newline at end of file diff --git a/calico/trajectory.cpp b/calico/trajectory.cpp index 24b5c23..b2cf621 100644 --- a/calico/trajectory.cpp +++ b/calico/trajectory.cpp @@ -52,11 +52,21 @@ absl::Status Trajectory::FitSpline( int Trajectory::AddParametersToProblem(ceres::Problem& problem) { int num_parameters; - num_parameters += spline_position_world_to_body_.AddParametersToProblem(problem); num_parameters += spline_rotation_world_from_body_.AddParametersToProblem(problem); + num_parameters += spline_position_world_to_body_.AddParametersToProblem(problem); + spline_rotation_world_from_body_.EnableControlPointsEstimation(rotation_enabled_); + spline_position_world_to_body_.EnableControlPointsEstimation(position_enabled_); return num_parameters; } +void Trajectory::EnablePositionEstimation(bool enable) { + position_enabled_ = enable; +} + +void Trajectory::EnableRotationEstimation(bool enable) { + rotation_enabled_ = enable; +} + const absl::flat_hash_map& Trajectory::trajectory() const { return pose_id_to_pose_world_body_; } diff --git a/calico/trajectory.h b/calico/trajectory.h index 56b3d49..9025d54 100644 --- a/calico/trajectory.h +++ b/calico/trajectory.h @@ -63,6 +63,9 @@ class Trajectory { /// parameters added to the problem. int AddParametersToProblem(ceres::Problem& problem); + void EnablePositionEstimation(bool enable); + void EnableRotationEstimation(bool enable); + /// Accessors for the rotation spline. const BSpline<3>& rotation_spline() const { return spline_rotation_world_from_body_; } BSpline<3>& rotation_spline() { return spline_rotation_world_from_body_; } @@ -118,6 +121,8 @@ class Trajectory { absl::flat_hash_map pose_id_to_pose_world_body_; BSpline<3> spline_rotation_world_from_body_; BSpline<3> spline_position_world_to_body_; + bool position_enabled_; + bool rotation_enabled_; // Convenience function for unwrapping discrete axis-angle vectors in order to // get a more continuous signal. From ba6ceaf8c189e11220a4525f6f161224c42c09f2 Mon Sep 17 00:00:00 2001 From: James Yang Date: Mon, 16 Jun 2025 11:04:13 -0700 Subject: [PATCH 5/8] Allow for non-zero start id --- calico/chart_detectors/aprilgrid_detector.cpp | 3 ++- calico/chart_detectors/aprilgrid_detector.h | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/calico/chart_detectors/aprilgrid_detector.cpp b/calico/chart_detectors/aprilgrid_detector.cpp index 6558029..dda107a 100644 --- a/calico/chart_detectors/aprilgrid_detector.cpp +++ b/calico/chart_detectors/aprilgrid_detector.cpp @@ -17,6 +17,7 @@ AprilGridDetector::AprilGridDetector(const std::string& config_filename) { .tagRows = config_yaml["tagRows"].as(), .tagSize = config_yaml["tagSize"].as(), .tagSpacing = config_yaml["tagSpacing"].as(), + .startId = config_yaml["startId"].as(), }; 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); diff --git a/calico/chart_detectors/aprilgrid_detector.h b/calico/chart_detectors/aprilgrid_detector.h index 5f2526e..2fb4560 100644 --- a/calico/chart_detectors/aprilgrid_detector.h +++ b/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 { From b11db6b803df9ed2f3c03430d5256972c50022ea Mon Sep 17 00:00:00 2001 From: James Yang Date: Mon, 16 Jun 2025 11:11:14 -0700 Subject: [PATCH 6/8] Update cmakelists --- CMakeLists.txt | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 0cc13d7..1632c21 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -489,20 +489,20 @@ if (BUILD_TESTING) gmock ) add_test(BatchOptimizerTest batch_optimizer_test) - # # Python bindings test. - # add_test( - # NAME python_bindings_test - # COMMAND python3 -m pytest ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/python_bindings_test.py - # WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/calico/test - # ) - # # Python utilities test. - # add_test( - # NAME python_utils_test - # COMMAND python3 -m pytest ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/python_utils_test.py - # WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/calico/test - # ) - # set_tests_properties( - # python_bindings_test python_utils_test - # PROPERTIES ENVIRONMENT "PYTHONPATH=${PROJECT_BINARY_DIR}:$ENV{PYTHONPATH}" - # ) + # Python bindings test. + add_test( + NAME python_bindings_test + COMMAND python3 -m pytest ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/python_bindings_test.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/calico/test + ) + # Python utilities test. + add_test( + NAME python_utils_test + COMMAND python3 -m pytest ${CMAKE_CURRENT_SOURCE_DIR}/calico/test/python_utils_test.py + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/calico/test + ) + set_tests_properties( + python_bindings_test python_utils_test + PROPERTIES ENVIRONMENT "PYTHONPATH=${PROJECT_BINARY_DIR}:$ENV{PYTHONPATH}" + ) endif() From e71ce1553d920651fcb967f54c5eb4953783f8b2 Mon Sep 17 00:00:00 2001 From: James Yang Date: Mon, 16 Jun 2025 11:21:28 -0700 Subject: [PATCH 7/8] Add python bindings to trajectory --- calico/calico.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/calico/calico.cpp b/calico/calico.cpp index 76f6360..bb13f97 100644 --- a/calico/calico.cpp +++ b/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_>(m, "Landmark") From b58073dc9edece6d2be206371b1e38ea3a678ec4 Mon Sep 17 00:00:00 2001 From: James Yang Date: Wed, 18 Jun 2025 14:36:25 -0700 Subject: [PATCH 8/8] Bug fix --- calico/trajectory.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/calico/trajectory.cpp b/calico/trajectory.cpp index b2cf621..2ec6503 100644 --- a/calico/trajectory.cpp +++ b/calico/trajectory.cpp @@ -52,10 +52,10 @@ absl::Status Trajectory::FitSpline( int Trajectory::AddParametersToProblem(ceres::Problem& problem) { int num_parameters; - num_parameters += spline_rotation_world_from_body_.AddParametersToProblem(problem); - num_parameters += spline_position_world_to_body_.AddParametersToProblem(problem); spline_rotation_world_from_body_.EnableControlPointsEstimation(rotation_enabled_); spline_position_world_to_body_.EnableControlPointsEstimation(position_enabled_); + num_parameters += spline_rotation_world_from_body_.AddParametersToProblem(problem); + num_parameters += spline_position_world_to_body_.AddParametersToProblem(problem); return num_parameters; }