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

temporary disabled -

release/4.3a0
Luca Carlone 2013-10-18 06:17:01 +00:00
parent f6d4da5d40
commit f7e90bbe86
1 changed files with 10 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
gtsam_unstable/examples/SmartProjectionFactorTesting.cpp
View File

@ -16,6 +16,8 @@
* @author Luca Carlone * @author Luca Carlone
*/ */
#ifdef DEVELOP
// Use a map to store landmark/smart factor pairs // Use a map to store landmark/smart factor pairs
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
@ -113,36 +115,6 @@ void writeValues(string directory_, const Values& values){
} }
// 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) { void optimizeGraphLM(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr graphValues, Values &result, boost::shared_ptr<Ordering> &ordering) {
// Optimization parameters // Optimization parameters
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
@ -468,9 +440,15 @@ int main(int argc, char** argv) {
cout << "===================================================" << endl; cout << "===================================================" << endl;
writeValues("./", result); writeValues("./", result);
writeValuesBAL("./", result, vector_r, vector_l, vector_u, vector_v, totNumPoses, totNumLandmarks, totNumMeasurements);
if (debug) cout << numLandmarks << " " << numPoses << " " << optimized << endl; if (debug) cout << numLandmarks << " " << numPoses << " " << optimized << endl;
exit(0); exit(0);
} }
#endif
int main(){
return 1;
}