From 45e1fe832d7d8edcba865373b64d7cad356f8c7e Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Wed, 16 Oct 2013 00:54:56 +0000 Subject: [PATCH] fixed issues --- .../examples/SmartProjectionFactorTesting.cpp | 34 +++++++++++++------ gtsam_unstable/geometry/triangulation.h | 7 +++- .../slam/GenericProjectionFactorsCreator.h | 27 +++++++++++++-- .../slam/SmartProjectionFactorsCreator.h | 20 ++++++++--- .../slam/SmartProjectionHessianFactor.h | 8 ++++- 5 files changed, 75 insertions(+), 21 deletions(-) diff --git a/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp b/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp index 420d43b18..e7e1ac8d9 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp @@ -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(new Ordering()); // std::vector< boost::shared_ptr > K_cameras; // TODO: uncomment - std::vector< boost::shared_ptr > K_cameras; // boost::shared_ptr 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 > 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 Kbundler(new Cal3Bundler(f, k1, k2, 0.0, 0.0)); //TODO: uncomment // K_cameras.push_back(Kbundler); //TODO: uncomment boost::shared_ptr 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); } diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index ac99a9a2a..9210e1fd7 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -73,8 +73,11 @@ GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, std::vector 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& poses, // construct projection matrices from poses & calibration for(size_t i = 0; iK() * 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); diff --git a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h index 3167e0f0e..7616a81da 100644 --- a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h +++ b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h @@ -37,6 +37,7 @@ namespace gtsam { typedef GenericProjectionFactor ProjectionFactor; typedef FastMap > > ProjectionFactorMap; + typedef FastMap > CalibrationMap; typedef FastMap 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 > projectionFactorVector; typename std::vector >::iterator vfit; Point3 point; @@ -188,6 +199,7 @@ namespace gtsam { std::vector cameraPoses; std::vector measured; + typename std::vector< boost::shared_ptr > 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((*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(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 cameraPoseKeys; std::vector landmarkKeys; ProjectionFactorMap projectionFactors; + CalibrationMap keyCalibrationMap; boost::shared_ptr ordering; // orderingMethod: 0 - COLAMD, 1 - landmark first, then COLAMD on poses (constrained ordering) int orderingMethod; diff --git a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h index 758e5a63f..24bd0c0d8 100644 --- a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h +++ b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h @@ -89,12 +89,14 @@ namespace gtsam { void add(Key landmarkKey, Key poseKey, Point2 measurement, const SharedNoiseModel& model, - const boost::shared_ptr& K, + const boost::shared_ptr& Ki, NonlinearFactorGraph &graph) { std::vector views; std::vector 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 smartFactorState(new SmartProjectionHessianFactorState()); boost::shared_ptr 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; }; diff --git a/gtsam_unstable/slam/SmartProjectionHessianFactor.h b/gtsam_unstable/slam/SmartProjectionHessianFactor.h index ceb0c7514..b801cf954 100644 --- a/gtsam_unstable/slam/SmartProjectionHessianFactor.h +++ b/gtsam_unstable/slam/SmartProjectionHessianFactor.h @@ -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& 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 camera(pose, *K_all_.at(i));