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);
129 newFactors.
addPrior(0, pose0, priorNoise);
131 ofstream os2(
"rangeResultLM.txt");
132 ofstream os3(
"rangeResultSR.txt");
146 size_t ids[] = { 1, 6, 0, 5 };
147 typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
148 map<size_t, SmartPtr> smartFactors;
150 for(
size_t jj: ids) {
160 size_t countK = 0, totalCount=0;
168 boost::tie(t, odometry) = timedOdometry;
169 printf(
"step %d, time = %g\n",(
int)i,t);
177 lastPose = predictedPose;
178 initial.
insert(i, predictedPose);
179 landmarkEstimates.
insert(i, predictedPose);
182 while (k < K && t >= boost::get<0>(triples[k])) {
183 size_t j = boost::get<1>(triples[k]);
184 double range = boost::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,
201 if (k <= 200 ||
std::abs(error[0]) < 5)
211 if (k >= minK && countK >= incK) {
213 isam.
update(newFactors, initial);
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 << it.key <<
"\t" << it.value.x() <<
"\t" << it.value.y() <<
"\t1" 241 for(
size_t jj: ids) {
243 os3 << jj <<
"\t" << landmark.x() <<
"\t" << landmark.y() <<
"\t1" 259 ofstream
os(
"rangeResult.txt");
261 os << it.key <<
"\t" << it.value.x() <<
"\t" << it.value.y() <<
"\t" 262 << it.value.theta() << endl;
boost::tuple< double, size_t, double > RangeTriple
A smart factor for range-only SLAM that does initialization and marginalization.
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
void insert(Key j, const Value &val)
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
Vector unwhitenedError(const Values &x, boost::optional< std::vector< Matrix > & > H=boost::none) const override
IsDerived< DERIVEDFACTOR > push_back(boost::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
static shared_ptr Create(const RobustModel::shared_ptr &robust, const NoiseModel::shared_ptr noise)
pair< double, Pose2 > TimedOdometry
static Point3 landmark(0, 0, 5)
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
string findExampleDataFile(const string &name)
Values calculateEstimate() const
list< TimedOdometry > readOdometry()
Class compose(const Class &g) const
const ValueType at(Key j) const
Incremental update functionality (ISAM2) for BayesTree, with fluid relinearization.
vector< RangeTriple > readTriples()
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NonlinearISAM isam(relinearizeInterval)
Key symbol(unsigned char c, std::uint64_t j)
boost::shared_ptr< Base > shared_ptr
std::vector< float > Values
ofstream os("timeSchurFactors.csv")
static shared_ptr Sigmas(const Vector &sigmas, bool smart=true)
int main(int argc, char **argv)
virtual ISAM2Result update(const NonlinearFactorGraph &newFactors=NonlinearFactorGraph(), const Values &newTheta=Values(), const FactorIndices &removeFactorIndices=FactorIndices(), const boost::optional< FastMap< Key, int > > &constrainedKeys=boost::none, const boost::optional< FastList< Key > > &noRelinKeys=boost::none, const boost::optional< FastList< Key > > &extraReelimKeys=boost::none, bool force_relinearize=false)
pair< double, Pose2 > TimedOdometry
noiseModel::Diagonal::shared_ptr priorNoise
Pose2 odometry(2.0, 0.0, 0.0)
utility functions for loading datasets
static shared_ptr Sigma(size_t dim, double sigma, bool smart=true)
Filtered< Value > filter(const boost::function< bool(Key)> &filterFcn)
All noise models live in the noiseModel namespace.