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

fixed issues

release/4.3a0
Luca Carlone 2013-10-16 00:54:56 +00:00
parent 01f6ee56e4
commit 45e1fe832d
5 changed files with 75 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam_unstable/examples/SmartProjectionFactorTesting.cpp
View File

@ -203,6 +203,8 @@ int main(int argc, char** argv) {
// Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used
bool useSmartProjectionFactor = true;
bool doTriangulation = true; // we read points initial guess from file or we triangulate
bool useLM = true;
bool addNoise = false;
@ -221,8 +223,14 @@ int main(int argc, char** argv) {
// Get home directory and dataset
string HOME = getenv("HOME");
string input_dir = HOME + "/data/SfM/BAL/Ladybug/";
string datasetName = "problem-1031-110968-pre.txt";
string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1031-110968-pre.txt";
// string datasetFile = HOME + "/data/SfM/BAL/Ladybug/problem-1723-156502-pre.txt";
// string datasetFile = HOME + "/data/SfM/BAL/final/problem-1936-649673-pre.txt";
// 1936 649673 5213733 problem-1936-649673-pre.txt.bz2
// 3068 310854 1653812 problem-3068-310854-pre.txt.bz2
// 4585 1324582 9125125 problem-4585-1324582-pre.txt.bz2
// 13682 4456117 28987644 problem-13682-4456117-pre.txt.bz2
static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2));
NonlinearFactorGraph graphSmart, graphProjection;
@ -234,7 +242,7 @@ int main(int argc, char** argv) {
// Read in kitti dataset
ifstream fin;
fin.open((input_dir+datasetName).c_str());
fin.open((datasetFile).c_str());
if(!fin) {
cerr << "Could not open dataset" << endl;
exit(1);
@ -252,7 +260,6 @@ int main(int argc, char** argv) {
boost::shared_ptr<Ordering> ordering(new Ordering());
// std::vector< boost::shared_ptr<Cal3Bundler> > K_cameras; // TODO: uncomment
std::vector< boost::shared_ptr<Cal3_S2> > K_cameras;
// boost::shared_ptr<Cal3Bundler> K(new Cal3Bundler()); // TODO: uncomment
Cal3_S2::shared_ptr K(new Cal3_S2());
@ -285,6 +292,8 @@ int main(int argc, char** argv) {
cout << "last measurement: " << r << " " << l << " " << u << " " << v << endl;
std::vector< boost::shared_ptr<Cal3_S2> > K_cameras;
// create values
for(unsigned int i = 0; i < totNumPoses; i++){
// R,t,f,k1 and k2.
@ -292,6 +301,7 @@ int main(int argc, char** argv) {
// boost::shared_ptr<Cal3Bundler> Kbundler(new Cal3Bundler(f, k1, k2, 0.0, 0.0)); //TODO: uncomment
// K_cameras.push_back(Kbundler); //TODO: uncomment
boost::shared_ptr<Cal3_S2> K_S2(new Cal3_S2(f, f, 0.0, 0.0, 0.0));
// cout << "f "<< f << endl;
K_cameras.push_back(K_S2);
Vector3 rotVect(rotx,roty,rotz);
// FORMAT CONVERSION!! R -> R'
@ -336,7 +346,7 @@ int main(int argc, char** argv) {
if (useSmartProjectionFactor) {
smartCreator.add(L(l), X(r), Point2(u,v), graphSmart);
smartCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_cameras.at(r), graphSmart);
numLandmarks = smartCreator.getNumLandmarks();
// Add initial pose value if pose does not exist
@ -363,10 +373,8 @@ int main(int argc, char** argv) {
cout << "---------------------------------------------------------- " << endl;
if (!useSmartProjectionFactor) {
bool doTriangulation = false; // we read points initial guess from file
projectionCreator.update(graphProjection, loadedValues, graphProjectionValues, doTriangulation);
graphProjectionValues = loadedValues;
// graphProjectionValues = loadedValues;
ordering = projectionCreator.getOrdering();
}
@ -383,13 +391,16 @@ int main(int argc, char** argv) {
optimizeGraphLM(graphSmart, graphSmartValues, result, ordering);
else
optimizeGraphISAM2(graphSmart, graphSmartValues, result);
cout << "Final reprojection error (smart): " << graphSmart.error(result);
} else {
if (useLM)
optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering);
else
optimizeGraphISAM2(graphSmart, graphSmartValues, result); // ?
optimizeGraphISAM2(graphProjection, graphProjectionValues, result); // ?
cout << "Final reprojection error (standard): " << graphProjection.error(result);
}
// *graphSmartValues = result; // we use optimized solution as initial guess for the next one
optimized = true;
@ -398,6 +409,7 @@ int main(int argc, char** argv) {
cout << "===================================================" << endl;
writeValues("./", result);
// if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses);
if (debug) cout << numLandmarks << " " << numPoses << " " << optimized << endl;
exit(0);
}

7
gtsam_unstable/geometry/triangulation.h
View File

@ -73,8 +73,11 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector<Pose3>& poses,
std::vector<Matrix> projection_matrices;
// construct projection matrices from poses & calibration
BOOST_FOREACH(const Pose3& pose, poses)
BOOST_FOREACH(const Pose3& pose, poses){
projection_matrices.push_back( K.K() * sub(pose.inverse().matrix(),0,3,0,4) );
// std::cout << "Calibration i \n" << K.K() << std::endl;
// std::cout << "rank_tol i \n" << rank_tol << std::endl;
}
Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);
@ -104,6 +107,8 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
// construct projection matrices from poses & calibration
for(size_t i = 0; i<poses.size(); i++){
projection_matrices.push_back( Ks.at(i)->K() * sub(poses.at(i).inverse().matrix(),0,3,0,4) );
// std::cout << "2Calibration i \n" << Ks.at(i)->K() << std::endl;
// std::cout << "2rank_tol i \n" << rank_tol << std::endl;
}
Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol);

27
gtsam_unstable/slam/GenericProjectionFactorsCreator.h
View File

@ -37,6 +37,7 @@ namespace gtsam {
typedef GenericProjectionFactor<Pose3, Point3, CALIBRATION> ProjectionFactor;
typedef FastMap<Key, std::vector<boost::shared_ptr<ProjectionFactor> > > ProjectionFactorMap;
typedef FastMap<Key, boost::shared_ptr<CALIBRATION> > CalibrationMap;
typedef FastMap<Key, int> OrderingMap;
public:
@ -90,6 +91,12 @@ namespace gtsam {
NonlinearFactorGraph &graph) {
bool debug = false;
// Check if landmark exists in mapping
typename CalibrationMap::iterator calfit = keyCalibrationMap.find(poseKey);
if (calfit == keyCalibrationMap.end()){
keyCalibrationMap.insert( std::make_pair(poseKey, K) );
}
// Create projection factor
typename ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, model, poseKey, landmarkKey, K));
@ -167,6 +174,10 @@ namespace gtsam {
else
std::cout << "Reading initial guess for 3D points from file" << std::endl;
double rankTolerance=1;
std::cout << "rankTolerance " << rankTolerance << std::endl;
std::vector<boost::shared_ptr<ProjectionFactor> > projectionFactorVector;
typename std::vector<boost::shared_ptr<ProjectionFactor> >::iterator vfit;
Point3 point;
@ -188,6 +199,7 @@ namespace gtsam {
std::vector<Pose3> cameraPoses;
std::vector<Point2> measured;
typename std::vector< boost::shared_ptr<CALIBRATION> > Ks;
// Iterate through projection factors
for (vfit = projectionFactorVector.begin(); vfit != projectionFactorVector.end(); vfit++) { // for each factors connected to the landmark
@ -196,8 +208,16 @@ namespace gtsam {
if (debug) std::cout << "ProjectionFactor: " << std::endl;
if (debug) (*vfit)->print("ProjectionFactor");
// Iterate through poses
cameraPoses.push_back( loadedValues->at<Pose3>((*vfit)->key1() ) ); // get poses connected to the landmark
// Iterate through poses // find calibration
Key poseKey = (*vfit)->key1();
typename CalibrationMap::iterator calfit = keyCalibrationMap.find(poseKey);
if (calfit == keyCalibrationMap.end()){ // the pose is not there
std::cout << "ProjectionFactor: " << std::endl;
}else{
Ks.push_back(calfit->second);
}
cameraPoses.push_back( loadedValues->at<Pose3>(poseKey) ); // get poses connected to the landmark
measured.push_back( (*vfit)->measured() ); // get measurements of the landmark
}
@ -205,7 +225,7 @@ namespace gtsam {
if (doTriangualatePoints){
// std::cout << "Triangulating points " << std::endl;
try {
point = triangulatePoint3(cameraPoses, measured, *K_);
point = triangulatePoint3(cameraPoses, measured, Ks, rankTolerance);
if (debug) std::cout << "Triangulation succeeded: " << point << std::endl;
} catch( TriangulationUnderconstrainedException& e) {
if (debug) std::cout << "Triangulation failed because of unconstrained exception" << std::endl;
@ -272,6 +292,7 @@ namespace gtsam {
std::vector<Key> cameraPoseKeys;
std::vector<Key> landmarkKeys;
ProjectionFactorMap projectionFactors;
CalibrationMap keyCalibrationMap;
boost::shared_ptr<Ordering> ordering;
// orderingMethod: 0 - COLAMD, 1 - landmark first, then COLAMD on poses (constrained ordering)
int orderingMethod;

20
gtsam_unstable/slam/SmartProjectionFactorsCreator.h
View File

@ -89,12 +89,14 @@ namespace gtsam {
void add(Key landmarkKey, Key poseKey,
Point2 measurement,
const SharedNoiseModel& model,
const boost::shared_ptr<CALIBRATION>& K,
const boost::shared_ptr<CALIBRATION>& Ki,
NonlinearFactorGraph &graph) {
std::vector<Key> views;
std::vector<Point2> measurements;
// std::cout << "matrix : " << K->K() << std::endl;
bool debug = false;
// Check if landmark exists in mapping
@ -102,24 +104,31 @@ namespace gtsam {
typename SmartFactorMap::iterator fit = smartFactors.find(landmarkKey);
if (fsit != smartFactorStates.end() && fit != smartFactors.end()) {
if (debug) fprintf(stderr,"Adding measurement to existing landmark\n");
// (*fit).second->print("before: ");
// Add measurement to smart factor
(*fit).second->add(measurement, poseKey, model, K);
(*fit).second->add(measurement, poseKey, model, Ki);
// (*fit).second->print("after: ");
totalNumMeasurements++;
if (debug) (*fit).second->print();
} else {
if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), fit != smartFactors.end());
if (debug) std::cout <<"landmark " << DefaultKeyFormatter(landmarkKey) << "pose " << DefaultKeyFormatter(poseKey) << std::endl;
// if (debug) fprintf(stderr,"landmarkKey %d poseKey %d measurement\n", landmarkKey, fit != smartFactors.end());
// This is a new landmark, create a new factor and add to mapping
boost::shared_ptr<SmartProjectionHessianFactorState> smartFactorState(new SmartProjectionHessianFactorState());
boost::shared_ptr<SmartFactor> smartFactor(new SmartFactor(rankTolerance_, linearizationThreshold_));
smartFactor->add(measurement, poseKey, model, K);
smartFactor->add(measurement, poseKey, model, Ki);
// smartFactor->print("created: ");
// smartFactor->print(" ");
smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) );
smartFactors.insert( std::make_pair(landmarkKey, smartFactor) );
graph.push_back(smartFactor);
if (debug) std::cout <<" graph size " << graph.size() << std::endl;
numLandmarks++;
totalNumMeasurements++;
@ -144,6 +153,7 @@ namespace gtsam {
SmartFactorMap smartFactors;
unsigned int totalNumMeasurements;
//landmarkKeys.push_back( L(l) );
unsigned int numLandmarks;
};

8
gtsam_unstable/slam/SmartProjectionHessianFactor.h
View File

@ -40,7 +40,7 @@ namespace gtsam {
// default threshold for rank deficient triangulation
static double defaultRankTolerance = 1; // this value may be scenario-dependent and has to be larger in presence of larger noise
// if set to true will use the rotation-only version for degenerate cases
static bool manageDegeneracy = true;
static bool manageDegeneracy = false;
/**
* Structure for storing some state memory, used to speed up optimization
@ -250,6 +250,10 @@ namespace gtsam {
BOOST_FOREACH(const SharedNoiseModel& noise_i, noise_) {
noise_i->print("noise model = ");
}
BOOST_FOREACH(const boost::shared_ptr<CALIBRATION>& K, K_all_) {
K->print("calibration = ");
}
if(this->body_P_sensor_){
this->body_P_sensor_->print(" sensor pose in body frame: ");
}
@ -318,6 +322,7 @@ namespace gtsam {
if (retriangulate) {
// We triangulate the 3D position of the landmark
try {
std::cout << "triangulatePoint3 i \n" << rankTolerance << std::endl;
state_->point = triangulatePoint3(cameraPoses, measured_, K_all_, rankTolerance);
state_->degenerate = false;
state_->cheiralityException = false;
@ -521,6 +526,7 @@ namespace gtsam {
}
if(state_->degenerate){
// return 0.0; // TODO: this maybe should be zero?
for(size_t i = 0; i < measured_.size(); i++) {
Pose3 pose = cameraPoses.at(i);
PinholeCamera<CALIBRATION> camera(pose, *K_all_.at(i));