Go to the documentation of this file.
46 using namespace gtsam;
49 int main(
int argc,
char** argv){
53 bool output_poses =
true;
54 string poseOutput(
"../../../examples/data/optimized_poses.txt");
55 string init_poseOutput(
"../../../examples/data/initial_poses.txt");
59 ofstream pose3Out, init_pose3Out;
61 bool add_initial_noise =
true;
69 cout <<
"Reading calibration info" << endl;
70 ifstream calibration_file(calibration_loc.c_str());
73 calibration_file >>
fx >>
fy >>
s >>
u0 >>
v0 >>
b;
76 cout <<
"Reading camera poses" << endl;
77 ifstream pose_file(pose_loc.c_str());
82 while (pose_file >> pose_id) {
83 for (
int i = 0;
i < 16;
i++) {
84 pose_file >>
m.data()[
i];
86 if(add_initial_noise){
87 m(1,3) += (pose_id % 10)/10.0;
92 const auto initialPoses = initial_estimate.
extract<
Pose3>();
94 init_pose3Out.open(init_poseOutput.c_str(),
ios::out);
95 for (
size_t i = 1;
i <= initialPoses.size();
i++) {
98 << initialPoses.at(
X(
i)).
matrix().format(
108 double uL, uR,
v, _X,
Y,
Z;
109 ifstream factor_file(factor_loc.c_str());
110 cout <<
"Reading stereo factors" << endl;
115 size_t current_l = 3;
117 while (factor_file >>
x >>
l >> uL >> uR >>
v >> _X >>
Y >>
Z) {
134 params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
135 params.verbosity = NonlinearOptimizerParams::ERROR;
137 cout <<
"Optimizing" << endl;
142 cout <<
"Final result sample:" << endl;
144 pose_values.
print(
"Final camera poses:\n");
147 pose3Out.open(poseOutput.c_str(),
ios::out);
148 for(
size_t i = 1;
i<=pose_values.
size();
i++){
152 cout <<
"Writing output" << endl;
Contains generic global functions designed particularly for the matlab interface.
Smart stereo factor on poses, assuming camera calibration is fixed.
Values allPose3s(const Values &values)
Extract all Pose3 values.
virtual const Values & optimize()
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
GaussianFactorGraphValuePair Y
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
static const SmartProjectionParams params
const ValueType at(Key j) const
utility functions for loading datasets
std::map< Key, ValueType > extract(const std::function< bool(Key)> &filterFcn=&_truePredicate< Key >) const
static const Line3 l(Rot3(), 1, 1)
std::shared_ptr< Cal3_S2Stereo > shared_ptr
The most common 5DOF 3D->2D calibration + Stereo baseline.
SmartProjectionPoseFactor< Cal3_S2 > SmartFactor
noiseModel::Diagonal::shared_ptr model
std::ofstream out("Result.txt")
Map< Matrix< T, Dynamic, Dynamic, ColMajor >, 0, OuterStride<> > matrix(T *data, int rows, int cols, int stride)
int main(int argc, char **argv)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
The most common 5DOF 3D->2D calibration, stereo version.
Factor Graph consisting of non-linear factors.
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
void insert(Key j, const Value &val)
Array< int, Dynamic, 1 > v
The matrix class, also used for vectors and row-vectors.
GTSAM_EXPORT std::string findExampleDataFile(const std::string &name)
std::shared_ptr< Isotropic > shared_ptr
std::shared_ptr< This > shared_ptr
shorthand for a smart pointer to a factor
NonlinearFactorGraph graph
A non-templated config holding any types of Manifold-group elements.
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:31