Go to the documentation of this file.
38 using namespace gtsam;
42 int main(
int argc,
char* argv[]) {
45 throw runtime_error(
"Usage: timeShonanFactor [g2oFile]");
51 g2oFile = argv[argc - 1];
54 }
catch (
const exception&
e) {
55 cerr <<
e.what() <<
'\n';
60 const auto measurements = parseMeasurements<Rot3>(g2oFile);
66 auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
68 auto G = std::make_shared<Matrix>(SOn::VectorizedGenerators(4));
70 const auto &
keys =
m.keys();
71 const Rot3 &Rij =
m.measured();
72 const auto &
model =
m.noiseModel();
81 LevenbergMarquardtParams::SetCeresDefaults(&
params);
82 params.setLinearSolverType(
"MULTIFRONTAL_QR");
92 for (
size_t i = 0;
i < 100;
i++) {
95 initial.insert(0, SOn::Identity(4));
96 for (
size_t j = 1;
j < poses.size();
j++) {
static SharedNoiseModel gNoiseModel
virtual const Values & optimize()
int main(int argc, char *argv[])
Array< double, 1, 3 > e(1./3., 0.5, 2.)
int optimize(const SfmData &db, const NonlinearFactorGraph &graph, const Values &initial, bool separateCalibration=false)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
Main factor type in Shonan averaging, on SO(n) pairs.
static const SmartProjectionParams params
double error(const Values &values) const
utility functions for loading datasets
Rot3 is a 3D rotation represented as a rotation matrix if the preprocessor symbol GTSAM_USE_QUATERNIO...
void tictoc_finishedIteration_()
noiseModel::Base::shared_ptr SharedNoiseModel
noiseModel::Diagonal::shared_ptr model
std::vector< double > measurements
Factor Graph consisting of non-linear factors.
JacobiRotation< float > G
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
GTSAM_EXPORT std::map< size_t, Pose3 > parseVariables< Pose3 >(const std::string &filename, size_t maxIndex)
IsDerived< DERIVEDFACTOR > add(std::shared_ptr< DERIVEDFACTOR > factor)
add is a synonym for push_back.
NonlinearFactorGraph graph
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.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:42:30