OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
HomogeneousPointParameterBlock.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_HOMOGENEOUSPOINTPARAMETERBLOCK_HPP_
40 #define INCLUDE_OKVIS_CERES_HOMOGENEOUSPOINTPARAMETERBLOCK_HPP_
41 
44 #include <Eigen/Core>
45 
47 namespace okvis {
49 namespace ceres {
50 
53  Eigen::Vector4d>
54 {
55  public:
56 
58  typedef Eigen::Vector4d estimate_t;
59 
62 
65 
70  HomogeneousPointParameterBlock(const Eigen::Vector4d& point, uint64_t id,
71  bool initialized = true);
72 
77  HomogeneousPointParameterBlock(const Eigen::Vector3d& point, uint64_t id,
78  bool initialized = true);
79 
82 
85 
88  virtual void setEstimate(const Eigen::Vector4d& point);
89 
93  {
95  }
96 
98 
101 
104  virtual Eigen::Vector4d estimate() const;
105 
108  bool initialized() const
109  {
110  return initialized_;
111  }
112 
114 
115  // minimal internal parameterization
116  // x0_plus_Delta=Delta_Chi[+]x0
123  virtual void plus(const double* x0, const double* Delta_Chi,
124  double* x0_plus_Delta) const
125  {
126  HomogeneousPointLocalParameterization::plus(x0, Delta_Chi, x0_plus_Delta);
127  }
128 
132  virtual void plusJacobian(const double* x0, double* jacobian) const
133  {
135  }
136 
137  // Delta_Chi=x0_plus_Delta[-]x0
143  virtual void minus(const double* x0, const double* x0_plus_Delta,
144  double* Delta_Chi) const
145  {
146  HomogeneousPointLocalParameterization::minus(x0, x0_plus_Delta, Delta_Chi);
147  }
148 
153  virtual void liftJacobian(const double* x0, double* jacobian) const
154  {
156  }
157 
159  virtual std::string typeInfo() const
160  {
161  return "HomogeneousPointParameterBlock";
162  }
163 
164  private:
166 
167 };
168 
169 } // namespace ceres
170 } // namespace okvis
171 
172 #endif /* INCLUDE_OKVIS_CERES_HOMOGENEOUSPOINTPARAMETERBLOCK_HPP_ */
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: HomogeneousPointLocalParameterization.cpp:86
Header file for the HomogeneousPointLocalParameterization class.
virtual std::string typeInfo() const
Return parameter block type as string.
Definition: HomogeneousPointParameterBlock.hpp:159
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: HomogeneousPointParameterBlock.hpp:123
virtual void liftJacobian(const double *x0, double *jacobian) const
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: HomogeneousPointParameterBlock.hpp:153
bool initialized_
Whether or not the 3d position is considered initialised.
Definition: HomogeneousPointParameterBlock.hpp:165
bool initialized() const
Get initialisaiton status.
Definition: HomogeneousPointParameterBlock.hpp:108
Wraps the parameter block for a speed / IMU biases estimate.
Definition: HomogeneousPointParameterBlock.hpp:52
Header file for the ParameterBlockSized class.
Base class providing the interface for parameter blocks.
Definition: ParameterBlockSized.hpp:59
virtual ~HomogeneousPointParameterBlock()
Trivial destructor.
Definition: HomogeneousPointParameterBlock.cpp:53
static bool plusJacobian(const double *x, double *jacobian)
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: HomogeneousPointLocalParameterization.cpp:109
void setInitialized(bool initialized)
Set initialisaiton status.
Definition: HomogeneousPointParameterBlock.hpp:92
HomogeneousPointParameterBlock()
Default constructor (assumes not fixed).
Definition: HomogeneousPointParameterBlock.cpp:47
static bool liftJacobian(const double *x, double *jacobian)
Computes the Jacobian from minimal space to naively overparameterised space as used by ceres...
Definition: HomogeneousPointLocalParameterization.cpp:122
virtual Eigen::Vector4d estimate() const
Get estimate.
Definition: HomogeneousPointParameterBlock.cpp:84
ParameterBlockSized< 4, 3, estimate_t > base_t
The base class type.
Definition: HomogeneousPointParameterBlock.hpp:61
virtual void plusJacobian(const double *x0, double *jacobian) const
The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
Definition: HomogeneousPointParameterBlock.hpp:132
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: HomogeneousPointLocalParameterization.cpp:59
Eigen::Vector4d estimate_t
The estimate type (4D vector).
Definition: HomogeneousPointParameterBlock.hpp:58
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: HomogeneousPointParameterBlock.hpp:143
virtual void setEstimate(const Eigen::Vector4d &point)
Set estimate of this parameter block.
Definition: HomogeneousPointParameterBlock.cpp:76