42 using namespace gtsam;
44 int main(
int argc,
char** argv) {
47 const auto model = noiseModel::Isotropic::Sigma(3, 1);
56 ifstream calibration_file(calibration_loc.c_str());
57 cout <<
"Reading calibration info" << endl;
58 calibration_file >> fx >> fy >> s >> u0 >> v0 >>
b;
63 ifstream pose_file(pose_loc.c_str());
64 cout <<
"Reading camera poses" << endl;
69 while (pose_file >> pose_id) {
70 for (
int i = 0;
i < 16;
i++) {
71 pose_file >> m.
data()[
i];
82 double uL, uR,
v,
X,
Y,
Z;
83 ifstream factor_file(factor_loc.c_str());
84 cout <<
"Reading stereo factors" << endl;
87 while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
108 cout <<
"Optimizing" << endl;
115 cout <<
"Final result sample:" << endl;
117 pose_values.
print(
"Final camera poses:\n");
virtual const Values & optimize()
A non-templated config holding any types of Manifold-group elements.
Factor Graph consisting of non-linear factors.
noiseModel::Diagonal::shared_ptr model
void insert(Key j, const Value &val)
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar * data() const
static Cal3_S2 K(500, 500, 0.1, 640/2, 480/2)
NonlinearFactorGraph graph
string findExampleDataFile(const string &name)
Point3 transformFrom(const Point3 &point, OptionalJacobian< 3, 6 > Hself=boost::none, OptionalJacobian< 3, 3 > Hpoint=boost::none) const
takes point in Pose coordinates and transforms it to world coordinates
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&...args)
Emplace a shared pointer to factor of given type.
static const Line3 l(Rot3(), 1, 1)
boost::shared_ptr< Cal3_S2Stereo > shared_ptr
const ValueType at(Key j) const
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
static SmartStereoProjectionParams params
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
The most common 5DOF 3D->2D calibration + Stereo baseline.
static Pose3 camPose(Rot3((Matrix3()<< 1., 0., 0., 0.,-1., 0., 0., 0.,-1.).finished()), Point3(0, 0, 6.25))
Ordering::OrderingType orderingType
The method of ordering use during variable elimination (default COLAMD)
The matrix class, also used for vectors and row-vectors.
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
utility functions for loading datasets
int main(int argc, char **argv)
Filtered< Value > filter(const boost::function< bool(Key)> &filterFcn)