Go to the documentation of this file.
32 using namespace gtsam;
41 int main(
int argc,
char* argv[]) {
47 auto measurementNoise =
48 noiseModel::Isotropic::Sigma(2, 1.0);
58 for (
size_t j = 0;
j < points.size(); ++
j) {
63 for (
size_t i = 0;
i < poses.size(); ++
i) {
83 auto noise = noiseModel::Diagonal::Sigmas(
84 (
Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
99 for (
size_t i = 0;
i < poses.size(); ++
i)
101 initialEstimate.
print(
"Initial Estimates:\n");
112 for (
size_t j = 0;
j < points.size(); ++
j) {
125 landmark_result.
print(
"Landmark results:\n");
127 cout <<
"number of iterations: " << optimizer.
iterations() << endl;
virtual const Values & optimize()
std::vector< Point3 > createPoints()
Create a set of ground-truth landmarks.
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
double error(const Values &values) const
int main(int argc, char *argv[])
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
std::shared_ptr< Cal3_S2 > 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
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
void insert(Key j, const Value &val)
PinholePose< Cal3_S2 > Camera
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
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
void print(const std::string &str="NonlinearFactorGraph: ", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
gtsam
Author(s):
autogenerated on Sat Nov 16 2024 04:04:11