OKVIS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
PoseParameterBlock.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_POSEPARAMETERBLOCK_HPP_
40 #define INCLUDE_OKVIS_CERES_POSEPARAMETERBLOCK_HPP_
41 
42 #include <Eigen/Core>
46 #include <okvis/Time.hpp>
47 
48 namespace okvis {
49 namespace ceres{
50 
52 class PoseParameterBlock : public ParameterBlockSized<7,6,okvis::kinematics::Transformation>{
53 public:
54 
57 
60 
63 
69 
71  virtual ~PoseParameterBlock();
72 
73  // setters
76  virtual void setEstimate(const okvis::kinematics::Transformation& T_WS);
77 
80 
81  // getters
85 
88  okvis::Time timestamp() const {return timestamp_;}
89 
90  // minimal internal parameterization
91  // x0_plus_Delta=Delta_Chi[+]x0
98  virtual void plus(const double* x0, const double* Delta_Chi, double* x0_plus_Delta) const {
99  PoseLocalParameterization::plus(x0,Delta_Chi,x0_plus_Delta);
100  }
101 
105  virtual void plusJacobian(const double* x0, double* jacobian) const {
107  }
108 
109  // Delta_Chi=x0_plus_Delta[-]x0
115  virtual void minus(const double* x0, const double* x0_plus_Delta, double* Delta_Chi) const {
116  PoseLocalParameterization::minus(x0, x0_plus_Delta, Delta_Chi);
117  }
118 
123  virtual void liftJacobian(const double* x0, double* jacobian) const {
125  }
126 
128  virtual std::string typeInfo() const {return "PoseParameterBlock";}
129 
130 private:
132 };
133 
134 } // namespace ceres
135 } // namespace okvis
136 
137 #endif /* INCLUDE_OKVIS_CERES_POSEPARAMETERBLOCK_HPP_ */
ParameterBlockSized< 7, 6, estimate_t > base_t
The base class type.
Definition: PoseParameterBlock.hpp:59
virtual okvis::kinematics::Transformation estimate() const
Get estimate.
Definition: PoseParameterBlock.cpp:83
virtual void liftJacobian(const double *x0, double *jacobian) const
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: PoseParameterBlock.hpp:123
virtual void plus(const double *x0, const double *Delta_Chi, double *x0_plus_Delta) const
Generalization of the addition operation, x_plus_delta = Plus(x, delta) with the condition that Plus(...
Definition: PoseParameterBlock.hpp:98
okvis::kinematics::Transformation estimate_t
The estimate type (okvis::kinematics::Transformation ).
Definition: PoseParameterBlock.hpp:56
virtual ~PoseParameterBlock()
Trivial destructor.
Definition: PoseParameterBlock.cpp:53
Header file for the PoseLocalParemerization class.
okvis::Time timestamp() const
Get the time.
Definition: PoseParameterBlock.hpp:88
Wraps the parameter block for a pose estimate.
Definition: PoseParameterBlock.hpp:52
okvis::Time timestamp_
Time of this state.
Definition: PoseParameterBlock.hpp:131
virtual void minus(const double *x0, const double *x0_plus_Delta, double *Delta_Chi) const
Computes the minimal difference between a variable x and a perturbed variable x_plus_delta.
Definition: PoseParameterBlock.hpp:115
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
Header file for the TimeBase, Time and WallTime class.
virtual std::string typeInfo() const
Return parameter block type as string.
Definition: PoseParameterBlock.hpp:128
void setTimestamp(const okvis::Time &timestamp)
Definition: PoseParameterBlock.hpp:79
Header file for the ParameterBlockSized class.
Base class providing the interface for parameter blocks.
Definition: ParameterBlockSized.hpp:59
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
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 void setEstimate(const okvis::kinematics::Transformation &T_WS)
Set estimate of this parameter block.
Definition: PoseParameterBlock.cpp:68
PoseParameterBlock()
Default constructor (assumes not fixed).
Definition: PoseParameterBlock.cpp:47
Header file for the Transformation class.
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
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
virtual void plusJacobian(const double *x0, double *jacobian) const
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: PoseParameterBlock.hpp:105
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