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