OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Estimator.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: Dec 30, 2014
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  * Modified: Andreas Forster (an.forster@gmail.com)
32  *********************************************************************************/
33 
41 #ifndef INCLUDE_OKVIS_ESTIMATOR_HPP_
42 #define INCLUDE_OKVIS_ESTIMATOR_HPP_
43 
44 #include <memory>
45 #include <mutex>
46 #include <array>
47 
48 #include <ceres/ceres.h>
50 
51 #include <okvis/assert_macros.hpp>
53 #include <okvis/MultiFrame.hpp>
54 #include <okvis/FrameTypedefs.hpp>
55 #include <okvis/Measurements.hpp>
56 #include <okvis/Variables.hpp>
60 #include <okvis/ceres/Map.hpp>
64 
66 namespace okvis {
67 
69 
78 {
79  public:
80  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error)
81  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
82 
86  Estimator();
87 
92  Estimator(std::shared_ptr<okvis::ceres::Map> mapPtr);
93  virtual ~Estimator();
94 
97 
102  int addCamera(
103  const okvis::ExtrinsicsEstimationParameters & extrinsicsEstimationParameters);
104 
111  int addImu(const okvis::ImuParameters & imuParameters);
112 
116  void clearCameras();
117 
121  void clearImus();
122 
124 
133  const okvis::ImuMeasurementDeque & imuMeasurements,
134  bool asKeyframe);
135 
141  void printStates(uint64_t poseId, std::ostream & buffer) const;
142 
149  bool addLandmark(uint64_t landmarkId,
150  const Eigen::Vector4d & landmark);
151 
161  template<class GEOMETRY_TYPE>
162  ::ceres::ResidualBlockId addObservation(uint64_t landmarkId, uint64_t poseId,
163  size_t camIdx, size_t keypointIdx);
172  bool removeObservation(uint64_t landmarkId, uint64_t poseId, size_t camIdx,
173  size_t keypointIdx);
174 
183  bool applyMarginalizationStrategy(size_t numKeyframes, size_t numImuFrames,
184  okvis::MapPointVector& removedLandmarks);
185 
192  static bool initPoseFromImu(
193  const okvis::ImuMeasurementDeque & imuMeasurements,
195 
202  void optimize(size_t numIter, size_t numThreads = 1, bool verbose = false);
203 
211  bool setOptimizationTimeLimit(double timeLimit, int minIterations);
212 
218  bool isLandmarkAdded(uint64_t landmarkId) const {
219  bool isAdded = landmarksMap_.find(landmarkId) != landmarksMap_.end();
220  OKVIS_ASSERT_TRUE_DBG(Exception, isAdded == mapPtr_->parameterBlockExists(landmarkId),
221  "id="<<landmarkId<<" inconsistent. isAdded = " << isAdded);
222  return isAdded;
223  }
224 
230  bool isLandmarkInitialized(uint64_t landmarkId) const;
231 
234 
240  bool getLandmark(uint64_t landmarkId, okvis::MapPoint& mapPoint) const;
241 
247  size_t getLandmarks(okvis::PointMap & landmarks) const;
248 
256  size_t getLandmarks(okvis::MapPointVector & landmarks) const;
257 
263  okvis::MultiFramePtr multiFrame(uint64_t frameId) const {
264  OKVIS_ASSERT_TRUE_DBG(Exception, multiFramePtrMap_.find(frameId)!=multiFramePtrMap_.end(),
265  "Requested multi-frame does not exist in estimator.");
266  return multiFramePtrMap_.at(frameId);
267  }
268 
275  bool get_T_WS(uint64_t poseId, okvis::kinematics::Transformation & T_WS) const;
276 
277  // the following access the optimization graph, so are not very fast.
278  // Feel free to implement caching for them...
287  bool getSpeedAndBias(uint64_t poseId, uint64_t imuIdx, okvis::SpeedAndBias & speedAndBias) const;
288 
297  bool getCameraSensorStates(uint64_t poseId, size_t cameraIdx,
299 
302  size_t numFrames() const {
303  return statesMap_.size();
304  }
305 
308  size_t numLandmarks() const {
309  return landmarksMap_.size();
310  }
311 
314  uint64_t currentKeyframeId() const;
315 
321  uint64_t frameIdByAge(size_t age) const;
322 
325  uint64_t currentFrameId() const;
326 
328 
334  bool isKeyframe(uint64_t frameId) const {
335  return statesMap_.at(frameId).isKeyframe;
336  }
337 
343  bool isInImuWindow(uint64_t frameId) const;
344 
347 
352  okvis::Time timestamp(uint64_t frameId) const {
353  return statesMap_.at(frameId).timestamp;
354  }
355 
359 
366  bool set_T_WS(uint64_t poseId, const okvis::kinematics::Transformation & T_WS);
367 
376  bool setSpeedAndBias(uint64_t poseId, size_t imuIdx, const okvis::SpeedAndBias & speedAndBias);
377 
386  bool setCameraSensorStates(uint64_t poseId, size_t cameraIdx,
388 
393  bool setLandmark(uint64_t landmarkId, const Eigen::Vector4d & landmark);
394 
398  void setLandmarkInitialized(uint64_t landmarkId, bool initialized);
399 
403  void setKeyframe(uint64_t frameId, bool isKeyframe){
404  statesMap_.at(frameId).isKeyframe = isKeyframe;
405  }
406 
409  void setMap(std::shared_ptr<okvis::ceres::Map> mapPtr) {
410  mapPtr_ = mapPtr;
411  }
413 
414  private:
415 
421  bool removeObservation(::ceres::ResidualBlockId residualBlockId);
422 
424  struct StateInfo
425  {
430  StateInfo(uint64_t id = 0, bool isRequired = true, bool exists = false)
431  : id(id),
433  exists(exists)
434  {
435  }
436  uint64_t id;
437  bool isRequired;
438  bool exists;
439  };
440 
443  {
444  T_WS = 0,
446  Qff = 2,
447  T_GW = 3,
448  };
449 
452  {
453  Camera = 0,
454  Imu = 1,
455  Position = 2,
456  Gps = 3,
460  };
461 
464  {
465  T_SCi = 0,
467  };
468 
472  {
474  };
475 
478  {
479  T_PiW = 0,
481  };
482 
485  {
487  };
488 
491  {
493  };
494 
497  {
499  };
500 
503  {
505  };
506 
507  // getters
508  bool getGlobalStateParameterBlockPtr(uint64_t poseId, int stateType,
509  std::shared_ptr<ceres::ParameterBlock>& stateParameterBlockPtr) const;
510  template<class PARAMETER_BLOCK_T>
511  bool getGlobalStateParameterBlockAs(uint64_t poseId, int stateType,
512  PARAMETER_BLOCK_T & stateParameterBlock) const;
513  template<class PARAMETER_BLOCK_T>
514  bool getGlobalStateEstimateAs(uint64_t poseId, int stateType,
515  typename PARAMETER_BLOCK_T::estimate_t & state) const;
516 
517  bool getSensorStateParameterBlockPtr(uint64_t poseId, int sensorIdx,
518  int sensorType, int stateType,
519  std::shared_ptr<ceres::ParameterBlock>& stateParameterBlockPtr) const;
520  template<class PARAMETER_BLOCK_T>
521  bool getSensorStateParameterBlockAs(uint64_t poseId, int sensorIdx,
522  int sensorType, int stateType,
523  PARAMETER_BLOCK_T & stateParameterBlock) const;
524  template<class PARAMETER_BLOCK_T>
525  bool getSensorStateEstimateAs(uint64_t poseId, int sensorIdx, int sensorType,
526  int stateType, typename PARAMETER_BLOCK_T::estimate_t & state) const;
527 
528  // setters
529  template<class PARAMETER_BLOCK_T>
530  bool setGlobalStateEstimateAs(uint64_t poseId, int stateType,
531  const typename PARAMETER_BLOCK_T::estimate_t & state);
532  template<class PARAMETER_BLOCK_T>
533  bool setSensorStateEstimateAs(uint64_t poseId, int sensorIdx, int sensorType,
534  int stateType, const typename PARAMETER_BLOCK_T::estimate_t & state);
535 
536  // the following are just fixed-size containers for related parameterBlockIds:
537  typedef std::array<StateInfo, 6> GlobalStatesContainer;
538  typedef std::vector<StateInfo> SpecificSensorStatesContainer;
539  typedef std::array<std::vector<SpecificSensorStatesContainer>, 7> AllSensorStatesContainer;
540 
542  struct States
543  {
544  States() : isKeyframe(false), id(0) {}
546  : isKeyframe(isKeyframe), id(id), timestamp(timestamp) {}
550  uint64_t id;
552  };
553 
554  // the following keeps track of all the states at different time instances (key=poseId)
555  std::map<uint64_t, States> statesMap_;
556  std::map<uint64_t, okvis::MultiFramePtr> multiFramePtrMap_;
557  std::shared_ptr<okvis::ceres::Map> mapPtr_;
558 
559  // this is the reference pose
560  uint64_t referencePoseId_;
561 
562  // the following are updated after the optimization
564  mutable std::mutex statesMutex_;
565 
566  // parameters
568  Eigen::aligned_allocator<okvis::ExtrinsicsEstimationParameters> > extrinsicsEstimationParametersVec_;
569  std::vector<okvis::ImuParameters, Eigen::aligned_allocator<okvis::ImuParameters> > imuParametersVec_;
570 
571  // loss function for reprojection errors
572  std::shared_ptr< ::ceres::LossFunction> cauchyLossFunctionPtr_;
573  std::shared_ptr< ::ceres::LossFunction> huberLossFunctionPtr_;
574 
575  // the marginalized error term
576  std::shared_ptr<ceres::MarginalizationError> marginalizationErrorPtr_;
577  ::ceres::ResidualBlockId marginalizationResidualId_;
578 
579  // ceres iteration callback object
580  std::unique_ptr<okvis::ceres::CeresIterationCallback> ceresCallback_;
581 };
582 
583 } // namespace okvis
584 
586 
587 #endif /* INCLUDE_OKVIS_ESTIMATOR_HPP_ */
Pose.
Definition: Estimator.hpp:444
void clearCameras()
Remove all cameras from the configuration.
Definition: Estimator.cpp:100
The estimator class.
Definition: Estimator.hpp:77
StateInfo(uint64_t id=0, bool isRequired=true, bool exists=false)
Constructor.
Definition: Estimator.hpp:430
bool setCameraSensorStates(uint64_t poseId, size_t cameraIdx, const okvis::kinematics::Transformation &T_SCi)
Set the transformation from sensor to camera frame for a given pose ID.
Definition: Estimator.cpp:1067
bool setLandmark(uint64_t landmarkId, const Eigen::Vector4d &landmark)
Set the homogeneous coordinates for a landmark.
Definition: Estimator.cpp:1076
uint64_t frameIdByAge(size_t age) const
Get the ID of an older frame.
Definition: Estimator.cpp:1022
std::map< uint64_t, States > statesMap_
Buffer for currently considered states.
Definition: Estimator.hpp:555
std::shared_ptr< ::ceres::LossFunction > huberLossFunctionPtr_
Huber loss.
Definition: Estimator.hpp:573
std::shared_ptr< okvis::ceres::Map > mapPtr_
The underlying okvis::Map.
Definition: Estimator.hpp:557
Struct to define the behavior of the camera extrinsics.
Definition: Parameters.hpp:60
Eigen::Matrix< double, 1, 1 > Qff
Definition: Variables.hpp:59
std::mutex statesMutex_
Regulate access of landmarksMap_.
Definition: Estimator.hpp:564
States()
Definition: Estimator.hpp:544
A type to store information about a point in the world map.
Definition: FrameTypedefs.hpp:139
Camera.
Definition: Estimator.hpp:453
bool getGlobalStateParameterBlockPtr(uint64_t poseId, int stateType, std::shared_ptr< ceres::ParameterBlock > &stateParameterBlockPtr) const
Definition: Estimator.cpp:1110
bool getSensorStateParameterBlockAs(uint64_t poseId, int sensorIdx, int sensorType, int stateType, PARAMETER_BLOCK_T &stateParameterBlock) const
Definition: Estimator.cpp:1192
std::vector< okvis::ExtrinsicsEstimationParameters, Eigen::aligned_allocator< okvis::ExtrinsicsEstimationParameters > > extrinsicsEstimationParametersVec_
Extrinsics parameters.
Definition: Estimator.hpp:568
std::vector< StateInfo > SpecificSensorStatesContainer
Container for sensor states. The dimension can vary from sensor to sensor...
Definition: Estimator.hpp:538
std::array< std::vector< SpecificSensorStatesContainer >, 7 > AllSensorStatesContainer
Union of all sensor states.
Definition: Estimator.hpp:539
AllSensorStatesContainer sensors
Definition: Estimator.hpp:548
StaticPressureSensorStates
GpsSensorStates, currently unused.
Definition: Estimator.hpp:496
Magnetometer z-bias, currently unused.
Definition: Estimator.hpp:445
void setLandmarkInitialized(uint64_t landmarkId, bool initialized)
Set the landmark initialization state.
Definition: Estimator.cpp:1100
std::deque< ImuMeasurement, Eigen::aligned_allocator< ImuMeasurement > > ImuMeasurementDeque
Definition: Measurements.hpp:163
GpsSensorStates
GpsSensorStates, currently unused.
Definition: Estimator.hpp:484
uint64_t id
Definition: Estimator.hpp:550
Static pressure, currently unused.
Definition: Estimator.hpp:458
Header file for the CeresIterationCallback class. Used to enforce a time limit on the ceres optimizat...
Header file for the PoseParameterBlock class.
Header file for the MultiFrame class.
Header file for the Map class. This essentially encapsulates the ceres::Problem.
bool isLandmarkInitialized(uint64_t landmarkId) const
Checks whether the landmark is initialized.
Definition: Estimator.cpp:946
bool getSensorStateParameterBlockPtr(uint64_t poseId, int sensorIdx, int sensorType, int stateType, std::shared_ptr< ceres::ParameterBlock > &stateParameterBlockPtr) const
Definition: Estimator.cpp:1171
DynamicPressureSensorStates
GpsSensorStates, currently unused.
Definition: Estimator.hpp:502
Dynamic pressure, currently unused.
Definition: Estimator.hpp:459
position sensor frame to world, currently unused
Definition: Estimator.hpp:479
bool setSensorStateEstimateAs(uint64_t poseId, int sensorIdx, int sensorType, int stateType, const typename PARAMETER_BLOCK_T::estimate_t &state)
Definition: Estimator.cpp:1269
okvis::PointMap landmarksMap_
Contains all the current landmarks (synched after optimisation).
Definition: Estimator.hpp:563
Eigen::Matrix< double, 9, 1 > SpeedAndBias
Definition: FrameTypedefs.hpp:234
PositionSensorStates
PositionSensorStates, currently unused.
Definition: Estimator.hpp:477
uint64_t currentKeyframeId() const
Get the ID of the current keyframe.
Definition: Estimator.cpp:1010
bool setOptimizationTimeLimit(double timeLimit, int minIterations)
Set a time limit for the optimization process.
Definition: Estimator.cpp:909
std::shared_ptr< MultiFrame > MultiFramePtr
For convenience.
Definition: MultiFrame.hpp:272
This file contains the templated measurement structs, structs encapsulating Sensor data and related t...
void setMap(std::shared_ptr< okvis::ceres::Map > mapPtr)
set ceres map
Definition: Estimator.hpp:409
Position, currently unused.
Definition: Estimator.hpp:455
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
Header implementation file for the Estimator class.
okvis::Time timestamp
Definition: Estimator.hpp:551
int addImu(const okvis::ImuParameters &imuParameters)
Add an IMU to the configuration.
Definition: Estimator.cpp:89
void setKeyframe(uint64_t frameId, bool isKeyframe)
Set whether a frame is a keyframe or not.
Definition: Estimator.hpp:403
std::map< uint64_t, okvis::MultiFramePtr > multiFramePtrMap_
remember all needed okvis::MultiFrame.
Definition: Estimator.hpp:556
bool setSpeedAndBias(uint64_t poseId, size_t imuIdx, const okvis::SpeedAndBias &speedAndBias)
Set the speeds and IMU biases for a given pose ID.
Definition: Estimator.cpp:1060
Header file for the ReprojectionError class.
States This summarizes all the possible states – i.e. their ids:
Definition: Estimator.hpp:542
bool removeObservation(uint64_t landmarkId, uint64_t poseId, size_t camIdx, size_t keypointIdx)
Remove an observation from a landmark, if available.
Definition: Estimator.cpp:388
std::vector< MapPoint, Eigen::aligned_allocator< MapPoint > > MapPointVector
Definition: FrameTypedefs.hpp:172
Header file for the HomogeneousPointParameterBlock class.
bool getSpeedAndBias(uint64_t poseId, uint64_t imuIdx, okvis::SpeedAndBias &speedAndBias) const
Get speeds and IMU biases for a given pose ID.
Definition: Estimator.cpp:989
bool isInImuWindow(uint64_t frameId) const
Checks if a particular frame is still in the IMU window.
Definition: Estimator.cpp:1039
antenna offset, currently unused
Definition: Estimator.hpp:480
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
Magnetometer, currently unused.
Definition: Estimator.hpp:457
::ceres::ResidualBlockId marginalizationResidualId_
Remembers the marginalisation object's Id.
Definition: Estimator.hpp:577
bool setGlobalStateEstimateAs(uint64_t poseId, int stateType, const typename PARAMETER_BLOCK_T::estimate_t &state)
Definition: Estimator.cpp:1234
Extrinsics as T_SC.
Definition: Estimator.hpp:465
okvis::MultiFramePtr multiFrame(uint64_t frameId) const
Get a multiframe.
Definition: Estimator.hpp:263
bool addLandmark(uint64_t landmarkId, const Eigen::Vector4d &landmark)
Add a landmark.
Definition: Estimator.cpp:346
void optimize(size_t numIter, size_t numThreads=1, bool verbose=false)
Start ceres optimization.
Definition: Estimator.cpp:847
This file contains some useful assert macros.
bool isLandmarkAdded(uint64_t landmarkId) const
Checks whether the landmark is added to the estimator.
Definition: Estimator.hpp:218
bool isRequired
Whether or not we require the state.
Definition: Estimator.hpp:437
GPS, currently unused.
Definition: Estimator.hpp:456
IMU.
Definition: Estimator.hpp:454
bool applyMarginalizationStrategy(size_t numKeyframes, size_t numImuFrames, okvis::MapPointVector &removedLandmarks)
Applies the dropping/marginalization strategy according to the RSS'13/IJRR'14 paper. The new number of frames in the window will be numKeyframes+numImuFrames.
Definition: Estimator.cpp:434
Header file for the SpeedAndBiasParameterBlock class.
SensorStates
SensorStates The sensor-internal states enumerated.
Definition: Estimator.hpp:451
static bool initPoseFromImu(const okvis::ImuMeasurementDeque &imuMeasurements, okvis::kinematics::Transformation &T_WS)
Initialise pose from IMU measurements. For convenience as static.
Definition: Estimator.cpp:811
size_t getLandmarks(okvis::PointMap &landmarks) const
Get a copy of all the landmarks as a PointMap.
Definition: Estimator.cpp:954
MagnetometerSensorStates
MagnetometerSensorStates, currently unused.
Definition: Estimator.hpp:490
bool getSensorStateEstimateAs(uint64_t poseId, int sensorIdx, int sensorType, int stateType, typename PARAMETER_BLOCK_T::estimate_t &state) const
Definition: Estimator.cpp:1220
std::vector< okvis::ImuParameters, Eigen::aligned_allocator< okvis::ImuParameters > > imuParametersVec_
IMU parameters.
Definition: Estimator.hpp:569
ImuSensorStates
ImuSensorStates The IMU-internal states enumerated.
Definition: Estimator.hpp:471
void clearImus()
Remove all IMUs from the configuration.
Definition: Estimator.cpp:105
Alignment of global frame, currently unused.
Definition: Estimator.hpp:447
virtual ~Estimator()
Definition: Estimator.cpp:76
This file contains some typedefs related to state variables.
uint64_t currentFrameId() const
Get the ID of the newest frame added to the state.
Definition: Estimator.cpp:1033
bool getLandmark(uint64_t landmarkId, okvis::MapPoint &mapPoint) const
Get a specific landmark.
Definition: Estimator.cpp:933
bool addStates(okvis::MultiFramePtr multiFrame, const okvis::ImuMeasurementDeque &imuMeasurements, bool asKeyframe)
Add a pose to the state.
Definition: Estimator.cpp:110
bool exists
Whether or not this exists in the ceres problem.
Definition: Estimator.hpp:438
bool getGlobalStateParameterBlockAs(uint64_t poseId, int stateType, PARAMETER_BLOCK_T &stateParameterBlock) const
Definition: Estimator.cpp:1131
void printStates(uint64_t poseId, std::ostream &buffer) const
Prints state information to buffer.
Definition: Estimator.cpp:776
currently unused
Definition: Estimator.hpp:498
currently unused
Definition: Estimator.hpp:504
GlobalStates
GlobalStates The global states enumerated.
Definition: Estimator.hpp:442
Intrinsics.
Definition: Estimator.hpp:466
IMU parameters.
Definition: Parameters.hpp:105
Header file for the Transformation class.
size_t numFrames() const
Get the number of states/frames in the estimator.
Definition: Estimator.hpp:302
std::map< uint64_t, MapPoint, std::less< uint64_t >, Eigen::aligned_allocator< MapPoint > > PointMap
Definition: FrameTypedefs.hpp:174
CameraSensorStates
CameraSensorStates The camera-internal states enumerated.
Definition: Estimator.hpp:463
uint64_t referencePoseId_
The pose ID of the reference (currently not changing)
Definition: Estimator.hpp:560
GlobalStatesContainer global
Definition: Estimator.hpp:547
bool set_T_WS(uint64_t poseId, const okvis::kinematics::Transformation &T_WS)
Set pose for a given pose ID.
Definition: Estimator.cpp:1047
Time representation. May either represent wall clock time or ROS clock time.
Definition: Time.hpp:187
::ceres::ResidualBlockId addObservation(uint64_t landmarkId, uint64_t poseId, size_t camIdx, size_t keypointIdx)
Add an observation to a landmark.
int addCamera(const okvis::ExtrinsicsEstimationParameters &extrinsicsEstimationParameters)
Add a camera to the configuration. Sensors can only be added and never removed.
Definition: Estimator.cpp:81
std::array< StateInfo, 6 > GlobalStatesContainer
Container for global states.
Definition: Estimator.hpp:537
bool getCameraSensorStates(uint64_t poseId, size_t cameraIdx, okvis::kinematics::Transformation &T_SCi) const
Get camera states for a given pose ID.
Definition: Estimator.cpp:1001
uint64_t id
The ID.
Definition: Estimator.hpp:436
bool isKeyframe(uint64_t frameId) const
Checks if a particular frame is a keyframe.
Definition: Estimator.hpp:334
bool get_T_WS(uint64_t poseId, okvis::kinematics::Transformation &T_WS) const
Get pose for a given pose ID.
Definition: Estimator.cpp:975
Header file for the VioBackendInterface class.
StateInfo This configures the state vector ordering.
Definition: Estimator.hpp:424
States(bool isKeyframe, uint64_t id, okvis::Time timestamp)
Definition: Estimator.hpp:545
std::shared_ptr< ::ceres::LossFunction > cauchyLossFunctionPtr_
Cauchy loss.
Definition: Estimator.hpp:572
#define OKVIS_ASSERT_TRUE_DBG(exceptionType, condition, message)
Definition: assert_macros.hpp:211
std::shared_ptr< ceres::MarginalizationError > marginalizationErrorPtr_
The marginalisation class.
Definition: Estimator.hpp:576
antenna offset, currently unused
Definition: Estimator.hpp:486
bool isKeyframe
Definition: Estimator.hpp:549
size_t numLandmarks() const
Get the number of landmarks in the estimator.
Definition: Estimator.hpp:308
Header file for the MarginalizationError class.
bool getGlobalStateEstimateAs(uint64_t poseId, int stateType, typename PARAMETER_BLOCK_T::estimate_t &state) const
Definition: Estimator.cpp:1159
An abstract interface for backends.
Definition: VioBackendInterface.hpp:67
okvis::Time timestamp(uint64_t frameId) const
Get the timestamp for a particular frame.
Definition: Estimator.hpp:352
Eigen::Matrix< double, 3, 1 > MagnetometerBias
Definition: Variables.hpp:55
This file contains useful typedefs and structs related to frames.
std::unique_ptr< okvis::ceres::CeresIterationCallback > ceresCallback_
Maybe there was a callback registered, store it here.
Definition: Estimator.hpp:580
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Estimator()
The default constructor.
Definition: Estimator.cpp:67