12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

intermediate changes (work in progress)

release/4.3a0
Luca Carlone 2013-10-18 01:25:18 +00:00
parent eff198372b
commit 06812ed964
1 changed files with 34 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam_unstable/examples/SmartProjectionFactorTesting.cpp
View File

@ -58,7 +58,7 @@ using namespace gtsam;
using namespace boost::assign;
namespace NM = gtsam::noiseModel;
// #define USE_BUNDLER
#define USE_BUNDLER
using symbol_shorthand::X;
using symbol_shorthand::L;
@ -109,8 +109,38 @@ void writeValues(string directory_, const Values& values){
fout << endl;
}
fout.close();
} // end of if on landmarks
}
// Write key values to file
void writeValuesBAL(string directory_, const Values& values, std::vector<int> vector_r, std::vector<int> vector_l,
std::vector<double> vector_u, std::vector<double> vector_v, int totNumPoses, int totNumLandmarks, int totNumMeasurements){
string filename = directory_ + "est_BAL_poses.txt";
ofstream fout;
fout.open(filename.c_str());
fout.precision(20);
fout << totNumPoses << " " << totNumLandmarks << " " << totNumMeasurements << endl;
for(size_t i=0; i<totNumMeasurements;i++){
fout << vector_r.at(i) << " " << vector_l.at(i) << " " << vector_u.at(i) << " " << vector_v.at(i) << endl;
}
// write out camera poses
BOOST_FOREACH(Values::ConstFiltered<Pose3>::value_type key_value, values.filter<Pose3>()) {
Pose3 CurrPose = key_value.value;
fout << Pose3::Logmap(CurrPose) << endl;
}
if(values.filter<Point3>().size() > 0) {
// write landmarks
BOOST_FOREACH(Values::ConstFiltered<Point3>::value_type key_value, values.filter<Point3>()) {
fout << key_value.value.vector() << endl;
}
fout.close();
} // end of if on landmarks
}
void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result, boost::shared_ptr<Ordering> &ordering) {
@ -326,6 +356,7 @@ int main(int argc, char** argv) {
fin >> rotx >> roty >> rotz >> x >> y >> z >> f >> k1 >> k2;
#ifdef USE_BUNDLER
boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(f, k1, k2, 0.0, 0.0));
// cout << k1 << " " << k2 << endl;
K_cameras.push_back(Kbundler);
#else
boost::shared_ptr<Cal3_S2> K_S2(new Cal3_S2(f, f, 0.0, 0.0, 0.0));
@ -437,6 +468,8 @@ int main(int argc, char** argv) {
cout << "===================================================" << endl;
writeValues("./", result);
writeValuesBAL("./", result, vector_r, vector_l, vector_u, vector_v, totNumPoses, totNumLandmarks, totNumMeasurements);
if (debug) cout << numLandmarks << " " << numPoses << " " << optimized << endl;
exit(0);