Add Homogeneous vector parameterization.

Change-Id: I42f68a0665a62e2c7dcc7584e5581a05c9b849b0

docs/source/nnls_modeling.rst

@@ -1140,6 +1140,29 @@ Instances
    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`

include/ceres/local_parameterization.h

@@ -210,6 +210,37 @@ class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
   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"

internal/ceres/CMakeLists.txt

@@ -260,6 +260,7 @@ IF (BUILD_TESTING AND GFLAGS)
   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)

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: [email protected] (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_

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: [email protected] (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

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 @@ bool QuaternionParameterization::ComputeJacobian(const double* x,
   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