├── examples ├── 0.gif ├── 1.gif ├── 2.gif └── 4.gif ├── README.md ├── LICENSE ├── sim.py ├── FastMarginals.h ├── FastMarginals.cpp ├── slam.py ├── da.h └── gtsam.h /examples/0.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jinkunw/mhjcbb/HEAD/examples/0.gif -------------------------------------------------------------------------------- /examples/1.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jinkunw/mhjcbb/HEAD/examples/1.gif -------------------------------------------------------------------------------- /examples/2.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jinkunw/mhjcbb/HEAD/examples/2.gif -------------------------------------------------------------------------------- /examples/4.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jinkunw/mhjcbb/HEAD/examples/4.gif -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MHJCBB 2 | 3 | Implementation of multiple hypothesis joint compatibility branch and bound (MHJCBB) in [Robust Exploration with Multiple Hypothesis Data Association](http://personal.stevens.edu/~benglot/Wang_Englot_IROS_2018_Preprint.pdf). 4 | 5 | ![](./examples/0.gif) ![](./examples/1.gif) 6 | ![](./examples/2.gif) ![](./examples/4.gif) 7 | 8 | ## Installation 9 | 10 | Install [da branch](https://bitbucket.org/jinkunw/gtsam/src/f4684fa5798494f46792c8aa829c16bf21ecbac5?at=da) in GTSAM. This will also build [cython wrapper](https://bitbucket.org/jinkunw/gtsam/src/f4684fa5798494f46792c8aa829c16bf21ecbac5/cython/?at=da) of GTSAM, which includes `da_JCBB2` and `da_MHJCBB2`. Modify `PYTHONPATH` to include the GTSAM_CYTHON_INSTALL_PATH (usually `/usr/local/cython`), 11 | ``` 12 | export PYTHONPATH=$PYTHONPATH: 13 | ``` 14 | 15 | ## Usage 16 | 17 | See [slam.py](./slam.py) for detailed usage. 18 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2018, Jinkun Wang 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /sim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import gtsam 3 | 4 | 5 | class Simulator(object): 6 | def __init__(self, seed=0): 7 | self.sigma_x = 0.1 8 | self.sigma_y = 0.1 9 | self.sigma_theta = 0.05 10 | self.sigma_bearing = 0.05 11 | self.sigma_range = 0.1 12 | 13 | self.max_range = 5.0 14 | self.max_bearing = np.pi / 3.0 15 | self.seed = seed 16 | self.random_state = np.random.RandomState(self.seed) 17 | 18 | # Define env and traj here 19 | self.env = {} # l -> gtsam.Point2 20 | self.traj = [] # gtsam.Pose2 21 | 22 | def reset(self): 23 | self.random_state = np.random.RandomState(self.seed) 24 | 25 | def random_map(self, size, limit): 26 | """ 27 | size: num of landmarks 28 | limit: l, r, b, t 29 | """ 30 | self.env = {} 31 | l, r, b, t = limit 32 | for i in range(size): 33 | x = self.random_state.uniform(l, r) 34 | y = self.random_state.uniform(b, t) 35 | self.env[i] = gtsam.Point2(x, y) 36 | 37 | def step(self): 38 | """ 39 | return: 40 | odom: odom between two poses (initial pose is returned for the first step) 41 | obs: dict of (landmark -> (bearing, range)) 42 | """ 43 | for i in range(len(self.traj)): 44 | if i == 0: 45 | odom = gtsam.Pose2() 46 | else: 47 | odom = self.traj[i - 1].between(self.traj[i]) 48 | nx = self.random_state.normal(0.0, self.sigma_x) 49 | ny = self.random_state.normal(0.0, self.sigma_y) 50 | nt = self.random_state.normal(0.0, self.sigma_theta) 51 | odom = odom.compose(gtsam.Pose2(nx, ny, nt)) 52 | 53 | obs = {} 54 | for l, point in self.env.items(): 55 | b = self.traj[i].bearing(point).theta() 56 | r = self.traj[i].range(point) 57 | b += self.random_state.normal(0.0, self.sigma_bearing) 58 | r += self.random_state.normal(0.0, self.sigma_range) 59 | 60 | if 0 < r < self.max_range and abs(b) < self.max_bearing: 61 | obs[l] = b, r 62 | 63 | if i == 0: 64 | yield self.traj[0].compose(odom), obs 65 | else: 66 | yield odom, obs 67 | -------------------------------------------------------------------------------- /FastMarginals.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace gtsam { 14 | 15 | /* 16 | * Implementation of the covariance recovery algorithm in 17 | * Covariance Recovery from a Square Root Information Matrix for Data Association 18 | * See code in iSAM for details 19 | * http://people.csail.mit.edu/kaess/isam/ 20 | */ 21 | struct SparseBlockVector { 22 | std::map vector; 23 | 24 | void insert(Key i, const Matrix &block) { 25 | vector[i] = block; 26 | } 27 | 28 | std::map::const_iterator begin() const { return vector.begin(); }; 29 | std::map::const_iterator end() const { return vector.end(); }; 30 | 31 | SparseBlockVector() {} 32 | }; 33 | 34 | std::ostream &operator<<(std::ostream &os, const SparseBlockVector &vector); 35 | 36 | typedef std::pair KeyPair; 37 | std::size_t hash_value(const KeyPair &key_pair); 38 | 39 | struct CovarianceCache { 40 | boost::unordered_map entries; 41 | std::unordered_map diag; 42 | std::unordered_map rows; 43 | 44 | CovarianceCache() {} 45 | }; 46 | 47 | class FastMarginals { 48 | public: 49 | FastMarginals(const ISAM2 &isam2) : isam2_(isam2) { 50 | initialize(); 51 | } 52 | 53 | Matrix marginalCovariance(const Key &variable); 54 | 55 | Matrix jointMarginalCovariance(const KeyVector &variables); 56 | 57 | protected: 58 | void initialize(); 59 | 60 | Matrix getRBlock(const Key &key_i, const Key &key_j); 61 | 62 | const SparseBlockVector &getRRow(const Key &key); 63 | 64 | Matrix getR(const std::vector &variables); 65 | 66 | Matrix getKeyDiag(const Key &key); 67 | 68 | size_t getKeyDim(const Key &key); 69 | 70 | Matrix sumJ(const Key key_l, const Key key_i); 71 | 72 | Matrix recover(const Key &key_i, const Key &key_l); 73 | 74 | const ISAM2 &isam2_; 75 | std::vector ordering_; 76 | boost::unordered_map key_idx_; 77 | CovarianceCache cov_cache_; 78 | std::unordered_map Fs_; 79 | std::unordered_map F_; 80 | Key last_key_; 81 | size_t size0_; 82 | }; 83 | 84 | } // namespace gtsam 85 | -------------------------------------------------------------------------------- /FastMarginals.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace gtsam { 5 | std::ostream &operator<<(std::ostream &os, const SparseBlockVector &vector) { 6 | for (auto it = vector.begin(); it != vector.end(); ++it) 7 | os << it->first << std::endl 8 | << it->second << std::endl; 9 | return os; 10 | } 11 | 12 | std::size_t hash_value(const KeyPair &key_pair) { 13 | std::size_t seed = 0; 14 | boost::hash_combine(seed, key_pair.first); 15 | boost::hash_combine(seed, key_pair.second); 16 | return seed; 17 | } 18 | 19 | Matrix FastMarginals::marginalCovariance(const Key &variable) { 20 | return recover(variable, variable); 21 | } 22 | 23 | Matrix FastMarginals::jointMarginalCovariance(const KeyVector &variables) { 24 | size_t dim = 0; 25 | std::vector variable_acc_dim; 26 | for (Key key : variables) { 27 | variable_acc_dim.push_back(dim); 28 | dim += getKeyDim(key); 29 | } 30 | 31 | std::vector variable_idx(variables.size()); 32 | std::iota(variable_idx.begin(), variable_idx.end(), 0); 33 | std::sort(variable_idx.begin(), variable_idx.end(), 34 | [this, &variables](int i, int j) { 35 | return key_idx_[variables[i]] < key_idx_[variables[j]]; 36 | }); 37 | 38 | Matrix cov = Matrix::Zero(dim, dim); 39 | for (int j = variable_idx.size() - 1; j >= 0; --j) { 40 | Key key_j = variables[variable_idx[j]]; 41 | size_t col = variable_acc_dim[variable_idx[j]]; 42 | 43 | for (int i = j; i >= 0; --i) { 44 | Key key_i = variables[variable_idx[i]]; 45 | size_t row = variable_acc_dim[variable_idx[i]]; 46 | 47 | if (row > col) { 48 | cov.block(col, row, getKeyDim(key_j), getKeyDim(key_i)) = recover(key_i, key_j).transpose(); 49 | } else { 50 | cov.block(row, col, getKeyDim(key_i), getKeyDim(key_j)) = recover(key_i, key_j); 51 | } 52 | } 53 | } 54 | cov.triangularView() = cov.transpose(); 55 | return cov; 56 | } 57 | 58 | Matrix FastMarginals::getRBlock(const Key &key_i, const Key &key_j) { 59 | ISAM2Clique::shared_ptr clique = isam2_[key_i]; 60 | const ISAM2Clique::sharedConditional conditional = clique->conditional(); 61 | if (conditional->find(key_j) == conditional->end()) 62 | return Matrix(); 63 | 64 | size_t block_row = conditional->find(key_i) - conditional->begin(); 65 | size_t block_col = conditional->find(key_j) - conditional->begin(); 66 | const auto &m = conditional->matrixObject(); 67 | DenseIndex row = m.offset(block_row); 68 | DenseIndex col = m.offset(block_col); 69 | return m.matrix().block(row, col, getKeyDim(key_i), getKeyDim(key_j)); 70 | } 71 | 72 | const SparseBlockVector &FastMarginals::getRRow(const Key &key) { 73 | const auto &it = cov_cache_.rows.find(key); 74 | if (it == cov_cache_.rows.cend()) { 75 | auto ret = cov_cache_.rows.insert(std::make_pair(key, SparseBlockVector())); 76 | bool started = false; 77 | for (Key key_i : ordering_) { 78 | if (key_i == key) 79 | started = true; 80 | if (!started) 81 | continue; 82 | 83 | Matrix block = getRBlock(key, key_i); 84 | if (block.size() > 0) 85 | ret.first->second.insert(key_i, block); 86 | } 87 | return ret.first->second; 88 | } else 89 | return it->second; 90 | } 91 | 92 | Matrix FastMarginals::getR(const std::vector &variables) { 93 | size_t dim = 0; 94 | for (Key key : variables) 95 | dim += getKeyDim(key); 96 | 97 | Matrix R = Matrix::Zero(dim, dim); 98 | size_t row = 0; 99 | for (size_t i = 0; i < variables.size(); ++i) { 100 | Key key_i = variables[i]; 101 | size_t col = row; 102 | size_t dim_i = getKeyDim(key_i); 103 | for (size_t j = i; j < variables.size(); ++j) { 104 | Key key_j = variables[j]; 105 | size_t dim_j = getKeyDim(key_j); 106 | Matrix block = getRBlock(key_i, key_j); 107 | if (block.size() > 0) 108 | R.block(row, col, dim_i, dim_j) = block; 109 | col += dim_j; 110 | } 111 | row += dim_i; 112 | } 113 | return R; 114 | } 115 | 116 | size_t FastMarginals::getKeyDim(const Key &key) { 117 | return isam2_.getLinearizationPoint().at(key).dim(); 118 | } 119 | 120 | Matrix FastMarginals::getKeyDiag(const Key &key) { 121 | auto it = cov_cache_.diag.find(key); 122 | if (it == cov_cache_.diag.end()) { 123 | auto ret = cov_cache_.diag.insert(std::make_pair(key, getRBlock(key, key).inverse())); 124 | return ret.first->second; 125 | } else 126 | return it->second; 127 | } 128 | 129 | void FastMarginals::initialize() { 130 | std::queue q; 131 | assert(isam2_.roots().size() == 1); 132 | q.push(isam2_.roots()[0]); 133 | while (!q.empty()) { 134 | ISAM2Clique::shared_ptr c = q.front(); 135 | q.pop(); 136 | std::vector sub; 137 | assert(c->conditional() != nullptr); 138 | for (Key key : c->conditional()->frontals()) { 139 | sub.push_back(key); 140 | } 141 | ordering_.insert(ordering_.begin(), sub.begin(), sub.end()); 142 | for (auto child : c->children) 143 | q.push(child); 144 | } 145 | 146 | for (size_t i = 0; i < ordering_.size(); ++i) { 147 | Key key = ordering_[i]; 148 | key_idx_[key] = i; 149 | } 150 | } 151 | 152 | Matrix FastMarginals::sumJ(const Key key_l, const Key key_i) { 153 | Matrix sum = Matrix::Zero(getKeyDim(key_i), getKeyDim(key_l)); 154 | const SparseBlockVector &Ri = getRRow(key_i); 155 | 156 | size_t idx_l = key_idx_[key_l]; 157 | size_t idx_i = key_idx_[key_i]; 158 | for (auto it = Ri.begin(); it != Ri.end(); ++it) { 159 | Key key_j = it->first; 160 | size_t idx_j = key_idx_[key_j]; 161 | if (idx_j > idx_i) { 162 | sum += it->second * (idx_j > idx_l ? recover(key_l, key_j).transpose() : recover(key_j, key_l)); 163 | } 164 | } 165 | return sum; 166 | } 167 | 168 | Matrix FastMarginals::recover(const Key &key_i, const Key &key_l) { 169 | KeyPair key_pair = std::make_pair(key_i, key_l); 170 | auto entry_iter = cov_cache_.entries.find(key_pair); 171 | if (entry_iter == cov_cache_.entries.end()) { 172 | Matrix res; 173 | if (key_i == key_l) 174 | res = getKeyDiag(key_l) * (getKeyDiag(key_l).transpose() - sumJ(key_l, key_l)); 175 | else 176 | res = -getKeyDiag(key_i) * sumJ(key_l, key_i); 177 | 178 | cov_cache_.entries.insert(std::make_pair(key_pair, res)); 179 | return res; 180 | } else { 181 | return entry_iter->second; 182 | } 183 | } 184 | 185 | } // namespace gtsam 186 | -------------------------------------------------------------------------------- /slam.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import gtsam 3 | 4 | from sim import * 5 | 6 | 7 | def X(x): 8 | return gtsam.symbol(ord('x'), x) 9 | 10 | 11 | def L(l): 12 | return gtsam.symbol(ord('l'), l) 13 | 14 | 15 | def slam(sim, da='jcbb', prob=0.95): 16 | isam2 = gtsam.ISAM2() 17 | graph = gtsam.NonlinearFactorGraph() 18 | values = gtsam.Values() 19 | 20 | observed = set() 21 | for x, (odom, obs) in enumerate(sim.step()): 22 | if x == 0: 23 | prior_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) 24 | prior_factor = gtsam.PriorFactorPose2(X(0), odom, prior_model) 25 | graph.add(prior_factor) 26 | values.insert(X(0), odom) 27 | else: 28 | odom_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) 29 | odom_factor = gtsam.BetweenFactorPose2(X(x - 1), X(x), odom, odom_model) 30 | graph.add(odom_factor) 31 | pose0 = isam2.calculateEstimatePose2(X(x - 1)) 32 | values.insert(X(x), pose0.compose(odom)) 33 | 34 | isam2.update(graph, values) 35 | graph.resize(0) 36 | values.clear() 37 | estimate = isam2.calculateEstimate() 38 | 39 | if da == 'dr': 40 | for l_true, br in obs.items(): 41 | l = len(observed) 42 | br_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_bearing, sim.sigma_range])) 43 | br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) 44 | graph.add(br_factor) 45 | if l not in observed: 46 | pose1 = isam2.calculateEstimatePose2(X(x)) 47 | point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) 48 | values.insert(L(l), pose1.transform_from(point)) 49 | observed.add(l) 50 | elif da == 'perfect': 51 | for l_true, br in obs.items(): 52 | br_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_bearing, sim.sigma_range])) 53 | br_factor = gtsam.BearingRangeFactor2D(X(x), L(l_true), gtsam.Rot2(br[0]), br[1], br_model) 54 | graph.add(br_factor) 55 | if l_true not in observed: 56 | pose1 = isam2.calculateEstimatePose2(X(x)) 57 | point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) 58 | values.insert(L(l_true), pose1.transform_from(point)) 59 | observed.add(l_true) 60 | elif da == 'jcbb': 61 | ################################################################################ 62 | jcbb = gtsam.da_JCBB2(isam2, prob) 63 | for l, br in obs.items(): 64 | br_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_bearing, sim.sigma_range])) 65 | jcbb.add(gtsam.Rot2(br[0]), br[1], br_model) 66 | 67 | keys = jcbb.match() 68 | ################################################################################ 69 | 70 | keys = [gtsam.symbolIndex(keys.at(i)) for i in range(keys.size())] 71 | for (l_true, br), l in zip(obs.items(), keys): 72 | br_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_bearing, sim.sigma_range])) 73 | br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) 74 | graph.add(br_factor) 75 | if l not in observed: 76 | pose1 = isam2.calculateEstimatePose2(X(x)) 77 | point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) 78 | values.insert(L(l), pose1.transform_from(point)) 79 | observed.add(l) 80 | 81 | isam2.update(graph, values) 82 | graph.resize(0) 83 | values.clear() 84 | 85 | traj_est = [isam2.calculateEstimatePose2(X(x)) for x in range(len(sim.traj))] 86 | traj_est = np.array([(p.x(), p.y(), p.theta()) for p in traj_est]) 87 | landmark_est = [isam2.calculateEstimatePoint2(L(l)) for l in observed] 88 | landmark_est = np.array([(p.x(), p.y()) for p in landmark_est]) 89 | return [[traj_est, landmark_est]] 90 | 91 | 92 | def prune1(slams, x, threshold): 93 | deleted = set() 94 | for i, (isam2_i, observed_i) in enumerate(slams): 95 | if i in deleted: 96 | continue 97 | pose_i = isam2_i.calculateEstimatePose2(X(x)) 98 | cov_i = isam2_i.marginalCovariance(X(x)) 99 | for j, (isam2_j, observed_j) in enumerate(slams): 100 | if j <= i or j in deleted: 101 | continue 102 | pose_j = isam2_j.calculateEstimatePose2(X(x)) 103 | 104 | e = pose_i.localCoordinates(pose_j).reshape(-1, 1) 105 | if e.T.dot(np.linalg.inv(cov_i)).dot(e) < threshold**2: 106 | if len(observed_i) < len(observed_j): 107 | deleted.add(j) 108 | else: 109 | deleted.add(i) 110 | break 111 | 112 | slams = [slams[i] for i in range(len(slams)) if i not in deleted] 113 | return sorted(slams, key=lambda x: x[1]) 114 | 115 | 116 | def prune2(slams, max_observed_diff=3): 117 | pruned = [slams[0]] 118 | for isam2, observed in slams[1:]: 119 | if len(observed) - len(slams[0][1]) < max_observed_diff: 120 | pruned.append([isam2, observed]) 121 | return pruned 122 | 123 | 124 | def mhjcbb(sim, num_tracks=10, prob=0.95, posterior_pose_md_threshold=1.5, prune2_skip=10, max_observed_diff=3): 125 | slams = [[gtsam.ISAM2(), set()]] 126 | 127 | prune2_count = 1 128 | observed = set() 129 | for x, (odom, obs) in enumerate(sim.step()): 130 | for isam2, observed in slams: 131 | graph = gtsam.NonlinearFactorGraph() 132 | values = gtsam.Values() 133 | if x == 0: 134 | prior_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) 135 | prior_factor = gtsam.PriorFactorPose2(X(0), odom, prior_model) 136 | graph.add(prior_factor) 137 | values.insert(X(0), odom) 138 | else: 139 | odom_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta])) 140 | odom_factor = gtsam.BetweenFactorPose2(X(x - 1), X(x), odom, odom_model) 141 | graph.add(odom_factor) 142 | pose0 = isam2.calculateEstimatePose2(X(x - 1)) 143 | values.insert(X(x), pose0.compose(odom)) 144 | 145 | isam2.update(graph, values) 146 | 147 | ################################################################################ 148 | mhjcbb = gtsam.da_MHJCBB2(num_tracks, prob, posterior_pose_md_threshold) 149 | for isam2, observed, in slams: 150 | mhjcbb.initialize(isam2) 151 | 152 | for l, br in obs.items(): 153 | br_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_bearing, sim.sigma_range])) 154 | mhjcbb.add(gtsam.Rot2(br[0]), br[1], br_model) 155 | 156 | mhjcbb.match() 157 | ################################################################################ 158 | 159 | new_slams = [] 160 | for i in range(mhjcbb.size()): 161 | track, keys = mhjcbb.get(i) 162 | keys = [gtsam.symbolIndex(keys.at(i)) for i in range(keys.size())] 163 | 164 | isam2 = gtsam.ISAM2() 165 | isam2.update(slams[track][0].getFactorsUnsafe(), slams[track][0].calculateEstimate()) 166 | graph = gtsam.NonlinearFactorGraph() 167 | values = gtsam.Values() 168 | observed = set(slams[track][1]) 169 | for (l_true, br), l in zip(obs.items(), keys): 170 | br_model = gtsam.noiseModel_Diagonal.Sigmas(np.array([sim.sigma_bearing, sim.sigma_range])) 171 | br_factor = gtsam.BearingRangeFactor2D(X(x), L(l), gtsam.Rot2(br[0]), br[1], br_model) 172 | 173 | graph.add(br_factor) 174 | if l not in observed: 175 | pose1 = isam2.calculateEstimatePose2(X(x)) 176 | point = gtsam.Point2(br[1] * np.cos(br[0]), br[1] * np.sin(br[0])) 177 | values.insert(L(l), pose1.transform_from(point)) 178 | observed.add(l) 179 | isam2.update(graph, values) 180 | new_slams.append([isam2, observed]) 181 | slams = new_slams 182 | slams = prune1(slams, x, posterior_pose_md_threshold) 183 | 184 | if len(slams[0][1]) > prune2_count * prune2_skip: 185 | slams = prune2(slams, max_observed_diff) 186 | prune2_count += 1 187 | 188 | result = [] 189 | for isam2, observed in slams: 190 | traj_est = [isam2.calculateEstimatePose2(X(x)) for x in range(len(sim.traj))] 191 | traj_est = np.array([(p.x(), p.y(), p.theta()) for p in traj_est]) 192 | landmark_est = [isam2.calculateEstimatePoint2(L(l)) for l in observed] 193 | landmark_est = np.array([(p.x(), p.y()) for p in landmark_est]) 194 | result.append((traj_est, landmark_est)) 195 | return result 196 | 197 | 198 | def plot_sim(ax, sim): 199 | traj = np.array([(p.x(), p.y(), p.theta()) for p in sim.traj]) 200 | landmark = np.array([(p.x(), p.y()) for p in sim.env.values()]) 201 | p1 = ax.scatter(landmark[:, 0], landmark[:, 1], marker='x', lw=2, s=100) 202 | (p2, ) = ax.plot(traj[:, 0], traj[:, 1], 'k-', lw=2) 203 | ax.set_aspect('equal') 204 | return p1, p2 205 | 206 | 207 | def plot_est(ax, result): 208 | for i, (traj_est, landmark_est) in enumerate(result): 209 | if i == 0: 210 | p1 = ax.scatter(landmark_est[:, 0], landmark_est[:, 1], marker='+', lw=2, alpha=0.5) 211 | alpha = 1.0 212 | else: 213 | alpha = 0.1 214 | (p2, ) = ax.plot(traj_est[:, 0], traj_est[:, 1], 'g-', lw=2, alpha=alpha) 215 | ax.set_aspect('equal') 216 | return p1, p2 217 | 218 | 219 | if __name__ == '__main__': 220 | import matplotlib.pyplot as plt 221 | 222 | # Design a simulator 223 | sim = Simulator() 224 | sim.random_map(50, (-10, 10, -10, 10)) 225 | pose = gtsam.Pose2(5, -5, np.pi / 2.0) 226 | for i in range(50): 227 | sim.traj.append(pose) 228 | if i % 10 == 0 and i > 0: 229 | u = 0.0, 1.0, np.pi / 2.0 230 | else: 231 | u = 1.0, 0.0, 0.0 232 | pose = pose.compose(gtsam.Pose2(*u)) 233 | 234 | fig, (ax1, ax2, ax3) = plt.subplots(1, 3, True, True, figsize=(8, 3)) 235 | 236 | # Perfect DA 237 | sim.reset() 238 | result = slam(sim, 'perfect') 239 | plot_sim(ax1, sim) 240 | plot_est(ax1, result) 241 | ax1.set_title('Perfect DA') 242 | ax1.set_xlim(-10, 10) 243 | ax1.set_ylim(-10, 10) 244 | 245 | # # JCBB 246 | sim.reset() 247 | result = slam(sim, 'jcbb') 248 | p1, p2 = plot_sim(ax2, sim) 249 | p3, p4 = plot_est(ax2, result) 250 | ax2.set_title('JCBB') 251 | ax2.set_xlim(-10, 10) 252 | ax2.set_ylim(-10, 10) 253 | 254 | # MHJCBB 255 | sim.reset() 256 | result = mhjcbb(sim) 257 | plot_sim(ax3, sim) 258 | plot_est(ax3, result) 259 | ax3.set_title('MHJCBB') 260 | ax3.set_xlim(-10, 10) 261 | ax3.set_ylim(-10, 10) 262 | 263 | fig.legend((p1, p2, p3, p4), ('Landmarks - True', 'Trajectory - True', 'Landmarks - SLAM', 'Trajectory - SLAM'), 264 | 'lower center', ncol=4, fontsize='medium') 265 | 266 | plt.tight_layout() 267 | plt.show() -------------------------------------------------------------------------------- /da.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #define USE_FAST_MARGINALS 8 | #ifdef USE_FAST_MARGINALS 9 | #include 10 | #endif 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | namespace gtsam { 21 | namespace da { 22 | 23 | using symbol_shorthand::L; 24 | using symbol_shorthand::X; 25 | 26 | /// Chi-square inverse cumulative distribution function 27 | double chi2inv(double P, unsigned int dim); 28 | 29 | struct Innovation { 30 | typedef std::shared_ptr shared_ptr; 31 | Key l; // landmark key 32 | Vector error; // residual error 33 | Matrix Hx; // Jacobian w.r.t. pose 34 | Matrix Hl; // Jacobian w.r.t. point 35 | Vector sigmas; // measurement noise 36 | double md; // Mahalanobis distance 37 | }; 38 | 39 | template ::result_type, 41 | typename RANGE = typename Range::result_type> 42 | class JCBB { 43 | public: 44 | /// Initialize JCBB with a complete factor graph and an estimate. 45 | /// Assume 46 | /// 1. Pose and landmark keys are represented by X(x) and L(l). 47 | /// 2. Pose keys are incremental from 0 to N. 48 | /// 3. The associated landmark keys are also incremental. 49 | JCBB(const ISAM2 &isam2, double prob) 50 | : values_(isam2.calculateEstimate()), prob_(prob), 51 | #ifdef USE_FAST_MARGINALS 52 | marginals_(isam2) { 53 | #else 54 | marginals_(isam2.getFactorsUnsafe(), values_) { 55 | #endif 56 | for (Key key : values_.keys()) 57 | if (symbolChr(key) == 'l') keys_.push_back(key); 58 | 59 | // Find the latest pose key 60 | for (int x = 0;; ++x) { 61 | if (!values_.exists(X(x))) { 62 | assert(x > 0); 63 | x0_ = X(x - 1); 64 | pose0_ = values_.at(x0_); 65 | break; 66 | } 67 | } 68 | } 69 | 70 | /// Add current measurement with noise model. 71 | /// Only diagonal noise model is supported. 72 | void add(BEARING measuredBearing, RANGE measuredRange, 73 | const SharedNoiseModel &model) { 74 | // Search landmark candidates that is likely to be independently compatible. 75 | innovations_.push_back({}); 76 | for (Key l : keys_) { 77 | POINT point = values_.at(l); 78 | BearingRangeFactor factor(x0_, l, measuredBearing, measuredRange, model); 79 | 80 | Innovation::shared_ptr inn(new Innovation); 81 | inn->l = l; 82 | inn->error = factor.evaluateError(pose0_, point, inn->Hx, inn->Hl); 83 | inn->sigmas = model->sigmas(); 84 | inn->md = model->distance(inn->error); 85 | 86 | if (jc_(inn)) { 87 | innovations_.back().push_back(inn); 88 | } 89 | } 90 | } 91 | 92 | /// Perform JCBB data association. 93 | /// Return landmark keys for measurements. 94 | KeyVector match() { 95 | KeyVector keys; 96 | keys.push_back(x0_); 97 | for (const std::vector &obs_inn : innovations_) { 98 | for (const Innovation::shared_ptr &inn : obs_inn) 99 | if (std::find(keys.begin(), keys.end(), inn->l) == keys.end()) 100 | keys.push_back(inn->l); 101 | } 102 | #ifndef USE_FAST_MARGINALS 103 | joint_marginals_ = marginals_.jointMarginalCovariance(keys); 104 | #endif 105 | 106 | // Sort candidate landmarks by its residual. 107 | // A heuristic to quickly obtain a lower bound. 108 | for (FastVector &linn : innovations_) 109 | std::sort(linn.begin(), linn.end(), 110 | [](Innovation::shared_ptr lhs, Innovation::shared_ptr rhs) { 111 | return lhs->md < rhs->md; 112 | }); 113 | 114 | // Recursive search in interpretation tree starting with an empty hypothesis. 115 | jcbb({}); 116 | 117 | // Map innovations to keys. 118 | KeyVector matched_keys; 119 | int new_l = 0; 120 | for (Innovation::shared_ptr &inn : best_hypothesis_) 121 | matched_keys.push_back(inn ? inn->l : keys_.size() + new_l++); 122 | return matched_keys; 123 | } 124 | 125 | private: 126 | void jcbb(const FastVector &hypothesis) { 127 | int k = hypothesis.size(); 128 | int h = pairings(hypothesis); 129 | if (k == innovations_.size()) { 130 | if (best_hypothesis_.empty() || h > pairings(best_hypothesis_)) 131 | best_hypothesis_ = hypothesis; 132 | return; 133 | } 134 | 135 | FastSet existing; 136 | for (const Innovation::shared_ptr &inn : hypothesis) 137 | if (inn) existing.insert(inn->l); 138 | 139 | for (Innovation::shared_ptr &inn : innovations_[k]) { 140 | // Make sure keys are used only once. 141 | if (existing.find(inn->l) != existing.end()) continue; 142 | 143 | // Get remaining keys if we associate k-th measurement with inn->l. 144 | FastSet remaining; 145 | for (int j = k + 1; j < innovations_.size(); ++j) { 146 | for (Innovation::shared_ptr &future_inn : innovations_[j]) { 147 | if (future_inn->l != inn->l && 148 | existing.find(future_inn->l) == existing.end()) 149 | remaining.insert(future_inn->l); 150 | } 151 | } 152 | 153 | // Calculate the max pairings (upper bound) we can achieve with this association. 154 | int max_remaining = 155 | std::min(remaining.size(), innovations_.size() - k - 1); 156 | // Stop searching if upper bound <= current lower bound. 157 | if (h + 1 + max_remaining <= pairings(best_hypothesis_)) continue; 158 | 159 | // Keep searching in interpretation tree if current hypothesis is JC. 160 | FastVector extended = hypothesis; 161 | extended.push_back(inn); 162 | if (jc(extended)) jcbb(extended); 163 | } 164 | 165 | // Same as above but with a null pairing. 166 | FastSet remaining; 167 | for (int j = k + 1; j < innovations_.size(); ++j) { 168 | for (Innovation::shared_ptr &future_inn : innovations_[j]) { 169 | if (existing.find(future_inn->l) == existing.end()) 170 | remaining.insert(future_inn->l); 171 | } 172 | } 173 | int max_remaining = std::min(remaining.size(), innovations_.size() - k - 1); 174 | if (best_hypothesis_.empty() || 175 | h + max_remaining > pairings(best_hypothesis_)) { 176 | FastVector extended = hypothesis; 177 | extended.push_back(nullptr); 178 | jcbb(extended); 179 | } 180 | } 181 | 182 | /// Calculate non-null pairings in the hypothesis. 183 | int pairings(const FastVector &hypothesis) const { 184 | return std::count_if( 185 | hypothesis.cbegin(), hypothesis.cend(), 186 | [](const Innovation::shared_ptr &inn) { return inn != nullptr; }); 187 | } 188 | 189 | /// Fast independent compatibility (IC) test. 190 | bool jc_(const Innovation::shared_ptr &inn) const { 191 | // Assume two variables are independent 192 | Matrix S = Matrix::Zero(5, 5); 193 | S.block<3, 3>(0, 0) = marginals_.marginalCovariance(x0_); 194 | S.block<2, 2>(3, 3) = marginals_.marginalCovariance(inn->l); 195 | 196 | Matrix H(2, 5); 197 | H.block<2, 3>(0, 0) = inn->Hx; 198 | H.block<2, 2>(0, 3) = inn->Hl; 199 | 200 | Matrix R = inn->sigmas.asDiagonal(); 201 | Vector e = inn->error; 202 | 203 | Matrix C = H * S * H.transpose() + R * R; 204 | double chi2 = e.transpose() * C.llt().solve(e); 205 | return chi2 < chi2inv(prob_, 2); 206 | } 207 | 208 | /// Joint compatibility (JC) test. 209 | bool jc(const FastVector &hypothesis) const { 210 | if (hypothesis.empty() || pairings(hypothesis) == 0) 211 | return true; 212 | 213 | // Calculate covariance from GTSAM joint marginals. 214 | // TODO: incremental update 215 | int XD = POSE::dimension, LD = POINT::dimension, 216 | FD = BearingRange::dimension; 217 | int N = XD, M = 0; 218 | KeyVector keys; 219 | keys.push_back(x0_); 220 | for (const Innovation::shared_ptr &inn : hypothesis) { 221 | if (!inn) continue; 222 | keys.push_back(inn->l); 223 | N += LD; 224 | M += FD; 225 | } 226 | 227 | Matrix S(N, N); 228 | #ifdef USE_FAST_MARGINALS 229 | S = marginals_.jointMarginalCovariance(keys); 230 | #else 231 | for (int i = 0, p = 0; i < keys.size(); ++i) { 232 | Key ki = keys[i]; 233 | int pi = values_.at(ki).dim(); 234 | for (int j = i, q = p; j < keys.size(); ++j) { 235 | Key kj = keys[j]; 236 | int qj = values_.at(kj).dim(); 237 | S.block(p, q, pi, qj) = joint_marginals_.at(ki, kj); 238 | q += qj; 239 | } 240 | p += pi; 241 | } 242 | S.triangularView() = S.transpose(); 243 | #endif 244 | 245 | Matrix H = Matrix::Zero(M, N); 246 | Matrix R = Matrix::Zero(M, M); 247 | Vector e(M); 248 | for (int i = 0, j = 0; i < hypothesis.size(); ++i) { 249 | if (!hypothesis[i]) continue; 250 | H.block(j * FD, 0, FD, XD) = hypothesis[i]->Hx; 251 | H.block(j * FD, XD + j * LD, FD, LD) = hypothesis[i]->Hl; 252 | R.block(j * FD, j * FD, FD, FD) = hypothesis[i]->sigmas.asDiagonal(); 253 | e.segment(j * FD, FD) = hypothesis[i]->error; 254 | j += 1; 255 | } 256 | 257 | Matrix C = H * S * H.transpose() + R * R; 258 | double chi2 = e.transpose() * C.llt().solve(e); 259 | return chi2 < chi2inv(prob_, M); 260 | } 261 | 262 | private: 263 | double prob_; 264 | 265 | FastVector> innovations_; 266 | FastVector best_hypothesis_; 267 | 268 | KeyVector keys_; 269 | Values values_; 270 | Key x0_; 271 | POSE pose0_; 272 | 273 | #ifdef USE_FAST_MARGINALS 274 | mutable FastMarginals marginals_; 275 | #else 276 | Marginals marginals_; 277 | JointMarginal joint_marginals_; 278 | #endif 279 | }; 280 | 281 | template ::result_type, 283 | typename RANGE = typename Range::result_type> 284 | class MHJCBB { 285 | private: 286 | /// Match information 287 | struct MatchInfo { 288 | int track; // track index 289 | std::vector hypothesis; // hypothesis 290 | int num_pairings; // current pairings 291 | double md; // chi-squared error 292 | POSE pose; // posterior pose after update 293 | Matrix covariance; // posterior covariance 294 | }; 295 | 296 | /// Compare two matching results. 297 | struct MatchInfoCmp { 298 | bool operator()(const MatchInfo &a, const MatchInfo &b) const { 299 | return (a.num_pairings > b.num_pairings) || 300 | ((a.num_pairings == b.num_pairings) && (a.md < b.md)); 301 | } 302 | }; 303 | 304 | /// Track information 305 | struct TrackInfo { 306 | int index; // track index 307 | FastVector> innovations; // candidates for each measurement 308 | KeyVector keys; // Same as members in JCBB 309 | #ifdef USE_FAST_MARGINALS 310 | mutable FastMarginals marginals; 311 | 312 | TrackInfo(const ISAM2 &isam2) : marginals(isam2) {} 313 | #else 314 | Marginals marginals; 315 | JointMarginal joint_marginals; 316 | #endif 317 | Values values; 318 | Key x0; 319 | POSE pose0; 320 | 321 | std::stack stack; // use stack to search in the interpretation tree 322 | }; 323 | 324 | public: 325 | /// MHJCBB 326 | MHJCBB(int max_tracks, 327 | double prob, 328 | double posterior_pose_md_threshold) // threshold used for screening match 329 | : max_tracks_(max_tracks), 330 | prob_(prob), 331 | posterior_pose_md_threshold_(posterior_pose_md_threshold) {} 332 | 333 | /// Initialize a track with a complete factor graph and estimate. 334 | /// Call multiple times if there are multiple tracks. 335 | void initialize(const ISAM2 &isam2) { 336 | #ifdef USE_FAST_MARGINALS 337 | tracks_.push_back(TrackInfo(isam2)); 338 | TrackInfo &track = tracks_.back(); 339 | track.values = isam2.calculateEstimate(); 340 | #else 341 | tracks_.push_back(TrackInfo()); 342 | TrackInfo &track = tracks_.back(); 343 | track.values = isam2.calculateEstimate(); 344 | track.marginals = Marginals(isam2.getFactorsUnsafe(), track.values); 345 | #endif 346 | 347 | for (Key key : track.values.keys()) 348 | if (symbolChr(key) == 'l') track.keys.push_back(key); 349 | 350 | for (int x = 0;; ++x) { 351 | if (!track.values.exists(X(x))) { 352 | assert(x > 0); 353 | track.x0 = X(x - 1); 354 | track.pose0 = track.values.template at(track.x0); 355 | break; 356 | } 357 | } 358 | track.stack.push(MatchInfo()); 359 | track.stack.top().track = tracks_.size() - 1; 360 | track.stack.top().pose = track.pose0; 361 | track.stack.top().covariance = track.marginals.marginalCovariance(track.x0); 362 | } 363 | 364 | /// Add measurement to every track. 365 | void add(BEARING measuredBearing, RANGE measuredRange, 366 | const SharedNoiseModel &model) { 367 | for (TrackInfo &track : tracks_) { 368 | track.innovations.push_back({}); 369 | for (Key l : track.keys) { 370 | POINT point = track.values.template at(l); 371 | BearingRangeFactor factor(track.x0, l, measuredBearing, measuredRange, model); 372 | 373 | Innovation::shared_ptr inn(new Innovation); 374 | inn->l = l; 375 | inn->error = factor.evaluateError(track.pose0, point, inn->Hx, inn->Hl); 376 | inn->sigmas = model->sigmas(); 377 | inn->md = model->distance(inn->error); 378 | 379 | if (jc_(track, inn)) 380 | track.innovations.back().push_back(inn); 381 | } 382 | } 383 | } 384 | 385 | /// MHJCBB match 386 | void match() { 387 | for (TrackInfo &track : tracks_) { 388 | // Same as JCBB 389 | KeyVector keys; 390 | keys.push_back(track.x0); 391 | for (const std::vector &obs_inn : track.innovations) { 392 | for (const Innovation::shared_ptr &inn : obs_inn) 393 | if (std::find(keys.begin(), keys.end(), inn->l) == keys.end()) 394 | keys.push_back(inn->l); 395 | } 396 | #ifndef USE_FAST_MARGINALS 397 | track.joint_marginals = track.marginals.jointMarginalCovariance(keys); 398 | #endif 399 | 400 | // Sort in reverse order due to stack. 401 | for (FastVector &linn : track.innovations) 402 | std::sort(linn.begin(), linn.end(), 403 | [](Innovation::shared_ptr lhs, Innovation::shared_ptr rhs) { 404 | return lhs->md > rhs->md; 405 | }); 406 | } 407 | 408 | mhjcbb(); 409 | 410 | // Map to keys. 411 | while (!best_hypotheses_.empty()) { 412 | MatchInfo mi = best_hypotheses_.top(); 413 | best_hypotheses_.pop(); 414 | 415 | KeyVector matched_keys; 416 | int new_l = 0; 417 | for (const Innovation::shared_ptr &inn : mi.hypothesis) 418 | matched_keys.push_back(inn ? inn->l : tracks_[mi.track].keys.size() + new_l++); 419 | result_.insert(result_.begin(), std::make_pair(mi.track, matched_keys)); 420 | } 421 | } 422 | 423 | /// Access matching result. 424 | int size() const { return result_.size(); } 425 | std::pair get(int i) const { return result_[i]; } 426 | 427 | private: 428 | /// MHJCBB search in interpretation forest. 429 | void mhjcbb() { 430 | int tracks_done = 0; 431 | int i = 0; 432 | while (tracks_done < max_tracks_) { 433 | for (TrackInfo &ti : tracks_) { 434 | // JCBB will return when one complete hypothesis is met. 435 | tracks_done += jcbb(ti); 436 | } 437 | } 438 | 439 | screen2(); 440 | } 441 | 442 | /// JCBB in a track. 443 | /// Return true if the search is done. 444 | bool jcbb(TrackInfo &ti) { 445 | while (!ti.stack.empty()) { 446 | MatchInfo mi = ti.stack.top(); 447 | ti.stack.pop(); 448 | 449 | int k = mi.hypothesis.size(); 450 | int h = pairings(mi.hypothesis); 451 | if (k == ti.innovations.size()) { 452 | if (jc(ti, mi) && screen1(mi)) { 453 | best_hypotheses_.push(mi); 454 | if (best_hypotheses_.size() > max_tracks_) 455 | best_hypotheses_.pop(); 456 | return false; 457 | } 458 | } else { 459 | FastSet existing; 460 | for (const Innovation::shared_ptr &inn : mi.hypothesis) 461 | if (inn) existing.insert(inn->l); 462 | 463 | FastSet remaining; 464 | for (int j = k + 1; j < ti.innovations.size(); ++j) { 465 | for (Innovation::shared_ptr &future_inn : ti.innovations[j]) { 466 | if (existing.find(future_inn->l) == existing.end()) 467 | remaining.insert(future_inn->l); 468 | } 469 | } 470 | int max_remaining = std::min(remaining.size(), ti.innovations.size() - k - 1); 471 | if (best_hypotheses_.size() < max_tracks_ || 472 | h + max_remaining >= best_hypotheses_.top().num_pairings) { 473 | MatchInfo extended = mi; 474 | extended.hypothesis.push_back(nullptr); 475 | ti.stack.push(extended); 476 | } 477 | 478 | for (Innovation::shared_ptr &inn : ti.innovations[k]) { 479 | if (existing.find(inn->l) != existing.end()) continue; 480 | 481 | FastSet remaining; 482 | for (int j = k + 1; j < ti.innovations.size(); ++j) { 483 | for (Innovation::shared_ptr &future_inn : ti.innovations[j]) { 484 | if (future_inn->l != inn->l && 485 | existing.find(future_inn->l) == existing.end()) 486 | remaining.insert(future_inn->l); 487 | } 488 | } 489 | int max_remaining = 490 | std::min(remaining.size(), ti.innovations.size() - k - 1); 491 | int future_pairings = h + 1 + max_remaining; 492 | if (best_hypotheses_.size() == max_tracks_) { 493 | int min_pairings = best_hypotheses_.top().num_pairings; 494 | if (future_pairings < min_pairings) 495 | continue; 496 | if (best_hypotheses_.size() == ti.innovations.size() && 497 | future_pairings == min_pairings && mi.md > best_hypotheses_.top().md) 498 | continue; 499 | } 500 | 501 | MatchInfo extended = mi; 502 | extended.hypothesis.push_back(inn); 503 | extended.num_pairings += 1; 504 | if (jc(ti, mi)) { 505 | ti.stack.push(extended); 506 | } 507 | } 508 | } 509 | } 510 | return true; 511 | } 512 | 513 | /// Remove redundant matching result from the same track. 514 | bool screen1(const MatchInfo &mi) { 515 | decltype(best_hypotheses_) copy; 516 | 517 | bool valid = true; 518 | while (!best_hypotheses_.empty()) { 519 | MatchInfo existing_mi = best_hypotheses_.top(); 520 | best_hypotheses_.pop(); 521 | 522 | if (!valid || mi.track != existing_mi.track) { 523 | copy.push(existing_mi); 524 | continue; 525 | } 526 | 527 | Vector e = existing_mi.pose.localCoordinates(mi.pose); 528 | double md = e.transpose() * existing_mi.covariance.inverse() * e; 529 | if (sqrt(md) < posterior_pose_md_threshold_) { 530 | if (MatchInfoCmp()(existing_mi, mi)) { 531 | copy.push(existing_mi); 532 | valid = false; 533 | } 534 | } else { 535 | copy.push(existing_mi); 536 | } 537 | } 538 | best_hypotheses_ = copy; 539 | return valid; 540 | } 541 | 542 | /// Remove redundant empty matching result. 543 | void screen2() { 544 | decltype(best_hypotheses_) copy; 545 | 546 | FastVector null, nonnull; 547 | while (!best_hypotheses_.empty()) { 548 | MatchInfo mi = best_hypotheses_.top(); 549 | best_hypotheses_.pop(); 550 | 551 | if (mi.num_pairings) { 552 | copy.push(mi); 553 | nonnull.push_back(mi); 554 | } else { 555 | null.push_back(mi); 556 | } 557 | } 558 | 559 | for (const MatchInfo &mi1 : null) { 560 | bool valid = true; 561 | for (const MatchInfo &mi2 : nonnull) { 562 | Vector e = mi2.pose.localCoordinates(mi1.pose); 563 | double md = e.transpose() * mi2.covariance.inverse() * e; 564 | if (sqrt(md) < posterior_pose_md_threshold_) { 565 | valid = false; 566 | break; 567 | } 568 | } 569 | 570 | if (valid) 571 | copy.push(mi1); 572 | } 573 | best_hypotheses_ = copy; 574 | } 575 | 576 | int pairings(const FastVector &hypothesis) const { 577 | return std::count_if( 578 | hypothesis.cbegin(), hypothesis.cend(), 579 | [](const Innovation::shared_ptr &inn) { return inn != nullptr; }); 580 | } 581 | 582 | bool jc_(const TrackInfo &ti, const Innovation::shared_ptr &inn) const { 583 | Matrix S = Matrix::Zero(5, 5); 584 | S.block<3, 3>(0, 0) = ti.marginals.marginalCovariance(ti.x0); 585 | S.block<2, 2>(3, 3) = ti.marginals.marginalCovariance(inn->l); 586 | 587 | Matrix H(2, 5); 588 | H.block<2, 3>(0, 0) = inn->Hx; 589 | H.block<2, 2>(0, 3) = inn->Hl; 590 | 591 | Matrix R = inn->sigmas.asDiagonal(); 592 | Vector e = inn->error; 593 | 594 | Matrix C = H * S * H.transpose() + R * R; 595 | double chi2 = e.transpose() * C.llt().solve(e); 596 | return chi2 < chi2inv(prob_, 2); 597 | } 598 | 599 | /// JC test. 600 | /// Update posterior pose and covariance in MatchInfo. 601 | bool jc(const TrackInfo &ti, MatchInfo &mi) const { 602 | if (!mi.num_pairings) return true; 603 | 604 | int XD = POSE::dimension, LD = POINT::dimension, 605 | FD = BearingRange::dimension; 606 | int N = XD, M = 0; 607 | KeyVector keys; 608 | keys.push_back(ti.x0); 609 | for (const Innovation::shared_ptr &inn : mi.hypothesis) { 610 | if (!inn) continue; 611 | keys.push_back(inn->l); 612 | N += LD; 613 | M += FD; 614 | } 615 | 616 | Matrix S(N, N); 617 | #ifdef USE_FAST_MARGINALS 618 | S = ti.marginals.jointMarginalCovariance(keys); 619 | #else 620 | for (int i = 0, p = 0; i < keys.size(); ++i) { 621 | Key ki = keys[i]; 622 | int pi = ti.values.at(ki).dim(); 623 | for (int j = i, q = p; j < keys.size(); ++j) { 624 | Key kj = keys[j]; 625 | int qj = ti.values.at(kj).dim(); 626 | S.block(p, q, pi, qj) = ti.joint_marginals.at(ki, kj); 627 | q += qj; 628 | } 629 | p += pi; 630 | } 631 | S.triangularView() = S.transpose(); 632 | #endif 633 | 634 | Matrix H = Matrix::Zero(M, N); 635 | Matrix R = Matrix::Zero(M, M); 636 | Vector e(M); 637 | for (int i = 0, j = 0; i < mi.hypothesis.size(); ++i) { 638 | if (!mi.hypothesis[i]) continue; 639 | H.block(j * FD, 0, FD, XD) = mi.hypothesis[i]->Hx; 640 | H.block(j * FD, XD + j * LD, FD, LD) = mi.hypothesis[i]->Hl; 641 | R.block(j * FD, j * FD, FD, FD) = mi.hypothesis[i]->sigmas.asDiagonal(); 642 | e.segment(j * FD, FD) = mi.hypothesis[i]->error; 643 | j += 1; 644 | } 645 | 646 | // update step in Kalman filter 647 | Matrix C = H * S * H.transpose() + R * R; 648 | Matrix C_1 = C.inverse(); 649 | Matrix K = S * H.transpose() * C_1; 650 | Vector d = -K * e; 651 | 652 | mi.pose = ti.values.template at(ti.x0).retract(d.head(XD)); 653 | mi.covariance = S.topLeftCorner(XD, XD) - K.topRows(XD) * H * S.leftCols(XD); 654 | 655 | double chi2 = e.transpose() * C_1 * e; 656 | return chi2 < chi2inv(prob_, M); 657 | } 658 | 659 | private: 660 | int max_tracks_; 661 | double prob_; 662 | double posterior_pose_md_threshold_; 663 | 664 | FastVector tracks_; 665 | std::priority_queue, MatchInfoCmp> best_hypotheses_; 666 | FastVector> result_; 667 | }; 668 | 669 | double normalCDF(double u) { 670 | static const double a[5] = {1.161110663653770e-002, 3.951404679838207e-001, 671 | 2.846603853776254e+001, 1.887426188426510e+002, 672 | 3.209377589138469e+003}; 673 | static const double b[5] = {1.767766952966369e-001, 8.344316438579620e+000, 674 | 1.725514762600375e+002, 1.813893686502485e+003, 675 | 8.044716608901563e+003}; 676 | static const double c[9] = { 677 | 2.15311535474403846e-8, 5.64188496988670089e-1, 8.88314979438837594e00, 678 | 6.61191906371416295e01, 2.98635138197400131e02, 8.81952221241769090e02, 679 | 1.71204761263407058e03, 2.05107837782607147e03, 1.23033935479799725E03}; 680 | static const double d[9] = { 681 | 1.00000000000000000e00, 1.57449261107098347e01, 1.17693950891312499e02, 682 | 5.37181101862009858e02, 1.62138957456669019e03, 3.29079923573345963e03, 683 | 4.36261909014324716e03, 3.43936767414372164e03, 1.23033935480374942e03}; 684 | static const double p[6] = {1.63153871373020978e-2, 3.05326634961232344e-1, 685 | 3.60344899949804439e-1, 1.25781726111229246e-1, 686 | 1.60837851487422766e-2, 6.58749161529837803e-4}; 687 | static const double q[6] = {1.00000000000000000e00, 2.56852019228982242e00, 688 | 1.87295284992346047e00, 5.27905102951428412e-1, 689 | 6.05183413124413191e-2, 2.33520497626869185e-3}; 690 | double y, z; 691 | 692 | y = fabs(u); 693 | // clang-format off 694 | if (y <= 0.46875 * 1.4142135623730950488016887242097) { 695 | /* evaluate erf() for |u| <= sqrt(2)*0.46875 */ 696 | z = y * y; 697 | y = u * ((((a[0] * z + a[1]) * z + a[2]) * z + a[3]) * z + a[4]) / ((((b[0] * z + b[1]) * z + b[2]) * z + b[3]) * z + b[4]); 698 | return 0.5 + y; 699 | } 700 | 701 | z = exp(-y * y / 2) / 2; 702 | if (y <= 4.0) { 703 | /* evaluate erfc() for sqrt(2)*0.46875 <= |u| <= sqrt(2)*4.0 */ 704 | y = y / 1.4142135623730950488016887242097; 705 | y = ((((((((c[0] * y + c[1]) * y + c[2]) * y + c[3]) * y + c[4]) * y + c[5]) * y + c[6]) * y + c[7]) * y + c[8]) / ((((((((d[0] * y + d[1]) * y + d[2]) * y + d[3]) * y + d[4]) * y + d[5]) * y + d[6]) * y + d[7]) * y + d[8]); 706 | y = z * y; 707 | } else { 708 | /* evaluate erfc() for |u| > sqrt(2)*4.0 */ 709 | z = z * 1.4142135623730950488016887242097 / y; 710 | y = 2 / (y * y); 711 | y = y * (((((p[0] * y + p[1]) * y + p[2]) * y + p[3]) * y + p[4]) * y + p[5]) / (((((q[0] * y + q[1]) * y + q[2]) * y + q[3]) * y + q[4]) * y + q[5]); 712 | y = z * (0.564189583547756286948 - y); 713 | } 714 | return (u < 0.0 ? y : 1 - y); 715 | } 716 | 717 | double normalQuantile(double p) { 718 | double q, t, u; 719 | 720 | static const double a[6] = {-3.969683028665376e+01, 2.209460984245205e+02, 721 | -2.759285104469687e+02, 1.383577518672690e+02, 722 | -3.066479806614716e+01, 2.506628277459239e+00}; 723 | static const double b[5] = {-5.447609879822406e+01, 1.615858368580409e+02, 724 | -1.556989798598866e+02, 6.680131188771972e+01, 725 | -1.328068155288572e+01}; 726 | static const double c[6] = {-7.784894002430293e-03, -3.223964580411365e-01, 727 | -2.400758277161838e+00, -2.549732539343734e+00, 728 | 4.374664141464968e+00, 2.938163982698783e+00}; 729 | static const double d[4] = {7.784695709041462e-03, 3.224671290700398e-01, 730 | 2.445134137142996e+00, 3.754408661907416e+00}; 731 | 732 | q = std::min(p, 1 - p); 733 | 734 | if (q > 0.02425) { 735 | /* Rational approximation for central region. */ 736 | u = q - 0.5; 737 | t = u * u; 738 | u = u * (((((a[0] * t + a[1]) * t + a[2]) * t + a[3]) * t + a[4]) * t + a[5]) / (((((b[0] * t + b[1]) * t + b[2]) * t + b[3]) * t + b[4]) * t + 1); 739 | } else { 740 | /* Rational approximation for tail region. */ 741 | t = sqrt(-2 * log(q)); 742 | u = (((((c[0] * t + c[1]) * t + c[2]) * t + c[3]) * t + c[4]) * t + c[5]) / ((((d[0] * t + d[1]) * t + d[2]) * t + d[3]) * t + 1); 743 | } 744 | 745 | /* The relative error of the approximation has absolute value less 746 | than 1.15e-9. One iteration of Halley's rational method (third 747 | order) gives full machine precision... */ 748 | t = normalCDF(u) - q; /* error */ 749 | t = t * 2.506628274631000502415765284811 * exp(u * u / 2); /* f(u)/df(u) */ 750 | u = u - t / (1 + u * t / 2); /* Halley's method */ 751 | 752 | return (p > 0.5 ? -u : u); 753 | } 754 | 755 | double chi2inv(double P, unsigned int dim) { 756 | if (P == 0) 757 | return 0; 758 | else 759 | return dim * pow(1.0 - 2.0 / (9 * dim) + sqrt(2.0 / (9 * dim)) * normalQuantile(P), 3); 760 | } 761 | } // namespace da 762 | } // namespace gtsam -------------------------------------------------------------------------------- /gtsam.h: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | * GTSAM Wrap Module Definition 4 | * 5 | * These are the current classes available through the matlab and python wrappers, 6 | * add more functions/classes as they are available. 7 | * 8 | * IMPORTANT: the python wrapper supports keyword arguments for functions/methods. Hence, the 9 | * argument names matter. An implementation restriction is that in overloaded methods 10 | * or functions, arguments of different types *have* to have different names. 11 | * 12 | * Requirements: 13 | * Classes must start with an uppercase letter 14 | * - Can wrap a typedef 15 | * Only one Method/Constructor per line, though methods/constructors can extend across multiple lines 16 | * Methods can return 17 | * - Eigen types: Matrix, Vector 18 | * - C/C++ basic types: string, bool, size_t, int, double, char, unsigned char 19 | * - void 20 | * - Any class with which be copied with boost::make_shared() 21 | * - boost::shared_ptr of any object type 22 | * Constructors 23 | * - Overloads are supported, but arguments of different types *have* to have different names 24 | * - A class with no constructors can be returned from other functions but not allocated directly in MATLAB 25 | * Methods 26 | * - Constness has no effect 27 | * - Specify by-value (not reference) return types, even if C++ method returns reference 28 | * - Must start with a letter (upper or lowercase) 29 | * - Overloads are supported 30 | * Static methods 31 | * - Must start with a letter (upper or lowercase) and use the "static" keyword 32 | * - The first letter will be made uppercase in the generated MATLAB interface 33 | * - Overloads are supported, but arguments of different types *have* to have different names 34 | * Arguments to functions any of 35 | * - Eigen types: Matrix, Vector 36 | * - Eigen types and classes as an optionally const reference 37 | * - C/C++ basic types: string, bool, size_t, size_t, double, char, unsigned char 38 | * - Any class with which be copied with boost::make_shared() (except Eigen) 39 | * - boost::shared_ptr of any object type (except Eigen) 40 | * Comments can use either C++ or C style, with multiple lines 41 | * Namespace definitions 42 | * - Names of namespaces must start with a lowercase letter 43 | * - start a namespace with "namespace {" 44 | * - end a namespace with exactly "}" 45 | * - Namespaces can be nested 46 | * Namespace usage 47 | * - Namespaces can be specified for classes in arguments and return values 48 | * - In each case, the namespace must be fully specified, e.g., "namespace1::namespace2::ClassName" 49 | * Includes in C++ wrappers 50 | * - All includes will be collected and added in a single file 51 | * - All namespaces must have angle brackets: 52 | * - No default includes will be added 53 | * Global/Namespace functions 54 | * - Functions specified outside of a class are global 55 | * - Can be overloaded with different arguments 56 | * - Can have multiple functions of the same name in different namespaces 57 | * Using classes defined in other modules 58 | * - If you are using a class 'OtherClass' not wrapped in this definition file, add "class OtherClass;" to avoid a dependency error 59 | * Virtual inheritance 60 | * - Specify fully-qualified base classes, i.e. "virtual class Derived : ns::Base {" where "ns" is the namespace 61 | * - Mark with 'virtual' keyword, e.g. "virtual class Base {", and also "virtual class Derived : ns::Base {" 62 | * - Forward declarations must also be marked virtual, e.g. "virtual class ns::Base;" and 63 | * also "virtual class ns::Derived;" 64 | * - Pure virtual (abstract) classes should list no constructors in this interface file 65 | * - Virtual classes must have a clone() function in C++ (though it does not have to be included 66 | * in the MATLAB interface). clone() will be called whenever an object copy is needed, instead 67 | * of using the copy constructor (which is used for non-virtual objects). 68 | * - Signature of clone function - will be called virtually, so must appear at least at the top of the inheritance tree 69 | * virtual boost::shared_ptr clone() const; 70 | * Class Templates 71 | * - Basic templates are supported either with an explicit list of types to instantiate, 72 | * e.g. template class Class1 { ... }; 73 | * or with typedefs, e.g. 74 | * template class Class2 { ... }; 75 | * typedef Class2 MyInstantiatedClass; 76 | * - In the class definition, appearances of the template argument(s) will be replaced with their 77 | * instantiated types, e.g. 'void setValue(const T& value);'. 78 | * - To refer to the instantiation of the template class itself, use 'This', i.e. 'static This Create();' 79 | * - To create new instantiations in other modules, you must copy-and-paste the whole class definition 80 | * into the new module, but use only your new instantiation types. 81 | * - When forward-declaring template instantiations, use the generated/typedefed name, e.g. 82 | * class gtsam::Class1Pose2; 83 | * class gtsam::MyInstantiatedClass; 84 | * Boost.serialization within Matlab: 85 | * - you need to mark classes as being serializable in the markup file (see this file for an example). 86 | * - There are two options currently, depending on the class. To "mark" a class as serializable, 87 | * add a function with a particular signature so that wrap will catch it. 88 | * - Add "void serialize()" to a class to create serialization functions for a class. 89 | * Adding this flag subsumes the serializable() flag below. Requirements: 90 | * - A default constructor must be publicly accessible 91 | * - Must not be an abstract base class 92 | * - The class must have an actual boost.serialization serialize() function. 93 | * - Add "void serializable()" to a class if you only want the class to be serialized as a 94 | * part of a container (such as noisemodel). This version does not require a publicly 95 | * accessible default constructor. 96 | * Forward declarations and class definitions for Cython: 97 | * - Need to specify the base class (both this forward class and base class are declared in an external cython header) 98 | * This is so Cython can generate proper inheritance. 99 | * Example when wrapping a gtsam-based project: 100 | * // forward declarations 101 | * virtual class gtsam::NonlinearFactor 102 | * virtual class gtsam::NoiseModelFactor : gtsam::NonlinearFactor 103 | * // class definition 104 | * #include 105 | * virtual class MyFactor : gtsam::NoiseModelFactor {...}; 106 | * - *DO NOT* re-define overriden function already declared in the external (forward-declared) base class 107 | * - This will cause an ambiguity problem in Cython pxd header file 108 | */ 109 | 110 | /** 111 | * Status: 112 | * - TODO: default values for arguments 113 | * - WORKAROUND: make multiple versions of the same function for different configurations of default arguments 114 | * - TODO: Handle gtsam::Rot3M conversions to quaternions 115 | * - TODO: Parse return of const ref arguments 116 | * - TODO: Parse std::string variants and convert directly to special string 117 | * - TODO: Add enum support 118 | * - TODO: Add generalized serialization support via boost.serialization with hooks to matlab save/load 119 | */ 120 | 121 | namespace gtsam { 122 | 123 | // Actually a FastList 124 | #include 125 | class KeyList { 126 | KeyList(); 127 | KeyList(const gtsam::KeyList& other); 128 | 129 | // Note: no print function 130 | 131 | // common STL methods 132 | size_t size() const; 133 | bool empty() const; 134 | void clear(); 135 | 136 | // structure specific methods 137 | size_t front() const; 138 | size_t back() const; 139 | void push_back(size_t key); 140 | void push_front(size_t key); 141 | void pop_back(); 142 | void pop_front(); 143 | void sort(); 144 | void remove(size_t key); 145 | 146 | void serialize() const; 147 | }; 148 | 149 | // Actually a FastSet 150 | class KeySet { 151 | KeySet(); 152 | KeySet(const gtsam::KeySet& set); 153 | KeySet(const gtsam::KeyVector& vector); 154 | KeySet(const gtsam::KeyList& list); 155 | 156 | // Testable 157 | void print(string s) const; 158 | bool equals(const gtsam::KeySet& other) const; 159 | 160 | // common STL methods 161 | size_t size() const; 162 | bool empty() const; 163 | void clear(); 164 | 165 | // structure specific methods 166 | void insert(size_t key); 167 | void merge(const gtsam::KeySet& other); 168 | bool erase(size_t key); // returns true if value was removed 169 | bool count(size_t key) const; // returns true if value exists 170 | 171 | void serialize() const; 172 | }; 173 | 174 | // Actually a vector 175 | class KeyVector { 176 | KeyVector(); 177 | KeyVector(const gtsam::KeyVector& other); 178 | 179 | // Note: no print function 180 | 181 | // common STL methods 182 | size_t size() const; 183 | bool empty() const; 184 | void clear(); 185 | 186 | // structure specific methods 187 | size_t at(size_t i) const; 188 | size_t front() const; 189 | size_t back() const; 190 | void push_back(size_t key) const; 191 | 192 | void serialize() const; 193 | }; 194 | 195 | // Actually a FastMap 196 | class KeyGroupMap { 197 | KeyGroupMap(); 198 | 199 | // Note: no print function 200 | 201 | // common STL methods 202 | size_t size() const; 203 | bool empty() const; 204 | void clear(); 205 | 206 | // structure specific methods 207 | size_t at(size_t key) const; 208 | int erase(size_t key); 209 | bool insert2(size_t key, int val); 210 | }; 211 | 212 | //************************************************************************* 213 | // base 214 | //************************************************************************* 215 | 216 | /** gtsam namespace functions */ 217 | #include 218 | bool linear_independent(Matrix A, Matrix B, double tol); 219 | 220 | #include 221 | virtual class Value { 222 | // No constructors because this is an abstract class 223 | 224 | // Testable 225 | void print(string s) const; 226 | 227 | // Manifold 228 | size_t dim() const; 229 | }; 230 | 231 | #include 232 | class LieScalar { 233 | // Standard constructors 234 | LieScalar(); 235 | LieScalar(double d); 236 | 237 | // Standard interface 238 | double value() const; 239 | 240 | // Testable 241 | void print(string s) const; 242 | bool equals(const gtsam::LieScalar& expected, double tol) const; 243 | 244 | // Group 245 | static gtsam::LieScalar identity(); 246 | gtsam::LieScalar inverse() const; 247 | gtsam::LieScalar compose(const gtsam::LieScalar& p) const; 248 | gtsam::LieScalar between(const gtsam::LieScalar& l2) const; 249 | 250 | // Manifold 251 | size_t dim() const; 252 | gtsam::LieScalar retract(Vector v) const; 253 | Vector localCoordinates(const gtsam::LieScalar& t2) const; 254 | 255 | // Lie group 256 | static gtsam::LieScalar Expmap(Vector v); 257 | static Vector Logmap(const gtsam::LieScalar& p); 258 | }; 259 | 260 | #include 261 | class LieVector { 262 | // Standard constructors 263 | LieVector(); 264 | LieVector(Vector v); 265 | 266 | // Standard interface 267 | Vector vector() const; 268 | 269 | // Testable 270 | void print(string s) const; 271 | bool equals(const gtsam::LieVector& expected, double tol) const; 272 | 273 | // Group 274 | static gtsam::LieVector identity(); 275 | gtsam::LieVector inverse() const; 276 | gtsam::LieVector compose(const gtsam::LieVector& p) const; 277 | gtsam::LieVector between(const gtsam::LieVector& l2) const; 278 | 279 | // Manifold 280 | size_t dim() const; 281 | gtsam::LieVector retract(Vector v) const; 282 | Vector localCoordinates(const gtsam::LieVector& t2) const; 283 | 284 | // Lie group 285 | static gtsam::LieVector Expmap(Vector v); 286 | static Vector Logmap(const gtsam::LieVector& p); 287 | 288 | // enabling serialization functionality 289 | void serialize() const; 290 | }; 291 | 292 | #include 293 | class LieMatrix { 294 | // Standard constructors 295 | LieMatrix(); 296 | LieMatrix(Matrix v); 297 | 298 | // Standard interface 299 | Matrix matrix() const; 300 | 301 | // Testable 302 | void print(string s) const; 303 | bool equals(const gtsam::LieMatrix& expected, double tol) const; 304 | 305 | // Group 306 | static gtsam::LieMatrix identity(); 307 | gtsam::LieMatrix inverse() const; 308 | gtsam::LieMatrix compose(const gtsam::LieMatrix& p) const; 309 | gtsam::LieMatrix between(const gtsam::LieMatrix& l2) const; 310 | 311 | // Manifold 312 | size_t dim() const; 313 | gtsam::LieMatrix retract(Vector v) const; 314 | Vector localCoordinates(const gtsam::LieMatrix & t2) const; 315 | 316 | // Lie group 317 | static gtsam::LieMatrix Expmap(Vector v); 318 | static Vector Logmap(const gtsam::LieMatrix& p); 319 | 320 | // enabling serialization functionality 321 | void serialize() const; 322 | }; 323 | 324 | //************************************************************************* 325 | // geometry 326 | //************************************************************************* 327 | 328 | #include 329 | class Point2 { 330 | // Standard Constructors 331 | Point2(); 332 | Point2(double x, double y); 333 | Point2(Vector v); 334 | 335 | // Testable 336 | void print(string s) const; 337 | bool equals(const gtsam::Point2& point, double tol) const; 338 | 339 | // Group 340 | static gtsam::Point2 identity(); 341 | 342 | // Standard Interface 343 | double x() const; 344 | double y() const; 345 | Vector vector() const; 346 | double distance(const gtsam::Point2& p2) const; 347 | double norm() const; 348 | 349 | // enabling serialization functionality 350 | void serialize() const; 351 | }; 352 | 353 | // std::vector 354 | #include 355 | class Point2Vector 356 | { 357 | // Constructors 358 | Point2Vector(); 359 | Point2Vector(const gtsam::Point2Vector& v); 360 | 361 | //Capacity 362 | size_t size() const; 363 | size_t max_size() const; 364 | void resize(size_t sz); 365 | size_t capacity() const; 366 | bool empty() const; 367 | void reserve(size_t n); 368 | 369 | //Element access 370 | gtsam::Point2 at(size_t n) const; 371 | gtsam::Point2 front() const; 372 | gtsam::Point2 back() const; 373 | 374 | //Modifiers 375 | void assign(size_t n, const gtsam::Point2& u); 376 | void push_back(const gtsam::Point2& x); 377 | void pop_back(); 378 | }; 379 | 380 | #include 381 | class StereoPoint2 { 382 | // Standard Constructors 383 | StereoPoint2(); 384 | StereoPoint2(double uL, double uR, double v); 385 | 386 | // Testable 387 | void print(string s) const; 388 | bool equals(const gtsam::StereoPoint2& point, double tol) const; 389 | 390 | // Group 391 | static gtsam::StereoPoint2 identity(); 392 | gtsam::StereoPoint2 inverse() const; 393 | gtsam::StereoPoint2 compose(const gtsam::StereoPoint2& p2) const; 394 | gtsam::StereoPoint2 between(const gtsam::StereoPoint2& p2) const; 395 | 396 | // Manifold 397 | gtsam::StereoPoint2 retract(Vector v) const; 398 | Vector localCoordinates(const gtsam::StereoPoint2& p) const; 399 | 400 | // Lie Group 401 | static gtsam::StereoPoint2 Expmap(Vector v); 402 | static Vector Logmap(const gtsam::StereoPoint2& p); 403 | 404 | // Standard Interface 405 | Vector vector() const; 406 | double uL() const; 407 | double uR() const; 408 | double v() const; 409 | 410 | // enabling serialization functionality 411 | void serialize() const; 412 | }; 413 | 414 | #include 415 | class Point3 { 416 | // Standard Constructors 417 | Point3(); 418 | Point3(double x, double y, double z); 419 | Point3(Vector v); 420 | 421 | // Testable 422 | void print(string s) const; 423 | bool equals(const gtsam::Point3& p, double tol) const; 424 | 425 | // Group 426 | static gtsam::Point3 identity(); 427 | 428 | // Standard Interface 429 | Vector vector() const; 430 | double x() const; 431 | double y() const; 432 | double z() const; 433 | 434 | // enabling serialization functionality 435 | void serialize() const; 436 | }; 437 | 438 | #include 439 | class Rot2 { 440 | // Standard Constructors and Named Constructors 441 | Rot2(); 442 | Rot2(double theta); 443 | static gtsam::Rot2 fromAngle(double theta); 444 | static gtsam::Rot2 fromDegrees(double theta); 445 | static gtsam::Rot2 fromCosSin(double c, double s); 446 | 447 | // Testable 448 | void print(string s) const; 449 | bool equals(const gtsam::Rot2& rot, double tol) const; 450 | 451 | // Group 452 | static gtsam::Rot2 identity(); 453 | gtsam::Rot2 inverse(); 454 | gtsam::Rot2 compose(const gtsam::Rot2& p2) const; 455 | gtsam::Rot2 between(const gtsam::Rot2& p2) const; 456 | 457 | // Manifold 458 | gtsam::Rot2 retract(Vector v) const; 459 | Vector localCoordinates(const gtsam::Rot2& p) const; 460 | 461 | // Lie Group 462 | static gtsam::Rot2 Expmap(Vector v); 463 | static Vector Logmap(const gtsam::Rot2& p); 464 | 465 | // Group Action on Point2 466 | gtsam::Point2 rotate(const gtsam::Point2& point) const; 467 | gtsam::Point2 unrotate(const gtsam::Point2& point) const; 468 | 469 | // Standard Interface 470 | static gtsam::Rot2 relativeBearing(const gtsam::Point2& d); // Ignoring derivative 471 | static gtsam::Rot2 atan2(double y, double x); 472 | double theta() const; 473 | double degrees() const; 474 | double c() const; 475 | double s() const; 476 | Matrix matrix() const; 477 | 478 | // enabling serialization functionality 479 | void serialize() const; 480 | }; 481 | 482 | #include 483 | class Rot3 { 484 | // Standard Constructors and Named Constructors 485 | Rot3(); 486 | Rot3(Matrix R); 487 | Rot3(const gtsam::Point3& col1, const gtsam::Point3& col2, const gtsam::Point3& col3); 488 | Rot3(double R11, double R12, double R13, 489 | double R21, double R22, double R23, 490 | double R31, double R32, double R33); 491 | 492 | static gtsam::Rot3 Rx(double t); 493 | static gtsam::Rot3 Ry(double t); 494 | static gtsam::Rot3 Rz(double t); 495 | static gtsam::Rot3 RzRyRx(double x, double y, double z); 496 | static gtsam::Rot3 RzRyRx(Vector xyz); 497 | static gtsam::Rot3 Yaw(double t); // positive yaw is to right (as in aircraft heading) 498 | static gtsam::Rot3 Pitch(double t); // positive pitch is up (increasing aircraft altitude) 499 | static gtsam::Rot3 Roll(double t); // positive roll is to right (increasing yaw in aircraft) 500 | static gtsam::Rot3 Ypr(double y, double p, double r); 501 | static gtsam::Rot3 Quaternion(double w, double x, double y, double z); 502 | static gtsam::Rot3 Rodrigues(Vector v); 503 | static gtsam::Rot3 Rodrigues(double wx, double wy, double wz); 504 | 505 | // Testable 506 | void print(string s) const; 507 | bool equals(const gtsam::Rot3& rot, double tol) const; 508 | 509 | // Group 510 | static gtsam::Rot3 identity(); 511 | gtsam::Rot3 inverse() const; 512 | gtsam::Rot3 compose(const gtsam::Rot3& p2) const; 513 | gtsam::Rot3 between(const gtsam::Rot3& p2) const; 514 | 515 | // Manifold 516 | //gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options 517 | gtsam::Rot3 retract(Vector v) const; 518 | Vector localCoordinates(const gtsam::Rot3& p) const; 519 | 520 | // Group Action on Point3 521 | gtsam::Point3 rotate(const gtsam::Point3& p) const; 522 | gtsam::Point3 unrotate(const gtsam::Point3& p) const; 523 | 524 | // Standard Interface 525 | static gtsam::Rot3 Expmap(Vector v); 526 | static Vector Logmap(const gtsam::Rot3& p); 527 | Matrix matrix() const; 528 | Matrix transpose() const; 529 | gtsam::Point3 column(size_t index) const; 530 | Vector xyz() const; 531 | Vector ypr() const; 532 | Vector rpy() const; 533 | double roll() const; 534 | double pitch() const; 535 | double yaw() const; 536 | // Vector toQuaternion() const; // FIXME: Can't cast to Vector properly 537 | Vector quaternion() const; 538 | 539 | // enabling serialization functionality 540 | void serialize() const; 541 | }; 542 | 543 | #include 544 | class Pose2 { 545 | // Standard Constructor 546 | Pose2(); 547 | Pose2(const gtsam::Pose2& other); 548 | Pose2(double x, double y, double theta); 549 | Pose2(double theta, const gtsam::Point2& t); 550 | Pose2(const gtsam::Rot2& r, const gtsam::Point2& t); 551 | Pose2(Vector v); 552 | 553 | // Testable 554 | void print(string s) const; 555 | bool equals(const gtsam::Pose2& pose, double tol) const; 556 | 557 | // Group 558 | static gtsam::Pose2 identity(); 559 | gtsam::Pose2 inverse() const; 560 | gtsam::Pose2 compose(const gtsam::Pose2& p2) const; 561 | gtsam::Pose2 between(const gtsam::Pose2& p2) const; 562 | 563 | // Manifold 564 | gtsam::Pose2 retract(Vector v) const; 565 | Vector localCoordinates(const gtsam::Pose2& p) const; 566 | 567 | // Lie Group 568 | static gtsam::Pose2 Expmap(Vector v); 569 | static Vector Logmap(const gtsam::Pose2& p); 570 | Matrix AdjointMap() const; 571 | Vector Adjoint(Vector xi) const; 572 | static Matrix wedge(double vx, double vy, double w); 573 | 574 | // Group Actions on Point2 575 | gtsam::Point2 transform_from(const gtsam::Point2& p) const; 576 | gtsam::Point2 transform_to(const gtsam::Point2& p) const; 577 | 578 | // Standard Interface 579 | double x() const; 580 | double y() const; 581 | double theta() const; 582 | gtsam::Rot2 bearing(const gtsam::Point2& point) const; 583 | double range(const gtsam::Point2& point) const; 584 | gtsam::Point2 translation() const; 585 | gtsam::Rot2 rotation() const; 586 | Matrix matrix() const; 587 | 588 | // enabling serialization functionality 589 | void serialize() const; 590 | }; 591 | 592 | #include 593 | class Pose3 { 594 | // Standard Constructors 595 | Pose3(); 596 | Pose3(const gtsam::Pose3& other); 597 | Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); 598 | Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) 599 | Pose3(Matrix mat); 600 | 601 | // Testable 602 | void print(string s) const; 603 | bool equals(const gtsam::Pose3& pose, double tol) const; 604 | 605 | // Group 606 | static gtsam::Pose3 identity(); 607 | gtsam::Pose3 inverse() const; 608 | gtsam::Pose3 compose(const gtsam::Pose3& pose) const; 609 | gtsam::Pose3 between(const gtsam::Pose3& pose) const; 610 | 611 | // Manifold 612 | gtsam::Pose3 retract(Vector v) const; 613 | Vector localCoordinates(const gtsam::Pose3& pose) const; 614 | 615 | // Lie Group 616 | static gtsam::Pose3 Expmap(Vector v); 617 | static Vector Logmap(const gtsam::Pose3& pose); 618 | Matrix AdjointMap() const; 619 | Vector Adjoint(Vector xi) const; 620 | static Matrix wedge(double wx, double wy, double wz, double vx, double vy, double vz); 621 | 622 | // Group Action on Point3 623 | gtsam::Point3 transform_from(const gtsam::Point3& point) const; 624 | gtsam::Point3 transform_to(const gtsam::Point3& point) const; 625 | 626 | // Standard Interface 627 | gtsam::Rot3 rotation() const; 628 | gtsam::Point3 translation() const; 629 | double x() const; 630 | double y() const; 631 | double z() const; 632 | Matrix matrix() const; 633 | gtsam::Pose3 transform_to(const gtsam::Pose3& pose) const; // FIXME: shadows other transform_to() 634 | double range(const gtsam::Point3& point); 635 | double range(const gtsam::Pose3& pose); 636 | 637 | // enabling serialization functionality 638 | void serialize() const; 639 | }; 640 | 641 | // std::vector 642 | #include 643 | class Pose3Vector 644 | { 645 | Pose3Vector(); 646 | size_t size() const; 647 | bool empty() const; 648 | gtsam::Pose3 at(size_t n) const; 649 | void push_back(const gtsam::Pose3& pose); 650 | }; 651 | 652 | #include 653 | class Unit3 { 654 | // Standard Constructors 655 | Unit3(); 656 | Unit3(const gtsam::Point3& pose); 657 | 658 | // Testable 659 | void print(string s) const; 660 | bool equals(const gtsam::Unit3& pose, double tol) const; 661 | 662 | // Other functionality 663 | Matrix basis() const; 664 | Matrix skew() const; 665 | 666 | // Manifold 667 | static size_t Dim(); 668 | size_t dim() const; 669 | gtsam::Unit3 retract(Vector v) const; 670 | Vector localCoordinates(const gtsam::Unit3& s) const; 671 | }; 672 | 673 | #include 674 | class EssentialMatrix { 675 | // Standard Constructors 676 | EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); 677 | 678 | // Testable 679 | void print(string s) const; 680 | bool equals(const gtsam::EssentialMatrix& pose, double tol) const; 681 | 682 | // Manifold 683 | static size_t Dim(); 684 | size_t dim() const; 685 | gtsam::EssentialMatrix retract(Vector v) const; 686 | Vector localCoordinates(const gtsam::EssentialMatrix& s) const; 687 | 688 | // Other methods: 689 | gtsam::Rot3 rotation() const; 690 | gtsam::Unit3 direction() const; 691 | Matrix matrix() const; 692 | double error(Vector vA, Vector vB); 693 | }; 694 | 695 | #include 696 | class Cal3_S2 { 697 | // Standard Constructors 698 | Cal3_S2(); 699 | Cal3_S2(double fx, double fy, double s, double u0, double v0); 700 | Cal3_S2(Vector v); 701 | Cal3_S2(double fov, int w, int h); 702 | 703 | // Testable 704 | void print(string s) const; 705 | bool equals(const gtsam::Cal3_S2& rhs, double tol) const; 706 | 707 | // Manifold 708 | static size_t Dim(); 709 | size_t dim() const; 710 | gtsam::Cal3_S2 retract(Vector v) const; 711 | Vector localCoordinates(const gtsam::Cal3_S2& c) const; 712 | 713 | // Action on Point2 714 | gtsam::Point2 calibrate(const gtsam::Point2& p) const; 715 | gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; 716 | 717 | // Standard Interface 718 | double fx() const; 719 | double fy() const; 720 | double skew() const; 721 | double px() const; 722 | double py() const; 723 | gtsam::Point2 principalPoint() const; 724 | Vector vector() const; 725 | Matrix matrix() const; 726 | Matrix matrix_inverse() const; 727 | 728 | // enabling serialization functionality 729 | void serialize() const; 730 | }; 731 | 732 | #include 733 | virtual class Cal3DS2_Base { 734 | // Standard Constructors 735 | Cal3DS2_Base(); 736 | 737 | // Testable 738 | void print(string s) const; 739 | 740 | // Standard Interface 741 | double fx() const; 742 | double fy() const; 743 | double skew() const; 744 | double px() const; 745 | double py() const; 746 | double k1() const; 747 | double k2() const; 748 | Matrix K() const; 749 | Vector k() const; 750 | Vector vector() const; 751 | 752 | // Action on Point2 753 | gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; 754 | gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; 755 | 756 | // enabling serialization functionality 757 | void serialize() const; 758 | }; 759 | 760 | #include 761 | virtual class Cal3DS2 : gtsam::Cal3DS2_Base { 762 | // Standard Constructors 763 | Cal3DS2(); 764 | Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2); 765 | Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2); 766 | Cal3DS2(Vector v); 767 | 768 | // Testable 769 | bool equals(const gtsam::Cal3DS2& rhs, double tol) const; 770 | 771 | // Manifold 772 | size_t dim() const; 773 | static size_t Dim(); 774 | gtsam::Cal3DS2 retract(Vector v) const; 775 | Vector localCoordinates(const gtsam::Cal3DS2& c) const; 776 | 777 | // enabling serialization functionality 778 | void serialize() const; 779 | }; 780 | 781 | #include 782 | virtual class Cal3Unified : gtsam::Cal3DS2_Base { 783 | // Standard Constructors 784 | Cal3Unified(); 785 | Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2); 786 | Cal3Unified(double fx, double fy, double s, double u0, double v0, double k1, double k2, double p1, double p2, double xi); 787 | Cal3Unified(Vector v); 788 | 789 | // Testable 790 | bool equals(const gtsam::Cal3Unified& rhs, double tol) const; 791 | 792 | // Standard Interface 793 | double xi() const; 794 | gtsam::Point2 spaceToNPlane(const gtsam::Point2& p) const; 795 | gtsam::Point2 nPlaneToSpace(const gtsam::Point2& p) const; 796 | 797 | // Manifold 798 | size_t dim() const; 799 | static size_t Dim(); 800 | gtsam::Cal3Unified retract(Vector v) const; 801 | Vector localCoordinates(const gtsam::Cal3Unified& c) const; 802 | 803 | // enabling serialization functionality 804 | void serialize() const; 805 | }; 806 | 807 | #include 808 | class Cal3_S2Stereo { 809 | // Standard Constructors 810 | Cal3_S2Stereo(); 811 | Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); 812 | Cal3_S2Stereo(Vector v); 813 | 814 | // Testable 815 | void print(string s) const; 816 | bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; 817 | 818 | // Standard Interface 819 | double fx() const; 820 | double fy() const; 821 | double skew() const; 822 | double px() const; 823 | double py() const; 824 | gtsam::Point2 principalPoint() const; 825 | double baseline() const; 826 | }; 827 | 828 | #include 829 | class Cal3Bundler { 830 | // Standard Constructors 831 | Cal3Bundler(); 832 | Cal3Bundler(double fx, double k1, double k2, double u0, double v0); 833 | 834 | // Testable 835 | void print(string s) const; 836 | bool equals(const gtsam::Cal3Bundler& rhs, double tol) const; 837 | 838 | // Manifold 839 | static size_t Dim(); 840 | size_t dim() const; 841 | gtsam::Cal3Bundler retract(Vector v) const; 842 | Vector localCoordinates(const gtsam::Cal3Bundler& c) const; 843 | 844 | // Action on Point2 845 | gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const; 846 | gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; 847 | 848 | // Standard Interface 849 | double fx() const; 850 | double fy() const; 851 | double k1() const; 852 | double k2() const; 853 | double u0() const; 854 | double v0() const; 855 | Vector vector() const; 856 | Vector k() const; 857 | //Matrix K() const; //FIXME: Uppercase 858 | 859 | // enabling serialization functionality 860 | void serialize() const; 861 | }; 862 | 863 | #include 864 | class CalibratedCamera { 865 | // Standard Constructors and Named Constructors 866 | CalibratedCamera(); 867 | CalibratedCamera(const gtsam::Pose3& pose); 868 | CalibratedCamera(Vector v); 869 | static gtsam::CalibratedCamera Level(const gtsam::Pose2& pose2, double height); 870 | 871 | // Testable 872 | void print(string s) const; 873 | bool equals(const gtsam::CalibratedCamera& camera, double tol) const; 874 | 875 | // Manifold 876 | static size_t Dim(); 877 | size_t dim() const; 878 | gtsam::CalibratedCamera retract(Vector d) const; 879 | Vector localCoordinates(const gtsam::CalibratedCamera& T2) const; 880 | 881 | // Action on Point3 882 | gtsam::Point2 project(const gtsam::Point3& point) const; 883 | static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); 884 | 885 | // Standard Interface 886 | gtsam::Pose3 pose() const; 887 | double range(const gtsam::Point3& p) const; // TODO: Other overloaded range methods 888 | 889 | // enabling serialization functionality 890 | void serialize() const; 891 | }; 892 | 893 | #include 894 | template 895 | class PinholeCamera { 896 | // Standard Constructors and Named Constructors 897 | PinholeCamera(); 898 | PinholeCamera(const gtsam::Pose3& pose); 899 | PinholeCamera(const gtsam::Pose3& pose, const CALIBRATION& K); 900 | static This Level(const CALIBRATION& K, const gtsam::Pose2& pose, double height); 901 | static This Level(const gtsam::Pose2& pose, double height); 902 | static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, 903 | const gtsam::Point3& upVector, const CALIBRATION& K); 904 | 905 | // Testable 906 | void print(string s) const; 907 | bool equals(const This& camera, double tol) const; 908 | 909 | // Standard Interface 910 | gtsam::Pose3 pose() const; 911 | CALIBRATION calibration() const; 912 | 913 | // Manifold 914 | This retract(Vector d) const; 915 | Vector localCoordinates(const This& T2) const; 916 | size_t dim() const; 917 | static size_t Dim(); 918 | 919 | // Transformations and measurement functions 920 | static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); 921 | pair projectSafe(const gtsam::Point3& pw) const; 922 | gtsam::Point2 project(const gtsam::Point3& point); 923 | gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; 924 | double range(const gtsam::Point3& point); 925 | double range(const gtsam::Pose3& pose); 926 | 927 | // enabling serialization functionality 928 | void serialize() const; 929 | }; 930 | 931 | #include 932 | virtual class SimpleCamera { 933 | // Standard Constructors and Named Constructors 934 | SimpleCamera(); 935 | SimpleCamera(const gtsam::Pose3& pose); 936 | SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K); 937 | static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height); 938 | static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height); 939 | static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, 940 | const gtsam::Point3& upVector, const gtsam::Cal3_S2& K); 941 | static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, 942 | const gtsam::Point3& upVector); 943 | 944 | // Testable 945 | void print(string s) const; 946 | bool equals(const gtsam::SimpleCamera& camera, double tol) const; 947 | 948 | // Standard Interface 949 | gtsam::Pose3 pose() const; 950 | gtsam::Cal3_S2 calibration() const; 951 | 952 | // Manifold 953 | gtsam::SimpleCamera retract(Vector d) const; 954 | Vector localCoordinates(const gtsam::SimpleCamera& T2) const; 955 | size_t dim() const; 956 | static size_t Dim(); 957 | 958 | // Transformations and measurement functions 959 | static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); 960 | pair projectSafe(const gtsam::Point3& pw) const; 961 | gtsam::Point2 project(const gtsam::Point3& point); 962 | gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; 963 | double range(const gtsam::Point3& point); 964 | double range(const gtsam::Pose3& pose); 965 | 966 | // enabling serialization functionality 967 | void serialize() const; 968 | 969 | }; 970 | 971 | // Some typedefs for common camera types 972 | // PinholeCameraCal3_S2 is the same as SimpleCamera above 973 | typedef gtsam::PinholeCamera PinholeCameraCal3_S2; 974 | // due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified 975 | //typedef gtsam::PinholeCamera PinholeCameraCal3DS2; 976 | //typedef gtsam::PinholeCamera PinholeCameraCal3Unified; 977 | //typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; 978 | 979 | #include 980 | class StereoCamera { 981 | // Standard Constructors and Named Constructors 982 | StereoCamera(); 983 | StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); 984 | 985 | // Testable 986 | void print(string s) const; 987 | bool equals(const gtsam::StereoCamera& camera, double tol) const; 988 | 989 | // Standard Interface 990 | gtsam::Pose3 pose() const; 991 | double baseline() const; 992 | gtsam::Cal3_S2Stereo calibration() const; 993 | 994 | // Manifold 995 | gtsam::StereoCamera retract(Vector d) const; 996 | Vector localCoordinates(const gtsam::StereoCamera& T2) const; 997 | size_t dim() const; 998 | static size_t Dim(); 999 | 1000 | // Transformations and measurement functions 1001 | gtsam::StereoPoint2 project(const gtsam::Point3& point); 1002 | gtsam::Point3 backproject(const gtsam::StereoPoint2& p) const; 1003 | 1004 | // enabling serialization functionality 1005 | void serialize() const; 1006 | }; 1007 | 1008 | #include 1009 | 1010 | // Templates appear not yet supported for free functions 1011 | gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, 1012 | gtsam::Cal3_S2* sharedCal, const gtsam::Point2Vector& measurements, 1013 | double rank_tol, bool optimize); 1014 | gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, 1015 | gtsam::Cal3Bundler* sharedCal, const gtsam::Point2Vector& measurements, 1016 | double rank_tol, bool optimize); 1017 | 1018 | //************************************************************************* 1019 | // Symbolic 1020 | //************************************************************************* 1021 | 1022 | #include 1023 | virtual class SymbolicFactor { 1024 | // Standard Constructors and Named Constructors 1025 | SymbolicFactor(const gtsam::SymbolicFactor& f); 1026 | SymbolicFactor(); 1027 | SymbolicFactor(size_t j); 1028 | SymbolicFactor(size_t j1, size_t j2); 1029 | SymbolicFactor(size_t j1, size_t j2, size_t j3); 1030 | SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4); 1031 | SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5); 1032 | SymbolicFactor(size_t j1, size_t j2, size_t j3, size_t j4, size_t j5, size_t j6); 1033 | static gtsam::SymbolicFactor FromKeys(const gtsam::KeyVector& js); 1034 | 1035 | // From Factor 1036 | size_t size() const; 1037 | void print(string s) const; 1038 | bool equals(const gtsam::SymbolicFactor& other, double tol) const; 1039 | gtsam::KeyVector keys(); 1040 | }; 1041 | 1042 | #include 1043 | virtual class SymbolicFactorGraph { 1044 | SymbolicFactorGraph(); 1045 | SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); 1046 | SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); 1047 | 1048 | // From FactorGraph 1049 | void push_back(gtsam::SymbolicFactor* factor); 1050 | void print(string s) const; 1051 | bool equals(const gtsam::SymbolicFactorGraph& rhs, double tol) const; 1052 | size_t size() const; 1053 | bool exists(size_t idx) const; 1054 | 1055 | // Standard interface 1056 | gtsam::KeySet keys() const; 1057 | void push_back(const gtsam::SymbolicFactorGraph& graph); 1058 | void push_back(const gtsam::SymbolicBayesNet& bayesNet); 1059 | void push_back(const gtsam::SymbolicBayesTree& bayesTree); 1060 | 1061 | //Advanced Interface 1062 | void push_factor(size_t key); 1063 | void push_factor(size_t key1, size_t key2); 1064 | void push_factor(size_t key1, size_t key2, size_t key3); 1065 | void push_factor(size_t key1, size_t key2, size_t key3, size_t key4); 1066 | 1067 | gtsam::SymbolicBayesNet* eliminateSequential(); 1068 | gtsam::SymbolicBayesNet* eliminateSequential(const gtsam::Ordering& ordering); 1069 | gtsam::SymbolicBayesTree* eliminateMultifrontal(); 1070 | gtsam::SymbolicBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); 1071 | pair eliminatePartialSequential( 1072 | const gtsam::Ordering& ordering); 1073 | pair eliminatePartialSequential( 1074 | const gtsam::KeyVector& keys); 1075 | pair eliminatePartialMultifrontal( 1076 | const gtsam::Ordering& ordering); 1077 | pair eliminatePartialMultifrontal( 1078 | const gtsam::KeyVector& keys); 1079 | gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); 1080 | gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); 1081 | gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, 1082 | const gtsam::Ordering& marginalizedVariableOrdering); 1083 | gtsam::SymbolicBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, 1084 | const gtsam::Ordering& marginalizedVariableOrdering); 1085 | gtsam::SymbolicFactorGraph* marginal(const gtsam::KeyVector& key_vector); 1086 | }; 1087 | 1088 | #include 1089 | virtual class SymbolicConditional : gtsam::SymbolicFactor { 1090 | // Standard Constructors and Named Constructors 1091 | SymbolicConditional(); 1092 | SymbolicConditional(const gtsam::SymbolicConditional& other); 1093 | SymbolicConditional(size_t key); 1094 | SymbolicConditional(size_t key, size_t parent); 1095 | SymbolicConditional(size_t key, size_t parent1, size_t parent2); 1096 | SymbolicConditional(size_t key, size_t parent1, size_t parent2, size_t parent3); 1097 | static gtsam::SymbolicConditional FromKeys(const gtsam::KeyVector& keys, size_t nrFrontals); 1098 | 1099 | // Testable 1100 | void print(string s) const; 1101 | bool equals(const gtsam::SymbolicConditional& other, double tol) const; 1102 | 1103 | // Standard interface 1104 | size_t nrFrontals() const; 1105 | size_t nrParents() const; 1106 | }; 1107 | 1108 | #include 1109 | class SymbolicBayesNet { 1110 | SymbolicBayesNet(); 1111 | SymbolicBayesNet(const gtsam::SymbolicBayesNet& other); 1112 | // Testable 1113 | void print(string s) const; 1114 | bool equals(const gtsam::SymbolicBayesNet& other, double tol) const; 1115 | 1116 | // Standard interface 1117 | size_t size() const; 1118 | void saveGraph(string s) const; 1119 | gtsam::SymbolicConditional* at(size_t idx) const; 1120 | gtsam::SymbolicConditional* front() const; 1121 | gtsam::SymbolicConditional* back() const; 1122 | void push_back(gtsam::SymbolicConditional* conditional); 1123 | void push_back(const gtsam::SymbolicBayesNet& bayesNet); 1124 | }; 1125 | 1126 | #include 1127 | class SymbolicBayesTree { 1128 | 1129 | //Constructors 1130 | SymbolicBayesTree(); 1131 | SymbolicBayesTree(const gtsam::SymbolicBayesTree& other); 1132 | 1133 | // Testable 1134 | void print(string s); 1135 | bool equals(const gtsam::SymbolicBayesTree& other, double tol) const; 1136 | 1137 | //Standard Interface 1138 | //size_t findParentClique(const gtsam::IndexVector& parents) const; 1139 | size_t size(); 1140 | void saveGraph(string s) const; 1141 | void clear(); 1142 | void deleteCachedShortcuts(); 1143 | size_t numCachedSeparatorMarginals() const; 1144 | 1145 | gtsam::SymbolicConditional* marginalFactor(size_t key) const; 1146 | gtsam::SymbolicFactorGraph* joint(size_t key1, size_t key2) const; 1147 | gtsam::SymbolicBayesNet* jointBayesNet(size_t key1, size_t key2) const; 1148 | }; 1149 | 1150 | // class SymbolicBayesTreeClique { 1151 | // BayesTreeClique(); 1152 | // BayesTreeClique(CONDITIONAL* conditional); 1153 | // // BayesTreeClique(const std::pair& result) : Base(result) {} 1154 | // 1155 | // bool equals(const This& other, double tol) const; 1156 | // void print(string s) const; 1157 | // void printTree() const; // Default indent of "" 1158 | // void printTree(string indent) const; 1159 | // size_t numCachedSeparatorMarginals() const; 1160 | // 1161 | // CONDITIONAL* conditional() const; 1162 | // bool isRoot() const; 1163 | // size_t treeSize() const; 1164 | // // const std::list& children() const { return children_; } 1165 | // // derived_ptr parent() const { return parent_.lock(); } 1166 | // 1167 | // // FIXME: need wrapped versions graphs, BayesNet 1168 | // // BayesNet shortcut(derived_ptr root, Eliminate function) const; 1169 | // // FactorGraph marginal(derived_ptr root, Eliminate function) const; 1170 | // // FactorGraph joint(derived_ptr C2, derived_ptr root, Eliminate function) const; 1171 | // 1172 | // void deleteCachedShortcuts(); 1173 | // }; 1174 | 1175 | #include 1176 | class VariableIndex { 1177 | // Standard Constructors and Named Constructors 1178 | VariableIndex(); 1179 | // TODO: Templetize constructor when wrap supports it 1180 | //template 1181 | //VariableIndex(const T& factorGraph, size_t nVariables); 1182 | //VariableIndex(const T& factorGraph); 1183 | VariableIndex(const gtsam::SymbolicFactorGraph& sfg); 1184 | VariableIndex(const gtsam::GaussianFactorGraph& gfg); 1185 | VariableIndex(const gtsam::NonlinearFactorGraph& fg); 1186 | VariableIndex(const gtsam::VariableIndex& other); 1187 | 1188 | // Testable 1189 | bool equals(const gtsam::VariableIndex& other, double tol) const; 1190 | void print(string s) const; 1191 | 1192 | // Standard interface 1193 | size_t size() const; 1194 | size_t nFactors() const; 1195 | size_t nEntries() const; 1196 | }; 1197 | 1198 | //************************************************************************* 1199 | // linear 1200 | //************************************************************************* 1201 | 1202 | namespace noiseModel { 1203 | #include 1204 | virtual class Base { 1205 | }; 1206 | 1207 | virtual class Gaussian : gtsam::noiseModel::Base { 1208 | static gtsam::noiseModel::Gaussian* SqrtInformation(Matrix R); 1209 | static gtsam::noiseModel::Gaussian* Covariance(Matrix R); 1210 | Matrix R() const; 1211 | bool equals(gtsam::noiseModel::Base& expected, double tol); 1212 | void print(string s) const; 1213 | 1214 | // enabling serialization functionality 1215 | void serializable() const; 1216 | }; 1217 | 1218 | virtual class Diagonal : gtsam::noiseModel::Gaussian { 1219 | static gtsam::noiseModel::Diagonal* Sigmas(Vector sigmas); 1220 | static gtsam::noiseModel::Diagonal* Variances(Vector variances); 1221 | static gtsam::noiseModel::Diagonal* Precisions(Vector precisions); 1222 | Matrix R() const; 1223 | void print(string s) const; 1224 | 1225 | // enabling serialization functionality 1226 | void serializable() const; 1227 | }; 1228 | 1229 | virtual class Constrained : gtsam::noiseModel::Diagonal { 1230 | static gtsam::noiseModel::Constrained* MixedSigmas(Vector mu, Vector sigmas); 1231 | static gtsam::noiseModel::Constrained* MixedSigmas(double m, Vector sigmas); 1232 | static gtsam::noiseModel::Constrained* MixedVariances(Vector mu, Vector variances); 1233 | static gtsam::noiseModel::Constrained* MixedVariances(Vector variances); 1234 | static gtsam::noiseModel::Constrained* MixedPrecisions(Vector mu, Vector precisions); 1235 | static gtsam::noiseModel::Constrained* MixedPrecisions(Vector precisions); 1236 | 1237 | static gtsam::noiseModel::Constrained* All(size_t dim); 1238 | static gtsam::noiseModel::Constrained* All(size_t dim, double mu); 1239 | 1240 | gtsam::noiseModel::Constrained* unit() const; 1241 | 1242 | // enabling serialization functionality 1243 | void serializable() const; 1244 | }; 1245 | 1246 | virtual class Isotropic : gtsam::noiseModel::Diagonal { 1247 | static gtsam::noiseModel::Isotropic* Sigma(size_t dim, double sigma); 1248 | static gtsam::noiseModel::Isotropic* Variance(size_t dim, double varianace); 1249 | static gtsam::noiseModel::Isotropic* Precision(size_t dim, double precision); 1250 | void print(string s) const; 1251 | 1252 | // enabling serialization functionality 1253 | void serializable() const; 1254 | }; 1255 | 1256 | virtual class Unit : gtsam::noiseModel::Isotropic { 1257 | static gtsam::noiseModel::Unit* Create(size_t dim); 1258 | void print(string s) const; 1259 | 1260 | // enabling serialization functionality 1261 | void serializable() const; 1262 | }; 1263 | 1264 | namespace mEstimator { 1265 | virtual class Base { 1266 | }; 1267 | 1268 | virtual class Null: gtsam::noiseModel::mEstimator::Base { 1269 | Null(); 1270 | void print(string s) const; 1271 | static gtsam::noiseModel::mEstimator::Null* Create(); 1272 | 1273 | // enabling serialization functionality 1274 | void serializable() const; 1275 | }; 1276 | 1277 | virtual class Fair: gtsam::noiseModel::mEstimator::Base { 1278 | Fair(double c); 1279 | void print(string s) const; 1280 | static gtsam::noiseModel::mEstimator::Fair* Create(double c); 1281 | 1282 | // enabling serialization functionality 1283 | void serializable() const; 1284 | }; 1285 | 1286 | virtual class Huber: gtsam::noiseModel::mEstimator::Base { 1287 | Huber(double k); 1288 | void print(string s) const; 1289 | static gtsam::noiseModel::mEstimator::Huber* Create(double k); 1290 | 1291 | // enabling serialization functionality 1292 | void serializable() const; 1293 | }; 1294 | 1295 | virtual class Tukey: gtsam::noiseModel::mEstimator::Base { 1296 | Tukey(double k); 1297 | void print(string s) const; 1298 | static gtsam::noiseModel::mEstimator::Tukey* Create(double k); 1299 | 1300 | // enabling serialization functionality 1301 | void serializable() const; 1302 | }; 1303 | 1304 | }///\namespace mEstimator 1305 | 1306 | virtual class Robust : gtsam::noiseModel::Base { 1307 | Robust(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); 1308 | static gtsam::noiseModel::Robust* Create(const gtsam::noiseModel::mEstimator::Base* robust, const gtsam::noiseModel::Base* noise); 1309 | void print(string s) const; 1310 | 1311 | // enabling serialization functionality 1312 | void serializable() const; 1313 | }; 1314 | 1315 | }///\namespace noiseModel 1316 | 1317 | #include 1318 | class Sampler { 1319 | //Constructors 1320 | Sampler(gtsam::noiseModel::Diagonal* model, int seed); 1321 | Sampler(Vector sigmas, int seed); 1322 | Sampler(int seed); 1323 | 1324 | //Standard Interface 1325 | size_t dim() const; 1326 | Vector sigmas() const; 1327 | gtsam::noiseModel::Diagonal* model() const; 1328 | Vector sample(); 1329 | Vector sampleNewModel(gtsam::noiseModel::Diagonal* model); 1330 | }; 1331 | 1332 | #include 1333 | class VectorValues { 1334 | //Constructors 1335 | VectorValues(); 1336 | VectorValues(const gtsam::VectorValues& other); 1337 | 1338 | //Named Constructors 1339 | static gtsam::VectorValues Zero(const gtsam::VectorValues& model); 1340 | 1341 | //Standard Interface 1342 | size_t size() const; 1343 | size_t dim(size_t j) const; 1344 | bool exists(size_t j) const; 1345 | void print(string s) const; 1346 | bool equals(const gtsam::VectorValues& expected, double tol) const; 1347 | void insert(size_t j, Vector value); 1348 | Vector vector() const; 1349 | Vector at(size_t j) const; 1350 | void update(const gtsam::VectorValues& values); 1351 | 1352 | //Advanced Interface 1353 | void setZero(); 1354 | 1355 | gtsam::VectorValues add(const gtsam::VectorValues& c) const; 1356 | void addInPlace(const gtsam::VectorValues& c); 1357 | gtsam::VectorValues subtract(const gtsam::VectorValues& c) const; 1358 | gtsam::VectorValues scale(double a) const; 1359 | void scaleInPlace(double a); 1360 | 1361 | bool hasSameStructure(const gtsam::VectorValues& other) const; 1362 | double dot(const gtsam::VectorValues& V) const; 1363 | double norm() const; 1364 | double squaredNorm() const; 1365 | 1366 | // enabling serialization functionality 1367 | void serialize() const; 1368 | }; 1369 | 1370 | #include 1371 | virtual class GaussianFactor { 1372 | gtsam::KeyVector keys() const; 1373 | void print(string s) const; 1374 | bool equals(const gtsam::GaussianFactor& lf, double tol) const; 1375 | double error(const gtsam::VectorValues& c) const; 1376 | gtsam::GaussianFactor* clone() const; 1377 | gtsam::GaussianFactor* negate() const; 1378 | Matrix augmentedInformation() const; 1379 | Matrix information() const; 1380 | Matrix augmentedJacobian() const; 1381 | pair jacobian() const; 1382 | size_t size() const; 1383 | bool empty() const; 1384 | }; 1385 | 1386 | #include 1387 | virtual class JacobianFactor : gtsam::GaussianFactor { 1388 | //Constructors 1389 | JacobianFactor(); 1390 | JacobianFactor(const gtsam::GaussianFactor& factor); 1391 | JacobianFactor(Vector b_in); 1392 | JacobianFactor(size_t i1, Matrix A1, Vector b, 1393 | const gtsam::noiseModel::Diagonal* model); 1394 | JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, 1395 | const gtsam::noiseModel::Diagonal* model); 1396 | JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, Matrix A3, 1397 | Vector b, const gtsam::noiseModel::Diagonal* model); 1398 | JacobianFactor(const gtsam::GaussianFactorGraph& graph); 1399 | 1400 | //Testable 1401 | void print(string s) const; 1402 | void printKeys(string s) const; 1403 | bool equals(const gtsam::GaussianFactor& lf, double tol) const; 1404 | size_t size() const; 1405 | Vector unweighted_error(const gtsam::VectorValues& c) const; 1406 | Vector error_vector(const gtsam::VectorValues& c) const; 1407 | double error(const gtsam::VectorValues& c) const; 1408 | 1409 | //Standard Interface 1410 | Matrix getA() const; 1411 | Vector getb() const; 1412 | size_t rows() const; 1413 | size_t cols() const; 1414 | bool isConstrained() const; 1415 | pair jacobianUnweighted() const; 1416 | Matrix augmentedJacobianUnweighted() const; 1417 | 1418 | void transposeMultiplyAdd(double alpha, Vector e, gtsam::VectorValues& x) const; 1419 | gtsam::JacobianFactor whiten() const; 1420 | 1421 | pair eliminate(const gtsam::Ordering& keys) const; 1422 | 1423 | void setModel(bool anyConstrained, Vector sigmas); 1424 | 1425 | gtsam::noiseModel::Diagonal* get_model() const; 1426 | 1427 | // enabling serialization functionality 1428 | void serialize() const; 1429 | }; 1430 | 1431 | #include 1432 | virtual class HessianFactor : gtsam::GaussianFactor { 1433 | //Constructors 1434 | HessianFactor(); 1435 | HessianFactor(const gtsam::GaussianFactor& factor); 1436 | HessianFactor(size_t j, Matrix G, Vector g, double f); 1437 | HessianFactor(size_t j, Vector mu, Matrix Sigma); 1438 | HessianFactor(size_t j1, size_t j2, Matrix G11, Matrix G12, Vector g1, Matrix G22, 1439 | Vector g2, double f); 1440 | HessianFactor(size_t j1, size_t j2, size_t j3, Matrix G11, Matrix G12, Matrix G13, 1441 | Vector g1, Matrix G22, Matrix G23, Vector g2, Matrix G33, Vector g3, 1442 | double f); 1443 | HessianFactor(const gtsam::GaussianFactorGraph& factors); 1444 | 1445 | //Testable 1446 | size_t size() const; 1447 | void print(string s) const; 1448 | void printKeys(string s) const; 1449 | bool equals(const gtsam::GaussianFactor& lf, double tol) const; 1450 | double error(const gtsam::VectorValues& c) const; 1451 | 1452 | //Standard Interface 1453 | size_t rows() const; 1454 | Matrix information() const; 1455 | double constantTerm() const; 1456 | Vector linearTerm() const; 1457 | 1458 | // enabling serialization functionality 1459 | void serialize() const; 1460 | }; 1461 | 1462 | #include 1463 | class GaussianFactorGraph { 1464 | GaussianFactorGraph(); 1465 | GaussianFactorGraph(const gtsam::GaussianBayesNet& bayesNet); 1466 | GaussianFactorGraph(const gtsam::GaussianBayesTree& bayesTree); 1467 | 1468 | // From FactorGraph 1469 | void print(string s) const; 1470 | bool equals(const gtsam::GaussianFactorGraph& lfgraph, double tol) const; 1471 | size_t size() const; 1472 | gtsam::GaussianFactor* at(size_t idx) const; 1473 | gtsam::KeySet keys() const; 1474 | gtsam::KeyVector keyVector() const; 1475 | bool exists(size_t idx) const; 1476 | 1477 | // Building the graph 1478 | void push_back(const gtsam::GaussianFactor* factor); 1479 | void push_back(const gtsam::GaussianConditional* conditional); 1480 | void push_back(const gtsam::GaussianFactorGraph& graph); 1481 | void push_back(const gtsam::GaussianBayesNet& bayesNet); 1482 | void push_back(const gtsam::GaussianBayesTree& bayesTree); 1483 | void add(const gtsam::GaussianFactor& factor); 1484 | void add(Vector b); 1485 | void add(size_t key1, Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); 1486 | void add(size_t key1, Matrix A1, size_t key2, Matrix A2, Vector b, 1487 | const gtsam::noiseModel::Diagonal* model); 1488 | void add(size_t key1, Matrix A1, size_t key2, Matrix A2, size_t key3, Matrix A3, 1489 | Vector b, const gtsam::noiseModel::Diagonal* model); 1490 | 1491 | // error and probability 1492 | double error(const gtsam::VectorValues& c) const; 1493 | double probPrime(const gtsam::VectorValues& c) const; 1494 | 1495 | gtsam::GaussianFactorGraph clone() const; 1496 | gtsam::GaussianFactorGraph negate() const; 1497 | 1498 | // Optimizing and linear algebra 1499 | gtsam::VectorValues optimize() const; 1500 | gtsam::VectorValues optimize(const gtsam::Ordering& ordering) const; 1501 | gtsam::VectorValues optimizeGradientSearch() const; 1502 | gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; 1503 | gtsam::VectorValues gradientAtZero() const; 1504 | 1505 | // Elimination and marginals 1506 | gtsam::GaussianBayesNet* eliminateSequential(); 1507 | gtsam::GaussianBayesNet* eliminateSequential(const gtsam::Ordering& ordering); 1508 | gtsam::GaussianBayesTree* eliminateMultifrontal(); 1509 | gtsam::GaussianBayesTree* eliminateMultifrontal(const gtsam::Ordering& ordering); 1510 | pair eliminatePartialSequential( 1511 | const gtsam::Ordering& ordering); 1512 | pair eliminatePartialSequential( 1513 | const gtsam::KeyVector& keys); 1514 | pair eliminatePartialMultifrontal( 1515 | const gtsam::Ordering& ordering); 1516 | pair eliminatePartialMultifrontal( 1517 | const gtsam::KeyVector& keys); 1518 | gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering); 1519 | gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector); 1520 | gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::Ordering& ordering, 1521 | const gtsam::Ordering& marginalizedVariableOrdering); 1522 | gtsam::GaussianBayesNet* marginalMultifrontalBayesNet(const gtsam::KeyVector& key_vector, 1523 | const gtsam::Ordering& marginalizedVariableOrdering); 1524 | gtsam::GaussianFactorGraph* marginal(const gtsam::KeyVector& key_vector); 1525 | 1526 | // Conversion to matrices 1527 | Matrix sparseJacobian_() const; 1528 | Matrix augmentedJacobian() const; 1529 | Matrix augmentedJacobian(const gtsam::Ordering& ordering) const; 1530 | pair jacobian() const; 1531 | pair jacobian(const gtsam::Ordering& ordering) const; 1532 | Matrix augmentedHessian() const; 1533 | Matrix augmentedHessian(const gtsam::Ordering& ordering) const; 1534 | pair hessian() const; 1535 | pair hessian(const gtsam::Ordering& ordering) const; 1536 | 1537 | // enabling serialization functionality 1538 | void serialize() const; 1539 | }; 1540 | 1541 | #include 1542 | virtual class GaussianConditional : gtsam::GaussianFactor { 1543 | //Constructors 1544 | GaussianConditional(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); 1545 | GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, 1546 | const gtsam::noiseModel::Diagonal* sigmas); 1547 | GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, 1548 | size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); 1549 | 1550 | //Constructors with no noise model 1551 | GaussianConditional(size_t key, Vector d, Matrix R); 1552 | GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); 1553 | GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, 1554 | size_t name2, Matrix T); 1555 | 1556 | //Standard Interface 1557 | void print(string s) const; 1558 | bool equals(const gtsam::GaussianConditional &cg, double tol) const; 1559 | 1560 | //Advanced Interface 1561 | gtsam::VectorValues solve(const gtsam::VectorValues& parents) const; 1562 | gtsam::VectorValues solveOtherRHS(const gtsam::VectorValues& parents, const gtsam::VectorValues& rhs) const; 1563 | void solveTransposeInPlace(gtsam::VectorValues& gy) const; 1564 | void scaleFrontalsBySigma(gtsam::VectorValues& gy) const; 1565 | 1566 | // enabling serialization functionality 1567 | void serialize() const; 1568 | }; 1569 | 1570 | #include 1571 | virtual class GaussianDensity : gtsam::GaussianConditional { 1572 | //Constructors 1573 | GaussianDensity(size_t key, Vector d, Matrix R, const gtsam::noiseModel::Diagonal* sigmas); 1574 | 1575 | //Standard Interface 1576 | void print(string s) const; 1577 | bool equals(const gtsam::GaussianDensity &cg, double tol) const; 1578 | Vector mean() const; 1579 | Matrix covariance() const; 1580 | }; 1581 | 1582 | #include 1583 | virtual class GaussianBayesNet { 1584 | //Constructors 1585 | GaussianBayesNet(); 1586 | GaussianBayesNet(const gtsam::GaussianConditional* conditional); 1587 | 1588 | // Testable 1589 | void print(string s) const; 1590 | bool equals(const gtsam::GaussianBayesNet& other, double tol) const; 1591 | size_t size() const; 1592 | 1593 | // FactorGraph derived interface 1594 | // size_t size() const; 1595 | gtsam::GaussianConditional* at(size_t idx) const; 1596 | gtsam::KeySet keys() const; 1597 | bool exists(size_t idx) const; 1598 | 1599 | gtsam::GaussianConditional* front() const; 1600 | gtsam::GaussianConditional* back() const; 1601 | void push_back(gtsam::GaussianConditional* conditional); 1602 | void push_back(const gtsam::GaussianBayesNet& bayesNet); 1603 | 1604 | gtsam::VectorValues optimize() const; 1605 | gtsam::VectorValues optimize(gtsam::VectorValues& solutionForMissing) const; 1606 | gtsam::VectorValues optimizeGradientSearch() const; 1607 | gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; 1608 | gtsam::VectorValues gradientAtZero() const; 1609 | double error(const gtsam::VectorValues& x) const; 1610 | double determinant() const; 1611 | double logDeterminant() const; 1612 | gtsam::VectorValues backSubstitute(const gtsam::VectorValues& gx) const; 1613 | gtsam::VectorValues backSubstituteTranspose(const gtsam::VectorValues& gx) const; 1614 | }; 1615 | 1616 | #include 1617 | virtual class GaussianBayesTree { 1618 | // Standard Constructors and Named Constructors 1619 | GaussianBayesTree(); 1620 | GaussianBayesTree(const gtsam::GaussianBayesTree& other); 1621 | bool equals(const gtsam::GaussianBayesTree& other, double tol) const; 1622 | void print(string s); 1623 | size_t size() const; 1624 | bool empty() const; 1625 | size_t numCachedSeparatorMarginals() const; 1626 | void saveGraph(string s) const; 1627 | 1628 | gtsam::VectorValues optimize() const; 1629 | gtsam::VectorValues optimizeGradientSearch() const; 1630 | gtsam::VectorValues gradient(const gtsam::VectorValues& x0) const; 1631 | gtsam::VectorValues gradientAtZero() const; 1632 | double error(const gtsam::VectorValues& x) const; 1633 | double determinant() const; 1634 | double logDeterminant() const; 1635 | Matrix marginalCovariance(size_t key) const; 1636 | gtsam::GaussianConditional* marginalFactor(size_t key) const; 1637 | gtsam::GaussianFactorGraph* joint(size_t key1, size_t key2) const; 1638 | gtsam::GaussianBayesNet* jointBayesNet(size_t key1, size_t key2) const; 1639 | }; 1640 | 1641 | #include 1642 | class Errors { 1643 | //Constructors 1644 | Errors(); 1645 | Errors(const gtsam::VectorValues& V); 1646 | 1647 | //Testable 1648 | void print(string s); 1649 | bool equals(const gtsam::Errors& expected, double tol) const; 1650 | }; 1651 | 1652 | #include 1653 | class GaussianISAM { 1654 | //Constructor 1655 | GaussianISAM(); 1656 | 1657 | //Standard Interface 1658 | void update(const gtsam::GaussianFactorGraph& newFactors); 1659 | void saveGraph(string s) const; 1660 | void clear(); 1661 | }; 1662 | 1663 | #include 1664 | virtual class IterativeOptimizationParameters { 1665 | string getVerbosity() const; 1666 | void setVerbosity(string s) ; 1667 | void print() const; 1668 | }; 1669 | 1670 | //virtual class IterativeSolver { 1671 | // IterativeSolver(); 1672 | // gtsam::VectorValues optimize (); 1673 | //}; 1674 | 1675 | #include 1676 | virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters { 1677 | ConjugateGradientParameters(); 1678 | int getMinIterations() const ; 1679 | int getMaxIterations() const ; 1680 | int getReset() const; 1681 | double getEpsilon_rel() const; 1682 | double getEpsilon_abs() const; 1683 | 1684 | void setMinIterations(int value); 1685 | void setMaxIterations(int value); 1686 | void setReset(int value); 1687 | void setEpsilon_rel(double value); 1688 | void setEpsilon_abs(double value); 1689 | void print() const; 1690 | }; 1691 | 1692 | #include 1693 | virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { 1694 | SubgraphSolverParameters(); 1695 | void print() const; 1696 | }; 1697 | 1698 | virtual class SubgraphSolver { 1699 | SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); 1700 | SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); 1701 | gtsam::VectorValues optimize() const; 1702 | }; 1703 | 1704 | #include 1705 | class KalmanFilter { 1706 | KalmanFilter(size_t n); 1707 | // gtsam::GaussianDensity* init(Vector x0, const gtsam::SharedDiagonal& P0); 1708 | gtsam::GaussianDensity* init(Vector x0, Matrix P0); 1709 | void print(string s) const; 1710 | static size_t step(gtsam::GaussianDensity* p); 1711 | gtsam::GaussianDensity* predict(gtsam::GaussianDensity* p, Matrix F, 1712 | Matrix B, Vector u, const gtsam::noiseModel::Diagonal* modelQ); 1713 | gtsam::GaussianDensity* predictQ(gtsam::GaussianDensity* p, Matrix F, 1714 | Matrix B, Vector u, Matrix Q); 1715 | gtsam::GaussianDensity* predict2(gtsam::GaussianDensity* p, Matrix A0, 1716 | Matrix A1, Vector b, const gtsam::noiseModel::Diagonal* model); 1717 | gtsam::GaussianDensity* update(gtsam::GaussianDensity* p, Matrix H, 1718 | Vector z, const gtsam::noiseModel::Diagonal* model); 1719 | gtsam::GaussianDensity* updateQ(gtsam::GaussianDensity* p, Matrix H, 1720 | Vector z, Matrix Q); 1721 | }; 1722 | 1723 | //************************************************************************* 1724 | // nonlinear 1725 | //************************************************************************* 1726 | 1727 | #include 1728 | size_t symbol(char chr, size_t index); 1729 | char symbolChr(size_t key); 1730 | size_t symbolIndex(size_t key); 1731 | 1732 | // Default keyformatter 1733 | void PrintKeyList (const gtsam::KeyList& keys); 1734 | void PrintKeyList (const gtsam::KeyList& keys, string s); 1735 | void PrintKeyVector(const gtsam::KeyVector& keys); 1736 | void PrintKeyVector(const gtsam::KeyVector& keys, string s); 1737 | void PrintKeySet (const gtsam::KeySet& keys); 1738 | void PrintKeySet (const gtsam::KeySet& keys, string s); 1739 | 1740 | #include 1741 | class LabeledSymbol { 1742 | LabeledSymbol(size_t full_key); 1743 | LabeledSymbol(const gtsam::LabeledSymbol& key); 1744 | LabeledSymbol(unsigned char valType, unsigned char label, size_t j); 1745 | 1746 | size_t key() const; 1747 | unsigned char label() const; 1748 | unsigned char chr() const; 1749 | size_t index() const; 1750 | 1751 | gtsam::LabeledSymbol upper() const; 1752 | gtsam::LabeledSymbol lower() const; 1753 | gtsam::LabeledSymbol newChr(unsigned char c) const; 1754 | gtsam::LabeledSymbol newLabel(unsigned char label) const; 1755 | 1756 | void print(string s) const; 1757 | }; 1758 | 1759 | size_t mrsymbol(unsigned char c, unsigned char label, size_t j); 1760 | unsigned char mrsymbolChr(size_t key); 1761 | unsigned char mrsymbolLabel(size_t key); 1762 | size_t mrsymbolIndex(size_t key); 1763 | 1764 | #include 1765 | class Ordering { 1766 | // Standard Constructors and Named Constructors 1767 | Ordering(); 1768 | Ordering(const gtsam::Ordering& other); 1769 | 1770 | // Testable 1771 | void print(string s) const; 1772 | bool equals(const gtsam::Ordering& ord, double tol) const; 1773 | 1774 | // Standard interface 1775 | size_t size() const; 1776 | size_t at(size_t key) const; 1777 | void push_back(size_t key); 1778 | 1779 | // enabling serialization functionality 1780 | void serialize() const; 1781 | }; 1782 | 1783 | #include 1784 | class NonlinearFactorGraph { 1785 | NonlinearFactorGraph(); 1786 | NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph); 1787 | 1788 | // FactorGraph 1789 | void print(string s) const; 1790 | bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const; 1791 | size_t size() const; 1792 | bool empty() const; 1793 | void remove(size_t i); 1794 | void replace(size_t i, gtsam::NonlinearFactor* factors); 1795 | void resize(size_t size); 1796 | size_t nrFactors() const; 1797 | gtsam::NonlinearFactor* at(size_t idx) const; 1798 | void push_back(const gtsam::NonlinearFactorGraph& factors); 1799 | void push_back(gtsam::NonlinearFactor* factor); 1800 | void add(gtsam::NonlinearFactor* factor); 1801 | bool exists(size_t idx) const; 1802 | gtsam::KeySet keys() const; 1803 | gtsam::KeyVector keyVector() const; 1804 | 1805 | // NonlinearFactorGraph 1806 | double error(const gtsam::Values& values) const; 1807 | double probPrime(const gtsam::Values& values) const; 1808 | gtsam::Ordering orderingCOLAMD() const; 1809 | // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map& constraints) const; 1810 | gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values) const; 1811 | gtsam::NonlinearFactorGraph clone() const; 1812 | 1813 | // enabling serialization functionality 1814 | void serialize() const; 1815 | }; 1816 | 1817 | #include 1818 | virtual class NonlinearFactor { 1819 | // Factor base class 1820 | size_t size() const; 1821 | gtsam::KeyVector keys() const; 1822 | void print(string s) const; 1823 | void printKeys(string s) const; 1824 | // NonlinearFactor 1825 | bool equals(const gtsam::NonlinearFactor& other, double tol) const; 1826 | double error(const gtsam::Values& c) const; 1827 | size_t dim() const; 1828 | bool active(const gtsam::Values& c) const; 1829 | gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; 1830 | gtsam::NonlinearFactor* clone() const; 1831 | // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen 1832 | }; 1833 | 1834 | #include 1835 | virtual class NoiseModelFactor: gtsam::NonlinearFactor { 1836 | bool equals(const gtsam::NoiseModelFactor& other, double tol) const; 1837 | gtsam::noiseModel::Base* get_noiseModel() const; // deprecated by below 1838 | gtsam::noiseModel::Base* noiseModel() const; 1839 | Vector unwhitenedError(const gtsam::Values& x) const; 1840 | Vector whitenedError(const gtsam::Values& x) const; 1841 | }; 1842 | 1843 | #include 1844 | class Values { 1845 | Values(); 1846 | Values(const gtsam::Values& other); 1847 | 1848 | size_t size() const; 1849 | bool empty() const; 1850 | void clear(); 1851 | size_t dim() const; 1852 | 1853 | void print(string s) const; 1854 | bool equals(const gtsam::Values& other, double tol) const; 1855 | 1856 | void insert(const gtsam::Values& values); 1857 | void update(const gtsam::Values& values); 1858 | void erase(size_t j); 1859 | void swap(gtsam::Values& values); 1860 | 1861 | bool exists(size_t j) const; 1862 | gtsam::KeyVector keys() const; 1863 | 1864 | gtsam::VectorValues zeroVectors() const; 1865 | 1866 | gtsam::Values retract(const gtsam::VectorValues& delta) const; 1867 | gtsam::VectorValues localCoordinates(const gtsam::Values& cp) const; 1868 | 1869 | // enabling serialization functionality 1870 | void serialize() const; 1871 | 1872 | // New in 4.0, we have to specialize every insert/update/at to generate wrappers 1873 | // Instead of the old: 1874 | // void insert(size_t j, const gtsam::Value& value); 1875 | // void update(size_t j, const gtsam::Value& val); 1876 | // gtsam::Value at(size_t j) const; 1877 | 1878 | void insert(size_t j, const gtsam::Point2& point2); 1879 | void insert(size_t j, const gtsam::Point3& point3); 1880 | void insert(size_t j, const gtsam::Rot2& rot2); 1881 | void insert(size_t j, const gtsam::Pose2& pose2); 1882 | void insert(size_t j, const gtsam::Rot3& rot3); 1883 | void insert(size_t j, const gtsam::Pose3& pose3); 1884 | void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); 1885 | void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); 1886 | void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); 1887 | void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); 1888 | void insert(size_t j, const gtsam::SimpleCamera& simpel_camera); 1889 | void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); 1890 | void insert(size_t j, Vector vector); 1891 | void insert(size_t j, Matrix matrix); 1892 | 1893 | void update(size_t j, const gtsam::Point2& point2); 1894 | void update(size_t j, const gtsam::Point3& point3); 1895 | void update(size_t j, const gtsam::Rot2& rot2); 1896 | void update(size_t j, const gtsam::Pose2& pose2); 1897 | void update(size_t j, const gtsam::Rot3& rot3); 1898 | void update(size_t j, const gtsam::Pose3& pose3); 1899 | void update(size_t j, const gtsam::Cal3_S2& cal3_s2); 1900 | void update(size_t j, const gtsam::Cal3DS2& cal3ds2); 1901 | void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); 1902 | void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); 1903 | void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); 1904 | void update(size_t j, Vector vector); 1905 | void update(size_t j, Matrix matrix); 1906 | 1907 | template 1908 | T at(size_t j); 1909 | 1910 | /// version for double 1911 | void insertDouble(size_t j, double c); 1912 | double atDouble(size_t j) const; 1913 | }; 1914 | 1915 | #include 1916 | class Marginals { 1917 | Marginals(const gtsam::NonlinearFactorGraph& graph, 1918 | const gtsam::Values& solution); 1919 | 1920 | void print(string s) const; 1921 | Matrix marginalCovariance(size_t variable) const; 1922 | Matrix marginalInformation(size_t variable) const; 1923 | gtsam::JointMarginal jointMarginalCovariance(const gtsam::KeyVector& variables) const; 1924 | gtsam::JointMarginal jointMarginalInformation(const gtsam::KeyVector& variables) const; 1925 | }; 1926 | 1927 | class JointMarginal { 1928 | Matrix at(size_t iVariable, size_t jVariable) const; 1929 | Matrix fullMatrix() const; 1930 | void print(string s) const; 1931 | void print() const; 1932 | }; 1933 | 1934 | #include 1935 | virtual class LinearContainerFactor : gtsam::NonlinearFactor { 1936 | 1937 | LinearContainerFactor(gtsam::GaussianFactor* factor, const gtsam::Values& linearizationPoint); 1938 | LinearContainerFactor(gtsam::GaussianFactor* factor); 1939 | 1940 | gtsam::GaussianFactor* factor() const; 1941 | // const boost::optional& linearizationPoint() const; 1942 | 1943 | bool isJacobian() const; 1944 | gtsam::JacobianFactor* toJacobian() const; 1945 | gtsam::HessianFactor* toHessian() const; 1946 | 1947 | static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph, 1948 | const gtsam::Values& linearizationPoint); 1949 | 1950 | static gtsam::NonlinearFactorGraph ConvertLinearGraph(const gtsam::GaussianFactorGraph& linear_graph); 1951 | 1952 | // enabling serialization functionality 1953 | void serializable() const; 1954 | }; // \class LinearContainerFactor 1955 | 1956 | // Summarization functionality 1957 | //#include 1958 | // 1959 | //// Uses partial QR approach by default 1960 | //gtsam::GaussianFactorGraph summarize( 1961 | // const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, 1962 | // const gtsam::KeySet& saved_keys); 1963 | // 1964 | //gtsam::NonlinearFactorGraph summarizeAsNonlinearContainer( 1965 | // const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values, 1966 | // const gtsam::KeySet& saved_keys); 1967 | 1968 | //************************************************************************* 1969 | // Nonlinear optimizers 1970 | //************************************************************************* 1971 | #include 1972 | virtual class NonlinearOptimizerParams { 1973 | NonlinearOptimizerParams(); 1974 | void print(string s) const; 1975 | 1976 | int getMaxIterations() const; 1977 | double getRelativeErrorTol() const; 1978 | double getAbsoluteErrorTol() const; 1979 | double getErrorTol() const; 1980 | string getVerbosity() const; 1981 | 1982 | void setMaxIterations(int value); 1983 | void setRelativeErrorTol(double value); 1984 | void setAbsoluteErrorTol(double value); 1985 | void setErrorTol(double value); 1986 | void setVerbosity(string s); 1987 | 1988 | string getLinearSolverType() const; 1989 | 1990 | void setLinearSolverType(string solver); 1991 | void setOrdering(const gtsam::Ordering& ordering); 1992 | void setIterativeParams(gtsam::IterativeOptimizationParameters* params); 1993 | 1994 | bool isMultifrontal() const; 1995 | bool isSequential() const; 1996 | bool isCholmod() const; 1997 | bool isIterative() const; 1998 | }; 1999 | 2000 | bool checkConvergence(double relativeErrorTreshold, 2001 | double absoluteErrorTreshold, double errorThreshold, 2002 | double currentError, double newError); 2003 | 2004 | #include 2005 | virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams { 2006 | GaussNewtonParams(); 2007 | }; 2008 | 2009 | #include 2010 | virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams { 2011 | LevenbergMarquardtParams(); 2012 | 2013 | double getlambdaInitial() const; 2014 | double getlambdaFactor() const; 2015 | double getlambdaUpperBound() const; 2016 | string getVerbosityLM() const; 2017 | 2018 | void setlambdaInitial(double value); 2019 | void setlambdaFactor(double value); 2020 | void setlambdaUpperBound(double value); 2021 | void setVerbosityLM(string s); 2022 | }; 2023 | 2024 | #include 2025 | virtual class DoglegParams : gtsam::NonlinearOptimizerParams { 2026 | DoglegParams(); 2027 | 2028 | double getDeltaInitial() const; 2029 | string getVerbosityDL() const; 2030 | 2031 | void setDeltaInitial(double deltaInitial) const; 2032 | void setVerbosityDL(string verbosityDL) const; 2033 | }; 2034 | 2035 | #include 2036 | virtual class NonlinearOptimizer { 2037 | gtsam::Values optimize(); 2038 | gtsam::Values optimizeSafely(); 2039 | double error() const; 2040 | int iterations() const; 2041 | gtsam::Values values() const; 2042 | void iterate() const; 2043 | }; 2044 | 2045 | #include 2046 | virtual class GaussNewtonOptimizer : gtsam::NonlinearOptimizer { 2047 | GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); 2048 | GaussNewtonOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::GaussNewtonParams& params); 2049 | }; 2050 | 2051 | #include 2052 | virtual class DoglegOptimizer : gtsam::NonlinearOptimizer { 2053 | DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); 2054 | DoglegOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::DoglegParams& params); 2055 | double getDelta() const; 2056 | }; 2057 | 2058 | #include 2059 | virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { 2060 | LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues); 2061 | LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, const gtsam::LevenbergMarquardtParams& params); 2062 | double lambda() const; 2063 | void print(string str) const; 2064 | }; 2065 | 2066 | #include 2067 | class ISAM2GaussNewtonParams { 2068 | ISAM2GaussNewtonParams(); 2069 | 2070 | void print(string str) const; 2071 | 2072 | /** Getters and Setters for all properties */ 2073 | double getWildfireThreshold() const; 2074 | void setWildfireThreshold(double wildfireThreshold); 2075 | }; 2076 | 2077 | class ISAM2DoglegParams { 2078 | ISAM2DoglegParams(); 2079 | 2080 | void print(string str) const; 2081 | 2082 | /** Getters and Setters for all properties */ 2083 | double getWildfireThreshold() const; 2084 | void setWildfireThreshold(double wildfireThreshold); 2085 | double getInitialDelta() const; 2086 | void setInitialDelta(double initialDelta); 2087 | string getAdaptationMode() const; 2088 | void setAdaptationMode(string adaptationMode); 2089 | bool isVerbose() const; 2090 | void setVerbose(bool verbose); 2091 | }; 2092 | 2093 | class ISAM2ThresholdMapValue { 2094 | ISAM2ThresholdMapValue(char c, Vector thresholds); 2095 | ISAM2ThresholdMapValue(const gtsam::ISAM2ThresholdMapValue& other); 2096 | }; 2097 | 2098 | class ISAM2ThresholdMap { 2099 | ISAM2ThresholdMap(); 2100 | ISAM2ThresholdMap(const gtsam::ISAM2ThresholdMap& other); 2101 | 2102 | // Note: no print function 2103 | 2104 | // common STL methods 2105 | size_t size() const; 2106 | bool empty() const; 2107 | void clear(); 2108 | 2109 | // structure specific methods 2110 | void insert(const gtsam::ISAM2ThresholdMapValue& value) const; 2111 | }; 2112 | 2113 | class ISAM2Params { 2114 | ISAM2Params(); 2115 | 2116 | void print(string str) const; 2117 | 2118 | /** Getters and Setters for all properties */ 2119 | void setOptimizationParams(const gtsam::ISAM2GaussNewtonParams& gauss_newton__params); 2120 | void setOptimizationParams(const gtsam::ISAM2DoglegParams& dogleg_params); 2121 | void setRelinearizeThreshold(double threshold); 2122 | void setRelinearizeThreshold(const gtsam::ISAM2ThresholdMap& threshold_map); 2123 | int getRelinearizeSkip() const; 2124 | void setRelinearizeSkip(int relinearizeSkip); 2125 | bool isEnableRelinearization() const; 2126 | void setEnableRelinearization(bool enableRelinearization); 2127 | bool isEvaluateNonlinearError() const; 2128 | void setEvaluateNonlinearError(bool evaluateNonlinearError); 2129 | string getFactorization() const; 2130 | void setFactorization(string factorization); 2131 | bool isCacheLinearizedFactors() const; 2132 | void setCacheLinearizedFactors(bool cacheLinearizedFactors); 2133 | bool isEnableDetailedResults() const; 2134 | void setEnableDetailedResults(bool enableDetailedResults); 2135 | bool isEnablePartialRelinearizationCheck() const; 2136 | void setEnablePartialRelinearizationCheck(bool enablePartialRelinearizationCheck); 2137 | }; 2138 | 2139 | class ISAM2Clique { 2140 | 2141 | //Constructors 2142 | ISAM2Clique(); 2143 | 2144 | //Standard Interface 2145 | Vector gradientContribution() const; 2146 | void print(string s); 2147 | }; 2148 | 2149 | class ISAM2Result { 2150 | ISAM2Result(); 2151 | 2152 | void print(string str) const; 2153 | 2154 | /** Getters and Setters for all properties */ 2155 | size_t getVariablesRelinearized() const; 2156 | size_t getVariablesReeliminated() const; 2157 | size_t getCliques() const; 2158 | }; 2159 | 2160 | class FactorIndices {}; 2161 | 2162 | class ISAM2 { 2163 | ISAM2(); 2164 | ISAM2(const gtsam::ISAM2Params& params); 2165 | ISAM2(const gtsam::ISAM2& other); 2166 | 2167 | bool equals(const gtsam::ISAM2& other, double tol) const; 2168 | void print(string s) const; 2169 | void printStats() const; 2170 | void saveGraph(string s) const; 2171 | 2172 | gtsam::ISAM2Result update(); 2173 | gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); 2174 | gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices); 2175 | gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::FactorIndices& removeFactorIndices, const gtsam::KeyGroupMap& constrainedKeys); 2176 | // TODO: wrap the full version of update 2177 | //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys); 2178 | //void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyVector& removeFactorIndices, FastMap& constrainedKeys, bool force_relinearize); 2179 | 2180 | gtsam::Values getLinearizationPoint() const; 2181 | gtsam::Values calculateEstimate() const; 2182 | template 2186 | VALUE calculateEstimate(size_t key) const; 2187 | gtsam::Values calculateBestEstimate() const; 2188 | Matrix marginalCovariance(size_t key) const; 2189 | gtsam::VectorValues getDelta() const; 2190 | gtsam::NonlinearFactorGraph getFactorsUnsafe() const; 2191 | gtsam::VariableIndex getVariableIndex() const; 2192 | gtsam::ISAM2Params params() const; 2193 | }; 2194 | 2195 | #include 2196 | class NonlinearISAM { 2197 | NonlinearISAM(); 2198 | NonlinearISAM(int reorderInterval); 2199 | void print(string s) const; 2200 | void printStats() const; 2201 | void saveGraph(string s) const; 2202 | gtsam::Values estimate() const; 2203 | Matrix marginalCovariance(size_t key) const; 2204 | int reorderInterval() const; 2205 | int reorderCounter() const; 2206 | void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues); 2207 | void reorder_relinearize(); 2208 | 2209 | // These might be expensive as instead of a reference the wrapper will make a copy 2210 | gtsam::GaussianISAM bayesTree() const; 2211 | gtsam::Values getLinearizationPoint() const; 2212 | gtsam::NonlinearFactorGraph getFactorsUnsafe() const; 2213 | }; 2214 | 2215 | //************************************************************************* 2216 | // Nonlinear factor types 2217 | //************************************************************************* 2218 | #include 2219 | #include 2220 | #include 2221 | 2222 | #include 2223 | template 2224 | virtual class PriorFactor : gtsam::NoiseModelFactor { 2225 | PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); 2226 | T prior() const; 2227 | 2228 | // enabling serialization functionality 2229 | void serialize() const; 2230 | }; 2231 | 2232 | 2233 | #include 2234 | template 2235 | virtual class BetweenFactor : gtsam::NoiseModelFactor { 2236 | BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); 2237 | T measured() const; 2238 | 2239 | // enabling serialization functionality 2240 | void serialize() const; 2241 | }; 2242 | 2243 | 2244 | 2245 | #include 2246 | template 2247 | virtual class NonlinearEquality : gtsam::NoiseModelFactor { 2248 | // Constructor - forces exact evaluation 2249 | NonlinearEquality(size_t j, const T& feasible); 2250 | // Constructor - allows inexact evaluation 2251 | NonlinearEquality(size_t j, const T& feasible, double error_gain); 2252 | 2253 | // enabling serialization functionality 2254 | void serialize() const; 2255 | }; 2256 | 2257 | 2258 | #include 2259 | template 2260 | virtual class RangeFactor : gtsam::NoiseModelFactor { 2261 | RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); 2262 | }; 2263 | 2264 | typedef gtsam::RangeFactor RangeFactorPosePoint2; 2265 | typedef gtsam::RangeFactor RangeFactorPosePoint3; 2266 | typedef gtsam::RangeFactor RangeFactorPose2; 2267 | typedef gtsam::RangeFactor RangeFactorPose3; 2268 | typedef gtsam::RangeFactor RangeFactorCalibratedCameraPoint; 2269 | typedef gtsam::RangeFactor RangeFactorSimpleCameraPoint; 2270 | typedef gtsam::RangeFactor RangeFactorCalibratedCamera; 2271 | typedef gtsam::RangeFactor RangeFactorSimpleCamera; 2272 | 2273 | 2274 | #include 2275 | template 2276 | virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { 2277 | RangeFactorWithTransform(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel, const POSE& body_T_sensor); 2278 | }; 2279 | 2280 | typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint2; 2281 | typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPosePoint3; 2282 | typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose2; 2283 | typedef gtsam::RangeFactorWithTransform RangeFactorWithTransformPose3; 2284 | 2285 | #include 2286 | template 2287 | virtual class BearingFactor : gtsam::NoiseModelFactor { 2288 | BearingFactor(size_t key1, size_t key2, const BEARING& measured, const gtsam::noiseModel::Base* noiseModel); 2289 | 2290 | // enabling serialization functionality 2291 | void serialize() const; 2292 | }; 2293 | 2294 | typedef gtsam::BearingFactor BearingFactor2D; 2295 | 2296 | #include 2297 | template 2298 | virtual class BearingRangeFactor : gtsam::NoiseModelFactor { 2299 | BearingRangeFactor(size_t poseKey, size_t pointKey, 2300 | const BEARING& measuredBearing, const RANGE& measuredRange, 2301 | const gtsam::noiseModel::Base* noiseModel); 2302 | 2303 | // enabling serialization functionality 2304 | void serialize() const; 2305 | }; 2306 | 2307 | typedef gtsam::BearingRangeFactor BearingRangeFactor2D; 2308 | 2309 | 2310 | #include 2311 | template 2312 | virtual class GenericProjectionFactor : gtsam::NoiseModelFactor { 2313 | GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, 2314 | size_t poseKey, size_t pointKey, const CALIBRATION* k); 2315 | GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, 2316 | size_t poseKey, size_t pointKey, const CALIBRATION* k, const POSE& body_P_sensor); 2317 | 2318 | GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, 2319 | size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); 2320 | GenericProjectionFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, 2321 | size_t poseKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality, 2322 | const POSE& body_P_sensor); 2323 | 2324 | gtsam::Point2 measured() const; 2325 | CALIBRATION* calibration() const; 2326 | bool verboseCheirality() const; 2327 | bool throwCheirality() const; 2328 | 2329 | // enabling serialization functionality 2330 | void serialize() const; 2331 | }; 2332 | typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3_S2; 2333 | typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; 2334 | 2335 | 2336 | #include 2337 | template 2338 | virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { 2339 | GeneralSFMFactor(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t cameraKey, size_t landmarkKey); 2340 | gtsam::Point2 measured() const; 2341 | }; 2342 | typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; 2343 | // due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 2344 | //typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; 2345 | 2346 | template 2347 | virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { 2348 | GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey); 2349 | gtsam::Point2 measured() const; 2350 | 2351 | // enabling serialization functionality 2352 | void serialize() const; 2353 | }; 2354 | 2355 | #include 2356 | class SmartProjectionParams { 2357 | SmartProjectionParams(); 2358 | // TODO(frank): make these work: 2359 | // void setLinearizationMode(LinearizationMode linMode); 2360 | // void setDegeneracyMode(DegeneracyMode degMode); 2361 | void setRankTolerance(double rankTol); 2362 | void setEnableEPI(bool enableEPI); 2363 | void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold); 2364 | void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold); 2365 | }; 2366 | 2367 | #include 2368 | template 2369 | virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor { 2370 | 2371 | SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, 2372 | const CALIBRATION* K); 2373 | SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, 2374 | const CALIBRATION* K, 2375 | const gtsam::Pose3& body_P_sensor); 2376 | SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise, 2377 | const CALIBRATION* K, 2378 | const gtsam::Pose3& body_P_sensor, 2379 | const gtsam::SmartProjectionParams& params); 2380 | 2381 | void add(const gtsam::Point2& measured_i, size_t poseKey_i); 2382 | 2383 | // enabling serialization functionality 2384 | //void serialize() const; 2385 | }; 2386 | 2387 | typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; 2388 | 2389 | 2390 | #include 2391 | template 2392 | virtual class GenericStereoFactor : gtsam::NoiseModelFactor { 2393 | GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, 2394 | size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); 2395 | gtsam::StereoPoint2 measured() const; 2396 | gtsam::Cal3_S2Stereo* calibration() const; 2397 | 2398 | // enabling serialization functionality 2399 | void serialize() const; 2400 | }; 2401 | typedef gtsam::GenericStereoFactor GenericStereoFactor3D; 2402 | 2403 | #include 2404 | template 2405 | virtual class PoseTranslationPrior : gtsam::NoiseModelFactor { 2406 | PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); 2407 | }; 2408 | 2409 | typedef gtsam::PoseTranslationPrior PoseTranslationPrior2D; 2410 | typedef gtsam::PoseTranslationPrior PoseTranslationPrior3D; 2411 | 2412 | #include 2413 | template 2414 | virtual class PoseRotationPrior : gtsam::NoiseModelFactor { 2415 | PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); 2416 | }; 2417 | 2418 | typedef gtsam::PoseRotationPrior PoseRotationPrior2D; 2419 | typedef gtsam::PoseRotationPrior PoseRotationPrior3D; 2420 | 2421 | #include 2422 | virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor { 2423 | EssentialMatrixFactor(size_t key, const gtsam::Point2& pA, const gtsam::Point2& pB, 2424 | const gtsam::noiseModel::Base* noiseModel); 2425 | }; 2426 | 2427 | #include 2428 | pair load2D(string filename, 2429 | gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise, bool smart); 2430 | pair load2D(string filename, 2431 | gtsam::noiseModel::Diagonal* model, int maxID, bool addNoise); 2432 | pair load2D(string filename, 2433 | gtsam::noiseModel::Diagonal* model, int maxID); 2434 | pair load2D(string filename, 2435 | gtsam::noiseModel::Diagonal* model); 2436 | pair load2D(string filename); 2437 | pair load2D_robust(string filename, 2438 | gtsam::noiseModel::Base* model); 2439 | void save2D(const gtsam::NonlinearFactorGraph& graph, 2440 | const gtsam::Values& config, gtsam::noiseModel::Diagonal* model, 2441 | string filename); 2442 | 2443 | pair readG2o(string filename); 2444 | void writeG2o(const gtsam::NonlinearFactorGraph& graph, 2445 | const gtsam::Values& estimate, string filename); 2446 | 2447 | //************************************************************************* 2448 | // Navigation 2449 | //************************************************************************* 2450 | namespace imuBias { 2451 | #include 2452 | 2453 | class ConstantBias { 2454 | // Constructors 2455 | ConstantBias(); 2456 | ConstantBias(Vector biasAcc, Vector biasGyro); 2457 | 2458 | // Testable 2459 | void print(string s) const; 2460 | bool equals(const gtsam::imuBias::ConstantBias& expected, double tol) const; 2461 | 2462 | // Group 2463 | static gtsam::imuBias::ConstantBias identity(); 2464 | gtsam::imuBias::ConstantBias inverse() const; 2465 | gtsam::imuBias::ConstantBias compose(const gtsam::imuBias::ConstantBias& b) const; 2466 | gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const; 2467 | 2468 | // Manifold 2469 | gtsam::imuBias::ConstantBias retract(Vector v) const; 2470 | Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const; 2471 | 2472 | // Lie Group 2473 | static gtsam::imuBias::ConstantBias Expmap(Vector v); 2474 | static Vector Logmap(const gtsam::imuBias::ConstantBias& b); 2475 | 2476 | // Standard Interface 2477 | Vector vector() const; 2478 | Vector accelerometer() const; 2479 | Vector gyroscope() const; 2480 | Vector correctAccelerometer(Vector measurement) const; 2481 | Vector correctGyroscope(Vector measurement) const; 2482 | }; 2483 | 2484 | }///\namespace imuBias 2485 | 2486 | #include 2487 | class NavState { 2488 | // Constructors 2489 | NavState(); 2490 | NavState(const gtsam::Rot3& R, const gtsam::Point3& t, Vector v); 2491 | NavState(const gtsam::Pose3& pose, Vector v); 2492 | 2493 | // Testable 2494 | void print(string s) const; 2495 | bool equals(const gtsam::NavState& expected, double tol) const; 2496 | 2497 | // Access 2498 | gtsam::Rot3 attitude() const; 2499 | gtsam::Point3 position() const; 2500 | Vector velocity() const; 2501 | gtsam::Pose3 pose() const; 2502 | }; 2503 | 2504 | #include 2505 | virtual class PreintegratedRotationParams { 2506 | PreintegratedRotationParams(); 2507 | 2508 | // Testable 2509 | void print(string s) const; 2510 | bool equals(const gtsam::PreintegratedRotationParams& expected, double tol); 2511 | 2512 | void setGyroscopeCovariance(Matrix cov); 2513 | void setOmegaCoriolis(Vector omega); 2514 | void setBodyPSensor(const gtsam::Pose3& pose); 2515 | 2516 | Matrix getGyroscopeCovariance() const; 2517 | 2518 | // TODO(frank): allow optional 2519 | // boost::optional getOmegaCoriolis() const; 2520 | // boost::optional getBodyPSensor() const; 2521 | }; 2522 | 2523 | #include 2524 | virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { 2525 | PreintegrationParams(Vector n_gravity); 2526 | 2527 | static gtsam::PreintegrationParams* MakeSharedD(double g); 2528 | static gtsam::PreintegrationParams* MakeSharedU(double g); 2529 | static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 2530 | static gtsam::PreintegrationParams* MakeSharedU(); // default g = 9.81 2531 | 2532 | // Testable 2533 | void print(string s) const; 2534 | bool equals(const gtsam::PreintegrationParams& expected, double tol); 2535 | 2536 | void setAccelerometerCovariance(Matrix cov); 2537 | void setIntegrationCovariance(Matrix cov); 2538 | void setUse2ndOrderCoriolis(bool flag); 2539 | 2540 | Matrix getAccelerometerCovariance() const; 2541 | Matrix getIntegrationCovariance() const; 2542 | bool getUse2ndOrderCoriolis() const; 2543 | }; 2544 | 2545 | #include 2546 | class PreintegratedImuMeasurements { 2547 | // Constructors 2548 | PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params); 2549 | PreintegratedImuMeasurements(const gtsam::PreintegrationParams* params, 2550 | const gtsam::imuBias::ConstantBias& bias); 2551 | 2552 | // Testable 2553 | void print(string s) const; 2554 | bool equals(const gtsam::PreintegratedImuMeasurements& expected, double tol); 2555 | 2556 | // Standard Interface 2557 | void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, 2558 | double deltaT); 2559 | void resetIntegration(); 2560 | Matrix preintMeasCov() const; 2561 | double deltaTij() const; 2562 | gtsam::Rot3 deltaRij() const; 2563 | Vector deltaPij() const; 2564 | Vector deltaVij() const; 2565 | Vector biasHatVector() const; 2566 | gtsam::NavState predict(const gtsam::NavState& state_i, 2567 | const gtsam::imuBias::ConstantBias& bias) const; 2568 | }; 2569 | 2570 | virtual class ImuFactor: gtsam::NonlinearFactor { 2571 | ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, 2572 | size_t bias, 2573 | const gtsam::PreintegratedImuMeasurements& preintegratedMeasurements); 2574 | 2575 | // Standard Interface 2576 | gtsam::PreintegratedImuMeasurements preintegratedMeasurements() const; 2577 | Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, 2578 | const gtsam::Pose3& pose_j, Vector vel_j, 2579 | const gtsam::imuBias::ConstantBias& bias); 2580 | }; 2581 | 2582 | #include 2583 | class PreintegratedCombinedMeasurements { 2584 | // Testable 2585 | void print(string s) const; 2586 | bool equals(const gtsam::PreintegratedCombinedMeasurements& expected, 2587 | double tol); 2588 | 2589 | // Standard Interface 2590 | void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, 2591 | double deltaT); 2592 | void resetIntegration(); 2593 | Matrix preintMeasCov() const; 2594 | double deltaTij() const; 2595 | gtsam::Rot3 deltaRij() const; 2596 | Vector deltaPij() const; 2597 | Vector deltaVij() const; 2598 | Vector biasHatVector() const; 2599 | gtsam::NavState predict(const gtsam::NavState& state_i, 2600 | const gtsam::imuBias::ConstantBias& bias) const; 2601 | }; 2602 | 2603 | virtual class CombinedImuFactor: gtsam::NonlinearFactor { 2604 | CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, 2605 | size_t bias_i, size_t bias_j, 2606 | const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); 2607 | 2608 | // Standard Interface 2609 | gtsam::PreintegratedCombinedMeasurements preintegratedMeasurements() const; 2610 | Vector evaluateError(const gtsam::Pose3& pose_i, Vector vel_i, 2611 | const gtsam::Pose3& pose_j, Vector vel_j, 2612 | const gtsam::imuBias::ConstantBias& bias_i, 2613 | const gtsam::imuBias::ConstantBias& bias_j); 2614 | }; 2615 | 2616 | #include 2617 | class PreintegratedAhrsMeasurements { 2618 | // Standard Constructor 2619 | PreintegratedAhrsMeasurements(Vector bias, Matrix measuredOmegaCovariance); 2620 | PreintegratedAhrsMeasurements(const gtsam::PreintegratedAhrsMeasurements& rhs); 2621 | 2622 | // Testable 2623 | void print(string s) const; 2624 | bool equals(const gtsam::PreintegratedAhrsMeasurements& expected, double tol); 2625 | 2626 | // get Data 2627 | gtsam::Rot3 deltaRij() const; 2628 | double deltaTij() const; 2629 | Vector biasHat() const; 2630 | 2631 | // Standard Interface 2632 | void integrateMeasurement(Vector measuredOmega, double deltaT); 2633 | void resetIntegration() ; 2634 | }; 2635 | 2636 | virtual class AHRSFactor : gtsam::NonlinearFactor { 2637 | AHRSFactor(size_t rot_i, size_t rot_j,size_t bias, 2638 | const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis); 2639 | AHRSFactor(size_t rot_i, size_t rot_j, size_t bias, 2640 | const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, Vector omegaCoriolis, 2641 | const gtsam::Pose3& body_P_sensor); 2642 | 2643 | // Standard Interface 2644 | gtsam::PreintegratedAhrsMeasurements preintegratedMeasurements() const; 2645 | Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j, 2646 | Vector bias) const; 2647 | gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias, 2648 | const gtsam::PreintegratedAhrsMeasurements& preintegratedMeasurements, 2649 | Vector omegaCoriolis) const; 2650 | }; 2651 | 2652 | #include 2653 | //virtual class AttitudeFactor : gtsam::NonlinearFactor { 2654 | // AttitudeFactor(const Unit3& nZ, const Unit3& bRef); 2655 | // AttitudeFactor(); 2656 | //}; 2657 | virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{ 2658 | Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model, 2659 | const gtsam::Unit3& bRef); 2660 | Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model); 2661 | Rot3AttitudeFactor(); 2662 | void print(string s) const; 2663 | bool equals(const gtsam::NonlinearFactor& expected, double tol) const; 2664 | gtsam::Unit3 nZ() const; 2665 | gtsam::Unit3 bRef() const; 2666 | }; 2667 | 2668 | virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor { 2669 | Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, 2670 | const gtsam::noiseModel::Diagonal* model, 2671 | const gtsam::Unit3& bRef); 2672 | Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, 2673 | const gtsam::noiseModel::Diagonal* model); 2674 | Pose3AttitudeFactor(); 2675 | void print(string s) const; 2676 | bool equals(const gtsam::NonlinearFactor& expected, double tol) const; 2677 | gtsam::Unit3 nZ() const; 2678 | gtsam::Unit3 bRef() const; 2679 | }; 2680 | 2681 | #include 2682 | virtual class Scenario { 2683 | gtsam::Pose3 pose(double t) const; 2684 | Vector omega_b(double t) const; 2685 | Vector velocity_n(double t) const; 2686 | Vector acceleration_n(double t) const; 2687 | gtsam::Rot3 rotation(double t) const; 2688 | gtsam::NavState navState(double t) const; 2689 | Vector velocity_b(double t) const; 2690 | Vector acceleration_b(double t) const; 2691 | }; 2692 | 2693 | virtual class ConstantTwistScenario : gtsam::Scenario { 2694 | ConstantTwistScenario(Vector w, Vector v); 2695 | ConstantTwistScenario(Vector w, Vector v, 2696 | const gtsam::Pose3& nTb0); 2697 | }; 2698 | 2699 | virtual class AcceleratingScenario : gtsam::Scenario { 2700 | AcceleratingScenario(const gtsam::Rot3& nRb, const gtsam::Point3& p0, 2701 | Vector v0, Vector a_n, 2702 | Vector omega_b); 2703 | }; 2704 | 2705 | //************************************************************************* 2706 | // Utilities 2707 | //************************************************************************* 2708 | 2709 | namespace utilities { 2710 | 2711 | #include 2712 | gtsam::KeyList createKeyList(Vector I); 2713 | gtsam::KeyList createKeyList(string s, Vector I); 2714 | gtsam::KeyVector createKeyVector(Vector I); 2715 | gtsam::KeyVector createKeyVector(string s, Vector I); 2716 | gtsam::KeySet createKeySet(Vector I); 2717 | gtsam::KeySet createKeySet(string s, Vector I); 2718 | Matrix extractPoint2(const gtsam::Values& values); 2719 | Matrix extractPoint3(const gtsam::Values& values); 2720 | Matrix extractPose2(const gtsam::Values& values); 2721 | gtsam::Values allPose3s(gtsam::Values& values); 2722 | Matrix extractPose3(const gtsam::Values& values); 2723 | void perturbPoint2(gtsam::Values& values, double sigma, int seed); 2724 | void perturbPose2 (gtsam::Values& values, double sigmaT, double sigmaR, int seed); 2725 | void perturbPoint3(gtsam::Values& values, double sigma, int seed); 2726 | void insertBackprojections(gtsam::Values& values, const gtsam::SimpleCamera& c, Vector J, Matrix Z, double depth); 2727 | void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K); 2728 | void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor); 2729 | Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values); 2730 | gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base); 2731 | gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys); 2732 | 2733 | } //\namespace utilities 2734 | 2735 | #include 2736 | class RedirectCout { 2737 | RedirectCout(); 2738 | string str(); 2739 | }; 2740 | 2741 | namespace da { 2742 | #include 2743 | template 2744 | class JCBB { 2745 | JCBB(const gtsam::ISAM2 &isam2, double prob); 2746 | 2747 | void add(BEARING measureBearing, RANGE measuredRange, const gtsam::noiseModel::Base *model); 2748 | gtsam::KeyVector match() const; 2749 | }; 2750 | 2751 | template 2752 | class MHJCBB { 2753 | MHJCBB(int max_tracks, double prob, double posterior_pose_md_threshold); 2754 | 2755 | void initialize(const gtsam::ISAM2 &isam2); 2756 | void add(BEARING measureBearing, RANGE measuredRange, const gtsam::noiseModel::Base *model); 2757 | void match(); 2758 | int size() const; 2759 | pair get(int i) const; 2760 | }; 2761 | 2762 | typedef gtsam::da::JCBB JCBB2; 2763 | typedef gtsam::da::MHJCBB MHJCBB2; 2764 | } //\namespace da 2765 | 2766 | #include 2767 | class FastMarginals { 2768 | FastMarginals(const gtsam::ISAM2 &isam2); 2769 | Matrix marginalCovariance(const size_t &variable); 2770 | Matrix jointMarginalCovariance(const gtsam::KeyVector &variables); 2771 | }; 2772 | 2773 | } // namespace gtsam 2774 | --------------------------------------------------------------------------------