From 01f6ee56e43aa9bf9208b51464e265c977a6ea7b Mon Sep 17 00:00:00 2001 From: Luca Carlone Date: Tue, 15 Oct 2013 22:43:10 +0000 Subject: [PATCH] templated version for Kitti, triangulation, and factorCreators --- ...ProjectionFactorExample_kitti_nonbatch.cpp | 2 +- .../examples/SmartProjectionFactorTesting.cpp | 40 ++++++++---- gtsam_unstable/geometry/triangulation.cpp | 27 -------- gtsam_unstable/geometry/triangulation.h | 33 ++++++++-- .../slam/GenericProjectionFactorsCreator.h | 24 ++++---- .../slam/SmartProjectionFactorsCreator.h | 61 ++++++++++++++++--- 6 files changed, 121 insertions(+), 66 deletions(-) diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp index 7f37979ae..4a0562257 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample_kitti_nonbatch.cpp @@ -281,7 +281,7 @@ int main(int argc, char** argv) { unsigned int maxNumPoses = 1e+6; // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used - bool useSmartProjectionFactor = false; + bool useSmartProjectionFactor = true; bool useLM = true; double KittiLinThreshold = -1.0; // 0.005; // diff --git a/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp b/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp index 80c9f598f..420d43b18 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorTesting.cpp @@ -62,6 +62,8 @@ using symbol_shorthand::X; using symbol_shorthand::L; typedef PriorFactor Pose3Prior; +//typedef SmartProjectionFactorsCreator SmartFactorsCreator; +//typedef GenericProjectionFactorsCreator ProjectionFactorsCreator; typedef SmartProjectionFactorsCreator SmartFactorsCreator; typedef GenericProjectionFactorsCreator ProjectionFactorsCreator; typedef FastMap OrderingMap; @@ -199,12 +201,10 @@ void optimizeGraphISAM2(NonlinearFactorGraph &graph, gtsam::Values::shared_ptr g // main int main(int argc, char** argv) { - // unsigned int maxNumLandmarks = 1e+7; - // unsigned int maxNumPoses = 1e+7; - // Set to true to use SmartProjectionFactor. Otherwise GenericProjectionFactor will be used - bool useSmartProjectionFactor = false; + bool useSmartProjectionFactor = true; bool useLM = true; + bool addNoise = false; // Smart factors settings double linThreshold = -1.0; // negative is disabled @@ -216,6 +216,8 @@ int main(int argc, char** argv) { std::cout << "PARAM SmartFactor: " << useSmartProjectionFactor << std::endl; std::cout << "PARAM LM: " << useLM << std::endl; std::cout << "PARAM linThreshold (negative is disabled): " << linThreshold << std::endl; + if(addNoise) + std::cout << "PARAM Noise: " << addNoise << std::endl; // Get home directory and dataset string HOME = getenv("HOME"); @@ -249,10 +251,10 @@ int main(int argc, char** argv) { bool optimized = false; boost::shared_ptr ordering(new Ordering()); - // std::vector< boost::shared_ptr > K_bundler_cameras; // TODO: uncomment - std::vector< boost::shared_ptr > K_S2_cameras; +// std::vector< boost::shared_ptr > K_cameras; // TODO: uncomment + std::vector< boost::shared_ptr > K_cameras; - // boost::shared_ptr Kbund(new Cal3Bundler());// TODO: uncomment +// boost::shared_ptr K(new Cal3Bundler()); // TODO: uncomment Cal3_S2::shared_ptr K(new Cal3_S2()); SmartFactorsCreator smartCreator(pixel_sigma, K, rankTolerance, linThreshold); // this initial K is not used @@ -287,14 +289,23 @@ int main(int argc, char** argv) { for(unsigned int i = 0; i < totNumPoses; i++){ // R,t,f,k1 and 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 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); + K_cameras.push_back(K_S2); Vector3 rotVect(rotx,roty,rotz); // FORMAT CONVERSION!! R -> R' Rot3 R = Rot3::Expmap(rotVect); - loadedValues->insert(X(i), Pose3(R.inverse(), - R.unrotate(Point3(x,y,z)) ) ); + Matrix3 R_bal_gtsam_mat = Matrix3::Zero(3,3); + R_bal_gtsam_mat(0,0) = 1.0; R_bal_gtsam_mat(1,1) = -1.0; R_bal_gtsam_mat(2,2) = -1.0; + Rot3 R_bal_gtsam_ = Rot3(R_bal_gtsam_mat); + Pose3 CameraPose((R.inverse()).compose(R_bal_gtsam_), - R.unrotate(Point3(x,y,z))); + + if(addNoise){ + Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.3,0.1,0.3)); + CameraPose = CameraPose.compose(noise_pose); + } + loadedValues->insert(X(i), CameraPose ); } cout << "last pose: " << x << " " << y << " " << z << " " << rotx << " " @@ -319,7 +330,7 @@ int main(int argc, char** argv) { l = vector_l.at(i); r = vector_r.at(i); // FORMAT CONVERSION!! XPOINT - u = - vector_u.at(i); + u = vector_u.at(i); // FORMAT CONVERSION!! XPOINT v = - vector_v.at(i); @@ -337,7 +348,7 @@ int main(int argc, char** argv) { } else { // or STANDARD PROJECTION FACTORS - projectionCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_S2_cameras.at(r), graphProjection); + projectionCreator.add(L(l), X(r), Point2(u,v), pixel_sigma, K_cameras.at(r), graphProjection); numLandmarks = projectionCreator.getNumLandmarks(); optimized = false; } @@ -382,6 +393,9 @@ int main(int argc, char** argv) { optimized = true; + cout << "===================================================" << endl; + tictoc_print_(); + cout << "===================================================" << endl; writeValues("./", result); // if (1||debug) fprintf(stderr,"%d: %d > %d, %d > %d\n", count, numLandmarks, maxNumLandmarks, numPoses, maxNumPoses); diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index 76c1ecde9..8fa29b231 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -54,33 +54,6 @@ Point3 triangulateDLT(const vector& projection_matrices, return Point3(sub( (v / v(3)),0,3)); } -/* ************************************************************************* */ -Point3 triangulatePoint3(const vector& poses, - const vector& measurements, const Cal3_S2& K, double rank_tol) { - - assert(poses.size() == measurements.size()); - - if(poses.size() < 2) - throw(TriangulationUnderconstrainedException()); - - vector projection_matrices; - - // construct projection matrices from poses & calibration - BOOST_FOREACH(const Pose3& pose, poses) - projection_matrices += K.matrix() * sub(pose.inverse().matrix(),0,3,0,4); - - Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol); - - // verify that the triangulated point lies infront of all cameras - BOOST_FOREACH(const Pose3& pose, poses) { - const Point3& p_local = pose.transform_to(triangulated_point); - if(p_local.z() <= 0) - throw(TriangulationCheiralityException()); - } - - return triangulated_point; -} - /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam_unstable/geometry/triangulation.h b/gtsam_unstable/geometry/triangulation.h index 2d31e1b45..ac99a9a2a 100644 --- a/gtsam_unstable/geometry/triangulation.h +++ b/gtsam_unstable/geometry/triangulation.h @@ -47,6 +47,9 @@ public: } }; +Point3 triangulateDLT(const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol); + /** * Function to triangulate 3D landmark point from an arbitrary number * of poses (at least 2) using the DLT. The function checks that the @@ -58,12 +61,32 @@ public: * @param rank tolerance, default 1e-9 * @return Returns a Point3 on success, boost::none otherwise. */ +template GTSAM_UNSTABLE_EXPORT Point3 triangulatePoint3(const std::vector& poses, - const std::vector& measurements, const Cal3_S2& K, double rank_tol = - 1e-9); + const std::vector& measurements, const CALIBRATION& K, double rank_tol = 1e-9){ -Point3 triangulateDLT(const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol); + assert(poses.size() == measurements.size()); + + if(poses.size() < 2) + throw(TriangulationUnderconstrainedException()); + + std::vector projection_matrices; + + // construct projection matrices from poses & calibration + BOOST_FOREACH(const Pose3& pose, poses) + projection_matrices.push_back( K.K() * sub(pose.inverse().matrix(),0,3,0,4) ); + + Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol); + + // verify that the triangulated point lies infront of all cameras + BOOST_FOREACH(const Pose3& pose, poses) { + const Point3& p_local = pose.transform_to(triangulated_point); + if(p_local.z() <= 0) + throw(TriangulationCheiralityException()); + } + + return triangulated_point; +} /* ************************************************************************* */ template @@ -80,7 +103,7 @@ Point3 triangulatePoint3(const std::vector& poses, // construct projection matrices from poses & calibration for(size_t i = 0; imatrix() * sub(poses.at(i).inverse().matrix(),0,3,0,4) ); + projection_matrices.push_back( Ks.at(i)->K() * sub(poses.at(i).inverse().matrix(),0,3,0,4) ); } Point3 triangulated_point = triangulateDLT(projection_matrices, measurements, rank_tol); diff --git a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h index 006a10046..3167e0f0e 100644 --- a/gtsam_unstable/slam/GenericProjectionFactorsCreator.h +++ b/gtsam_unstable/slam/GenericProjectionFactorsCreator.h @@ -31,12 +31,14 @@ namespace gtsam { - typedef GenericProjectionFactor ProjectionFactor; - typedef FastMap > > ProjectionFactorMap; - typedef FastMap OrderingMap; template class GenericProjectionFactorsCreator { + + typedef GenericProjectionFactor ProjectionFactor; + typedef FastMap > > ProjectionFactorMap; + typedef FastMap OrderingMap; + public: GenericProjectionFactorsCreator(const SharedNoiseModel& model, const boost::shared_ptr& K, @@ -51,10 +53,10 @@ namespace gtsam { bool debug = false; // Create projection factor - ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, noise_, poseKey, landmarkKey, K_)); + boost::shared_ptr projectionFactor(new ProjectionFactor(measurement, noise_, poseKey, landmarkKey, K_)); // Check if landmark exists in mapping - ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey); + typename ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey); if (pfit != projectionFactors.end()) { if (debug) fprintf(stderr,"Adding measurement to existing landmark\n"); @@ -65,7 +67,7 @@ namespace gtsam { if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end()); // Create a new vector of projection factors - std::vector projectionFactorVector; + std::vector > projectionFactorVector; projectionFactorVector.push_back(projectionFactor); // Insert projection factor to NEW list of projection factors associated with this landmark @@ -89,10 +91,10 @@ namespace gtsam { bool debug = false; // Create projection factor - ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, model, poseKey, landmarkKey, K)); + typename ProjectionFactor::shared_ptr projectionFactor(new ProjectionFactor(measurement, model, poseKey, landmarkKey, K)); // Check if landmark exists in mapping - ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey); + typename ProjectionFactorMap::iterator pfit = projectionFactors.find(landmarkKey); if (pfit != projectionFactors.end()) { if (debug) fprintf(stderr,"Adding measurement to existing landmark\n"); @@ -103,7 +105,7 @@ namespace gtsam { if (debug) fprintf(stderr,"New landmark (%d)\n", pfit != projectionFactors.end()); // Create a new vector of projection factors - std::vector projectionFactorVector; + std::vector > projectionFactorVector; projectionFactorVector.push_back(projectionFactor); // Insert projection factor to NEW list of projection factors associated with this landmark @@ -166,11 +168,11 @@ namespace gtsam { std::cout << "Reading initial guess for 3D points from file" << std::endl; std::vector > projectionFactorVector; - std::vector >::iterator vfit; + typename std::vector >::iterator vfit; Point3 point; Pose3 cameraPose; - ProjectionFactorMap::iterator pfit; + typename ProjectionFactorMap::iterator pfit; if (debug) graphValues->print("graphValues \n"); if (debug) std::cout << " # END VALUES: " << std::endl; diff --git a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h index 19d710092..758e5a63f 100644 --- a/gtsam_unstable/slam/SmartProjectionFactorsCreator.h +++ b/gtsam_unstable/slam/SmartProjectionFactorsCreator.h @@ -14,7 +14,8 @@ // Use a map to store landmark/smart factor pairs #include -#include +// #include +#include #include #include @@ -25,12 +26,13 @@ namespace gtsam { - typedef SmartProjectionFactor SmartFactor; - typedef FastMap > SmartFactorToStateMap; - typedef FastMap > SmartFactorMap; - template class SmartProjectionFactorsCreator { + + typedef SmartProjectionHessianFactor SmartFactor; + typedef FastMap > SmartFactorToStateMap; + typedef FastMap > SmartFactorMap; + public: SmartProjectionFactorsCreator(const SharedNoiseModel& model, const boost::shared_ptr& K, @@ -51,12 +53,12 @@ namespace gtsam { // Check if landmark exists in mapping SmartFactorToStateMap::iterator fsit = smartFactorStates.find(landmarkKey); - SmartFactorMap::iterator fit = smartFactors.find(landmarkKey); + typename SmartFactorMap::iterator fit = smartFactors.find(landmarkKey); if (fsit != smartFactorStates.end() && fit != smartFactors.end()) { if (debug) fprintf(stderr,"Adding measurement to existing landmark\n"); // Add measurement to smart factor - (*fit).second->add(measurement, poseKey); + (*fit).second->add(measurement, poseKey, noise_, K_); totalNumMeasurements++; if (debug) (*fit).second->print(); @@ -68,8 +70,9 @@ namespace gtsam { measurements.push_back(measurement); // This is a new landmark, create a new factor and add to mapping - boost::shared_ptr smartFactorState(new SmartProjectionFactorState()); - SmartFactor::shared_ptr smartFactor(new SmartFactor(views, measurements, noise_, K_, rankTolerance_, linearizationThreshold_)); + boost::shared_ptr smartFactorState(new SmartProjectionHessianFactorState()); + boost::shared_ptr smartFactor(new SmartFactor(rankTolerance_, linearizationThreshold_)); + smartFactor->add(measurement, poseKey, noise_, K_); smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) ); smartFactors.insert( std::make_pair(landmarkKey, smartFactor) ); graph.push_back(smartFactor); @@ -83,6 +86,46 @@ namespace gtsam { } } + void add(Key landmarkKey, Key poseKey, + Point2 measurement, + const SharedNoiseModel& model, + const boost::shared_ptr& K, + NonlinearFactorGraph &graph) { + + std::vector views; + std::vector measurements; + + bool debug = false; + + // Check if landmark exists in mapping + SmartFactorToStateMap::iterator fsit = smartFactorStates.find(landmarkKey); + typename SmartFactorMap::iterator fit = smartFactors.find(landmarkKey); + if (fsit != smartFactorStates.end() && fit != smartFactors.end()) { + if (debug) fprintf(stderr,"Adding measurement to existing landmark\n"); + + // Add measurement to smart factor + (*fit).second->add(measurement, poseKey, model, K); + totalNumMeasurements++; + if (debug) (*fit).second->print(); + + } else { + + if (debug) fprintf(stderr,"New landmark (%d,%d)\n", fsit != smartFactorStates.end(), 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); + smartFactorStates.insert( std::make_pair(landmarkKey, smartFactorState) ); + smartFactors.insert( std::make_pair(landmarkKey, smartFactor) ); + graph.push_back(smartFactor); + + numLandmarks++; + totalNumMeasurements++; + + } + } + unsigned int getTotalNumMeasurements() { return totalNumMeasurements; } unsigned int getNumLandmarks() { return numLandmarks; }