saberbud/ceres-solver
1
0
Fork 0
mirror of https://github.com/ceres-solver/ceres-solver.git synced 2026-08-30 17:10:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sized cost function using variadic templates

Browse Source 
This PR changes the interface of sized_cost_fucntion,
autodiff_cost_function and numeric_diff_costfunction from using ten
hardcoded parameter blocks to a variable number of parameter blocks
using variadic templates.

Trailing parameter blocks of size zero are now considered as error.

Change-Id: I37b9a0a420ef0eda6476a46672bbf6bd57e19760
This commit is contained in:
Johannes Beck
2018-10-13 22:34:53 +02:00
parent f8e8951611
commit 8eef94de4e
12 changed files with 380 additions and 591 deletions
Download Patch File Download Diff File Expand all files Collapse all files
internal/ceres/autodiff_test.cc
+26 -36
View File
@@ -194,7 +194,7 @@ TEST(AutoDiff, ProjectiveCameraModel) {
{
double *parameters[] = { PX };
double *jacobians[] = { J_PX };
ASSERT_TRUE((AutoDiff<Projective, double, 12 + 4>::Differentiate(
ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<12 + 4>>(
b, parameters, 2, ad_x1, jacobians)));
for (int i = 0; i < 2; ++i) {
@@ -209,7 +209,7 @@ TEST(AutoDiff, ProjectiveCameraModel) {
double J_X[2 * 4];
double *parameters[] = { P, X };
double *jacobians[] = { J_P, J_X };
ASSERT_TRUE((AutoDiff<Projective, double, 12, 4>::Differentiate(
ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<12, 4>>(
b, parameters, 2, ad_x2, jacobians)));
for (int i = 0; i < 2; ++i) {
@@ -316,7 +316,7 @@ TEST(AutoDiff, Metric) {
double J_X[2 * 3];
double *parameters[] = { q, c, X };
double *jacobians[] = { J_q, J_c, J_X };
ASSERT_TRUE((AutoDiff<Metric, double, 4, 3, 3>::Differentiate(
ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<4, 3, 3>>(
b, parameters, 2, ad_x, jacobians)));
for (int i = 0; i < 2; ++i) {
@@ -366,7 +366,7 @@ TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
functor.num_residuals = num_residuals;
// Run autodiff with the new number of residuals.
ASSERT_TRUE((AutoDiff<VaryingResidualFunctor, double, 2>::Differentiate(
ASSERT_TRUE((AutoDifferentiate<StaticParameterDims<2>>(
functor, parameters, num_residuals, residuals, jacobians)));
const double kTolerance = 1e-14;
@@ -528,8 +528,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual1Param functor;
int num_variables = 1;
EXPECT_TRUE((AutoDiff<Residual1Param, double, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -539,8 +539,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual2Param functor;
int num_variables = 2;
EXPECT_TRUE((AutoDiff<Residual2Param, double, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -550,8 +550,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual3Param functor;
int num_variables = 3;
EXPECT_TRUE((AutoDiff<Residual3Param, double, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -561,8 +561,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual4Param functor;
int num_variables = 4;
EXPECT_TRUE((AutoDiff<Residual4Param, double, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -572,8 +572,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual5Param functor;
int num_variables = 5;
EXPECT_TRUE((AutoDiff<Residual5Param, double, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -583,10 +583,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual6Param functor;
int num_variables = 6;
EXPECT_TRUE((AutoDiff<Residual6Param,
double,
1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -596,10 +594,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual7Param functor;
int num_variables = 7;
EXPECT_TRUE((AutoDiff<Residual7Param,
double,
1, 1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -609,10 +605,8 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual8Param functor;
int num_variables = 8;
EXPECT_TRUE((AutoDiff<
Residual8Param,
double, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE((AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -622,11 +616,9 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual9Param functor;
int num_variables = 9;
EXPECT_TRUE((AutoDiff<
Residual9Param,
double,
1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE(
(AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
@@ -636,11 +628,9 @@ TEST(AutoDiff, VariadicAutoDiff) {
{
Residual10Param functor;
int num_variables = 10;
EXPECT_TRUE((AutoDiff<
Residual10Param,
double,
1, 1, 1, 1, 1, 1, 1, 1, 1, 1>::Differentiate(
functor, parameters, 1, &residual, jacobians)));
EXPECT_TRUE(
(AutoDifferentiate<StaticParameterDims<1, 1, 1, 1, 1, 1, 1, 1, 1, 1>>(
functor, parameters, 1, &residual, jacobians)));
EXPECT_EQ(residual, pow(2, num_variables + 1) - 2);
for (int i = 0; i < num_variables; ++i) {
EXPECT_EQ(jacobian_values[i], (i + 1) * pow(2, i));
internal/ceres/local_parameterization_test.cc
+6 -8
View File
@@ -265,14 +265,12 @@ void QuaternionParameterizationTestHelper(
double* jacobian_array[2] = { NULL, jacobian_ref };
// Autodiff jacobian at delta_x = 0.
internal::AutoDiff<Plus,
double,
kGlobalSize,
kLocalSize>::Differentiate(Plus(),
parameters,
kGlobalSize,
x_plus_delta,
jacobian_array);
internal::AutoDifferentiate<StaticParameterDims<kGlobalSize, kLocalSize>>(
Plus(),
parameters,
kGlobalSize,
x_plus_delta,
jacobian_array);
double jacobian[12];
parameterization.ComputeJacobian(x, jacobian);
internal/ceres/parameter_dims_test.cc
+3 -3
View File
@@ -89,11 +89,11 @@ TEST(ParameterDims, GetUnpackedParameters) {
constexpr int N1 = 4;
constexpr int N2 = 2;
using Params = StaticParameterDims<N0, N1, N2>;
using ParameterDims = StaticParameterDims<N0, N1, N2>;
std::array<double, Params::kNumParameters> packed_parameters{};
std::array<double, ParameterDims::kNumParameters> packed_parameters{};
std::array<double*, 3> unpacked_parameters =
Params::GetUnpackedParameters(packed_parameters.data());
ParameterDims::GetUnpackedParameters(packed_parameters.data());
EXPECT_EQ(packed_parameters.data(), unpacked_parameters[0]);
EXPECT_EQ(packed_parameters.data() + N0, unpacked_parameters[1]);