Go to the documentation of this file.
42 using namespace gtsam;
44 int main(
int argc,
char* argv[]) {
53 auto poseNoise = noiseModel::Diagonal::Sigmas(
54 (
Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
60 Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
61 auto measurementNoise =
62 noiseModel::Isotropic::Sigma(2, 1.0);
63 for (
size_t i = 0;
i < poses.size(); ++
i) {
64 for (
size_t j = 0;
j < points.size(); ++
j) {
78 noiseModel::Isotropic::Sigma(3, 0.1);
83 auto calNoise = noiseModel::Diagonal::Sigmas(
84 (
Vector(5) << 500, 500, 0.1, 100, 100).finished());
91 for (
size_t i = 0;
i < poses.size(); ++
i)
94 Point3(0.05, -0.10, 0.20))));
95 for (
size_t j = 0;
j < points.size(); ++
j)
97 points[
j] +
Point3(-0.25, 0.20, 0.15));
virtual const Values & optimize()
std::vector< Point3 > createPoints()
Create a set of ground-truth landmarks.
a general SFM factor with an unknown calibration
int main(int argc, char *argv[])
void addPrior(Key key, const T &prior, const SharedNoiseModel &model=nullptr)
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
Factor Graph consisting of non-linear factors.
T compose(const T &t1, const T &t2)
void insert(Key j, const Value &val)
The most common 5DOF 3D->2D calibration.
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)
A non-templated config holding any types of Manifold-group elements.
Point2 project(const Point3 &point, OptionalJacobian< 2, 6 > Dcamera={}, OptionalJacobian< 2, 3 > Dpoint={}) const
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:35:13