Go to the documentation of this file.
80 std::list<TimedOdometry> odometryList;
82 std::ifstream is(data_file.c_str());
85 double t, distance_traveled, delta_heading;
86 is >>
t >> distance_traveled >> delta_heading;
87 odometryList.emplace_back(
t,
Pose2(distance_traveled, 0, delta_heading));
97 std::vector<RangeTriple> triples;
99 std::ifstream is(data_file.c_str());
102 double t,
range, sender, receiver;
103 is >>
t >> sender >> receiver >>
range;
104 triples.emplace_back(
t, receiver,
range);
111 int main(
int argc,
char** argv) {
115 std::cout <<
"Read " <<
M <<
" odometry entries." << std::endl;
118 size_t K = triples.size();
119 std::cout <<
"Read " <<
K <<
" range triples." << std::endl;
138 rangeNoise = robust ? tukey : gaussian;
153 std::normal_distribution<double>
normal(0.0, 100.0);
154 std::set<Symbol> initializedLandmarks;
159 bool initialized =
false;
177 lastPose = predictedPose;
181 while (k < K && t >= std::get<0>(triples[k])) {
182 size_t j = std::get<1>(triples[k]);
184 double range = std::get<2>(triples[k]);
186 i, landmark_key,
range, rangeNoise);
187 if (initializedLandmarks.count(landmark_key) == 0) {
188 std::cout <<
"adding landmark " <<
j << std::endl;
191 initializedLandmarks.insert(landmark_key);
195 landmark_key,
Point2(0, 0), looseNoise);
202 if ((k > minK) && (countK > incK)) {
204 std::cout <<
"Initializing at time " << k << std::endl;
205 gttic_(batchInitialization);
208 gttoc_(batchInitialization);
214 gttic_(calculateEstimate);
216 gttoc_(calculateEstimate);
232 for (
auto&& landmark_key : initializedLandmarks) {
234 std::cout << landmark_key <<
":" <<
p.transpose() <<
"\n";
std::tuple< double, size_t, double > RangeTriple
virtual const Values & optimize()
typedef and functions to augment Eigen's VectorXd
std::shared_ptr< Base > shared_ptr
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
std::list< TimedOdometry > readOdometry()
Class compose(const Class &g) const
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Serializable factor induced by a range measurement.
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
int main(int argc, char **argv)
Double_ range(const Point2_ &p, const Point2_ &q)
Pose2 odometry(2.0, 0.0, 0.0)
const ValueType at(Key j) const
utility functions for loading datasets
NonlinearISAM isam(relinearizeInterval)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Unit3_ normal(const OrientedPlane3_ &p)
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
std::pair< double, Pose2 > TimedOdometry
std::vector< RangeTriple > readTriples()
std::vector< float > Values
Factor Graph consisting of non-linear factors.
All noise models live in the noiseModel namespace.
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A non-templated config holding any types of Manifold-group elements.
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Matrix< RealScalar, Dynamic, Dynamic > M
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:51