From aa3729b098d6de9090d780b480ecb532d2b9668b Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Tue, 15 Oct 2013 19:16:39 +0000 Subject: [PATCH] Running, but to test --- .../examples/SmartProjectionFactorTesting.cpp | 75 ++++++++++++++----- 1 file changed, 57 insertions(+), 18 deletions(-) diff --git a/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp b/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp index 3421a6d38..80c9f598f 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp @@ -206,11 +206,12 @@ int main(int argc, char** argv) { bool useSmartProjectionFactor = false; bool useLM = true; + // Smart factors settings double linThreshold = -1.0; // negative is disabled double rankTolerance = 1.0; - bool incrementalFlag = false; - int optSkip = 200; // we optimize the graph every optSkip poses + // bool incrementalFlag = false; + // int optSkip = 200; // we optimize the graph every optSkip poses std::cout << "PARAM SmartFactor: " << useSmartProjectionFactor << std::endl; std::cout << "PARAM LM: " << useLM << std::endl; @@ -219,14 +220,15 @@ 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-1723-156502-pre.txt"; + string datasetName = "problem-1031-110968-pre.txt"; static SharedNoiseModel pixel_sigma(noiseModel::Unit::Create(2)); NonlinearFactorGraph graphSmart, graphProjection; gtsam::Values::shared_ptr graphSmartValues(new gtsam::Values()); gtsam::Values::shared_ptr graphProjectionValues(new gtsam::Values()); - gtsam::Values::shared_ptr loadedValues(new gtsam::Values()); + gtsam::Values::shared_ptr loadedValues(new gtsam::Values()); // values we read from file + Values result; // Read in kitti dataset ifstream fin; @@ -243,24 +245,27 @@ int main(int argc, char** argv) { double u, v; double x, y, z, rotx, roty, rotz, f, k1, k2; std::vector landmarkKeys, cameraPoseKeys; - Values result; + bool optimized = false; boost::shared_ptr ordering(new Ordering()); - // std::vector< boost::shared_ptr > K_cameras; // TODO: uncomment - Cal3_S2::shared_ptr K(new Cal3_S2(1, 1, 0, 0, 0)); + // std::vector< boost::shared_ptr > K_bundler_cameras; // TODO: uncomment + std::vector< boost::shared_ptr > K_S2_cameras; // boost::shared_ptr Kbund(new Cal3Bundler());// TODO: uncomment + Cal3_S2::shared_ptr K(new Cal3_S2()); - SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); - ProjectionFactorsCreator projectionCreator(pixel_sigma, K); + SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); // this initial K is not used + ProjectionFactorsCreator projectionCreator(pixel_sigma, K); // this initial K is not used // main loop: reads measurements and adds factors (also performs optimization if desired) // r >> pose id // l >> landmark id // (u >> u) >> measurement unsigned int totNumLandmarks=0, totNumPoses=0, totNumMeasurements=0; - fin >> totNumPoses >> totNumPoses >> totNumMeasurements; + fin >> totNumPoses >> totNumLandmarks >> totNumMeasurements; + + cout << "Dataset: #poses: " << totNumPoses << " #points: " << totNumLandmarks << " #measurements: " << totNumMeasurements << " " << endl; std::vector vector_u; std::vector vector_v; @@ -276,32 +281,47 @@ int main(int argc, char** argv) { vector_l.push_back(l); } + cout << "last measurement: " << r << " " << l << " " << u << " " << v << endl; + // create values for(unsigned int i = 0; i < totNumPoses; i++){ // R,t,f,k1 and k2. - fin >> x >> y >> z >> rotx >> roty >> rotz >> f >> k1 >> k2; + fin >> rotx >> roty >> rotz >> x >> y >> z >> f >> k1 >> k2; // 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)); + K_S2_cameras.push_back(K_S2); Vector3 rotVect(rotx,roty,rotz); - loadedValues->insert(Symbol('x',i), Pose3(Rot3::Expmap(rotVect), Point3(x,y,z) ) ); + // FORMAT CONVERSION!! R -> R' + Rot3 R = Rot3::Expmap(rotVect); + loadedValues->insert(X(i), Pose3(R.inverse(), - R.unrotate(Point3(x,y,z)) ) ); } + cout << "last pose: " << x << " " << y << " " << z << " " << rotx << " " + << roty << " " << rotz << " " << f << " " << k1 << " " << k2 << endl; + // add landmarks in standard projection factors if(!useSmartProjectionFactor){ for(unsigned int i = 0; i < totNumLandmarks; i++){ fin >> x >> y >> z; - loadedValues->insert(Symbol('l',i), Point3(x,y,z) ); + // FORMAT CONVERSION!! XPOINT + loadedValues->insert(L(i), Point3(x,y,z) ); } } + cout << "last point: " << x << " " << y << " " << z << endl; + // 1: add values and factors to the graph // 1.1: add factors // SMART FACTORS .. for(size_t i = 0; i < vector_u.size(); i++){ + l = vector_l.at(i); r = vector_r.at(i); - u = vector_u.at(i); - v = vector_v.at(i); + // FORMAT CONVERSION!! XPOINT + u = - vector_u.at(i); + // FORMAT CONVERSION!! XPOINT + v = - vector_v.at(i); if (useSmartProjectionFactor) { @@ -317,17 +337,36 @@ int main(int argc, char** argv) { } else { // or STANDARD PROJECTION FACTORS - projectionCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K, graphProjection); + projectionCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_S2_cameras.at(r), graphProjection); numLandmarks = projectionCreator.getNumLandmarks(); optimized = false; } } + cout << "Before call to update: ------------------ " << endl; + cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl; + Values valuesProjPoses = graphProjectionValues->filter(); + cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl; + Values valuesProjPoints = graphProjectionValues->filter(); + cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl; + cout << "---------------------------------------------------------- " << endl; + if (!useSmartProjectionFactor) { - projectionCreator.update(graphProjection, loadedValues, graphProjectionValues); + bool doTriangulation = false; // we read points initial guess from file + projectionCreator.update(graphProjection, loadedValues, graphProjectionValues, doTriangulation); + + graphProjectionValues = loadedValues; ordering = projectionCreator.getOrdering(); } + cout << "After call to update: ------------------ " << endl; + cout << "Poses in SmartGraph values: "<< graphSmartValues->size() << endl; + valuesProjPoses = graphProjectionValues->filter(); + cout << "Poses in ProjectionGraph values: "<< valuesProjPoses.size() << endl; + valuesProjPoints = graphProjectionValues->filter(); + cout << "Points in ProjectionGraph values: "<< valuesProjPoints.size() << endl; + cout << "---------------------------------------------------------- " << endl; + if (useSmartProjectionFactor) { if (useLM) optimizeGraphLM(graphSmart, graphSmartValues, result, ordering); @@ -337,7 +376,7 @@ int main(int argc, char** argv) { if (useLM) optimizeGraphLM(graphProjection, graphProjectionValues, result, ordering); else - optimizeGraphISAM2(graphSmart, graphSmartValues, result); + optimizeGraphISAM2(graphSmart, graphSmartValues, result); // ? } // *graphSmartValues = result; // we use optimized solution as initial guess for the next one