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 = std::make_shared<BlockJacobiPreconditionerParameters>();
99 pcg->epsilon_abs = 1
e-10;
100 pcg->epsilon_rel = 1
e-10;
109 for (
size_t j = 0;
j < points.size(); ++
j) {
110 auto smart = std::dynamic_pointer_cast<SmartFactor>(
graph[
j]);
112 std::optional<Point3>
point = smart->point(
result);
118 landmark_result.
print(
"Landmark results:\n");
120 cout <<
"number of iterations: " << optimizer.
iterations() << endl;
virtual const Values & optimize()
std::vector< Point3 > createPoints()
Create a set of ground-truth landmarks.
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
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
std::vector< Pose3 > createPoses(const Pose3 &init=Pose3(Rot3::Ypr(M_PI_2, 0, -M_PI_2), {30, 0, 0}), const Pose3 &delta=Pose3(Rot3::Ypr(0, -M_PI_4, 0), {sin(M_PI_4) *30, 0, 30 *(1 - sin(M_PI_4))}), int steps=8)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
size_t maxIterations
maximum number of cg iterations
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)
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 Sat Nov 16 2024 04:04:11