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.
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 Oct 4 2024 03:05:28