OKVIS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
PoseLocalParameterization.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
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 Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without 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  * Created on: Aug 30, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_POSELOCALPARAMETERIZATION_HPP_
40 #define INCLUDE_OKVIS_CERES_POSELOCALPARAMETERIZATION_HPP_
41 
42 #include "ceres/ceres.h"
43 #include <okvis/assert_macros.hpp>
45 
47 namespace okvis {
49 namespace ceres {
50 
52 class PoseLocalParameterization : public ::ceres::LocalParameterization,
54  public:
55 
58  }
59 
66  virtual bool Plus(const double* x, const double* delta,
67  double* x_plus_delta) const;
68 
74  virtual bool Minus(const double* x, const double* x_plus_delta,
75  double* delta) const;
76 
80  virtual bool ComputeJacobian(const double* x, double* jacobian) const;
81 
86  virtual bool ComputeLiftJacobian(const double* x, double* jacobian) const;
87 
88  // provide these as static for easy use elsewhere:
89 
96  static bool plus(const double* x, const double* delta, double* x_plus_delta);
97 
101  static bool plusJacobian(const double* x, double* jacobian);
102 
108  static bool minus(const double* x, const double* x_plus_delta, double* delta);
109 
114  static bool liftJacobian(const double* x, double* jacobian);
115 
117  virtual int GlobalSize() const {
118  return 7;
119  }
120 
122  virtual int LocalSize() const {
123  return 6;
124  }
125 
126  // added convenient check
127  bool VerifyJacobianNumDiff(const double* x, double* jacobian,
128  double* jacobianNumDiff);
129 };
130 
133 class PoseLocalParameterization3d : public ::ceres::LocalParameterization,
135  public:
136 
139  }
140 
147  virtual bool Plus(const double* x, const double* delta,
148  double* x_plus_delta) const;
149 
155  virtual bool Minus(const double* x, const double* x_plus_delta,
156  double* delta) const;
157 
161  virtual bool ComputeJacobian(const double* x, double* jacobian) const;
162 
167  virtual bool ComputeLiftJacobian(const double* x, double* jacobian) const;
168 
169  // provide these as static for easy use elsewhere:
173  static bool plusJacobian(const double* x, double* jacobian);
174 
179  static bool liftJacobian(const double* x, double* jacobian);
180 
182  virtual int GlobalSize() const {
183  return 7;
184  }
185 
187  virtual int LocalSize() const {
188  return 3;
189  }
190 };
191 
194 class PoseLocalParameterization4d : public ::ceres::LocalParameterization,
196  public:
197 
200  }
201 
208  virtual bool Plus(const double* x, const double* delta,
209  double* x_plus_delta) const;
210 
216  virtual bool Minus(const double* x, const double* x_plus_delta,
217  double* delta) const;
218 
222  virtual bool ComputeJacobian(const double* x, double* jacobian) const;
223 
228  virtual bool ComputeLiftJacobian(const double* x, double* jacobian) const;
229 
230  // provide these as static for easy use elsewhere:
234  static bool plusJacobian(const double* x, double* jacobian);
235 
240  static bool liftJacobian(const double* x, double* jacobian);
241 
243  virtual int GlobalSize() const {
244  return 7;
245  }
246 
248  virtual int LocalSize() const {
249  return 4;
250  }
251 };
252 
255 class PoseLocalParameterization2d : public ::ceres::LocalParameterization,
257  public:
258 
261  }
262 
269  virtual bool Plus(const double* x, const double* delta,
270  double* x_plus_delta) const;
271 
277  virtual bool Minus(const double* x, const double* x_plus_delta,
278  double* delta) const;
279 
283  virtual bool ComputeJacobian(const double* x, double* jacobian) const;
284 
289  virtual bool ComputeLiftJacobian(const double* x, double* jacobian) const;
290 
291  // provide these as static for easy use elsewhere:
295  static bool plusJacobian(const double* x, double* jacobian);
296 
301  static bool liftJacobian(const double* x, double* jacobian);
302 
304  virtual int GlobalSize() const {
305  return 7;
306  }
308  virtual int LocalSize() const {
309  return 2;
310  }
311 };
312 
313 } // namespace ceres
314 } // namespace okvis
315 
316 #endif /* INCLUDE_OKVIS_CERES_POSELOCALPARAMETERIZATION_HPP_ */
static bool liftJacobian(const double *x, double *jacobian)
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:350
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const
Generalization of the addition operation, x_plus_delta = Plus(x, delta) with the condition that Plus(...
Definition: PoseLocalParameterization.cpp:52
virtual bool ComputeJacobian(const double *x, double *jacobian) const
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseLocalParameterization.cpp:366
virtual int LocalSize() const
The parameter block local dimension.
Definition: PoseLocalParameterization.hpp:248
virtual ~PoseLocalParameterization()
Trivial destructor.
Definition: PoseLocalParameterization.hpp:57
virtual int LocalSize() const
The parameter block local dimension.
Definition: PoseLocalParameterization.hpp:308
virtual ~PoseLocalParameterization2d()
Trivial destructor.
Definition: PoseLocalParameterization.hpp:260
Provides some additional interfaces to ceres' LocalParamization than are needed in the generic margin...
Definition: LocalParamizationAdditionalInterfaces.hpp:52
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const
Generalization of the addition operation, x_plus_delta = Plus(x, delta) with the condition that Plus(...
Definition: PoseLocalParameterization.cpp:281
virtual bool ComputeLiftJacobian(const double *x, double *jacobian) const
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:330
Header file for the LocalParamizationAdditionalInterfaces class.
virtual bool ComputeLiftJacobian(const double *x, double *jacobian) const
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:97
virtual int LocalSize() const
The parameter block local dimension.
Definition: PoseLocalParameterization.hpp:122
virtual bool ComputeJacobian(const double *x, double *jacobian) const
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseLocalParameterization.cpp:148
virtual bool ComputeLiftJacobian(const double *x, double *jacobian) const
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:422
Pose local parameterisation, i.e. for orientation dq(dalpha) x q_bar. Here, we only perturb roll and ...
Definition: PoseLocalParameterization.hpp:255
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const
Generalization of the addition operation, x_plus_delta = Plus(x, delta) with the condition that Plus(...
Definition: PoseLocalParameterization.cpp:374
virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const
Generalization of the addition operation, x_plus_delta = Plus(x, delta) with the condition that Plus(...
Definition: PoseLocalParameterization.cpp:182
static bool plusJacobian(const double *x, double *jacobian)
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseLocalParameterization.cpp:238
static bool plusJacobian(const double *x, double *jacobian)
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseLocalParameterization.cpp:336
virtual bool ComputeJacobian(const double *x, double *jacobian) const
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseLocalParameterization.cpp:273
Pose local parameterisation, i.e. for orientation dq(dalpha) x q_bar. Here, we only perturb the trans...
Definition: PoseLocalParameterization.hpp:194
virtual int GlobalSize() const
The parameter block dimension.
Definition: PoseLocalParameterization.hpp:243
static bool liftJacobian(const double *x, double *jacobian)
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:255
virtual ~PoseLocalParameterization3d()
Trivial destructor.
Definition: PoseLocalParameterization.hpp:138
static bool liftJacobian(const double *x, double *jacobian)
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:445
This file contains some useful assert macros.
virtual bool Minus(const double *x, const double *x_plus_delta, double *delta) const
Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
Definition: PoseLocalParameterization.cpp:314
virtual int GlobalSize() const
The parameter block dimension.
Definition: PoseLocalParameterization.hpp:117
virtual int GlobalSize() const
The parameter block dimension.
Definition: PoseLocalParameterization.hpp:182
static bool minus(const double *x, const double *x_plus_delta, double *delta)
Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
Definition: PoseLocalParameterization.cpp:103
Pose local parameterisation, i.e. for orientation dq(dalpha) x q_bar.
Definition: PoseLocalParameterization.hpp:52
virtual bool ComputeJacobian(const double *x, double *jacobian) const
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseLocalParameterization.cpp:462
virtual bool ComputeLiftJacobian(const double *x, double *jacobian) const
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:232
virtual bool Minus(const double *x, const double *x_plus_delta, double *delta) const
Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
Definition: PoseLocalParameterization.cpp:90
static bool liftJacobian(const double *x, double *jacobian)
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseLocalParameterization.cpp:131
virtual bool Minus(const double *x, const double *x_plus_delta, double *delta) const
Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
Definition: PoseLocalParameterization.cpp:214
virtual int LocalSize() const
The parameter block local dimension.
Definition: PoseLocalParameterization.hpp:187
static bool plusJacobian(const double *x, double *jacobian)
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseLocalParameterization.cpp:428
virtual int GlobalSize() const
The parameter block dimension.
Definition: PoseLocalParameterization.hpp:304
bool VerifyJacobianNumDiff(const double *x, double *jacobian, double *jacobianNumDiff)
Definition: PoseLocalParameterization.cpp:154
virtual bool Minus(const double *x, const double *x_plus_delta, double *delta) const
Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
Definition: PoseLocalParameterization.cpp:405
static bool plus(const double *x, const double *delta, double *x_plus_delta)
Generalization of the addition operation, x_plus_delta = Plus(x, delta) with the condition that Plus(...
Definition: PoseLocalParameterization.cpp:60
Pose local parameterisation, i.e. for orientation dq(dalpha) x q_bar. Here, we only perturb the trans...
Definition: PoseLocalParameterization.hpp:133
virtual ~PoseLocalParameterization4d()
Trivial destructor.
Definition: PoseLocalParameterization.hpp:199
static bool plusJacobian(const double *x, double *jacobian)
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseLocalParameterization.cpp:119