Add ProductParameterization
Often a parameter block is the Cartesian product of a number of
manifolds. For example, a rigid transformation SE(3) = SO(3) x R^3
In such cases, where you have the local parameterization
of the individual manifolds available,
ProductParameterization can be used to construct a local
parameterization of the cartesian product.
Change-Id: I4b5bcbd2407a38739c7725b129789db5c3d65a20
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst
index 9516795..8bc5c6b 100644
--- a/docs/source/nnls_modeling.rst
+++ b/docs/source/nnls_modeling.rst
@@ -677,7 +677,7 @@
CostFunctionToFunctor<2,5,3> intrinsic_projection_;
};
- Note that :class:`CostFunctionToFunctor` takes ownership of the
+ Note that :class:`CostFunctionToFunctor` takes ownership of the
:class:`CostFunction` that was passed in to the constructor.
In the above example, we assumed that ``IntrinsicProjection`` is a
@@ -1271,6 +1271,31 @@
vector.
+.. class:: ProductParameterization
+
+ Consider an optimization problem over the space of rigid
+ transformations :math:`SE(3)`, which is the Cartesian product of
+ :math:`SO(3)` and :math:`\mathbb{R}^3`. Suppose you are using
+ Quaternions to represent the rotation, Ceres ships with a local
+ parameterization for that and :math:`\mathbb{R}^3` requires no, or
+ :class:`IdentityParameterization` parameterization. So how do we
+ construct a local parameterization for a parameter block a rigid
+ transformation?
+
+ In cases, where a parameter block is the Cartesian product of a
+ number of manifolds and you have the local parameterization of the
+ individual manifolds available, :class:`ProductParameterization`
+ can be used to construct a local parameterization of the cartesian
+ product. For the case of the rigid transformation, where say you
+ have a parameter block of size 7, where the first four entries
+ represent the rotation as a quaternion, a local parameterization
+ can be constructed as
+
+ .. code-block:: c++
+
+ ProductParameterization product_param(new QuaternionParameterization(),
+ new IdentityTransformation(3));
+
:class:`AutoDiffLocalParameterization`
======================================
diff --git a/docs/source/version_history.rst b/docs/source/version_history.rst
index f9e9c7b..ef68276 100644
--- a/docs/source/version_history.rst
+++ b/docs/source/version_history.rst
@@ -9,12 +9,14 @@
New Features
------------
-#. Added ``CubicInterpolator`` and ``BiCubicInterpolator`` to allow
+#. Add ``CubicInterpolator`` and ``BiCubicInterpolator`` to allow
smooth interpolation of sampled functions and integration with
automatic differentiation.
#. Add method to return covariance in tangent space. (Michael Vitus &
Steve Hsu)
#. Add Homogeneous vector parameterization. (Michael Vitus)
+#. Add a local parameterization that can be constructed as a cartesian
+ product of other local parameterization.
Bug Fixes & Minor Changes
-------------------------
diff --git a/include/ceres/autodiff_local_parameterization.h b/include/ceres/autodiff_local_parameterization.h
index 96153b1..27397e2 100644
--- a/include/ceres/autodiff_local_parameterization.h
+++ b/include/ceres/autodiff_local_parameterization.h
@@ -33,9 +33,9 @@
#ifndef CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
#define CERES_PUBLIC_AUTODIFF_LOCAL_PARAMETERIZATION_H_
+#include "ceres/local_parameterization.h"
#include "ceres/internal/autodiff.h"
#include "ceres/internal/scoped_ptr.h"
-#include "ceres/local_parameterization.h"
namespace ceres {
diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h
index 4294677..46a3931 100644
--- a/include/ceres/local_parameterization.h
+++ b/include/ceres/local_parameterization.h
@@ -34,6 +34,7 @@
#include <vector>
#include "ceres/internal/port.h"
+#include "ceres/internal/scoped_ptr.h"
#include "ceres/internal/disable_warnings.h"
namespace ceres {
@@ -241,6 +242,58 @@
const int size_;
};
+// Construct a local parameterization by taking the Cartesian product
+// of a number of other local parameterizations. This is useful, when
+// a parameter block is the cartesian product of two or more
+// manifolds. For example the parameters of a camera consist of a
+// rotation and a translation, i.e., SO(3) x R^3.
+//
+// Currently this class supports taking the cartesian product of up to
+// four local parameterizations.
+//
+// Example usage:
+//
+// ProductParameterization product_param(new QuaterionionParameterization(),
+// new IdentityParameterization(3));
+//
+// is the local parameterization for a rigid transformation, where the
+// rotation is represented using a quaternion.
+class CERES_EXPORT ProductParameterization : public LocalParameterization {
+ public:
+ //
+ // NOTE: All the constructors take ownership of the input local
+ // parameterizations.
+ //
+ ProductParameterization(LocalParameterization* local_param1,
+ LocalParameterization* local_param2);
+
+ ProductParameterization(LocalParameterization* local_param1,
+ LocalParameterization* local_param2,
+ LocalParameterization* local_param3);
+
+ ProductParameterization(LocalParameterization* local_param1,
+ LocalParameterization* local_param2,
+ LocalParameterization* local_param3,
+ LocalParameterization* local_param4);
+
+ virtual ~ProductParameterization();
+ virtual bool Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const;
+ virtual bool ComputeJacobian(const double* x,
+ double* jacobian) const;
+ virtual int GlobalSize() const { return global_size_; }
+ virtual int LocalSize() const { return local_size_; }
+
+ private:
+ void Init();
+
+ std::vector<LocalParameterization*> local_params_;
+ int local_size_;
+ int global_size_;
+ int buffer_size_;
+};
+
} // namespace ceres
#include "ceres/internal/reenable_warnings.h"
diff --git a/internal/ceres/local_parameterization.cc b/internal/ceres/local_parameterization.cc
index 465a365..8200476 100644
--- a/internal/ceres/local_parameterization.cc
+++ b/internal/ceres/local_parameterization.cc
@@ -32,6 +32,7 @@
#include "ceres/householder_vector.h"
#include "ceres/internal/eigen.h"
+#include "ceres/internal/fixed_array.h"
#include "ceres/rotation.h"
#include "glog/logging.h"
@@ -247,4 +248,98 @@
return true;
}
+ProductParameterization::ProductParameterization(
+ LocalParameterization* local_param1,
+ LocalParameterization* local_param2) {
+ local_params_.push_back(local_param1);
+ local_params_.push_back(local_param2);
+ Init();
+}
+
+ProductParameterization::ProductParameterization(
+ LocalParameterization* local_param1,
+ LocalParameterization* local_param2,
+ LocalParameterization* local_param3) {
+ local_params_.push_back(local_param1);
+ local_params_.push_back(local_param2);
+ local_params_.push_back(local_param3);
+ Init();
+}
+
+ProductParameterization::ProductParameterization(
+ LocalParameterization* local_param1,
+ LocalParameterization* local_param2,
+ LocalParameterization* local_param3,
+ LocalParameterization* local_param4) {
+ local_params_.push_back(local_param1);
+ local_params_.push_back(local_param2);
+ local_params_.push_back(local_param3);
+ local_params_.push_back(local_param4);
+ Init();
+}
+
+ProductParameterization::~ProductParameterization() {
+ for (int i = 0; i < local_params_.size(); ++i) {
+ delete local_params_[i];
+ }
+}
+
+void ProductParameterization::Init() {
+ global_size_ = 0;
+ local_size_ = 0;
+ buffer_size_ = 0;
+ for (int i = 0; i < local_params_.size(); ++i) {
+ const LocalParameterization* param = local_params_[i];
+ buffer_size_ = std::max(buffer_size_,
+ param->LocalSize() * param->GlobalSize());
+ global_size_ += param->GlobalSize();
+ local_size_ += param->LocalSize();
+ }
+}
+
+bool ProductParameterization::Plus(const double* x,
+ const double* delta,
+ double* x_plus_delta) const {
+ int x_cursor = 0;
+ int delta_cursor = 0;
+ for (int i = 0; i < local_params_.size(); ++i) {
+ const LocalParameterization* param = local_params_[i];
+ if (!param->Plus(x + x_cursor,
+ delta + delta_cursor,
+ x_plus_delta + x_cursor)) {
+ return false;
+ }
+ delta_cursor += param->LocalSize();
+ x_cursor += param->GlobalSize();
+ }
+
+ return true;
+}
+
+bool ProductParameterization::ComputeJacobian(const double* x,
+ double* jacobian_ptr) const {
+ MatrixRef jacobian(jacobian_ptr, GlobalSize(), LocalSize());
+ jacobian.setZero();
+ internal::FixedArray<double> buffer(buffer_size_);
+
+ int x_cursor = 0;
+ int delta_cursor = 0;
+ for (int i = 0; i < local_params_.size(); ++i) {
+ const LocalParameterization* param = local_params_[i];
+ const int local_size = param->LocalSize();
+ const int global_size = param->GlobalSize();
+
+ if (!param->ComputeJacobian(x + x_cursor, buffer.get())) {
+ return false;
+ }
+
+ jacobian.block(x_cursor, delta_cursor, global_size, local_size)
+ = MatrixRef(buffer.get(), global_size, local_size);
+ delta_cursor += local_size;
+ x_cursor += global_size;
+ }
+
+ return true;
+}
+
} // namespace ceres
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc
index 46f131c..8eb8033 100644
--- a/internal/ceres/local_parameterization_test.cc
+++ b/internal/ceres/local_parameterization_test.cc
@@ -35,6 +35,7 @@
#include "ceres/internal/autodiff.h"
#include "ceres/internal/eigen.h"
#include "ceres/local_parameterization.h"
+#include "ceres/random.h"
#include "ceres/rotation.h"
#include "gtest/gtest.h"
@@ -442,5 +443,195 @@
EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size");
}
+
+class ProductParameterizationTest : public ::testing::Test {
+ protected :
+ virtual void SetUp() {
+ const int global_size1 = 5;
+ std::vector<int> constant_parameters1;
+ constant_parameters1.push_back(2);
+ param1_.reset(new SubsetParameterization(global_size1,
+ constant_parameters1));
+
+ const int global_size2 = 3;
+ std::vector<int> constant_parameters2;
+ constant_parameters2.push_back(0);
+ constant_parameters2.push_back(1);
+ param2_.reset(new SubsetParameterization(global_size2,
+ constant_parameters2));
+
+ const int global_size3 = 4;
+ std::vector<int> constant_parameters3;
+ constant_parameters3.push_back(1);
+ param3_.reset(new SubsetParameterization(global_size3,
+ constant_parameters3));
+
+ const int global_size4 = 2;
+ std::vector<int> constant_parameters4;
+ constant_parameters4.push_back(1);
+ param4_.reset(new SubsetParameterization(global_size4,
+ constant_parameters4));
+ }
+
+ scoped_ptr<LocalParameterization> param1_;
+ scoped_ptr<LocalParameterization> param2_;
+ scoped_ptr<LocalParameterization> param3_;
+ scoped_ptr<LocalParameterization> param4_;
+};
+
+TEST_F(ProductParameterizationTest, LocalAndGlobalSize2) {
+ LocalParameterization* param1 = param1_.release();
+ LocalParameterization* param2 = param2_.release();
+
+ ProductParameterization product_param(param1, param2);
+ EXPECT_EQ(product_param.LocalSize(),
+ param1->LocalSize() + param2->LocalSize());
+ EXPECT_EQ(product_param.GlobalSize(),
+ param1->GlobalSize() + param2->GlobalSize());
+}
+
+
+TEST_F(ProductParameterizationTest, LocalAndGlobalSize3) {
+ LocalParameterization* param1 = param1_.release();
+ LocalParameterization* param2 = param2_.release();
+ LocalParameterization* param3 = param3_.release();
+
+ ProductParameterization product_param(param1, param2, param3);
+ EXPECT_EQ(product_param.LocalSize(),
+ param1->LocalSize() + param2->LocalSize() + param3->LocalSize());
+ EXPECT_EQ(product_param.GlobalSize(),
+ param1->GlobalSize() + param2->GlobalSize() + param3->GlobalSize());
+}
+
+TEST_F(ProductParameterizationTest, LocalAndGlobalSize4) {
+ LocalParameterization* param1 = param1_.release();
+ LocalParameterization* param2 = param2_.release();
+ LocalParameterization* param3 = param3_.release();
+ LocalParameterization* param4 = param4_.release();
+
+ ProductParameterization product_param(param1, param2, param3, param4);
+ EXPECT_EQ(product_param.LocalSize(),
+ param1->LocalSize() +
+ param2->LocalSize() +
+ param3->LocalSize() +
+ param4->LocalSize());
+ EXPECT_EQ(product_param.GlobalSize(),
+ param1->GlobalSize() +
+ param2->GlobalSize() +
+ param3->GlobalSize() +
+ param4->GlobalSize());
+}
+
+TEST_F(ProductParameterizationTest, Plus) {
+ LocalParameterization* param1 = param1_.release();
+ LocalParameterization* param2 = param2_.release();
+ LocalParameterization* param3 = param3_.release();
+ LocalParameterization* param4 = param4_.release();
+
+ ProductParameterization product_param(param1, param2, param3, param4);
+ std::vector<double> x(product_param.GlobalSize(), 0.0);
+ std::vector<double> delta(product_param.LocalSize(), 0.0);
+ std::vector<double> x_plus_delta_expected(product_param.GlobalSize(), 0.0);
+ std::vector<double> x_plus_delta(product_param.GlobalSize(), 0.0);
+
+ for (int i = 0; i < product_param.GlobalSize(); ++i) {
+ x[i] = RandNormal();
+ }
+
+ for (int i = 0; i < product_param.LocalSize(); ++i) {
+ delta[i] = RandNormal();
+ }
+
+ EXPECT_TRUE(product_param.Plus(&x[0], &delta[0], &x_plus_delta_expected[0]));
+ int x_cursor = 0;
+ int delta_cursor = 0;
+
+ EXPECT_TRUE(param1->Plus(&x[x_cursor],
+ &delta[delta_cursor],
+ &x_plus_delta[x_cursor]));
+ x_cursor += param1->GlobalSize();
+ delta_cursor += param1->LocalSize();
+
+ EXPECT_TRUE(param2->Plus(&x[x_cursor],
+ &delta[delta_cursor],
+ &x_plus_delta[x_cursor]));
+ x_cursor += param2->GlobalSize();
+ delta_cursor += param2->LocalSize();
+
+ EXPECT_TRUE(param3->Plus(&x[x_cursor],
+ &delta[delta_cursor],
+ &x_plus_delta[x_cursor]));
+ x_cursor += param3->GlobalSize();
+ delta_cursor += param3->LocalSize();
+
+ EXPECT_TRUE(param4->Plus(&x[x_cursor],
+ &delta[delta_cursor],
+ &x_plus_delta[x_cursor]));
+ x_cursor += param4->GlobalSize();
+ delta_cursor += param4->LocalSize();
+
+ for (int i = 0; i < x.size(); ++i) {
+ EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]);
+ }
+}
+
+TEST_F(ProductParameterizationTest, ComputeJacobian) {
+ LocalParameterization* param1 = param1_.release();
+ LocalParameterization* param2 = param2_.release();
+ LocalParameterization* param3 = param3_.release();
+ LocalParameterization* param4 = param4_.release();
+
+ ProductParameterization product_param(param1, param2, param3, param4);
+ std::vector<double> x(product_param.GlobalSize(), 0.0);
+
+ for (int i = 0; i < product_param.GlobalSize(); ++i) {
+ x[i] = RandNormal();
+ }
+
+ Matrix jacobian = Matrix::Random(product_param.GlobalSize(),
+ product_param.LocalSize());
+ EXPECT_TRUE(product_param.ComputeJacobian(&x[0], jacobian.data()));
+ int x_cursor = 0;
+ int delta_cursor = 0;
+
+ Matrix jacobian1(param1->GlobalSize(), param1->LocalSize());
+ EXPECT_TRUE(param1->ComputeJacobian(&x[x_cursor], jacobian1.data()));
+ jacobian.block(x_cursor, delta_cursor,
+ param1->GlobalSize(),
+ param1->LocalSize())
+ -= jacobian1;
+ x_cursor += param1->GlobalSize();
+ delta_cursor += param1->LocalSize();
+
+ Matrix jacobian2(param2->GlobalSize(), param2->LocalSize());
+ EXPECT_TRUE(param2->ComputeJacobian(&x[x_cursor], jacobian2.data()));
+ jacobian.block(x_cursor, delta_cursor,
+ param2->GlobalSize(),
+ param2->LocalSize())
+ -= jacobian2;
+ x_cursor += param2->GlobalSize();
+ delta_cursor += param2->LocalSize();
+
+ Matrix jacobian3(param3->GlobalSize(), param3->LocalSize());
+ EXPECT_TRUE(param3->ComputeJacobian(&x[x_cursor], jacobian3.data()));
+ jacobian.block(x_cursor, delta_cursor,
+ param3->GlobalSize(),
+ param3->LocalSize())
+ -= jacobian3;
+ x_cursor += param3->GlobalSize();
+ delta_cursor += param3->LocalSize();
+
+ Matrix jacobian4(param4->GlobalSize(), param4->LocalSize());
+ EXPECT_TRUE(param4->ComputeJacobian(&x[x_cursor], jacobian4.data()));
+ jacobian.block(x_cursor, delta_cursor,
+ param4->GlobalSize(),
+ param4->LocalSize())
+ -= jacobian4;
+ x_cursor += param4->GlobalSize();
+ delta_cursor += param4->LocalSize();
+
+ EXPECT_NEAR(jacobian.norm(), 0.0, std::numeric_limits<double>::epsilon());
+}
+
} // namespace internal
} // namespace ceres
