1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3// http://code.google.com/p/ceres-solver/
4//
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are met:
7//
8// * Redistributions of source code must retain the above copyright notice,
9//   this list of conditions and the following disclaimer.
10// * Redistributions in binary form must reproduce the above copyright notice,
11//   this list of conditions and the following disclaimer in the documentation
12//   and/or other materials provided with the distribution.
13// * Neither the name of Google Inc. nor the names of its contributors may be
14//   used to endorse or promote products derived from this software without
15//   specific prior written permission.
16//
17// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27// POSSIBILITY OF SUCH DAMAGE.
28//
29// Author: sameeragarwal@google.com (Sameer Agarwal)
30
31#include "ceres/local_parameterization.h"
32
33#include "ceres/internal/eigen.h"
34#include "ceres/rotation.h"
35#include "glog/logging.h"
36
37namespace ceres {
38
39IdentityParameterization::IdentityParameterization(const int size)
40    : size_(size) {
41  CHECK_GT(size, 0);
42}
43
44bool IdentityParameterization::Plus(const double* x,
45                                    const double* delta,
46                                    double* x_plus_delta) const {
47  VectorRef(x_plus_delta, size_) =
48      ConstVectorRef(x, size_) + ConstVectorRef(delta, size_);
49  return true;
50}
51
52bool IdentityParameterization::ComputeJacobian(const double* x,
53                                               double* jacobian) const {
54  MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_);
55  return true;
56}
57
58SubsetParameterization::SubsetParameterization(
59    int size,
60    const vector<int>& constant_parameters)
61    : local_size_(size - constant_parameters.size()),
62      constancy_mask_(size, 0) {
63  CHECK_GT(constant_parameters.size(), 0)
64      << "The set of constant parameters should contain at least "
65      << "one element. If you do not wish to hold any parameters "
66      << "constant, then do not use a SubsetParameterization";
67
68  vector<int> constant = constant_parameters;
69  sort(constant.begin(), constant.end());
70  CHECK(unique(constant.begin(), constant.end()) == constant.end())
71      << "The set of constant parameters cannot contain duplicates";
72  CHECK_LT(constant_parameters.size(), size)
73      << "Number of parameters held constant should be less "
74      << "than the size of the parameter block. If you wish "
75      << "to hold the entire parameter block constant, then a "
76      << "efficient way is to directly mark it as constant "
77      << "instead of using a LocalParameterization to do so.";
78  CHECK_GE(*min_element(constant.begin(), constant.end()), 0);
79  CHECK_LT(*max_element(constant.begin(), constant.end()), size);
80
81  for (int i = 0; i < constant_parameters.size(); ++i) {
82    constancy_mask_[constant_parameters[i]] = 1;
83  }
84}
85
86bool SubsetParameterization::Plus(const double* x,
87                                  const double* delta,
88                                  double* x_plus_delta) const {
89  for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) {
90    if (constancy_mask_[i]) {
91      x_plus_delta[i] = x[i];
92    } else {
93      x_plus_delta[i] = x[i] + delta[j++];
94    }
95  }
96  return true;
97}
98
99bool SubsetParameterization::ComputeJacobian(const double* x,
100                                             double* jacobian) const {
101  MatrixRef m(jacobian, constancy_mask_.size(), local_size_);
102  m.setZero();
103  for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) {
104    if (!constancy_mask_[i]) {
105      m(i, j++) = 1.0;
106    }
107  }
108  return true;
109}
110
111bool QuaternionParameterization::Plus(const double* x,
112                                      const double* delta,
113                                      double* x_plus_delta) const {
114  const double norm_delta =
115      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
116  if (norm_delta > 0.0) {
117    const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
118    double q_delta[4];
119    q_delta[0] = cos(norm_delta);
120    q_delta[1] = sin_delta_by_delta * delta[0];
121    q_delta[2] = sin_delta_by_delta * delta[1];
122    q_delta[3] = sin_delta_by_delta * delta[2];
123    QuaternionProduct(q_delta, x, x_plus_delta);
124  } else {
125    for (int i = 0; i < 4; ++i) {
126      x_plus_delta[i] = x[i];
127    }
128  }
129  return true;
130}
131
132bool QuaternionParameterization::ComputeJacobian(const double* x,
133                                                 double* jacobian) const {
134  jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
135  jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
136  jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
137  jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT
138  return true;
139}
140
141}  // namespace ceres
142