| ARX 1.0 The next-generation open source augmented reality toolkit.

Loading...
Searching...
No Matches
mapper.hpp
Go to the documentation of this file.
1/*
2 * mapper.hpp
3 * arx_mapper
4 *
5 * Marker mapping using artoolkitX and GTSAM
6 *
7 * Copyright 2018 Eden Networks Ltd.
8 *
9 * Author(s): Philip Lamb.
10 *
11 */
12
13#ifndef __MAPPER_H__
14#define __MAPPER_H__
15
16// Each variable in the system (poses and landmarks) must be identified with a unique key.
17// We will use symbols (X1, X2, L1).
18#include <gtsam/inference/Symbol.h>
19// Camera observations of landmarks will be stored as Pose3.
20#include <gtsam/geometry/Pose3.h>
21// Use iSAM2 to solve the structure-from-motion problem incrementally.
22#include <gtsam/nonlinear/ISAM2.h>
23// iSAM2 requires as input a set set of new factors to be added stored in a factor graph,
24// and initial guesses for any new variables used in the added factors
25#include <gtsam/nonlinear/NonlinearFactorGraph.h>
26// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
27// nonlinear functions around an initial linearization point, then solve the linear system
28// to update the linearization point. This happens repeatedly until the solver converges
29// to a consistent set of variable values. This requires us to specify an initial guess
30// for each variable, held in a Values container.
31#include <gtsam/nonlinear/Values.h>
32
33// In GTSAM, measurement functions are represented as 'factors'.
34// Use Between factors to model the camera's landmark observations.
35#include <gtsam/slam/BetweenFactor.h>
36
37// Map is stored as an ARMultiMarkerInfoT structure.
38#include <ARX/AR/ar.h>
39#include <ARX/AR/arMulti.h>
40
41 namespace arx_mapper {
42
43 struct Marker {
44 int uid;
45 ARdouble trans[3][4]; // Marker pose in camera coordinates.
46 };
47
48 class Mapper {
49 public:
50 static int pose_cnt;
51
52 Mapper(double relinearize_thresh, int relinearize_skip);
53
54 void AddPose(const ARdouble trans[3][4]); // Map pose in camera coordinates.
55 void AddFactors(const std::vector<Marker>& markers);
56
57 bool inited() const { return inited_; }
58 void Optimize(int num_iterations = 1);
59 void Update(ARMultiMarkerInfoT* map) const;
60 void AddLandmarks(const std::vector<Marker>& markers);
61 void Initialize(int uid, ARdouble width);
62 void Clear();
63
64 private:
65 void AddLandmark(int uid, const gtsam::Pose3& pose);
66
67 ARdouble width_;
68 bool inited_;
69 gtsam::ISAM2Params params_;
70 gtsam::ISAM2 isam2_;
71 gtsam::NonlinearFactorGraph graph_;
72 gtsam::Values initial_estimates_;
73 gtsam::Pose3 pose_;
74 gtsam::noiseModel::Diagonal::shared_ptr marker_noise_;
75 gtsam::noiseModel::Diagonal::shared_ptr small_noise_;
76 std::set<int> all_uids_;
77 };
78
79 gtsam::Pose3 PoseFromARTrans(const ARdouble trans[3][4]);
80 void ARTransFromPose(ARdouble trans[3][4], const gtsam::Pose3 &pose);
81
82} // namespace arx_mapper
83
84#endif // __MAPPER_H__
85
ar.h
artoolkitX core routines.
ARdouble
double ARdouble
Definition: ar.h:99
arMulti.h
arx_mapper::Mapper
Definition: mapper.hpp:48
arx_mapper::Mapper::Optimize
void Optimize(int num_iterations=1)
arx_mapper::Mapper::AddLandmarks
void AddLandmarks(const std::vector< Marker > &markers)
arx_mapper::Mapper::Clear
void Clear()
arx_mapper::Mapper::Update
void Update(ARMultiMarkerInfoT *map) const
arx_mapper::Mapper::pose_cnt
static int pose_cnt
Definition: mapper.hpp:50
arx_mapper::Mapper::AddFactors
void AddFactors(const std::vector< Marker > &markers)
arx_mapper::Mapper::Mapper
Mapper(double relinearize_thresh, int relinearize_skip)
arx_mapper::Mapper::Initialize
void Initialize(int uid, ARdouble width)
arx_mapper::Mapper::AddPose
void AddPose(const ARdouble trans[3][4])
arx_mapper::Mapper::inited
bool inited() const
Definition: mapper.hpp:57
arx_mapper
Definition: mapper.hpp:41
arx_mapper::PoseFromARTrans
gtsam::Pose3 PoseFromARTrans(const ARdouble trans[3][4])
arx_mapper::ARTransFromPose
void ARTransFromPose(ARdouble trans[3][4], const gtsam::Pose3 &pose)
ARMultiMarkerInfoT
Definition: arMulti.h:85
arx_mapper::Marker
Definition: mapper.hpp:43
arx_mapper::Marker::trans
ARdouble trans[3][4]
Definition: mapper.hpp:45
arx_mapper::Marker::uid
int uid
Definition: mapper.hpp:44