Go to the documentation of this file.
51 using namespace gtsam;
62 list<TimedOdometry> odometryList;
65 if (!is)
throw runtime_error(
"Plaza2_DR.txt file not found");
68 double t, distance_traveled, delta_heading;
69 is >>
t >> distance_traveled >> delta_heading;
70 odometryList.push_back(
81 vector<RangeTriple> triples;
84 if (!is)
throw runtime_error(
"Plaza2_TD.txt file not found");
87 double t, sender, receiver,
range;
88 is >>
t >> sender >> receiver >>
range;
96 int main(
int argc,
char** argv) {
103 size_t K = triples.size();
118 rangeNoise = robust ? tukey : gaussian;
124 Pose2 pose0 =
Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000);
158 lastPose = predictedPose;
160 landmarkEstimates.
insert(
i, predictedPose);
163 while (k < K && t >= std::get<0>(triples[k])) {
164 size_t j = std::get<1>(triples[k]);
165 double range = std::get<2>(triples[k]);
180 gttic_(calculateEstimate);
182 gttoc_(calculateEstimate);
185 landmarkEstimates =
Values();
204 ofstream os2(
"rangeResultLM.txt");
206 os2 <<
key <<
"\t" <<
point.x() <<
"\t" <<
point.y() <<
"\t1" << endl;
207 ofstream
os(
"rangeResult.txt");
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
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.
Class compose(const Class &g) const
Serializable factor induced by a range measurement.
std::tuple< double, size_t, double > RangeTriple
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
ofstream os("timeSchurFactors.csv")
void update(const NonlinearFactorGraph &newFactors, const Values &initialValues)
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
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
pair< double, Pose2 > TimedOdometry
NonlinearISAM isam(relinearizeInterval)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
Key symbol(unsigned char c, std::uint64_t j)
vector< RangeTriple > readTriples()
const gtsam::Symbol key('X', 0)
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
std::pair< double, Pose2 > TimedOdometry
Vector unwhitenedError(const Values &x, OptionalMatrixVecType H=nullptr) const override
std::vector< float > Values
Factor Graph consisting of non-linear factors.
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
All noise models live in the noiseModel namespace.
int main(int argc, char **argv)
void insert(Key j, const Value &val)
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
list< TimedOdometry > readOdometry()
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
A non-templated config holding any types of Manifold-group elements.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:31