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

LocalParameterization -> Manifold #1

Browse Source 
Manifolds are now part of the public API and co-exist
with LocalParameterizations.

1. Add Manifolds to the Problem API.
   a. AddParameterBlock(double*, int, Manifold*)
   b. SetParameterization(double*, Manifold*)
   b. GetManifold(const double*)
   c. HasManifold(const double*)

2. Internally Ceres now only uses Manifolds. When the user uses
   a LocalParameterization, it is wrapped in a ManifoldAdapter.

3. To preserve the API semantics while keeping the internals clean
   we need a new map in ProblemImpl which stores the association
   between parameter blocks and local parameterizations. This
   is temporary, it will go away once this transition is complete.

4. There are NO algorithmic changes, as in we are not using
   any of the expanded interface of the Manifold objects yet.
   That will come later.

5. All tests that use LocalParameterization have been duplicated
   to use Manifolds, and when this transition is complete the
   LocalParameterization based tests will be deleted.

6. Public documentation for the API has been updated. Deprecation
   notices to the documentation as well as C++ annotations will come
   later.

7. Similar changes have been made to GradientProblem.

Change-Id: I8e03c8ced6e141876ef3eca5740c113afa788f0c
This commit is contained in:
Sameer Agarwal
2021-12-28 07:05:03 -08:00
parent 00bfbae11f
commit 125a0e9be5
37 changed files with 2465 additions and 799 deletions
Download Patch File Download Diff File Expand all files Collapse all files
internal/ceres/covariance_impl.cc
+36 -35
View File
@@ -162,10 +162,10 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace(
const int block1_size = block1->Size();
const int block2_size = block2->Size();
const int block1_local_size = block1->LocalSize();
const int block2_local_size = block2->LocalSize();
const int block1_tangent_size = block1->TangentSize();
const int block2_tangent_size = block2->TangentSize();
if (!lift_covariance_to_ambient_space) {
MatrixRef(covariance_block, block1_local_size, block2_local_size)
MatrixRef(covariance_block, block1_tangent_size, block2_tangent_size)
.setZero();
} else {
MatrixRef(covariance_block, block1_size, block2_size).setZero();
@@ -209,35 +209,34 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace(
FindOrDie(parameter_map, const_cast<double*>(parameter_block1));
ParameterBlock* block2 =
FindOrDie(parameter_map, const_cast<double*>(parameter_block2));
const LocalParameterization* local_param1 = block1->local_parameterization();
const LocalParameterization* local_param2 = block2->local_parameterization();
const Manifold* manifold1 = block1->manifold();
const Manifold* manifold2 = block2->manifold();
const int block1_size = block1->Size();
const int block1_local_size = block1->LocalSize();
const int block1_tangent_size = block1->TangentSize();
const int block2_size = block2->Size();
const int block2_local_size = block2->LocalSize();
const int block2_tangent_size = block2->TangentSize();
ConstMatrixRef cov(covariance_matrix_->values() + rows[row_begin],
block1_local_size,
block1_tangent_size,
row_size);
// Fast path when there are no local parameterizations or if the
// user does not want it lifted to the ambient space.
if ((local_param1 == NULL && local_param2 == NULL) ||
// Fast path when there are no manifolds or if the user does not want it
// lifted to the ambient space.
if ((manifold1 == NULL && manifold2 == NULL) ||
!lift_covariance_to_ambient_space) {
if (transpose) {
MatrixRef(covariance_block, block2_local_size, block1_local_size) =
cov.block(0, offset, block1_local_size, block2_local_size)
MatrixRef(covariance_block, block2_tangent_size, block1_tangent_size) =
cov.block(0, offset, block1_tangent_size, block2_tangent_size)
.transpose();
} else {
MatrixRef(covariance_block, block1_local_size, block2_local_size) =
cov.block(0, offset, block1_local_size, block2_local_size);
MatrixRef(covariance_block, block1_tangent_size, block2_tangent_size) =
cov.block(0, offset, block1_tangent_size, block2_tangent_size);
}
return true;
}
// If local parameterizations are used then the covariance that has
// been computed is in the tangent space and it needs to be lifted
// back to the ambient space.
// If manifolds are used then the covariance that has been computed is in the
// tangent space and it needs to be lifted back to the ambient space.
//
// This is given by the formula
//
@@ -250,36 +249,37 @@ bool CovarianceImpl::GetCovarianceBlockInTangentOrAmbientSpace(
// See Result 5.11 on page 142 of Hartley & Zisserman (2nd Edition)
// for a proof.
//
// TODO(sameeragarwal): Add caching of local parameterization, so
// that they are computed just once per parameter block.
Matrix block1_jacobian(block1_size, block1_local_size);
if (local_param1 == NULL) {
// TODO(sameeragarwal): Add caching the manifold plus_jacobian, so that they
// are computed just once per parameter block.
Matrix block1_jacobian(block1_size, block1_tangent_size);
if (manifold1 == NULL) {
block1_jacobian.setIdentity();
} else {
local_param1->ComputeJacobian(parameter_block1, block1_jacobian.data());
manifold1->PlusJacobian(parameter_block1, block1_jacobian.data());
}
Matrix block2_jacobian(block2_size, block2_local_size);
Matrix block2_jacobian(block2_size, block2_tangent_size);
// Fast path if the user is requesting a diagonal block.
if (parameter_block1 == parameter_block2) {
block2_jacobian = block1_jacobian;
} else {
if (local_param2 == NULL) {
if (manifold2 == NULL) {
block2_jacobian.setIdentity();
} else {
local_param2->ComputeJacobian(parameter_block2, block2_jacobian.data());
manifold2->PlusJacobian(parameter_block2, block2_jacobian.data());
}
}
if (transpose) {
MatrixRef(covariance_block, block2_size, block1_size) =
block2_jacobian *
cov.block(0, offset, block1_local_size, block2_local_size).transpose() *
cov.block(0, offset, block1_tangent_size, block2_tangent_size)
.transpose() *
block1_jacobian.transpose();
} else {
MatrixRef(covariance_block, block1_size, block2_size) =
block1_jacobian *
cov.block(0, offset, block1_local_size, block2_local_size) *
cov.block(0, offset, block1_tangent_size, block2_tangent_size) *
block2_jacobian.transpose();
}
@@ -310,7 +310,7 @@ bool CovarianceImpl::GetCovarianceMatrixInTangentOrAmbientSpace(
if (lift_covariance_to_ambient_space) {
parameter_sizes.push_back(block->Size());
} else {
parameter_sizes.push_back(block->LocalSize());
parameter_sizes.push_back(block->TangentSize());
}
}
std::partial_sum(parameter_sizes.begin(),
@@ -415,7 +415,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity(
for (int i = 0; i < active_parameter_blocks.size(); ++i) {
double* parameter_block = active_parameter_blocks[i];
const int parameter_block_size =
problem->ParameterBlockLocalSize(parameter_block);
problem->ParameterBlockTangentSize(parameter_block);
parameter_block_to_row_index_[parameter_block] = num_rows;
num_rows += parameter_block_size;
}
@@ -435,8 +435,8 @@ bool CovarianceImpl::ComputeCovarianceSparsity(
int index1 = FindOrDie(parameter_block_to_row_index_, block_pair.first);
int index2 = FindOrDie(parameter_block_to_row_index_, block_pair.second);
const int size1 = problem->ParameterBlockLocalSize(block_pair.first);
const int size2 = problem->ParameterBlockLocalSize(block_pair.second);
const int size1 = problem->ParameterBlockTangentSize(block_pair.first);
const int size2 = problem->ParameterBlockTangentSize(block_pair.second);
num_nonzeros += size1 * size2;
// Make sure we are constructing a block upper triangular matrix.
@@ -481,7 +481,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity(
int cursor = 0; // index into the covariance matrix.
for (const auto& entry : parameter_block_to_row_index_) {
const double* row_block = entry.first;
const int row_block_size = problem->ParameterBlockLocalSize(row_block);
const int row_block_size = problem->ParameterBlockTangentSize(row_block);
int row_begin = entry.second;
// Iterate over the covariance blocks contained in this row block
@@ -494,7 +494,7 @@ bool CovarianceImpl::ComputeCovarianceSparsity(
if (block_pair.first != row_block) {
break;
}
num_columns += problem->ParameterBlockLocalSize(block_pair.second);
num_columns += problem->ParameterBlockTangentSize(block_pair.second);
}
// Fill out all the compressed rows for this parameter block.
@@ -502,7 +502,8 @@ bool CovarianceImpl::ComputeCovarianceSparsity(
rows[row_begin + r] = cursor;
for (int c = 0; c < num_col_blocks; ++c) {
const double* col_block = covariance_blocks[i + c].second;
const int col_block_size = problem->ParameterBlockLocalSize(col_block);
const int col_block_size =
problem->ParameterBlockTangentSize(col_block);
int col_begin = FindOrDie(parameter_block_to_row_index_, col_block);
for (int k = 0; k < col_block_size; ++k) {
cols[cursor++] = col_begin++;