OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
SpeedAndBiasParameterBlock.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_SPEEDANDBIASPARAMETERBLOCK_HPP_
40 #define INCLUDE_OKVIS_CERES_SPEEDANDBIASPARAMETERBLOCK_HPP_
41 
44 #include <Eigen/Core>
45 #include <okvis/Time.hpp>
46 
48 namespace okvis {
50 namespace ceres {
51 
52 typedef Eigen::Matrix<double, 9, 1> SpeedAndBias;
53 
56  public ParameterBlockSized<9, 9, SpeedAndBias> {
57  public:
58 
61 
64 
67 
72  SpeedAndBiasParameterBlock(const SpeedAndBias& speedAndBias, uint64_t id,
73  const okvis::Time& timestamp);
74 
77 
78  // setters
81  virtual void setEstimate(const SpeedAndBias& speedAndBias);
82 
87  }
88 
89  // getters
92  virtual SpeedAndBias estimate() const;
93 
97  return timestamp_;
98  }
99 
100  // minimal internal parameterization
101  // x0_plus_Delta=Delta_Chi[+]x0
108  virtual void plus(const double* x0, const double* Delta_Chi,
109  double* x0_plus_Delta) const {
110  Eigen::Map<const Eigen::Matrix<double, 9, 1> > x0_(x0);
111  Eigen::Map<const Eigen::Matrix<double, 9, 1> > Delta_Chi_(Delta_Chi);
112  Eigen::Map<Eigen::Matrix<double, 9, 1> > x0_plus_Delta_(x0_plus_Delta);
113  x0_plus_Delta_ = x0_ + Delta_Chi_;
114  }
115 
117 // /// @param[in] x0 Variable.
119  virtual void plusJacobian(const double* /*unused: x*/,
120  double* jacobian) const {
121  Eigen::Map<Eigen::Matrix<double, 9, 9, Eigen::RowMajor> > identity(
122  jacobian);
123  identity.setIdentity();
124  }
125 
126  // Delta_Chi=x0_plus_Delta[-]x0
132  virtual void minus(const double* x0, const double* x0_plus_Delta,
133  double* Delta_Chi) const {
134  Eigen::Map<const Eigen::Matrix<double, 9, 1> > x0_(x0);
135  Eigen::Map<Eigen::Matrix<double, 9, 1> > Delta_Chi_(Delta_Chi);
136  Eigen::Map<const Eigen::Matrix<double, 9, 1> > x0_plus_Delta_(
137  x0_plus_Delta);
138  Delta_Chi_ = x0_plus_Delta_ - x0_;
139  }
140 
142 // /// @param[in] x0 Variable.
145  virtual void liftJacobian(const double* /*unused: x*/,
146  double* jacobian) const {
147  Eigen::Map<Eigen::Matrix<double, 9, 9, Eigen::RowMajor> > identity(
148  jacobian);
149  identity.setIdentity();
150  }
151 
153  virtual std::string typeInfo() const {
154  return "SpeedAndBiasParameterBlock";
155  }
156 
157  private:
159 };
160 
161 } // namespace ceres
162 } // namespace okvis
163 
164 #endif /* INCLUDE_OKVIS_CERES_SPEEDANDBIASPARAMETERBLOCK_HPP_ */
virtual std::string typeInfo() const
Return parameter block type as string.
Definition: SpeedAndBiasParameterBlock.hpp:153
okvis::Time timestamp_
Time of this state.
Definition: SpeedAndBiasParameterBlock.hpp:158
okvis::Time timestamp() const
Get the time.
Definition: SpeedAndBiasParameterBlock.hpp:96
virtual void liftJacobian(const double *, double *jacobian) const
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: SpeedAndBiasParameterBlock.hpp:145
Header file for the TimeBase, Time and WallTime class.
virtual ~SpeedAndBiasParameterBlock()
Trivial destructor.
Definition: SpeedAndBiasParameterBlock.cpp:53
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: SpeedAndBiasParameterBlock.hpp:108
Header file for the ParameterBlockSized class.
Base class providing the interface for parameter blocks.
Definition: ParameterBlockSized.hpp:59
Eigen::Matrix< double, 9, 1 > SpeedAndBias
Definition: SpeedAndBiasParameterBlock.hpp:52
void setTimestamp(const okvis::Time &timestamp)
Set the time.
Definition: SpeedAndBiasParameterBlock.hpp:85
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: SpeedAndBiasParameterBlock.hpp:132
virtual void setEstimate(const SpeedAndBias &speedAndBias)
Set estimate of this parameter block.
Definition: SpeedAndBiasParameterBlock.cpp:68
Header file for the Transformation class.
virtual void plusJacobian(const double *, double *jacobian) const
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: SpeedAndBiasParameterBlock.hpp:119
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
virtual SpeedAndBias estimate() const
Get estimate.
Definition: SpeedAndBiasParameterBlock.cpp:75
ParameterBlockSized< 9, 9, SpeedAndBias > base_t
The base class type.
Definition: SpeedAndBiasParameterBlock.hpp:60
SpeedAndBias estimate_t
The estimate type (9D vector).
Definition: SpeedAndBiasParameterBlock.hpp:63
SpeedAndBiasParameterBlock()
Default constructor (assumes not fixed).
Definition: SpeedAndBiasParameterBlock.cpp:47
Wraps the parameter block for a speed / IMU biases estimate.
Definition: SpeedAndBiasParameterBlock.hpp:55