Merge commit 'cfb09d18272bb3c30585042533cbe7d876ef7ce0'
Upgrade ceres to the latest.
Change-Id: I2d8fb7d506a98f704e4e2f30e60030a0d6763b43
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/imu/imu_calibrator.h b/frc971/imu/imu_calibrator.h
index be4c4d4..58ff12b 100644
--- a/frc971/imu/imu_calibrator.h
+++ b/frc971/imu/imu_calibrator.h
@@ -124,7 +124,7 @@
Scalar time_offset;
void PopulateParameters(
- ceres::EigenQuaternionParameterization *quaternion_local_parameterization,
+ ceres::EigenQuaternionManifold *quaternion_local_parameterization,
ceres::Problem *problem, ceres::DynamicCostFunction *cost_function,
std::vector<double *> *parameters,
std::vector<std::function<void()>> *post_populate_methods) {
@@ -134,8 +134,8 @@
parameters->push_back(&time_offset);
post_populate_methods->emplace_back(
[this, problem, quaternion_local_parameterization]() {
- problem->SetParameterization(rotation.coeffs().data(),
- quaternion_local_parameterization);
+ problem->SetManifold(rotation.coeffs().data(),
+ quaternion_local_parameterization);
problem->SetParameterLowerBound(&time_offset, 0, -0.03);
problem->SetParameterUpperBound(&time_offset, 0, 0.03);
});
@@ -189,7 +189,7 @@
std::vector<ImuConfig<Scalar>> imus;
std::tuple<std::vector<double *>, std::vector<std::function<void()>>>
PopulateParameters(
- ceres::EigenQuaternionParameterization *quaternion_local_parameterization,
+ ceres::EigenQuaternionManifold *quaternion_local_parameterization,
ceres::Problem *problem, ceres::DynamicCostFunction *cost_function) {
std::vector<std::function<void()>> post_populate_methods;
std::vector<double *> parameters;
diff --git a/frc971/imu/imu_calibrator_solver.cc b/frc971/imu/imu_calibrator_solver.cc
index 19b82e5..8948a47 100644
--- a/frc971/imu/imu_calibrator_solver.cc
+++ b/frc971/imu/imu_calibrator_solver.cc
@@ -38,8 +38,8 @@
const std::vector<ImuConfig<double>> &nominal_config) {
ceres::Problem problem;
- ceres::EigenQuaternionParameterization *quaternion_local_parameterization =
- new ceres::EigenQuaternionParameterization();
+ ceres::EigenQuaternionManifold *quaternion_local_parameterization =
+ new ceres::EigenQuaternionManifold();
AllParameters<double> parameters;
std::vector<size_t> num_readings;
CHECK_EQ(nominal_config.size(), readings.size());
diff --git a/frc971/vision/extrinsics_calibration.cc b/frc971/vision/extrinsics_calibration.cc
index 619ea89..2bbba9a 100644
--- a/frc971/vision/extrinsics_calibration.cc
+++ b/frc971/vision/extrinsics_calibration.cc
@@ -836,8 +836,8 @@
CalibrationParameters *calibration_parameters) {
ceres::Problem problem;
- ceres::EigenQuaternionParameterization *quaternion_local_parameterization =
- new ceres::EigenQuaternionParameterization();
+ ceres::EigenQuaternionManifold *quaternion_local_parameterization =
+ new ceres::EigenQuaternionManifold();
// Set up the only cost function (also known as residual). This uses
// auto-differentiation to obtain the derivative (jacobian).
@@ -874,9 +874,8 @@
if (calibration_parameters->has_pivot) {
// Constrain Z since it's along the rotation axis and therefore
// redundant.
- problem.SetParameterization(
- calibration_parameters->pivot_to_imu_translation.data(),
- new ceres::SubsetParameterization(3, {2}));
+ problem.SetManifold(calibration_parameters->pivot_to_imu_translation.data(),
+ new ceres::SubsetManifold(3, {2}));
} else {
problem.SetParameterBlockConstant(
calibration_parameters->pivot_to_imu.coeffs().data());
@@ -895,18 +894,15 @@
calibration_parameters->board_to_world.coeffs().data());
}
- problem.SetParameterization(
+ problem.SetManifold(
calibration_parameters->initial_orientation.coeffs().data(),
quaternion_local_parameterization);
- problem.SetParameterization(
- calibration_parameters->pivot_to_camera.coeffs().data(),
- quaternion_local_parameterization);
- problem.SetParameterization(
- calibration_parameters->pivot_to_imu.coeffs().data(),
- quaternion_local_parameterization);
- problem.SetParameterization(
- calibration_parameters->board_to_world.coeffs().data(),
- quaternion_local_parameterization);
+ problem.SetManifold(calibration_parameters->pivot_to_camera.coeffs().data(),
+ quaternion_local_parameterization);
+ problem.SetManifold(calibration_parameters->pivot_to_imu.coeffs().data(),
+ quaternion_local_parameterization);
+ problem.SetManifold(calibration_parameters->board_to_world.coeffs().data(),
+ quaternion_local_parameterization);
for (int i = 0; i < 3; ++i) {
problem.SetParameterLowerBound(calibration_parameters->gyro_bias.data(), i,
-0.05);
diff --git a/frc971/vision/target_mapper.cc b/frc971/vision/target_mapper.cc
index b9b3712..57ab6e4 100644
--- a/frc971/vision/target_mapper.cc
+++ b/frc971/vision/target_mapper.cc
@@ -313,8 +313,8 @@
}
ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
- ceres::LocalParameterization *quaternion_local_parameterization =
- new ceres::EigenQuaternionParameterization;
+ ceres::Manifold *quaternion_local_parameterization =
+ new ceres::EigenQuaternionManifold();
int min_constraint_id = std::numeric_limits<int>::max();
int max_constraint_id = std::numeric_limits<int>::min();
@@ -368,10 +368,10 @@
pose_end_iter->second.p.data(),
pose_end_iter->second.q.coeffs().data());
- problem->SetParameterization(pose_begin_iter->second.q.coeffs().data(),
- quaternion_local_parameterization);
- problem->SetParameterization(pose_end_iter->second.q.coeffs().data(),
- quaternion_local_parameterization);
+ problem->SetManifold(pose_begin_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
+ problem->SetManifold(pose_end_iter->second.q.coeffs().data(),
+ quaternion_local_parameterization);
}
// The pose graph optimization problem has six DOFs that are not fully
@@ -411,14 +411,14 @@
this, num_residuals, ceres::DO_NOT_TAKE_OWNERSHIP);
ceres::LossFunction *loss_function = new ceres::HuberLoss(2.0);
- ceres::LocalParameterization *quaternion_local_parameterization =
- new ceres::EigenQuaternionParameterization;
+ ceres::Manifold *quaternion_local_parameterization =
+ new ceres::EigenQuaternionManifold();
problem->AddResidualBlock(cost_function.get(), loss_function,
T_frozen_actual_.vector().data(),
R_frozen_actual_.coeffs().data());
- problem->SetParameterization(R_frozen_actual_.coeffs().data(),
- quaternion_local_parameterization);
+ problem->SetManifold(R_frozen_actual_.coeffs().data(),
+ quaternion_local_parameterization);
return cost_function;
}