OKVIS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Map.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: Sep 8, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_CERES_MAP_HPP_
40 #define INCLUDE_OKVIS_CERES_MAP_HPP_
41 
42 #include <memory>
43 
44 #include <ceres/ceres.h>
48 #include <okvis/Time.hpp>
49 #include <okvis/FrameTypedefs.hpp>
50 #include <okvis/assert_macros.hpp>
51 #include <unordered_map>
53 
55 namespace okvis {
57 namespace ceres {
58 
63 class Map {
64  public:
65  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
66 
67 
68  Map();
69 
70  // definitions
74  : residualBlockId(0),
75  lossFunctionPtr(0),
76  errorInterfacePtr(std::shared_ptr<ErrorInterface>()) {
77  }
78 
83  ResidualBlockSpec(::ceres::ResidualBlockId residualBlockId,
84  ::ceres::LossFunction* lossFunctionPtr,
85  std::shared_ptr<ErrorInterface> errorInterfacePtr)
86  : residualBlockId(residualBlockId),
87  lossFunctionPtr(lossFunctionPtr),
88  errorInterfacePtr(errorInterfacePtr) {
89  }
90 
91  ::ceres::ResidualBlockId residualBlockId;
92  ::ceres::LossFunction* lossFunctionPtr;
93  std::shared_ptr<ErrorInterface> errorInterfacePtr;
94  };
95  typedef std::pair<uint64_t, std::shared_ptr<okvis::ceres::ParameterBlock> > ParameterBlockSpec;
96 
97  typedef std::vector<ResidualBlockSpec> ResidualBlockCollection;
98  typedef std::vector<ParameterBlockSpec> ParameterBlockCollection;
99 
108  };
109 
115  bool parameterBlockExists(uint64_t parameterBlockId) const;
116 
119 
121  void printParameterBlockInfo(uint64_t parameterBlockId) const;
122 
124  void printResidualBlockInfo(::ceres::ResidualBlockId residualBlockId) const;
125 
127 
128  // for quality assessment
134  void getLhs(uint64_t parameterBlockId, Eigen::MatrixXd& H);
135 
138 
146  bool addParameterBlock(
147  std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock,
148  int parameterization = Parameterization::Trivial, const int group = -1);
149 
155  bool removeParameterBlock(uint64_t parameterBlockId);
156 
163  std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock);
164 
172  ::ceres::ResidualBlockId addResidualBlock(
173  std::shared_ptr< ::ceres::CostFunction> cost_function,
174  ::ceres::LossFunction* loss_function,
175  std::vector<std::shared_ptr<okvis::ceres::ParameterBlock> >& parameterBlockPtrs);
176 
182  void resetResidualBlock(
183  ::ceres::ResidualBlockId residualBlockId,
184  std::vector<std::shared_ptr<okvis::ceres::ParameterBlock> >& parameterBlockPtrs);
185 
202  ::ceres::ResidualBlockId addResidualBlock(
203  std::shared_ptr< ::ceres::CostFunction> cost_function,
204  ::ceres::LossFunction* loss_function,
205  std::shared_ptr<okvis::ceres::ParameterBlock> x0,
206  std::shared_ptr<okvis::ceres::ParameterBlock> x1 = std::shared_ptr<
208  std::shared_ptr<okvis::ceres::ParameterBlock> x2 = std::shared_ptr<
210  std::shared_ptr<okvis::ceres::ParameterBlock> x3 = std::shared_ptr<
212  std::shared_ptr<okvis::ceres::ParameterBlock> x4 = std::shared_ptr<
214  std::shared_ptr<okvis::ceres::ParameterBlock> x5 = std::shared_ptr<
216  std::shared_ptr<okvis::ceres::ParameterBlock> x6 = std::shared_ptr<
218  std::shared_ptr<okvis::ceres::ParameterBlock> x7 = std::shared_ptr<
220  std::shared_ptr<okvis::ceres::ParameterBlock> x8 = std::shared_ptr<
222  std::shared_ptr<okvis::ceres::ParameterBlock> x9 = std::shared_ptr<
224 
230  bool removeResidualBlock(::ceres::ResidualBlockId id);
231 
233 
236 
242  bool setParameterBlockConstant(uint64_t parameterBlockId);
243 
249  bool setParameterBlockVariable(uint64_t parameterBlockId);
250 
257  std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock) {
258  return setParameterBlockConstant(parameterBlock->id());
259  }
260 
267  std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock) {
268  return setParameterBlockVariable(parameterBlock->id());
269  }
270 
277  bool resetParameterization(uint64_t parameterBlockId, int parameterization);
278 
285  bool setParameterization(
286  uint64_t parameterBlockId,
287  ::ceres::LocalParameterization* local_parameterization);
288 
296  std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock,
297  ::ceres::LocalParameterization* local_parameterization) {
298  return setParameterization(parameterBlock->id(), local_parameterization);
299  }
300 
302 
305 
307  std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlockPtr(
308  uint64_t parameterBlockId); // get a vertex
309 
311  std::shared_ptr<const okvis::ceres::ParameterBlock> parameterBlockPtr(
312  uint64_t parameterBlockId) const; // get a vertex
313 
315  std::shared_ptr<okvis::ceres::ErrorInterface> errorInterfacePtr(
316  ::ceres::ResidualBlockId residualBlockId); // get a vertex
317 
319  std::shared_ptr<const okvis::ceres::ErrorInterface> errorInterfacePtr(
320  ::ceres::ResidualBlockId residualBlockId) const; // get a vertex
321 
325  ResidualBlockCollection residuals(uint64_t parameterBlockId) const;
326 
331  ::ceres::ResidualBlockId residualBlockId) const; // get the parameter blocks connected
332 
334 
335  // Jacobian checker
343  bool isJacobianCorrect(::ceres::ResidualBlockId residualBlockId,
344  double relTol = 1e-6) const;
345 
346  // access to the map as such
348  typedef std::unordered_map<uint64_t,
349  std::shared_ptr<okvis::ceres::ParameterBlock> > Id2ParameterBlock_Map;
350 
352  typedef std::unordered_map< ::ceres::ResidualBlockId, ResidualBlockSpec> ResidualBlockId2ResidualBlockSpec_Map;
353 
356  return id2ParameterBlock_Map_;
357  }
361  }
362 
363  // these are public for convenient manipulation
365  ::ceres::Solver::Options options;
366 
368  ::ceres::Solver::Summary summary;
369 
371  void solve() {
372  Solve(options, problem_.get(), &summary);
373  }
374 
375  protected:
376 
379 
380  // member variables related to optimization
382  std::shared_ptr< ::ceres::Problem> problem_;
383 
384  // the actual maps
386  typedef std::unordered_multimap<uint64_t, ResidualBlockSpec> Id2ResidualBlock_Multimap;
387 
389  typedef std::unordered_map< ::ceres::ResidualBlockId,
391 
394 
397 
400 
403 
406 
409 
412 
415 
418 
419 };
420 
421 } //namespace okvis
422 } //namespace ceres
423 
424 #endif /* INCLUDE_OKVIS_CERES_MAP_HPP_ */
std::pair< uint64_t, std::shared_ptr< okvis::ceres::ParameterBlock > > ParameterBlockSpec
Definition: Map.hpp:95
Header file for the HomogeneousPointLocalParameterization class.
void printParameterBlockInfo(uint64_t parameterBlockId) const
Log information on a parameter block.
Definition: Map.cpp:75
Header file for the ErrorInterface class. A simple interface class that other error classes should in...
const Id2ParameterBlock_Map & id2parameterBlockMap() const
Get map connecting parameter block IDs to parameter blocks.
Definition: Map.hpp:355
ResidualBlockSpec()
Definition: Map.hpp:73
::ceres::ResidualBlockId residualBlockId
ID of residual block.
Definition: Map.hpp:91
std::vector< ResidualBlockSpec > ResidualBlockCollection
Definition: Map.hpp:97
void getLhs(uint64_t parameterBlockId, Eigen::MatrixXd &H)
Obtain the Hessian block for a specific parameter block.
Definition: Map.cpp:101
std::shared_ptr< ErrorInterface > errorInterfacePtr
The pointer to the error interface of the respective residual block.
Definition: Map.hpp:93
ParameterBlockCollection parameters(::ceres::ResidualBlockId residualBlockId) const
Get the parameters of a residual block.
Definition: Map.cpp:706
bool parameterBlockExists(uint64_t parameterBlockId) const
Check whether a certain parameter block is part of the map.
Definition: Map.cpp:67
bool removeParameterBlock(uint64_t parameterBlockId)
Remove a parameter block from the map.
Definition: Map.cpp:366
ResidualBlockId2ParameterBlockCollection_Map residualBlockId2ParameterBlockCollection_Map_
Go from residual block id to its parameter blocks.
Definition: Map.hpp:402
ResidualBlockSpec(::ceres::ResidualBlockId residualBlockId,::ceres::LossFunction *lossFunctionPtr, std::shared_ptr< ErrorInterface > errorInterfacePtr)
Constructor.
Definition: Map.hpp:83
bool isJacobianCorrect(::ceres::ResidualBlockId residualBlockId, double relTol=1e-6) const
Check a Jacobian with numeric differences.
Definition: Map.cpp:159
uint64_t residualCounter_
count the inserted residual blocks.
Definition: Map.hpp:378
std::shared_ptr< ::ceres::Problem > problem_
The ceres problem.
Definition: Map.hpp:382
Header file for the PoseLocalParemerization class.
std::shared_ptr< okvis::ceres::ErrorInterface > errorInterfacePtr(::ceres::ResidualBlockId residualBlockId)
Get a shared pointer to an error term.
Definition: Map.cpp:684
Use okvis::ceres::PoseLocalParameterization.
Definition: Map.hpp:103
okvis::ceres::PoseLocalParameterization poseLocalParameterization_
Store parameterisation locally.
Definition: Map.hpp:408
Use okvis::ceres::PoseLocalParameterization3d (orientation varying).
Definition: Map.hpp:104
bool setParameterBlockConstant(std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock)
Do not optimise a certain parameter block.
Definition: Map.hpp:256
bool setParameterization(uint64_t parameterBlockId,::ceres::LocalParameterization *local_parameterization)
Set the (local) parameterisation of a parameter block.
Definition: Map.cpp:628
ResidualBlockId2ResidualBlockSpec_Map residualBlockId2ResidualBlockSpec_Map_
Go from residual ID to specs.
Definition: Map.hpp:396
bool setParameterBlockVariable(uint64_t parameterBlockId)
Optimise a certain parameter block (this is the default).
Definition: Map.cpp:579
::ceres::Solver::Summary summary
Ceres optimization summary.
Definition: Map.hpp:368
std::vector< ParameterBlockSpec > ParameterBlockCollection
Definition: Map.hpp:98
::ceres::Solver::Options options
Ceres options.
Definition: Map.hpp:365
bool addParameterBlock(std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock, int parameterization=Parameterization::Trivial, const int group=-1)
Add a parameter block to the map.
Definition: Map.cpp:292
Header file for the TimeBase, Time and WallTime class.
Pose local parameterisation, i.e. for orientation dq(dalpha) x q_bar. Here, we only perturb roll and ...
Definition: PoseLocalParameterization.hpp:255
Simple interface class the errors implemented here should inherit from.
Definition: ErrorInterface.hpp:53
okvis::ceres::HomogeneousPointLocalParameterization homogeneousPointLocalParameterization_
Store parameterisation locally.
Definition: Map.hpp:405
void solve()
Solve the optimization problem.
Definition: Map.hpp:371
::ceres::ResidualBlockId addResidualBlock(std::shared_ptr< ::ceres::CostFunction > cost_function,::ceres::LossFunction *loss_function, std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &parameterBlockPtrs)
Adds a residual block.
Definition: Map.cpp:388
bool setParameterBlockVariable(std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock)
Optimise a certain parameter block (this is the default).
Definition: Map.hpp:266
ResidualBlockCollection residuals(uint64_t parameterBlockId) const
Get the residual blocks of a parameter block.
Definition: Map.cpp:666
Local parameterisation of a homogeneous point [x,y,z,w]^T. We use a Euclidean-type perturbation...
Definition: HomogeneousPointLocalParameterization.hpp:53
Pose local parameterisation, i.e. for orientation dq(dalpha) x q_bar. Here, we only perturb the trans...
Definition: PoseLocalParameterization.hpp:194
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
bool resetParameterization(uint64_t parameterBlockId, int parameterization)
Reset the (local) parameterisation of a parameter block.
Definition: Map.cpp:590
void resetResidualBlock(::ceres::ResidualBlockId residualBlockId, std::vector< std::shared_ptr< okvis::ceres::ParameterBlock > > &parameterBlockPtrs)
Replace the parameters connected to a residual block ID.
Definition: Map.cpp:488
okvis::ceres::PoseLocalParameterization4d poseLocalParameterization4d_
Store parameterisation locally.
Definition: Map.hpp:417
This file contains some useful assert macros.
okvis::ceres::PoseLocalParameterization3d poseLocalParameterization3d_
Store parameterisation locally.
Definition: Map.hpp:414
Parameterization
The Parameterisation enum.
Definition: Map.hpp:101
Id2ResidualBlock_Multimap id2ResidualBlock_Multimap_
Go from Id to residual block pointer.
Definition: Map.hpp:399
Header file for the ParameterBlock class.
Use okvis::ceres::PoseLocalParameterization2d (roll/pitch varying).
Definition: Map.hpp:106
Use okvis::ceres::PoseLocalParameterization4d (position and yaw varying).
Definition: Map.hpp:105
const ResidualBlockId2ResidualBlockSpec_Map & residualBlockId2ResidualBlockSpecMap() const
Get the actual map from Id to residual block specs.
Definition: Map.hpp:359
Id2ParameterBlock_Map id2ParameterBlock_Map_
The map connecting parameter block ID's and parameter blocks.
Definition: Map.hpp:393
Pose local parameterisation, i.e. for orientation dq(dalpha) x q_bar.
Definition: PoseLocalParameterization.hpp:52
std::unordered_map< ::ceres::ResidualBlockId, ParameterBlockCollection > ResidualBlockId2ParameterBlockCollection_Map
Go from residual block id to its parameter blocks.
Definition: Map.hpp:390
std::unordered_map< ::ceres::ResidualBlockId, ResidualBlockSpec > ResidualBlockId2ResidualBlockSpec_Map
The actual map from Id to residual block specs.
Definition: Map.hpp:352
bool setParameterization(std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlock,::ceres::LocalParameterization *local_parameterization)
Set the (local) parameterisation of a parameter block.
Definition: Map.hpp:295
bool setParameterBlockConstant(uint64_t parameterBlockId)
Do not optimise a certain parameter block.
Definition: Map.cpp:568
::ceres::LossFunction * lossFunctionPtr
The m-estimator.
Definition: Map.hpp:92
void printResidualBlockInfo(::ceres::ResidualBlockId residualBlockId) const
Log information on a residual block.
Definition: Map.cpp:94
Use okvis::ceres::HomogeneousPointLocalParameterization.
Definition: Map.hpp:102
Struct to store some infos about a residual.
Definition: Map.hpp:72
std::unordered_map< uint64_t, std::shared_ptr< okvis::ceres::ParameterBlock > > Id2ParameterBlock_Map
The actual map from Id to parameter block pointer.
Definition: Map.hpp:349
The Map class. This keeps track of how parameter blocks are connected to residual blocks...
Definition: Map.hpp:63
No local parameterisation.
Definition: Map.hpp:107
Map()
Constructor.
Definition: Map.cpp:52
bool removeResidualBlock(::ceres::ResidualBlockId id)
Remove a residual block.
Definition: Map.cpp:537
okvis::ceres::PoseLocalParameterization2d poseLocalParameterization2d_
Store parameterisation locally.
Definition: Map.hpp:411
std::shared_ptr< okvis::ceres::ParameterBlock > parameterBlockPtr(uint64_t parameterBlockId)
Get a shared pointer to a parameter block.
Definition: Map.cpp:643
Pose local parameterisation, i.e. for orientation dq(dalpha) x q_bar. Here, we only perturb the trans...
Definition: PoseLocalParameterization.hpp:133
std::unordered_multimap< uint64_t, ResidualBlockSpec > Id2ResidualBlock_Multimap
Go from Id to residual block pointer.
Definition: Map.hpp:386
Base class providing the interface for parameter blocks.
Definition: ParameterBlock.hpp:53
This file contains useful typedefs and structs related to frames.