Add Homogeneous vector parameterization. Change-Id: I42f68a0665a62e2c7dcc7584e5581a05c9b849b0
diff --git a/docs/source/nnls_modeling.rst b/docs/source/nnls_modeling.rst index 45ccd98..6772efe 100644 --- a/docs/source/nnls_modeling.rst +++ b/docs/source/nnls_modeling.rst
@@ -1140,6 +1140,29 @@ product. :class:`QuaternionParameterization` is an implementation of :eq:`quaternion`. +.. class:: HomogeneousVectorParameterization + + In computer vision, homogeneous vectors are commonly used to + represent entities in projective geometry such as points in + projective space. One example where it is useful to use this + over-parameterization is in representing points whose triangulation + is ill-conditioned. Here it is advantageous to use homogeneous + vectors, instead of an Euclidean vector, because it can represent + points at infinity. + + When using homogeneous vectors it is useful to only make updates + orthogonal to that :math:`n`-vector defining the homogeneous + vector [HartleyZisserman]_. One way to do this is to let :math:`\Delta x` + be a :math:`n-1` dimensional vector and define :math:`\boxplus` to be + + .. math:: \boxplus(x, \Delta x) = \left[ \frac{\sin\left(0.5 |\Delta x|\right)}{|\Delta x|} \Delta x, \cos(0.5 |\Delta x|) \right] * x + + The multiplication between the two vectors on the right hand side + is defined as an operator which applies the update orthogonal to + :math:`x` to remain on the sphere. Note, it is assumed that + last element of :math:`x` is the scalar component of the homogeneous + vector. + :class:`AutoDiffLocalParameterization`
diff --git a/include/ceres/local_parameterization.h b/include/ceres/local_parameterization.h index 87a2b76..4294677 100644 --- a/include/ceres/local_parameterization.h +++ b/include/ceres/local_parameterization.h
@@ -210,6 +210,37 @@ virtual int LocalSize() const { return 3; } }; + +// This provides a parameterization for homogeneous vectors which are commonly +// used in Structure for Motion problems. One example where they are used is +// in representing points whose triangulation is ill-conditioned. Here +// it is advantageous to use an over-parameterization since homogeneous vectors +// can represent points at infinity. +// +// The plus operator is defined as +// Plus(x, delta) = +// [sin(0.5 * |delta|) * delta / |delta|, cos(0.5 * |delta|)] * x +// with * defined as an operator which applies the update orthogonal to x to +// remain on the sphere. We assume that the last element of x is the scalar +// component. The size of the homogeneous vector is required to be greater than +// 1. +class CERES_EXPORT HomogeneousVectorParameterization : + public LocalParameterization { + public: + explicit HomogeneousVectorParameterization(int size); + virtual ~HomogeneousVectorParameterization() {} + 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 size_; } + virtual int LocalSize() const { return size_ - 1; } + + private: + const int size_; +}; + } // namespace ceres #include "ceres/internal/reenable_warnings.h"
diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 710a3f2..fedeafe 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt
@@ -260,6 +260,7 @@ CERES_TEST(gradient_checking_cost_function) CERES_TEST(graph) CERES_TEST(graph_algorithms) + CERES_TEST(householder_vector) CERES_TEST(implicit_schur_complement) CERES_TEST(iterative_schur_complement_solver) CERES_TEST(jet)
diff --git a/internal/ceres/householder_vector.h b/internal/ceres/householder_vector.h new file mode 100644 index 0000000..f54feea --- /dev/null +++ b/internal/ceres/householder_vector.h
@@ -0,0 +1,85 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#ifndef CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ +#define CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_ + +#include "Eigen/Core" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// Algorithm 5.1.1 from 'Matrix Computations' by Golub et al. (Johns Hopkins +// Studies in Mathematical Sciences) but using the nth element of the input +// vector as pivot instead of first. This computes the vector v with v(n) = 1 +// and beta such that H = I - beta * v * v^T is orthogonal and +// H * x = ||x||_2 * e_n. +template <typename Scalar> +void ComputeHouseholderVector(const Eigen::Matrix<Scalar, Eigen::Dynamic, 1>& x, + Eigen::Matrix<Scalar, Eigen::Dynamic, 1>* v, + Scalar* beta) { + CHECK_NOTNULL(beta); + CHECK_NOTNULL(v); + CHECK_GT(x.rows(), 1); + CHECK_EQ(x.rows(), v->rows()); + + Scalar sigma = x.head(x.rows() - 1).squaredNorm(); + *v = x; + (*v)(v->rows() - 1) = Scalar(1.0); + + *beta = Scalar(0.0); + const Scalar& x_pivot = x(x.rows() - 1); + + if (sigma <= Scalar(std::numeric_limits<double>::epsilon())) { + if (x_pivot < Scalar(0.0)) { + *beta = Scalar(2.0); + } + return; + } + + const Scalar mu = sqrt(x_pivot * x_pivot + sigma); + Scalar v_pivot = Scalar(1.0); + + if (x_pivot <= Scalar(0.0)) { + v_pivot = x_pivot - mu; + } else { + v_pivot = -sigma / (x_pivot + mu); + } + + *beta = Scalar(2.0) * v_pivot * v_pivot / (sigma + v_pivot * v_pivot); + + v->head(v->rows() - 1) /= v_pivot; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_HOUSEHOLDER_VECTOR_H_
diff --git a/internal/ceres/householder_vector_test.cc b/internal/ceres/householder_vector_test.cc new file mode 100644 index 0000000..fca0360 --- /dev/null +++ b/internal/ceres/householder_vector_test.cc
@@ -0,0 +1,115 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2015 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: vitus@google.com (Michael Vitus) + +#include "ceres/householder_vector.h" +#include "ceres/internal/eigen.h" +#include "glog/logging.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +void HouseholderTestHelper(const Vector& x) { + const double kTolerance = 1e-14; + + // Check to ensure that H * x = ||x|| * [0 ... 0 1]'. + Vector v(x.rows()); + double beta; + ComputeHouseholderVector(x, &v, &beta); + Vector result = x - beta * v * (v.transpose() * x); + + Vector expected_result(x.rows()); + expected_result.setZero(); + expected_result(x.rows() - 1) = 1; + expected_result *= x.norm(); + + for (int i = 0; i < x.rows(); ++i) { + EXPECT_NEAR(expected_result[i], result[i], kTolerance); + } +} + +TEST(HouseholderVector, ZeroPositive) { + Vector x(3); + x << 0.0, 0.0, 0.25; + + HouseholderTestHelper(x); +} + +TEST(HouseholderVector, ZeroNegative) { + Vector x(3); + x << 0.0, 0.0, -0.25; + + HouseholderTestHelper(x); +} + +TEST(HouseholderVector, NearZeroPositive) { + Vector x(3); + x << 1e-18, 1e-18, 0.25; + + HouseholderTestHelper(x); +} + +TEST(HouseholderVector, NearZeroNegative) { + Vector x(3); + x << 1e-18, 1e-18, -0.25; + + HouseholderTestHelper(x); +} + +TEST(HouseholderVector, NonZeroNegative) { + Vector x(3); + x << 1.0, 0.0, -3.0; + + HouseholderTestHelper(x); +} + +TEST(HouseholderVector, NonZeroPositive) { + Vector x(3); + x << 1.0, 1.0, 1.0; + + HouseholderTestHelper(x); +} + +TEST(HouseholderVector, NonZeroPositive_Size4) { + Vector x(4); + x << 1.0, 1.0, 0.0, 2.0; + + HouseholderTestHelper(x); +} + +TEST(HouseholderVector, LastElementZero) { + Vector x(4); + x << 1.0, 1.0, 0.0, 0.0; + + HouseholderTestHelper(x); +} + +} // namespace internal +} // namespace ceres
diff --git a/internal/ceres/local_parameterization.cc b/internal/ceres/local_parameterization.cc index 5e938c7..aa21e6b 100644 --- a/internal/ceres/local_parameterization.cc +++ b/internal/ceres/local_parameterization.cc
@@ -30,6 +30,7 @@ #include "ceres/local_parameterization.h" +#include "ceres/householder_vector.h" #include "ceres/internal/eigen.h" #include "ceres/rotation.h" #include "glog/logging.h" @@ -182,4 +183,68 @@ return true; } +HomogeneousVectorParameterization::HomogeneousVectorParameterization(int size) + : size_(size) { + CHECK_GT(size_, 1) << "The size of the homogeneous vector needs to be " + << "greater than 1."; +} + +bool HomogeneousVectorParameterization::Plus(const double* x_ptr, + const double* delta_ptr, + double* x_plus_delta_ptr) const { + ConstVectorRef x(x_ptr, size_); + ConstVectorRef delta(delta_ptr, size_ - 1); + VectorRef x_plus_delta(x_plus_delta_ptr, size_); + + const double norm_delta = delta.norm(); + + if (norm_delta == 0.0) { + x_plus_delta = x; + return true; + } + + // Map the delta from the minimum representation to the over parameterized + // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman + // (2nd Edition) for a detailed description. Note there is a typo on Page + // 625, line 4 so check the book errata. + const double norm_delta_div_2 = 0.5 * norm_delta; + const double sin_delta_by_delta = sin(norm_delta_div_2) / + norm_delta_div_2; + + Vector y(size_); + y.head(size_ - 1) = 0.5 * sin_delta_by_delta * delta; + y(size_ - 1) = cos(norm_delta_div_2); + + Vector v(size_); + double beta; + internal::ComputeHouseholderVector<double>(x, &v, &beta); + + // Apply the delta update to remain on the unit sphere. See section A6.9.3 + // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed + // description. + x_plus_delta = x.norm() * (y - v * (beta *(v.transpose() * y))); + + return true; +} + +bool HomogeneousVectorParameterization::ComputeJacobian( + const double* x_ptr, double* jacobian_ptr) const { + ConstVectorRef x(x_ptr, size_); + MatrixRef jacobian(jacobian_ptr, size_, size_ - 1); + + Vector v(size_); + double beta; + internal::ComputeHouseholderVector<double>(x, &v, &beta); + + // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the + // Householder matrix (H = I - beta * v * v'). + for (int i = 0; i < size_ - 1; ++i) { + jacobian.col(i) = -0.5 * beta * v(i) * v; + jacobian.col(i)(i) += 0.5; + } + jacobian *= x.norm(); + + return true; +} + } // namespace ceres
diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc index c899046..46f131c 100644 --- a/internal/ceres/local_parameterization_test.cc +++ b/internal/ceres/local_parameterization_test.cc
@@ -29,7 +29,9 @@ // Author: sameeragarwal@google.com (Sameer Agarwal) #include <cmath> +#include "ceres/autodiff_local_parameterization.h" #include "ceres/fpclassify.h" +#include "ceres/householder_vector.h" #include "ceres/internal/autodiff.h" #include "ceres/internal/eigen.h" #include "ceres/local_parameterization.h" @@ -244,6 +246,11 @@ EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0); } +template <int N> +void Normalize(double* x) { + VectorRef(x, N).normalize(); +} + TEST(QuaternionParameterization, ZeroTest) { double x[4] = {0.5, 0.5, 0.5, 0.5}; double delta[3] = {0.0, 0.0, 0.0}; @@ -251,16 +258,9 @@ QuaternionParameterizationTestHelper(x, delta, q_delta); } - TEST(QuaternionParameterization, NearZeroTest) { double x[4] = {0.52, 0.25, 0.15, 0.45}; - double norm_x = sqrt(x[0] * x[0] + - x[1] * x[1] + - x[2] * x[2] + - x[3] * x[3]); - for (int i = 0; i < 4; ++i) { - x[i] = x[i] / norm_x; - } + Normalize<4>(x); double delta[3] = {0.24, 0.15, 0.10}; for (int i = 0; i < 3; ++i) { @@ -278,14 +278,7 @@ TEST(QuaternionParameterization, AwayFromZeroTest) { double x[4] = {0.52, 0.25, 0.15, 0.45}; - double norm_x = sqrt(x[0] * x[0] + - x[1] * x[1] + - x[2] * x[2] + - x[3] * x[3]); - - for (int i = 0; i < 4; ++i) { - x[i] = x[i] / norm_x; - } + Normalize<4>(x); double delta[3] = {0.24, 0.15, 0.10}; const double delta_norm = sqrt(delta[0] * delta[0] + @@ -300,6 +293,154 @@ QuaternionParameterizationTestHelper(x, delta, q_delta); } +// Functor needed to implement automatically differentiated Plus for +// homogeneous vectors. Note this explicitly defined for vectors of size 4. +struct HomogeneousVectorParameterizationPlus { + template<typename Scalar> + bool operator()(const Scalar* p_x, const Scalar* p_delta, + Scalar* p_x_plus_delta) const { + Eigen::Map<const Eigen::Matrix<Scalar, 4, 1> > x(p_x); + Eigen::Map<const Eigen::Matrix<Scalar, 3, 1> > delta(p_delta); + Eigen::Map<Eigen::Matrix<Scalar, 4, 1> > x_plus_delta(p_x_plus_delta); + + const Scalar squared_norm_delta = + delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]; + + Eigen::Matrix<Scalar, 4, 1> y; + Scalar one_half(0.5); + if (squared_norm_delta > Scalar(0.0)) { + Scalar norm_delta = sqrt(squared_norm_delta); + Scalar norm_delta_div_2 = 0.5 * norm_delta; + const Scalar sin_delta_by_delta = sin(norm_delta_div_2) / + norm_delta_div_2; + y[0] = sin_delta_by_delta * delta[0] * one_half; + y[1] = sin_delta_by_delta * delta[1] * one_half; + y[2] = sin_delta_by_delta * delta[2] * one_half; + y[3] = cos(norm_delta_div_2); + + } else { + // We do not just use y = [0,0,0,1] here because that is a + // constant and when used for automatic differentiation will + // lead to a zero derivative. Instead we take a first order + // approximation and evaluate it at zero. + y[0] = delta[0] * one_half; + y[1] = delta[1] * one_half; + y[2] = delta[2] * one_half; + y[3] = Scalar(1.0); + } + + Eigen::Matrix<Scalar, Eigen::Dynamic, 1> v(4); + Scalar beta; + internal::ComputeHouseholderVector<Scalar>(x, &v, &beta); + + x_plus_delta = x.norm() * (y - v * (beta * v.dot(y))); + + return true; + } +}; + +void HomogeneousVectorParameterizationHelper(const double* x, + const double* delta) { + const double kTolerance = 1e-14; + + HomogeneousVectorParameterization homogeneous_vector_parameterization(4); + + // Ensure the update maintains the norm. + double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0}; + homogeneous_vector_parameterization.Plus(x, delta, x_plus_delta); + + const double x_plus_delta_norm = + sqrt(x_plus_delta[0] * x_plus_delta[0] + + x_plus_delta[1] * x_plus_delta[1] + + x_plus_delta[2] * x_plus_delta[2] + + x_plus_delta[3] * x_plus_delta[3]); + + const double x_norm = sqrt(x[0] * x[0] + x[1] * x[1] + + x[2] * x[2] + x[3] * x[3]); + + EXPECT_NEAR(x_plus_delta_norm, x_norm, kTolerance); + + // Autodiff jacobian at delta_x = 0. + AutoDiffLocalParameterization<HomogeneousVectorParameterizationPlus, 4, 3> + autodiff_jacobian; + + double jacobian_autodiff[12]; + double jacobian_analytic[12]; + + homogeneous_vector_parameterization.ComputeJacobian(x, jacobian_analytic); + autodiff_jacobian.ComputeJacobian(x, jacobian_autodiff); + + for (int i = 0; i < 12; ++i) { + EXPECT_TRUE(ceres::IsFinite(jacobian_analytic[i])); + EXPECT_NEAR(jacobian_analytic[i], jacobian_autodiff[i], kTolerance) + << "Jacobian mismatch: i = " << i << ", " << jacobian_analytic[i] << " " + << jacobian_autodiff[i]; + } +} + +TEST(HomogeneousVectorParameterization, ZeroTest) { + double x[4] = {0.0, 0.0, 0.0, 1.0}; + Normalize<4>(x); + double delta[3] = {0.0, 0.0, 0.0}; + + HomogeneousVectorParameterizationHelper(x, delta); +} + +TEST(HomogeneousVectorParameterization, NearZeroTest1) { + double x[4] = {1e-5, 1e-5, 1e-5, 1.0}; + Normalize<4>(x); + double delta[3] = {0.0, 1.0, 0.0}; + + HomogeneousVectorParameterizationHelper(x, delta); +} + +TEST(HomogeneousVectorParameterization, NearZeroTest2) { + double x[4] = {0.001, 0.0, 0.0, 0.0}; + double delta[3] = {0.0, 1.0, 0.0}; + + HomogeneousVectorParameterizationHelper(x, delta); +} + +TEST(HomogeneousVectorParameterization, AwayFromZeroTest1) { + double x[4] = {0.52, 0.25, 0.15, 0.45}; + Normalize<4>(x); + double delta[3] = {0.0, 1.0, -0.5}; + + HomogeneousVectorParameterizationHelper(x, delta); +} + +TEST(HomogeneousVectorParameterization, AwayFromZeroTest2) { + double x[4] = {0.87, -0.25, -0.34, 0.45}; + Normalize<4>(x); + double delta[3] = {0.0, 0.0, -0.5}; + + HomogeneousVectorParameterizationHelper(x, delta); +} + +TEST(HomogeneousVectorParameterization, AwayFromZeroTest3) { + double x[4] = {0.0, 0.0, 0.0, 2.0}; + double delta[3] = {0.0, 0.0, 0}; + + HomogeneousVectorParameterizationHelper(x, delta); +} + +TEST(HomogeneousVectorParameterization, AwayFromZeroTest4) { + double x[4] = {0.2, -1.0, 0.0, 2.0}; + double delta[3] = {1.4, 0.0, -0.5}; + + HomogeneousVectorParameterizationHelper(x, delta); +} + +TEST(HomogeneousVectorParameterization, AwayFromZeroTest5) { + double x[4] = {2.0, 0.0, 0.0, 0.0}; + double delta[3] = {1.4, 0.0, -0.5}; + + HomogeneousVectorParameterizationHelper(x, delta); +} + +TEST(HomogeneousVectorParameterization, DeathTests) { + EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size"); +} } // namespace internal } // namespace ceres