Go to the documentation of this file.
39 using namespace gtsam;
40 using namespace noiseModel;
43 int main(
int argc,
char* argv[]) {
45 Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
68 for (
size_t i = 0;
i < poses.size(); ++
i) {
71 for (
size_t j = 0;
j < points.size(); ++
j) {
88 for (
size_t i = 0;
i < poses.size(); ++
i)
90 for (
size_t j = 0;
j < points.size(); ++
j)
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)
Expression< Point3 > Point3_
virtual const Values & optimize()
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
std::vector< gtsam::Point3 > createPoints()
Point2 uncalibrate(const CAL &K, const Point2 &p, OptionalJacobian< 2, 5 > Dcal, OptionalJacobian< 2, 2 > Dp)
double error(const Values &values) const
Line3 transformTo(const Pose3 &wTc, const Line3 &wL, OptionalJacobian< 4, 6 > Dpose, OptionalJacobian< 4, 4 > Dline)
Factor graph that supports adding ExpressionFactors directly.
int main(int argc, char *argv[])
static Point2 project(const Pose3 &pose, const Unit3 &pointAtInfinity, const Cal3_S2::shared_ptr &cal)
Common expressions for solving geometry/slam/sfm problems.
T compose(const T &t1, const T &t2)
std::shared_ptr< Diagonal > shared_ptr
The most common 5DOF 3D->2D calibration.
std::shared_ptr< Isotropic > shared_ptr
static const CalibratedCamera camera(kDefaultPose)
NonlinearFactorGraph graph
Simple example for the structure-from-motion problems.
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
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
gtsam
Author(s):
autogenerated on Wed Sep 25 2024 03:05:23