Go to the documentation of this file.
52 using namespace gtsam;
63 list<TimedOdometry> odometryList;
66 if (!is)
throw runtime_error(
"Plaza1_DR.txt file not found");
69 double t, distance_traveled, delta_heading;
70 is >>
t >> distance_traveled >> delta_heading;
71 odometryList.push_back(
82 vector<RangeTriple> triples;
85 if (!is)
throw runtime_error(
"Plaza1_TD.txt file not found");
88 double t, sender, receiver,
range;
89 is >>
t >> sender >> receiver >>
range;
97 int main(
int argc,
char** argv) {
102 size_t K = triples.size();
105 size_t start = 220,
end=3000;
120 rangeNoise = robust ? tukey : gaussian;
127 M_PI - 2.02108900000000);
131 ofstream os2(
"rangeResultLM.txt");
132 ofstream os3(
"rangeResultSR.txt");
146 size_t ids[] = { 1, 6, 0, 5 };
147 typedef std::shared_ptr<SmartRangeFactor> SmartPtr;
148 map<size_t, SmartPtr> smartFactors;
150 for(
size_t jj: ids) {
160 size_t countK = 0, totalCount=0;
169 printf(
"step %d, time = %g\n",(
int)
i,
t);
177 lastPose = predictedPose;
179 landmarkEstimates.
insert(
i, predictedPose);
182 while (k < K && t >= std::get<0>(triples[k])) {
183 size_t j = std::get<1>(triples[k]);
184 double range = std::get<2>(triples[k]);
186 if (smart && totalCount < minK) {
188 smartFactors[
j]->addRange(
i,
range);
189 printf(
"adding range %g for %d",
range,(
int)
j);
190 }
catch (
const invalid_argument&
e) {
191 printf(
"warning: omitting duplicate range %g for %d: %s",
range,
211 if (k >= minK && countK >= incK) {
215 gttic_(calculateEstimate);
217 gttoc_(calculateEstimate);
222 landmarkEstimates =
Values();
228 if (smart && !hasLandmarks) {
229 cout <<
"initialize from smart landmarks" << endl;
230 for(
size_t jj: ids) {
238 os2 <<
key <<
"\t" <<
point.x() <<
"\t" <<
point.y() <<
"\t1" << endl;
240 for(
size_t jj: ids) {
258 ofstream
os(
"rangeResult.txt");
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
std::tuple< double, size_t, double > RangeTriple
Array< double, 1, 3 > e(1./3., 0.5, 2.)
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.
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
NonlinearISAM isam(relinearizeInterval)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
static Point3 landmark(0, 0, 5)
Key symbol(unsigned char c, std::uint64_t j)
pair< double, Pose2 > TimedOdometry
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
A smart factor for range-only SLAM that does initialization and marginalization.
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.
list< TimedOdometry > readOdometry()
void insert(Key j, const Value &val)
vector< RangeTriple > readTriples()
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
int main(int argc, char **argv)
static const EIGEN_DEPRECATED end_t end
A non-templated config holding any types of Manifold-group elements.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:31