Go to the documentation of this file.
28 using namespace gtsam;
37 int main(
int argc,
char* argv[]) {
42 auto measurementNoise =
43 noiseModel::Isotropic::Sigma(2, 1.0);
53 for (
size_t j = 0;
j < points.size(); ++
j) {
58 for (
size_t i = 0;
i < poses.size(); ++
i) {
73 auto noise = noiseModel::Diagonal::Sigmas(
74 (
Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
83 for (
size_t i = 0;
i < poses.size(); ++
i)
91 parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
96 std::make_shared<PCGSolverParameters>();
97 pcg->preconditioner_ =
98 std::make_shared<BlockJacobiPreconditionerParameters>();
100 pcg->setEpsilon_abs(1
e-10);
101 pcg->setEpsilon_rel(1
e-10);
110 for (
size_t j = 0;
j < points.size(); ++
j) {
111 auto smart = std::dynamic_pointer_cast<SmartFactor>(
graph[
j]);
113 std::optional<Point3>
point = smart->point(
result);
119 landmark_result.
print(
"Landmark results:\n");
121 cout <<
"number of iterations: " << optimizer.
iterations() << endl;
std::vector< gtsam::Pose3 > createPoses(const gtsam::Pose3 &init=gtsam::Pose3(gtsam::Rot3::Ypr(M_PI/2, 0,-M_PI/2), gtsam::Point3(30, 0, 0)), const gtsam::Pose3 &delta=gtsam::Pose3(gtsam::Rot3::Ypr(0,-M_PI/4, 0), gtsam::Point3(sin(M_PI/4) *30, 0, 30 *(1-sin(M_PI/4)))), int steps=8)
virtual const Values & optimize()
Array< double, 1, 3 > e(1./3., 0.5, 2.)
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
PinholePose< Cal3_S2 > Camera
std::vector< gtsam::Point3 > createPoints()
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
double error(const Values &values) const
static ConjugateGradientParameters parameters
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
std::shared_ptr< Cal3_S2 > shared_ptr
std::shared_ptr< PCGSolverParameters > shared_ptr
Smart factor on poses, assuming camera calibration is fixed.
size_t iterations() const
return number of iterations in current optimizer state
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
int main(int argc, char *argv[])
void insert(Key j, const Value &val)
size_t maxIterations() const
The most common 5DOF 3D->2D calibration.
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
static const CalibratedCamera camera(kDefaultPose)
NonlinearFactorGraph graph
Simple example for the structure-from-motion problems.
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
static Point2 measurement(323.0, 240.0)
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
gtsam
Author(s):
autogenerated on Wed Sep 25 2024 03:05:23