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

Add static/compile time sizing to EuclideanManifold

Browse Source 
This brings it in line with other manifolds like SphereManifold
and LineManifold, where the user has the choice to specify the size
of the manifold at compile time or runtime.

Most of the time the size is known at compile time so this will
speed up the common case.

Change-Id: I0c7ff8b7a9a64a81203eb11afc074874e208815a
This commit is contained in:
Sameer Agarwal
2022-02-28 11:23:43 -08:00
parent 4ad787ce19
commit 6a37fbf9b4
8 changed files with 170 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
internal/ceres/gradient_problem.cc
+10 -6
View File
@@ -40,8 +40,8 @@ namespace ceres {
GradientProblem::GradientProblem(FirstOrderFunction* function)
: function_(function),
manifold_(
std::make_unique<EuclideanManifold>(function_->NumParameters())),
manifold_(std::make_unique<EuclideanManifold<DYNAMIC>>(
function_->NumParameters())),
scratch_(new double[function_->NumParameters()]) {
CHECK(function != nullptr);
}
@@ -56,7 +56,8 @@ GradientProblem::GradientProblem(FirstOrderFunction* function,
manifold_ =
std::make_unique<internal::ManifoldAdapter>(parameterization_.get());
} else {
manifold_ = std::make_unique<EuclideanManifold>(function_->NumParameters());
manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>(
function_->NumParameters());
}
CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize());
}
@@ -65,9 +66,12 @@ GradientProblem::GradientProblem(FirstOrderFunction* function,
Manifold* manifold)
: function_(function), scratch_(new double[function_->NumParameters()]) {
CHECK(function != nullptr);
manifold_.reset(manifold != nullptr
? manifold
: new EuclideanManifold(function_->NumParameters()));
if (manifold != nullptr) {
manifold_.reset(manifold);
} else {
manifold_ = std::make_unique<EuclideanManifold<DYNAMIC>>(
function_->NumParameters());
}
CHECK_EQ(function_->NumParameters(), manifold_->AmbientSize());
}
internal/ceres/gradient_problem_test.cc
+2 -2
View File
@@ -107,7 +107,7 @@ TEST(GradientProblem, EvaluationWithParameterizationAndWithGradient) {
TEST(GradientProblem, EvalutaionWithManifoldAndNoGradient) {
ceres::GradientProblem problem(new QuadraticTestFunction(),
new EuclideanManifold(1));
new EuclideanManifold<1>);
double x = 7.0;
double cost = 0;
problem.Evaluate(&x, &cost, nullptr);
@@ -125,7 +125,7 @@ TEST(GradientProblem, EvaluationWithoutManifoldAndWithGradient) {
TEST(GradientProblem, EvaluationWithManifoldAndWithGradient) {
ceres::GradientProblem problem(new QuadraticTestFunction(),
new EuclideanManifold(1));
new EuclideanManifold<1>);
double x = 7.0;
double cost = 0;
double gradient = 0;
internal/ceres/manifold.cc
-45
View File
@@ -159,51 +159,6 @@ bool Manifold::RightMultiplyByPlusJacobian(const double* x,
return true;
}
EuclideanManifold::EuclideanManifold(int size) : size_(size) {
CHECK_GE(size, 0);
}
int EuclideanManifold::AmbientSize() const { return size_; }
int EuclideanManifold::TangentSize() const { return size_; }
bool EuclideanManifold::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
for (int i = 0; i < size_; ++i) {
x_plus_delta[i] = x[i] + delta[i];
}
return true;
}
bool EuclideanManifold::PlusJacobian(const double* x, double* jacobian) const {
MatrixRef(jacobian, size_, size_).setIdentity();
return true;
}
bool EuclideanManifold::RightMultiplyByPlusJacobian(
const double* x,
const int num_rows,
const double* ambient_matrix,
double* tangent_matrix) const {
std::copy_n(ambient_matrix, num_rows * size_, tangent_matrix);
return true;
}
bool EuclideanManifold::Minus(const double* y,
const double* x,
double* y_minus_x) const {
for (int i = 0; i < size_; ++i) {
y_minus_x[i] = y[i] - x[i];
}
return true;
};
bool EuclideanManifold::MinusJacobian(const double* x, double* jacobian) const {
MatrixRef(jacobian, size_, size_).setIdentity();
return true;
}
SubsetManifold::SubsetManifold(const int size,
const std::vector<int>& constant_parameters)
internal/ceres/manifold_test.cc
+25 -4
View File
@@ -51,8 +51,29 @@ namespace internal {
constexpr int kNumTrials = 1000;
constexpr double kTolerance = 1e-9;
TEST(EuclideanManifold, NormalFunctionTest) {
EuclideanManifold manifold(3);
TEST(EuclideanManifold, StaticNormalFunctionTest) {
EuclideanManifold<3> manifold;
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 3);
Vector zero_tangent = Vector::Zero(manifold.TangentSize());
for (int trial = 0; trial < kNumTrials; ++trial) {
const Vector x = Vector::Random(manifold.AmbientSize());
const Vector y = Vector::Random(manifold.AmbientSize());
Vector delta = Vector::Random(manifold.TangentSize());
Vector x_plus_delta = Vector::Zero(manifold.AmbientSize());
manifold.Plus(x.data(), delta.data(), x_plus_delta.data());
EXPECT_NEAR((x_plus_delta - x - delta).norm() / (x + delta).norm(),
0.0,
kTolerance);
EXPECT_THAT_MANIFOLD_INVARIANTS_HOLD(manifold, x, delta, y, kTolerance);
}
}
TEST(EuclideanManifold, DynamicNormalFunctionTest) {
EuclideanManifold<DYNAMIC> manifold(3);
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 3);
@@ -230,7 +251,7 @@ TEST(ProductManifold, NormalFunctionTest) {
TEST(ProductManifold, ZeroTangentSizeAndEuclidean) {
Manifold* subset_manifold = new SubsetManifold(1, {0});
Manifold* euclidean_manifold = new EuclideanManifold(2);
Manifold* euclidean_manifold = new EuclideanManifold<2>;
ProductManifold manifold(subset_manifold, euclidean_manifold);
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 2);
@@ -254,7 +275,7 @@ TEST(ProductManifold, ZeroTangentSizeAndEuclidean) {
TEST(ProductManifold, EuclideanAndZeroTangentSize) {
Manifold* subset_manifold = new SubsetManifold(1, {0});
Manifold* euclidean_manifold = new EuclideanManifold(2);
Manifold* euclidean_manifold = new EuclideanManifold<2>;
ProductManifold manifold(euclidean_manifold, subset_manifold);
EXPECT_EQ(manifold.AmbientSize(), 3);
EXPECT_EQ(manifold.TangentSize(), 2);
internal/ceres/problem_test.cc
+3 -4
View File
@@ -67,7 +67,6 @@ class UnaryCostFunction : public CostFunction {
mutable_parameter_block_sizes()->push_back(parameter_block_size);
}
bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const final {
@@ -595,7 +594,7 @@ TEST(Problem, SetManifoldWithUnknownPtrDies) {
Problem problem;
problem.AddParameterBlock(x, 3);
EXPECT_DEATH_IF_SUPPORTED(problem.SetManifold(y, new EuclideanManifold(3)),
EXPECT_DEATH_IF_SUPPORTED(problem.SetManifold(y, new EuclideanManifold<3>),
"Parameter block not found:");
}
@@ -632,7 +631,7 @@ TEST(Problem, GetManifold) {
problem.AddParameterBlock(x, 3);
problem.AddParameterBlock(y, 2);
Manifold* manifold = new EuclideanManifold(3);
Manifold* manifold = new EuclideanManifold<3>;
problem.SetManifold(x, manifold);
EXPECT_EQ(problem.GetManifold(x), manifold);
EXPECT_TRUE(problem.GetManifold(y) == nullptr);
@@ -660,7 +659,7 @@ TEST(Problem, HasManifold) {
problem.AddParameterBlock(x, 3);
problem.AddParameterBlock(y, 2);
Manifold* manifold = new EuclideanManifold(3);
Manifold* manifold = new EuclideanManifold<3>;
problem.SetManifold(x, manifold);
EXPECT_TRUE(problem.HasManifold(x));
EXPECT_FALSE(problem.HasManifold(y));