diff --git a/CMakeLists.txt b/CMakeLists.txt index bf2614d9..10654dd5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -128,6 +128,7 @@ option(WITH_FREENECT2 "Include Freenect2 support" ON) option(WITH_OPENNI2 "Include OpenNI2 support" ON) option(WITH_DC1394 "Include dc1394 support" ON) option(WITH_G2O "Include g2o support" ON) +option(WITH_GTSAM "Include GTSAM support" ON) option(WITH_CVSBA "Include cvsba support" ON) option(WITH_FLYCAPTURE2 "Include FlyCapture2/Triclops support" ON) @@ -170,6 +171,10 @@ IF(WITH_G2O) FIND_PACKAGE(G2O) ENDIF(WITH_G2O) +IF(WITH_GTSAM) + FIND_PACKAGE(GTSAM) +ENDIF(WITH_GTSAM) + IF(WITH_FLYCAPTURE2) FIND_PACKAGE(FlyCapture2) ENDIF(WITH_FLYCAPTURE2) @@ -429,6 +434,14 @@ ELSE() MESSAGE(STATUS " With g2o = NO (g2o not found)") ENDIF() +IF(GTSAM_FOUND) +MESSAGE(STATUS " With GTSAM = YES") +ELSEIF(NOT WITH_GTSAM) +MESSAGE(STATUS " With GTSAM = NO (WITH_GTSAM=OFF)") +ELSE() +MESSAGE(STATUS " With GTSAM = NO (GTSAM not found)") +ENDIF() + IF(cvsba_FOUND) MESSAGE(STATUS " With cvsba = YES") ELSEIF(NOT WITH_CVSBA) diff --git a/app/src/main.cpp b/app/src/main.cpp index 9305a1b7..7bd30cef 100644 --- a/app/src/main.cpp +++ b/app/src/main.cpp @@ -41,7 +41,7 @@ int main(int argc, char* argv[]) { /* Set logger type */ ULogger::setType(ULogger::kTypeConsole); - ULogger::setLevel(ULogger::kInfo); + ULogger::setLevel(ULogger::kDebug); /* Create tasks */ QApplication * app = new QApplication(argc, argv); diff --git a/cmake_modules/FindG2O.cmake b/cmake_modules/FindG2O.cmake index 343ed530..892c0933 100644 --- a/cmake_modules/FindG2O.cmake +++ b/cmake_modules/FindG2O.cmake @@ -70,11 +70,13 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR}) SET(G2O_LIBRARIES ${G2O_STUFF_LIBRARY} - ${G2O_CORE_LIBRARY} + ${G2O_CORE_LIBRARY} + ${G2O_SOLVER_CHOLMOD} ${G2O_SOLVER_CSPARSE} ${G2O_SOLVER_CSPARSE_EXTENSION} ${G2O_TYPES_SLAM2D} ${G2O_TYPES_SLAM3D} - ${CSPARSE_LIBRARY}) + ${CSPARSE_LIBRARY} + cholmod) SET(G2O_FOUND "YES") ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND AND CSPARSE_FOUND) diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 0c92fb08..b6c6d508 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -51,7 +51,8 @@ public: kTypeUndef = -1, kTypeTORO = 0, kTypeG2O = 1, - kTypeCVSBA = 2 + kTypeGTSAM = 2, + kTypeCVSBA = 3 }; static Optimizer * create(const ParametersMap & parameters); static Optimizer * create(Optimizer::Type & type, const ParametersMap & parameters = ParametersMap()); @@ -74,6 +75,7 @@ public: bool isSlam2d() const {return slam2d_;} bool isCovarianceIgnored() const {return covarianceIgnored_;} double epsilon() const {return epsilon_;} + bool isRobust() const {return robust_;} // inherited classes should implement one of these methods virtual std::map optimize( @@ -94,7 +96,8 @@ protected: int iterations = Parameters::defaultRGBDOptimizeIterations(), bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), - double epsilon = Parameters::defaultRGBDOptimizeEpsilon()); + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()); Optimizer(const ParametersMap & parameters); private: @@ -102,6 +105,7 @@ private: bool slam2d_; bool covarianceIgnored_; double epsilon_; + bool robust_; }; class RTABMAP_EXP TOROOptimizer : public Optimizer @@ -117,8 +121,12 @@ public: std::multimap & edgeConstraints); public: - TOROOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) : - Optimizer(iterations, slam2d, covarianceIgnored) {} + TOROOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {} TOROOptimizer(const ParametersMap & parameters) : Optimizer(parameters) {} virtual ~TOROOptimizer() {} @@ -136,10 +144,21 @@ class RTABMAP_EXP G2OOptimizer : public Optimizer { public: static bool available(); + static bool saveGraph( + const std::string & fileName, + const std::map & poses, + const std::multimap & edgeConstraints, + bool useRobustConstraints = false); public: - G2OOptimizer(int iterations = 100, bool slam2d = false, bool covarianceIgnored = false) : - Optimizer(iterations, slam2d, covarianceIgnored) {} + G2OOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {} + G2OOptimizer(const ParametersMap & parameters) : Optimizer(parameters) {} virtual ~G2OOptimizer() {} @@ -153,6 +172,33 @@ public: std::list > * intermediateGraphes = 0); }; +class RTABMAP_EXP GTSAMOptimizer : public Optimizer +{ +public: + static bool available(); + +public: + GTSAMOptimizer( + int iterations = Parameters::defaultRGBDOptimizeIterations(), + bool slam2d = Parameters::defaultRGBDOptimizeSlam2D(), + bool covarianceIgnored = Parameters::defaultRGBDOptimizeVarianceIgnored(), + double epsilon = Parameters::defaultRGBDOptimizeEpsilon(), + bool robust = Parameters::defaultRGBDOptimizeRobust()) : + Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {} + + GTSAMOptimizer(const ParametersMap & parameters) : + Optimizer(parameters) {} + virtual ~GTSAMOptimizer() {} + + virtual Type type() const {return kTypeGTSAM;} + + virtual std::map optimize( + int rootId, + const std::map & poses, + const std::multimap & edgeConstraints, + std::list > * intermediateGraphes = 0); +}; + class RTABMAP_EXP CVSBAOptimizer : public Optimizer { public: diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index e14cb953..498dfc81 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -306,10 +306,11 @@ class RTABMAP_EXP Parameters // Graph optimization RTABMAP_PARAM(RGBD, OptimizeStrategy, int, 0, "Graph optimization strategy: 0=TORO and 1=g2o."); - RTABMAP_PARAM(RGBD, OptimizeIterations, int, 100, "Optimization iterations."); + RTABMAP_PARAM(RGBD, OptimizeIterations, int, 10, "Optimization iterations."); RTABMAP_PARAM(RGBD, OptimizeSlam2D, bool, false, "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses."); RTABMAP_PARAM(RGBD, OptimizeVarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links."); - RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.0001, "Stop optimizing when the error improvement is less than this value."); + RTABMAP_PARAM(RGBD, OptimizeEpsilon, double, 0.001, "Stop optimizing when the error improvement is less than this value."); + RTABMAP_PARAM(RGBD, OptimizeRobust, bool, true, "Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies)."); // Odometry RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Bag-of-words 1=Optical Flow"); diff --git a/corelib/include/rtabmap/core/RtabmapEvent.h b/corelib/include/rtabmap/core/RtabmapEvent.h index b21f8921..5eac0b2e 100644 --- a/corelib/include/rtabmap/core/RtabmapEvent.h +++ b/corelib/include/rtabmap/core/RtabmapEvent.h @@ -65,7 +65,7 @@ public: kCmdDumpMemory, kCmdDumpPrediction, kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin - kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO) + kCmdExportPoses, // params: [bool] global, [bool] optimized, [string] path, [int] type (0=raw format, 1=RGBD-SLAM format, 2=KITTI format, 3=TORO, 4=g2o) kCmdCleanDataBuffer, kCmdPublish3DMap, // params: [bool] global, [bool] optimized, [bool] graphOnly kCmdTriggerNewMap, diff --git a/corelib/include/rtabmap/core/Transform.h b/corelib/include/rtabmap/core/Transform.h index 2ed7ead8..682d45a5 100644 --- a/corelib/include/rtabmap/core/Transform.h +++ b/corelib/include/rtabmap/core/Transform.h @@ -51,6 +51,8 @@ public: Transform(const cv::Mat & transformationMatrix); // x,y,z, roll,pitch,yaw Transform(float x, float y, float z, float roll, float pitch, float yaw); + // x,y, theta + Transform(float x, float y, float theta); float r11() const {return data()[0];} float r12() const {return data()[1];} diff --git a/corelib/src/CMakeLists.txt b/corelib/src/CMakeLists.txt index 22a425d5..64b2a4cf 100644 --- a/corelib/src/CMakeLists.txt +++ b/corelib/src/CMakeLists.txt @@ -149,8 +149,34 @@ IF(G2O_FOUND) ${LIBRARIES} ${G2O_LIBRARIES} ) + #Newest versions require std11 + SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") + + IF(WITH_VERTIGO) + SET(SRC_FILES + ${SRC_FILES} + vertigo/g2o/edge_se2MaxMixture.cpp + vertigo/g2o/edge_se2Switchable.cpp + vertigo/g2o/edge_se3Switchable.cpp + vertigo/g2o/edge_switchPrior.cpp + vertigo/g2o/types_g2o_robust.cpp + vertigo/g2o/vertex_switchLinear.cpp + ) + ENDIF(WITH_VERTIGO) ENDIF(G2O_FOUND) +IF(GTSAM_FOUND) + ADD_DEFINITIONS("-DWITH_GTSAM") + SET(INCLUDE_DIRS + ${INCLUDE_DIRS} + ${GTSAM_INCLUDE_DIRS} + ) + SET(LIBRARIES + ${LIBRARIES} + gtsam + ) +ENDIF(GTSAM_FOUND) + IF(cvsba_FOUND) ADD_DEFINITIONS("-DWITH_CVSBA") SET(INCLUDE_DIRS diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 233f3b8b..4df24f10 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -48,11 +48,50 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "g2o/core/optimization_algorithm_gauss_newton.h" #include "g2o/core/optimization_algorithm_levenberg.h" #include "g2o/solvers/csparse/linear_solver_csparse.h" +#include "g2o/solvers/cholmod/linear_solver_cholmod.h" +#include "g2o/solvers/pcg/linear_solver_pcg.h" #include "g2o/types/slam3d/vertex_se3.h" #include "g2o/types/slam3d/edge_se3.h" #include "g2o/types/slam2d/vertex_se2.h" #include "g2o/types/slam2d/edge_se2.h" -#endif + +typedef g2o::BlockSolver< g2o::BlockSolverTraits<3, 3> > Slam2dBlockSolver; +typedef g2o::LinearSolverCSparse Slam2dLinearCSparseSolver; +typedef g2o::LinearSolverCholmod Slam2dLinearCholmodSolver; +typedef g2o::LinearSolverPCG Slam2dLinearPCGSolver; + +typedef g2o::BlockSolver< g2o::BlockSolverTraits<6, 3> > Slam3dBlockSolver; +typedef g2o::LinearSolverCSparse Slam3dLinearCSparseSolver; +typedef g2o::LinearSolverCholmod Slam3dLinearCholmodSolver; +typedef g2o::LinearSolverPCG Slam3dLinearPCGSolver; + +#include "vertigo/g2o/edge_switchPrior.h" +#include "vertigo/g2o/edge_se2Switchable.h" +#include "vertigo/g2o/edge_se3Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" + +#endif // end WITH_G2O + +#ifdef WITH_GTSAM +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "vertigo/gtsam/betweenFactorMaxMix.h" +#include "vertigo/gtsam/betweenFactorSwitchable.h" +#include "vertigo/gtsam/switchVariableLinear.h" +#include "vertigo/gtsam/switchVariableSigmoid.h" +#endif // end WITH_GTSAM #ifdef WITH_CVSBA #include @@ -80,16 +119,23 @@ Optimizer * Optimizer::create(const ParametersMap & parameters) UWARN("g2o optimizer not available. TORO will be used instead."); type = Optimizer::kTypeTORO; } + if(!GTSAMOptimizer::available() && type == Optimizer::kTypeGTSAM) + { + UWARN("GTSAM optimizer not available. TORO will be used instead."); + type = Optimizer::kTypeTORO; + } Optimizer * optimizer = 0; switch(type) { + case Optimizer::kTypeGTSAM: + optimizer = new GTSAMOptimizer(parameters); + break; case Optimizer::kTypeG2O: optimizer = new G2OOptimizer(parameters); break; case Optimizer::kTypeTORO: default: optimizer = new TOROOptimizer(parameters); - type = Optimizer::kTypeTORO; break; } @@ -103,9 +149,17 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para UWARN("g2o optimizer not available. TORO will be used instead."); type = Optimizer::kTypeTORO; } + if(!GTSAMOptimizer::available() && type == Optimizer::kTypeGTSAM) + { + UWARN("GTSAM optimizer not available. TORO will be used instead."); + type = Optimizer::kTypeTORO; + } Optimizer * optimizer = 0; switch(type) { + case Optimizer::kTypeGTSAM: + optimizer = new GTSAMOptimizer(parameters); + break; case Optimizer::kTypeG2O: optimizer = new G2OOptimizer(parameters); break; @@ -119,11 +173,12 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para return optimizer; } -Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon) : +Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) : iterations_(iterations), slam2d_(slam2d), covarianceIgnored_(covarianceIgnored), - epsilon_(epsilon) + epsilon_(epsilon), + robust_(robust) { } @@ -131,7 +186,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) : iterations_(Parameters::defaultRGBDOptimizeIterations()), slam2d_(Parameters::defaultRGBDOptimizeSlam2D()), covarianceIgnored_(Parameters::defaultRGBDOptimizeVarianceIgnored()), - epsilon_(Parameters::defaultRGBDOptimizeEpsilon()) + epsilon_(Parameters::defaultRGBDOptimizeEpsilon()), + robust_(Parameters::defaultRGBDOptimizeRobust()) { parseParameters(parameters); } @@ -142,6 +198,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRGBDOptimizeVarianceIgnored(), covarianceIgnored_); Parameters::parse(parameters, Parameters::kRGBDOptimizeSlam2D(), slam2d_); Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_); + Parameters::parse(parameters, Parameters::kRGBDOptimizeRobust(), robust_); } std::map Optimizer::optimize( @@ -373,9 +430,12 @@ std::map TOROOptimizer::optimize( pg3.initializeOptimization(); } - UINFO("TORO iterate begin (iterations=%d)", iterations()); + UINFO("TORO optimizing begin (iterations=%d)", iterations()); double lasterror = 0; - for (int i=0; i0) { @@ -429,7 +489,7 @@ std::map TOROOptimizer::optimize( } // early stop condition - double errorDelta = lasterror - error; + errorDelta = lasterror - error; if(i>0 && errorDelta < this->epsilon()) { UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon()); @@ -437,7 +497,7 @@ std::map TOROOptimizer::optimize( } lasterror = error; } - UINFO("TORO iterate end"); + UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, errorDelta, timer.ticks()); if(isSlam2d()) { @@ -670,20 +730,77 @@ std::map G2OOptimizer::optimize( { // Apply g2o optimization - // create the linear solver - g2o::BlockSolverX::LinearSolverType * linearSolver = new g2o::LinearSolverCSparse(); - - // create the block solver on top of the linear solver - g2o::BlockSolverX* blockSolver = new g2o::BlockSolverX(linearSolver); - - // create the algorithm to carry out the optimization - //g2o::OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new g2o::OptimizationAlgorithmGaussNewton(blockSolver); - g2o::OptimizationAlgorithmLevenberg* optimizationAlgorithm = new g2o::OptimizationAlgorithmLevenberg(blockSolver); - - // create the optimizer to load the data and carry out the optimization g2o::SparseOptimizer optimizer; optimizer.setVerbose(false); - optimizer.setAlgorithm(optimizationAlgorithm); + int solverApproach = 0; + int optimizationApproach = 0; + if(isSlam2d()) + { + Slam2dBlockSolver * blockSolver; + if(solverApproach == 1) + { + //pcg + Slam2dLinearPCGSolver * linearSolver = new Slam2dLinearPCGSolver(); + blockSolver = new Slam2dBlockSolver(linearSolver); + } + else if(solverApproach == 2) + { + //csparse + Slam2dLinearCSparseSolver* linearSolver = new Slam2dLinearCSparseSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new Slam2dBlockSolver(linearSolver); + } + else + { + //chmold + Slam2dLinearCholmodSolver * linearSolver = new Slam2dLinearCholmodSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new Slam2dBlockSolver(linearSolver); + } + + if(optimizationApproach == 1) + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver)); + } + else + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); + } + } + else + { + Slam3dBlockSolver * blockSolver; + if(solverApproach == 1) + { + //pcg + Slam3dLinearPCGSolver * linearSolver = new Slam3dLinearPCGSolver(); + blockSolver = new Slam3dBlockSolver(linearSolver); + } + else if(solverApproach == 2) + { + //csparse + Slam3dLinearCSparseSolver* linearSolver = new Slam3dLinearCSparseSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new Slam3dBlockSolver(linearSolver); + } + else + { + //chmold + Slam3dLinearCholmodSolver * linearSolver = new Slam3dLinearCholmodSolver(); + linearSolver->setBlockOrdering(false); + blockSolver = new Slam3dBlockSolver(linearSolver); + } + + if(optimizationApproach == 1) + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmGaussNewton(blockSolver)); + } + else + { + optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(blockSolver)); + } + } + UDEBUG("fill poses to g2o..."); for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) @@ -694,16 +811,25 @@ std::map G2OOptimizer::optimize( { g2o::VertexSE2 * v2 = new g2o::VertexSE2(); v2->setEstimate(g2o::SE2(iter->second.x(), iter->second.y(), iter->second.theta())); + if(iter->first == rootId) + { + v2->setFixed(true); + } vertex = v2; } else { g2o::VertexSE3 * v3 = new g2o::VertexSE3(); - Eigen::Isometry3d pose; + Eigen::Affine3d a = iter->second.toEigen3d(); + Eigen::Isometry3d pose; + pose = a.rotation(); pose.translation() = a.translation(); - pose.linear() = a.rotation(); v3->setEstimate(pose); + if(iter->first == rootId) + { + v3->setFixed(true); + } vertex = v3; } vertex->setId(iter->first); @@ -711,6 +837,7 @@ std::map G2OOptimizer::optimize( } UDEBUG("fill edges to g2o..."); + int vertigoVertexId = poses.rbegin()->first+1; for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) { int id1 = iter->first; @@ -720,6 +847,32 @@ std::map G2OOptimizer::optimize( g2o::HyperGraph::Edge * edge = 0; + VertexSwitchLinear * v = 0; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + // For loop closure links, add switchable edges + + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + v = new VertexSwitchLinear(); + v->setEstimate(1.0); + v->setId(vertigoVertexId++); + UASSERT_MSG(optimizer.addVertex(v), uFormat("cannot insert switchable vertex %d!?", v->id()).c_str()); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + EdgeSwitchPrior * prior = new EdgeSwitchPrior(); + prior->setMeasurement(1.0); + prior->setVertex(0, v); + UASSERT_MSG(optimizer.addEdge(prior), uFormat("cannot insert switchable prior edge %d!?", v->id()).c_str()); + } + if(isSlam2d()) { Eigen::Matrix information = Eigen::Matrix::Identity(); @@ -736,16 +889,33 @@ std::map G2OOptimizer::optimize( information(2,2) = iter->second.infMatrix().at(5,5); // theta-theta } - g2o::EdgeSE2 * e = new g2o::EdgeSE2(); - g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); - g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); - e->setInformation(information); - edge = e; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + EdgeSE2Switchable * e = new EdgeSE2Switchable(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setVertex(2, v); + e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); + e->setInformation(information); + edge = e; + } + else + { + g2o::EdgeSE2 * e = new g2o::EdgeSE2(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + g2o::VertexSE2* v2 = (g2o::VertexSE2*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); + e->setInformation(information); + edge = e; + } } else { @@ -757,19 +927,36 @@ std::map G2OOptimizer::optimize( Eigen::Affine3d a = iter->second.transform().toEigen3d(); Eigen::Isometry3d constraint; + constraint = a.rotation(); constraint.translation() = a.translation(); - constraint.linear() = a.rotation(); - g2o::EdgeSE3 * e = new g2o::EdgeSE3(); - g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); - g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(constraint); - e->setInformation(information); - edge = e; + if(this->isRobust() && iter->second.type() != Link::kNeighbor) + { + EdgeSE3Switchable * e = new EdgeSE3Switchable(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setVertex(2, v); + e->setMeasurement(constraint); + e->setInformation(information); + edge = e; + } + else + { + g2o::EdgeSE3 * e = new g2o::EdgeSE3(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(constraint); + e->setInformation(information); + edge = e; + } } if (!optimizer.addEdge(edge)) @@ -780,25 +967,13 @@ std::map G2OOptimizer::optimize( } UDEBUG("Initial optimization..."); - UASSERT(uContains(poses, rootId)); - if(isSlam2d()) - { - g2o::VertexSE2* firstRobotPose = (g2o::VertexSE2*)optimizer.vertex(rootId); - UASSERT(firstRobotPose != 0); - firstRobotPose->setFixed(true); - } - else - { - g2o::VertexSE3* firstRobotPose = (g2o::VertexSE3*)optimizer.vertex(rootId); - UASSERT(firstRobotPose != 0); - firstRobotPose->setFixed(true); - } + optimizer.initializeOptimization(); - UINFO("g2o iterate begin (max iterations=%d)", iterations()); + UINFO("g2o optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); int it = 0; + UTimer timer; if(intermediateGraphes) { - optimizer.initializeOptimization(); for(int i=0; i 0) @@ -847,18 +1022,17 @@ std::map G2OOptimizer::optimize( if(ULogger::level() == ULogger::kDebug) { optimizer.computeActiveErrors(); - UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2()); + UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2()); } } } else { - optimizer.initializeOptimization(); it = optimizer.optimize(iterations()); optimizer.computeActiveErrors(); - UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.chi2()); + UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2()); } - UINFO("g2o iterate end (%d iterations done)", it); + UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks()); if(isSlam2d()) { @@ -916,6 +1090,312 @@ std::map G2OOptimizer::optimize( return optimizedPoses; } +bool G2OOptimizer::saveGraph( + const std::string & fileName, + const std::map & poses, + const std::multimap & edgeConstraints, + bool useRobustConstraints) +{ + FILE * file = 0; + +#ifdef _MSC_VER + fopen_s(&file, fileName.c_str(), "w"); +#else + file = fopen(fileName.c_str(), "w"); +#endif + + if(file) + { + // VERTEX_SE3 id x y z qw qx qy qz + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + Eigen::Quaternionf q = iter->second.getQuaternionf(); + fprintf(file, "VERTEX_SE3:QUAT %d %f %f %f %f %f %f %f\n", + iter->first, + iter->second.x(), + iter->second.y(), + iter->second.z(), + q.x(), + q.y(), + q.z(), + q.w()); + } + + //EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66 + int virtualVertexId = poses.size()?poses.rbegin()->first+1:0; + for(std::multimap::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + std::string prefix = "EDGE_SE3:QUAT"; + std::string suffix = ""; + + if(useRobustConstraints && iter->second.type() != Link::kNeighbor) + { + prefix = "EDGE_SE3_SWITCHABLE"; + fprintf(file, "VERTEX_SWITCH %d 1\n", virtualVertexId); + fprintf(file, "EDGE_SWITCH_PRIOR %d 1 1.0\n", virtualVertexId); + suffix = uFormat(" %d", virtualVertexId++); + } + + Eigen::Quaternionf q = iter->second.transform().getQuaternionf(); + fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n", + prefix.c_str(), + iter->first, + iter->second.to(), + suffix.c_str(), + iter->second.transform().x(), + iter->second.transform().y(), + iter->second.transform().z(), + q.x(), + q.y(), + q.z(), + q.w(), + iter->second.infMatrix().at(0,0), + iter->second.infMatrix().at(0,1), + iter->second.infMatrix().at(0,2), + iter->second.infMatrix().at(0,3), + iter->second.infMatrix().at(0,4), + iter->second.infMatrix().at(0,5), + iter->second.infMatrix().at(1,1), + iter->second.infMatrix().at(1,2), + iter->second.infMatrix().at(1,3), + iter->second.infMatrix().at(1,4), + iter->second.infMatrix().at(1,5), + iter->second.infMatrix().at(2,2), + iter->second.infMatrix().at(2,3), + iter->second.infMatrix().at(2,4), + iter->second.infMatrix().at(2,5), + iter->second.infMatrix().at(3,3), + iter->second.infMatrix().at(3,4), + iter->second.infMatrix().at(3,5), + iter->second.infMatrix().at(4,4), + iter->second.infMatrix().at(4,5), + iter->second.infMatrix().at(5,5)); + } + UINFO("Graph saved to %s", fileName.c_str()); + fclose(file); + } + else + { + UERROR("Cannot save to file %s", fileName.c_str()); + return false; + } + return true; +} + +////////////////////// +// GTSAM +////////////////////// +bool GTSAMOptimizer::available() +{ +#ifdef WITH_GTSAM + return true; +#else + return false; +#endif +} + +std::map GTSAMOptimizer::optimize( + int rootId, + const std::map & poses, + const std::multimap & edgeConstraints, + std::list > * intermediateGraphes) +{ + std::map optimizedPoses; +#ifdef WITH_GTSAM + UDEBUG("Optimizing graph..."); + if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0) + { + gtsam::NonlinearFactorGraph graph; + + //prior first pose + UASSERT(uContains(poses, rootId)); + const Transform & initialPose = poses.at(rootId); + if(isSlam2d()) + { + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01)); + graph.add(gtsam::PriorFactor(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); + } + else + { + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); + } + + UDEBUG("fill poses to gtsam..."); + gtsam::Values initialEstimate; + for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) + { + UASSERT(!iter->second.isNull()); + if(isSlam2d()) + { + initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta())); + } + else + { + initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d())); + } + } + + UDEBUG("fill edges to gtsam..."); + int switchCounter = poses.rbegin()->first+1; + for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + int id1 = iter->first; + int id2 = iter->second.to(); + + UASSERT(!iter->second.transform().isNull()); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + double prior = 1.0; + initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior)); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); + graph.add(gtsam::PriorFactor (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel)); + } + + if(isSlam2d()) + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information(0,0) = iter->second.infMatrix().at(0,0)/1000.0; // x-x + information(0,1) = iter->second.infMatrix().at(0,1)/1000.0; // x-y + information(0,2) = iter->second.infMatrix().at(0,5)/1000.0; // x-theta + information(1,0) = iter->second.infMatrix().at(1,0)/1000.0; // y-x + information(1,1) = iter->second.infMatrix().at(1,1)/1000.0; // y-y + information(1,2) = iter->second.infMatrix().at(1,5)/1000.0; // y-theta + information(2,0) = iter->second.infMatrix().at(5,0)/1000.0; // theta-x + information(2,1) = iter->second.infMatrix().at(5,1)/1000.0; // theta-y + information(2,2) = iter->second.infMatrix().at(5,5)/1000.0; // theta-theta + } + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } + else + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } + } + else + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information = information / 1000.0; + } + + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + + if(this->isRobust() && iter->second.type()!=Link::kNeighbor) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } + else + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } + } + } + + UDEBUG("create optimizer"); + gtsam::GaussNewtonParams parameters; + parameters.relativeErrorTol = epsilon(); + parameters.maxIterations = iterations(); + gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + //gtsam::LevenbergMarquardtParams parametersLev; + //parametersLev.relativeErrorTol = epsilon(); + //parametersLev.maxIterations = iterations(); + //gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parametersLev); + //gtsam::DoglegParams parametersDogleg; + //parametersDogleg.relativeErrorTol = epsilon(); + //parametersDogleg.maxIterations = iterations(); + //gtsam::DoglegOptimizer optimizer(graph, initialEstimate, parametersDogleg); + + UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0); + UTimer timer; + for(int i=0; i 0) + { + std::map tmpPoses; + for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter) + { + if(iter->value.dim() > 1) + { + if(isSlam2d()) + { + gtsam::Pose2 p = iter->value.cast(); + tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta()))); + } + else + { + gtsam::Pose3 p = iter->value.cast(); + tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix()))); + } + } + } + intermediateGraphes->push_back(tmpPoses); + } + try + { + optimizer.iterate(); + } + catch(gtsam::IndeterminantLinearSystemException & e) + { + UERROR("GTSAM exception catched: %s", e.what()); + return optimizedPoses; + } + UDEBUG("iteration %d error =%f", i+1, optimizer.error()); + if(optimizer.error() < epsilon()) + { + break; + } + } + UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer.iterations(), optimizer.error(), graph.error(initialEstimate), graph.error(optimizer.values()), timer.ticks()); + + for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter) + { + if(iter->value.dim() > 1) + { + if(isSlam2d()) + { + gtsam::Pose2 p = iter->value.cast(); + optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta()))); + } + else + { + gtsam::Pose3 p = iter->value.cast(); + optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix()))); + } + } + } + } + UDEBUG("Optimizing graph...end!"); +#else + UERROR("Not built with GTSAM support!"); +#endif + return optimizedPoses; +} + ////////////////////// // cvsba ////////////////////// diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index b310d319..b3ec5bd1 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -742,6 +742,14 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, { graph::TOROOptimizer::saveGraph(path, poses, constraints); } + else if(type == 4) // g2o + { +#ifdef WITH_G2O + graph::G2OOptimizer::saveGraph(path, poses, constraints); +#else + UERROR("Cannot export in g2o format because RTAB-Map is not built with g2o support!"); +#endif + } else { //get timestamps @@ -2680,7 +2688,6 @@ void Rtabmap::optimizeCurrentMap( std::multimap * constraints) const { //Optimize the map - optimizedPoses.clear(); UINFO("Optimize map: around location %d", id); if(_memory && id > 0) { @@ -2692,14 +2699,23 @@ void Rtabmap::optimizeCurrentMap( } UINFO("get %d ids time %f s", (int)ids.size(), timer.ticks()); - optimizedPoses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints); - - if(_memory->getSignature(id) && uContains(optimizedPoses, id)) - { - Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse(); - UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str()); - } + std::map poses = Rtabmap::optimizeGraph(id, uKeysSet(ids), lookInDatabase, constraints); UINFO("optimize time %f s", timer.ticks()); + + if(poses.size()) + { + optimizedPoses = poses; + + if(_memory->getSignature(id) && uContains(optimizedPoses, id)) + { + Transform t = optimizedPoses.at(id) * _memory->getSignature(id)->getPose().inverse(); + UINFO("Correction (from node %d) %s", id, t.prettyPrint().c_str()); + } + } + else + { + UERROR("Failed to optimize the graph! Keeping the graph without optimization..."); + } } } diff --git a/corelib/src/Transform.cpp b/corelib/src/Transform.cpp index cb025873..7678e69b 100644 --- a/corelib/src/Transform.cpp +++ b/corelib/src/Transform.cpp @@ -66,6 +66,12 @@ Transform::Transform(float x, float y, float z, float roll, float pitch, float y *this = fromEigen3f(t); } +Transform::Transform(float x, float y, float theta) +{ + Eigen::Affine3f t = pcl::getTransformation (x, y, 0, 0, 0, theta); + *this = fromEigen3f(t); +} + bool Transform::isNull() const { return (data()[0] == 0.0f && diff --git a/corelib/src/sqlite3/readme.txt b/corelib/src/sqlite3/readme.txt new file mode 100644 index 00000000..5f1dcc60 --- /dev/null +++ b/corelib/src/sqlite3/readme.txt @@ -0,0 +1,3 @@ + +Info: https://www.sqlite.org/ +License: Public domain (https://www.sqlite.org/copyright.html) \ No newline at end of file diff --git a/corelib/src/toro3d/readme.txt b/corelib/src/toro3d/readme.txt new file mode 100644 index 00000000..8de8c6e8 --- /dev/null +++ b/corelib/src/toro3d/readme.txt @@ -0,0 +1,3 @@ + +Info: https://www.openslam.org/toro.html +License: Creative Commons (Attribution-NonCommercial-ShareAlike) \ No newline at end of file diff --git a/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp b/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp new file mode 100644 index 00000000..41407b83 --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se2MaxMixture.cpp @@ -0,0 +1,122 @@ +/* + * edge_se2MaxMixture.cpp + * + * Created on: 12.06.2012 + * Author: niko + */ + + +#include "edge_se2MaxMixture.h" + +#include +using namespace std; +using namespace Eigen; + +// ================================================ +EdgeSE2MaxMixture::EdgeSE2MaxMixture() : g2o::EdgeSE2::EdgeSE2() +{ + nullHypothesisMoreLikely = false; +} + +// ================================================ +bool EdgeSE2MaxMixture::read(std::istream& is) + { + Vector3d p; + is >> weight >> p[0] >> p[1] >> p[2]; + setMeasurement(g2o::SE2(p)); + _inverseMeasurement = measurement().inverse(); + //measurement().fromVector(p); + //inverseMeasurement() = measurement().inverse(); + for (int i = 0; i < 3; ++i) + for (int j = i; j < 3; ++j) { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + + information_constraint = _information; + nu_constraint = 1.0/sqrt(information_constraint.inverse().determinant()); + information_nullHypothesis = information_constraint*weight; + nu_nullHypothesis = 1.0/sqrt(information_nullHypothesis.inverse().determinant()); + + return true; + } +// ================================================ +bool EdgeSE2MaxMixture::write(std::ostream& os) const +{ + Vector3d p = measurement().toVector(); + os << p.x() << " " << p.y() << " " << p.z(); + for (int i = 0; i < 3; ++i) + for (int j = i; j < 3; ++j) + os << " " << information()(i, j); + return os.good(); +} + +// ================================================ +void EdgeSE2MaxMixture::linearizeOplus() +{ + g2o::EdgeSE2::linearizeOplus(); + if (nullHypothesisMoreLikely) { + _jacobianOplusXi *= weight; + _jacobianOplusXj *= weight; + } +} + +// ================================================ +void EdgeSE2MaxMixture::computeError() +{ + // calculate the error for this constraint + g2o::EdgeSE2::computeError(); + + // determine the likelihood for constraint and null hypothesis + double mahal_constraint = _error.transpose() * information_constraint * _error; + double likelihood_constraint = nu_constraint * exp(-mahal_constraint); + + double mahal_nullHypothesis = _error.transpose() * (information_nullHypothesis) * _error; + double likelihood_nullHypothesis = nu_nullHypothesis * exp(-mahal_nullHypothesis); + + // if the nullHypothesis is more likely ... + if (likelihood_nullHypothesis > likelihood_constraint) { + _information = information_nullHypothesis; + nullHypothesisMoreLikely = true; + } + else { + _information = information_constraint; + nullHypothesisMoreLikely = false; + } + + +} + + +// ================================================ +#ifdef G2O_HAVE_OPENGL + EdgeSE2MaxMixtureDrawAction::EdgeSE2MaxMixtureDrawAction(): DrawAction(typeid(EdgeSE2MaxMixture).name()){} + + g2o::HyperGraphElementAction* EdgeSE2MaxMixtureDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* /*params_*/){ + if (typeid(*element).name()!=_typeName) + return 0; + EdgeSE2MaxMixture* e = static_cast(element); + + + g2o::VertexSE2* fromEdge = static_cast(e->vertices()[0]); + g2o::VertexSE2* toEdge = static_cast(e->vertices()[1]); + + + if (e->nullHypothesisMoreLikely) glColor3f(0.0,0.0,0.0); + else glColor3f(1.0,0.5,0.2); + + glPushAttrib(GL_ENABLE_BIT); + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.); + glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.); + glEnd(); + glPopAttrib(); + return this; + } +#endif + + + diff --git a/corelib/src/vertigo/g2o/edge_se2MaxMixture.h b/corelib/src/vertigo/g2o/edge_se2MaxMixture.h new file mode 100644 index 00000000..48d134fe --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se2MaxMixture.h @@ -0,0 +1,46 @@ +/* + * edge_se2MaxMixture.h + * + * Created on: 12.06.2012 + * Author: niko + */ + +#ifndef EDGE_SE2MAXMIXTURE_H_ +#define EDGE_SE2MAXMIXTURE_H_ + +#include "g2o/types/slam2d/vertex_se2.h" +#include "g2o/types/slam2d/edge_se2.h" + + +class EdgeSE2MaxMixture : public g2o::EdgeSE2 +{ + public: + EdgeSE2MaxMixture(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + void computeError(); + void linearizeOplus(); + + double weight; + + bool nullHypothesisMoreLikely; + + InformationType information_nullHypothesis; + double nu_nullHypothesis; + InformationType information_constraint; + double nu_constraint ; +}; + + +#ifdef G2O_HAVE_OPENGL + class EdgeSE2MaxMixtureDrawAction: public g2o::DrawAction{ + public: + EdgeSE2MaxMixtureDrawAction(); + virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* params_); + }; +#endif + + +#endif /* EDGE_SE2MAXMIXTURE_H_ */ diff --git a/corelib/src/vertigo/g2o/edge_se2Switchable.cpp b/corelib/src/vertigo/g2o/edge_se2Switchable.cpp new file mode 100644 index 00000000..1e6deffd --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se2Switchable.cpp @@ -0,0 +1,131 @@ +/* + * edge_se2Switchable.cpp + * + * Created on: 13.07.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#include "vertigo/g2o/edge_se2Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" +#include + +using namespace std; +using namespace Eigen; + + +// ================================================ +EdgeSE2Switchable::EdgeSE2Switchable() : g2o::BaseMultiEdge<3, g2o::SE2>() +{ + resize(3); + _jacobianOplus[0].resize(3,3); + _jacobianOplus[1].resize(3,3); + _jacobianOplus[2].resize(3,1); + +} +// ================================================ +bool EdgeSE2Switchable::read(std::istream& is) + { + Vector3d p; + is >> p[0] >> p[1] >> p[2]; + setMeasurement(g2o::SE2(p)); + _inverseMeasurement = measurement().inverse(); + + for (int i = 0; i < 3; ++i) + for (int j = i; j < 3; ++j) { + is >> information()(i, j); + if (i != j) + information()(j, i) = information()(i, j); + } + return true; + } +// ================================================ +bool EdgeSE2Switchable::write(std::ostream& os) const +{ + Vector3d p = measurement().toVector(); + os << p.x() << " " << p.y() << " " << p.z(); + for (int i = 0; i < 3; ++i) + for (int j = i; j < 3; ++j) + os << " " << information()(i, j); + return os.good(); +} + +// ================================================ +void EdgeSE2Switchable::linearizeOplus() +{ + + const g2o::VertexSE2* vi = static_cast(_vertices[0]); + const g2o::VertexSE2* vj = static_cast(_vertices[1]); + const VertexSwitchLinear* vSwitch = static_cast(_vertices[2]); + + double thetai = vi->estimate().rotation().angle(); + + Vector2d dt = vj->estimate().translation() - vi->estimate().translation(); + double si=sin(thetai), ci=cos(thetai); + + _jacobianOplus[0](0, 0) = -ci; _jacobianOplus[0](0, 1) = -si; _jacobianOplus[0](0, 2) = -si*dt.x()+ci*dt.y(); + _jacobianOplus[0](1, 0) = si; _jacobianOplus[0](1, 1) = -ci; _jacobianOplus[0](1, 2) = -ci*dt.x()-si*dt.y(); + _jacobianOplus[0](2, 0) = 0; _jacobianOplus[0](2, 1) = 0; _jacobianOplus[0](2, 2) = -1; + + _jacobianOplus[1](0, 0) = ci; _jacobianOplus[1](0, 1)= si; _jacobianOplus[1](0, 2)= 0; + _jacobianOplus[1](1, 0) =-si; _jacobianOplus[1](1, 1)= ci; _jacobianOplus[1](1, 2)= 0; + _jacobianOplus[1](2, 0) = 0; _jacobianOplus[1](2, 1)= 0; _jacobianOplus[1](2, 2)= 1; + + const g2o::SE2& rmean = _inverseMeasurement; + Matrix3d z = Matrix3d::Zero(); + z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix(); + z(2, 2) = 1.; + _jacobianOplus[0] = z * _jacobianOplus[0]; + _jacobianOplus[1] = z * _jacobianOplus[1]; + + + _jacobianOplus[0]*=vSwitch->estimate(); + _jacobianOplus[1]*=vSwitch->estimate(); + + + // derivative w.r.t switch vertex + _jacobianOplus[2].setZero(); + g2o::SE2 delta = _inverseMeasurement * (vi->estimate().inverse()*vj->estimate()); + _jacobianOplus[2] = delta.toVector() * vSwitch->gradient(); +} + +// ================================================ +void EdgeSE2Switchable::computeError() +{ + const g2o::VertexSE2* v1 = static_cast(_vertices[0]); + const g2o::VertexSE2* v2 = static_cast(_vertices[1]); + const VertexSwitchLinear* v3 = static_cast(_vertices[2]); + + g2o::SE2 delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate()); + _error = delta.toVector() * v3->estimate(); +} + + +#ifdef G2O_HAVE_OPENGL + EdgeSE2SwitchableDrawAction::EdgeSE2SwitchableDrawAction(): DrawAction(typeid(EdgeSE2Switchable).name()){} + + g2o::HyperGraphElementAction* EdgeSE2SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* /*params_*/){ + if (typeid(*element).name()!=_typeName) + return 0; + EdgeSE2Switchable* e = static_cast(element); + + + g2o::VertexSE2* fromEdge = static_cast(e->vertices()[0]); + g2o::VertexSE2* toEdge = static_cast(e->vertices()[1]); + VertexSwitchLinear* s = static_cast(e->vertices()[2]); + + glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1); + glPushAttrib(GL_ENABLE_BIT); + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.); + glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.); + glEnd(); + glPopAttrib(); + return this; + } +#endif + diff --git a/corelib/src/vertigo/g2o/edge_se2Switchable.h b/corelib/src/vertigo/g2o/edge_se2Switchable.h new file mode 100644 index 00000000..a4004566 --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se2Switchable.h @@ -0,0 +1,50 @@ +/* + * edge_se2Switchable.h + * + * Created on: 13.07.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#ifndef EDGE_SE2SWITCHABLE_H_ +#define EDGE_SE2SWITCHABLE_H_ + +#include "g2o/types/slam2d/vertex_se2.h" +#include "g2o/core/base_multi_edge.h" +#include "g2o/core/hyper_graph_action.h" + +class EdgeSE2Switchable : public g2o::BaseMultiEdge<3, g2o::SE2> +{ + public: + EdgeSE2Switchable(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + void computeError(); + void linearizeOplus(); + + + virtual void setMeasurement(const g2o::SE2& m){ + _measurement = m; + _inverseMeasurement = m.inverse(); + } + + protected: + g2o::SE2 _inverseMeasurement; +}; + + +#ifdef G2O_HAVE_OPENGL + class EdgeSE2SwitchableDrawAction: public g2o::DrawAction{ + public: + EdgeSE2SwitchableDrawAction(); + virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* params_); + }; +#endif + + + +#endif /* EDGE_SE2SWITCHABLE_H_ */ diff --git a/corelib/src/vertigo/g2o/edge_se3Switchable.cpp b/corelib/src/vertigo/g2o/edge_se3Switchable.cpp new file mode 100644 index 00000000..d5b8a4ed --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se3Switchable.cpp @@ -0,0 +1,120 @@ +/* + * edge_se3Switchable.cpp + * + * Created on: 17.10.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#include "vertigo/g2o/edge_se3Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" +#include +#include "g2o/types/slam3d/vertex_se3.h" +#include "g2o/types/slam3d/isometry3d_gradients.h" + + +using namespace std; +using namespace Eigen; + + +// ================================================ +EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d>() +{ + resize(3); + _jacobianOplus[0].resize(6,6); + _jacobianOplus[1].resize(6,6); + _jacobianOplus[2].resize(6,1); + +} +// ================================================ +bool EdgeSE3Switchable::read(std::istream& is) + { + g2o::Vector7d meas; + for (int i=0; i<7; i++) + is >> meas[i]; + // normalize the quaternion to recover numerical precision lost by storing as human readable text + Vector4d::MapType(meas.data()+3).normalize(); + setMeasurement(g2o::internal::fromVectorQT(meas)); + + for (int i=0; i<6; i++) + for (int j=i; j<6; j++) { + is >> information()(i,j); + if (i!=j) + information()(j,i) = information()(i,j); + } + return true; + + } +// ================================================ +bool EdgeSE3Switchable::write(std::ostream& os) const +{ + g2o::Vector7d meas = g2o::internal::toVectorQT(measurement()); + for (int i=0; i<7; i++) os << meas[i] << " "; + for (int i = 0; i < 6; ++i) + for (int j = i; j < 6; ++j) + os << " " << information()(i, j); + return os.good(); +} + +// ================================================ +void EdgeSE3Switchable::linearizeOplus() +{ + + g2o::VertexSE3* from = static_cast(_vertices[0]); + g2o::VertexSE3* to = static_cast(_vertices[1]); + const VertexSwitchLinear* vSwitch = static_cast(_vertices[2]); + + Eigen::Isometry3d E; + const Eigen::Isometry3d& Xi=from->estimate(); + const Eigen::Isometry3d& Xj=to->estimate(); + const Eigen::Isometry3d& Z=_measurement; + g2o::internal::computeEdgeSE3Gradient(E, _jacobianOplus[0], _jacobianOplus[1], Z, Xi, Xj); + + _jacobianOplus[0]*=vSwitch->estimate(); + _jacobianOplus[1]*=vSwitch->estimate(); + + // derivative w.r.t switch vertex + _jacobianOplus[2].setZero(); + _jacobianOplus[2] = g2o::internal::toVectorMQT(E) * vSwitch->gradient(); +} + + +// ================================================ +void EdgeSE3Switchable::computeError() +{ + const g2o::VertexSE3* v1 = dynamic_cast(_vertices[0]); + const g2o::VertexSE3* v2 = dynamic_cast(_vertices[1]); + const VertexSwitchLinear* v3 = static_cast(_vertices[2]); + + Eigen::Isometry3d delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate()); + _error = g2o::internal::toVectorMQT(delta) * v3->estimate(); +} + + +#ifdef G2O_HAVE_OPENGL + EdgeSE3SwitchableDrawAction::EdgeSE3SwitchableDrawAction(): DrawAction(typeid(EdgeSE3Switchable).name()){} + + g2o::HyperGraphElementAction* EdgeSE3SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* /*params_*/){ + if (typeid(*element).name()!=_typeName) + return 0; + EdgeSE3Switchable* e = static_cast(element); + + + g2o::VertexSE3* fromEdge = static_cast(e->vertices()[0]); + g2o::VertexSE3* toEdge = static_cast(e->vertices()[1]); + VertexSwitchLinear* s = static_cast(e->vertices()[2]); + + glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1); + glPushAttrib(GL_ENABLE_BIT); + glDisable(GL_LIGHTING); + glBegin(GL_LINES); + glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),fromEdge->estimate().translation().z()); + glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),toEdge->estimate().translation().z()); + glEnd(); + glPopAttrib(); + return this; + } +#endif diff --git a/corelib/src/vertigo/g2o/edge_se3Switchable.h b/corelib/src/vertigo/g2o/edge_se3Switchable.h new file mode 100644 index 00000000..ee636edd --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_se3Switchable.h @@ -0,0 +1,48 @@ +/* + * edge_se3Switchable.h + * + * Created on: 17.10.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#ifndef EDGE_SE3SWITCHABLE_H_ +#define EDGE_SE3SWITCHABLE_H_ + +#include "g2o/types/slam3d/vertex_se3.h" +#include "g2o/core/base_multi_edge.h" +#include "g2o/core/hyper_graph_action.h" + +class EdgeSE3Switchable : public g2o::BaseMultiEdge<6, Eigen::Isometry3d> +{ + public: + EdgeSE3Switchable(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + void computeError(); + void linearizeOplus(); + + virtual void setMeasurement(const Eigen::Isometry3d& m){ + _measurement = m; + _inverseMeasurement = m.inverse(); + } + + protected: + Eigen::Isometry3d _inverseMeasurement; +}; + + +#ifdef G2O_HAVE_OPENGL + class EdgeSE3SwitchableDrawAction: public g2o::DrawAction{ + public: + EdgeSE3SwitchableDrawAction(); + virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element, + g2o::HyperGraphElementAction::Parameters* params_); + }; +#endif + + +#endif /* EDGE_SE3SWITCHABLE_H_ */ diff --git a/corelib/src/vertigo/g2o/edge_switchPrior.cpp b/corelib/src/vertigo/g2o/edge_switchPrior.cpp new file mode 100644 index 00000000..0af37b45 --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_switchPrior.cpp @@ -0,0 +1,44 @@ +#include "vertigo/g2o/edge_switchPrior.h" +using namespace std; + + EdgeSwitchPrior::EdgeSwitchPrior() + { + setMeasurement(1.0); + } + + bool EdgeSwitchPrior::read(std::istream &is) + { + double new_measurement; + is >> new_measurement; + + setMeasurement(new_measurement); + + is >> information()(0,0); + return true; + } + + bool EdgeSwitchPrior::write(std::ostream &os) const + { + os << measurement() << " " << information()(0,0); + return true; + } + + void EdgeSwitchPrior::setMeasurement(const double & m) + { + g2o::BaseEdge<1, double>::setMeasurement(m); + information()(0,0) = 1.0; + } + + void EdgeSwitchPrior::linearizeOplus() + { + _jacobianOplusXi[0]=-1.0; + } + + void EdgeSwitchPrior::computeError() + { + const VertexSwitchLinear* s = static_cast(_vertices[0]); + + _error[0] = measurement() - s->x(); + + } + diff --git a/corelib/src/vertigo/g2o/edge_switchPrior.h b/corelib/src/vertigo/g2o/edge_switchPrior.h new file mode 100644 index 00000000..b4505ffb --- /dev/null +++ b/corelib/src/vertigo/g2o/edge_switchPrior.h @@ -0,0 +1,22 @@ +#pragma once + + +#include "vertex_switchLinear.h" +#include "g2o/core/base_unary_edge.h" + + + +class EdgeSwitchPrior : public g2o::BaseUnaryEdge<1, double, VertexSwitchLinear> +{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EdgeSwitchPrior(); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + + virtual void setMeasurement(const double & m); + + virtual void linearizeOplus(); + void computeError(); +}; diff --git a/corelib/src/vertigo/g2o/types_g2o_robust.cpp b/corelib/src/vertigo/g2o/types_g2o_robust.cpp new file mode 100644 index 00000000..f1553f5f --- /dev/null +++ b/corelib/src/vertigo/g2o/types_g2o_robust.cpp @@ -0,0 +1,22 @@ +#include "g2o/core/factory.h" +#include "g2o/stuff/macros.h" + +#include "vertigo/g2o/edge_switchPrior.h" +#include "vertigo/g2o/edge_se2Switchable.h" +//#include "vertigo/g2o/edge_se2MaxMixture.h" +#include "vertigo/g2o/edge_se3Switchable.h" +#include "vertigo/g2o/vertex_switchLinear.h" + + +G2O_REGISTER_TYPE(EDGE_SWITCH_PRIOR, EdgeSwitchPrior); +G2O_REGISTER_TYPE(EDGE_SE2_SWITCHABLE, EdgeSE2Switchable); +//G2O_REGISTER_TYPE(EDGE_SE2_MAXMIX, EdgeSE2MaxMixture); +G2O_REGISTER_TYPE(EDGE_SE3_SWITCHABLE, EdgeSE3Switchable); +G2O_REGISTER_TYPE(VERTEX_SWITCH, VertexSwitchLinear); + +#ifdef G2O_HAVE_OPENGL + G2O_REGISTER_ACTION(EdgeSE2SwitchableDrawAction); + //G2O_REGISTER_ACTION(EdgeSE2MaxMixtureDrawAction); + G2O_REGISTER_ACTION(EdgeSE3SwitchableDrawAction); +#endif + diff --git a/corelib/src/vertigo/g2o/vertex_switchLinear.cpp b/corelib/src/vertigo/g2o/vertex_switchLinear.cpp new file mode 100644 index 00000000..de4c503f --- /dev/null +++ b/corelib/src/vertigo/g2o/vertex_switchLinear.cpp @@ -0,0 +1,59 @@ +/* + * vertex_switchLinear.cpp + * + * Created on: 17.10.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + +#include "vertigo/g2o/vertex_switchLinear.h" +#include + +using namespace std; + + VertexSwitchLinear::VertexSwitchLinear() : + _x(0) + { + setToOrigin(); + setEstimate(1.0); + } + + bool VertexSwitchLinear:: read(std::istream& is) + { + is >> _x; + _estimate=_x; + + return true; + } + + bool VertexSwitchLinear::write(std::ostream& os) const + { + os << _x; + return os.good(); + } + + void VertexSwitchLinear::setToOriginImpl() + { + _x=0; + _estimate=_x; + } + + + void VertexSwitchLinear::setEstimate(const double &et) + { + _x=et; + _estimate=_x; + } + + + void VertexSwitchLinear::oplusImpl(const double* update) + { + _x += update[0]; + + if (_x<0) _x=0; + if (_x>1) _x=1; + + _estimate=_x; + } diff --git a/corelib/src/vertigo/g2o/vertex_switchLinear.h b/corelib/src/vertigo/g2o/vertex_switchLinear.h new file mode 100644 index 00000000..7fa7edeb --- /dev/null +++ b/corelib/src/vertigo/g2o/vertex_switchLinear.h @@ -0,0 +1,43 @@ +/* + * vertex_switchLinear.h + * + * Created on: 17.10.2011 + * Author: niko + * + * Updated on: 14.01.2013 + * Author: Christian Kerl + */ + + +#pragma once + +#include "g2o/core/base_vertex.h" +#include + + + + class VertexSwitchLinear : public g2o::BaseVertex<1, double> + { + + public: + VertexSwitchLinear(); + + virtual void setToOriginImpl(); + + virtual void oplusImpl(const double* update); + + virtual bool read(std::istream& is); + virtual bool write(std::ostream& os) const; + virtual void setEstimate(const double &et); + + + double x() const { return _x; }; + + + //! The gradient at the current estimate is always 1; + double gradient() const { return 1; } ; + + private: + double _x; + + }; diff --git a/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h b/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h new file mode 100644 index 00000000..789609d5 --- /dev/null +++ b/corelib/src/vertigo/gtsam/betweenFactorMaxMix.h @@ -0,0 +1,67 @@ +/* + * betweenFactorMaxMix.h + * + * Created on: 14.08.2012 + * Author: niko + */ + +#ifndef BETWEENFACTORMAXMIX_H_ +#define BETWEENFACTORMAXMIX_H_ + +#include +#include + +namespace vertigo { + + template + class BetweenFactorMaxMix : public gtsam::NoiseModelFactor2 + { + public: + BetweenFactorMaxMix() : weight(0.0) {}; + BetweenFactorMaxMix(gtsam::Key key1, gtsam::Key key2, const VALUE& measured, const gtsam::SharedNoiseModel& model, const gtsam::SharedNoiseModel& model2, double w) + : gtsam::NoiseModelFactor2(model, key1, key2), weight(w), nullHypothesisModel(model2), + betweenFactor(key1, key2, measured, model) { }; + + gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const + { + + // calculate error + gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2); + + + + // which hypothesis is more likely + double m1 = this->noiseModel_->distance(error); + gtsam::noiseModel::Gaussian::shared_ptr g1 = this->noiseModel_; + gtsam::Matrix info1(g1->R().transpose()*g1->R()); + double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant()); + double l1 = nu1 * exp(-0.5*m1); + + double m2 = nullHypothesisModel->distance(error); + gtsam::noiseModel::Gaussian::shared_ptr g2 = nullHypothesisModel; + gtsam::Matrix info2(g2->R().transpose()*g2->R()); + double nu2 = 1.0/sqrt(gtsam::inverse(info2).determinant()); + double l2 = nu2 * exp(-0.5*m2); + + // if the null hypothesis is more likely, than proceed by applying the weight ... + if (l2>l1) { + if (H1) *H1 = *H1 * weight; + if (H2) *H2 = *H2 * weight; + error *= sqrt(weight); + } + + return error; + }; + + private: + gtsam::BetweenFactor betweenFactor; + gtsam::SharedNoiseModel nullHypothesisModel; + double weight; + + }; +} + + +#endif /* BETWEENFACTORMAXMIX_H_ */ diff --git a/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h b/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h new file mode 100644 index 00000000..ab5a4438 --- /dev/null +++ b/corelib/src/vertigo/gtsam/betweenFactorSwitchable.h @@ -0,0 +1,97 @@ +/* + * betweenFactorSwitchable.h + * + * Created on: 02.08.2012 + * Author: niko + */ + +#ifndef BETWEENFACTORSWITCHABLE_H_ +#define BETWEENFACTORSWITCHABLE_H_ + +#include + +#include +using std::cout; +using std::endl; + +#include "switchVariableLinear.h" +#include "switchVariableSigmoid.h" + + +namespace vertigo { + + template + class BetweenFactorSwitchableLinear : public gtsam::NoiseModelFactor3 + { + public: + BetweenFactorSwitchableLinear() {}; + BetweenFactorSwitchableLinear(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model) + : gtsam::NoiseModelFactor3(model, key1, key2, key3), + betweenFactor(key1, key2, measured, model) {}; + + gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const + { + + // calculate error + gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2); + error *= s.value(); + + // handle derivatives + if (H1) *H1 = *H1 * s.value(); + if (H2) *H2 = *H2 * s.value(); + if (H3) *H3 = error; + + return error; + }; + + private: + gtsam::BetweenFactor betweenFactor; + + }; + + + + template + class BetweenFactorSwitchableSigmoid : public gtsam::NoiseModelFactor3 + { + public: + BetweenFactorSwitchableSigmoid() {}; + BetweenFactorSwitchableSigmoid(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model) + : gtsam::NoiseModelFactor3(model, key1, key2, key3), + betweenFactor(key1, key2, measured, model) {}; + + gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s, + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const + { + + // calculate error + gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2); + + + double w = sigmoid(s.value()); + error *= w; + + // handle derivatives + if (H1) *H1 = *H1 * w; + if (H2) *H2 = *H2 * w; + if (H3) *H3 = error /* (w*(1.0-w))*/; // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x + + return error; + }; + + private: + gtsam::BetweenFactor betweenFactor; + + double sigmoid(double x) const { + return 1.0/(1.0+exp(-x)); + } + }; + +} + +#endif /* BETWEENFACTORSWITCHABLE_H_ */ diff --git a/corelib/src/vertigo/gtsam/switchVariableLinear.h b/corelib/src/vertigo/gtsam/switchVariableLinear.h new file mode 100644 index 00000000..34213884 --- /dev/null +++ b/corelib/src/vertigo/gtsam/switchVariableLinear.h @@ -0,0 +1,130 @@ +/* + * switchVariableLinear.h + * + * Created on: 02.08.2012 + * Author: niko + */ + +#ifndef SWITCHVARIABLELINEAR_H_ +#define SWITCHVARIABLELINEAR_H_ + +#pragma once + +#include +#include + +namespace vertigo { + + /** + * SwitchVariableLinear is a wrapper around double to allow it to be a Lie type + */ + struct SwitchVariableLinear : public gtsam::DerivedValue { + + /** default constructor */ + SwitchVariableLinear() : d_(0.0) {}; + + /** wrap a double */ + SwitchVariableLinear(double d) : d_(d) { + // if (d_ < 0.0) d_=0.0; + // else if(d_>1.0) d_=1.0; + }; + + /** access the underlying value */ + double value() const { return d_; } + + /** print @param s optional string naming the object */ + inline void print(const std::string& name="") const { + std::cout << name << ": " << d_ << std::endl; + } + + /** equality up to tolerance */ + inline bool equals(const SwitchVariableLinear& expected, double tol=1e-5) const { + return fabs(expected.d_ - d_) <= tol; + } + + // Manifold requirements + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return 1; } + inline static size_t Dim() { return 1; } + + /** Update the SwitchVariableLinear with a tangent space update */ + inline SwitchVariableLinear retract(const gtsam::Vector& v) const { + double x = value() + v(0); + + if (x>1.0) x=1.0; + else if (x<0.0) x=0.0; + + return SwitchVariableLinear(x); + } + + /** @return the local coordinates of another object */ + inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); } + + // Group requirements + + /** identity */ + inline static SwitchVariableLinear identity() { + return SwitchVariableLinear(); + } + + /** compose with another object */ + inline SwitchVariableLinear compose(const SwitchVariableLinear& p) const { + return SwitchVariableLinear(d_ + p.d_); + } + + /** between operation */ + inline SwitchVariableLinear between(const SwitchVariableLinear& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -gtsam::eye(1); + if(H2) *H2 = gtsam::eye(1); + return SwitchVariableLinear(l2.value() - value()); + } + + /** invert the object and yield a new one */ + inline SwitchVariableLinear inverse() const { + return SwitchVariableLinear(-1.0 * value()); + } + + // Lie functions + + /** Expmap around identity */ + static inline SwitchVariableLinear Expmap(const gtsam::Vector& v) { return SwitchVariableLinear(v(0)); } + + /** Logmap around identity - just returns with default cast back */ + static inline gtsam::Vector Logmap(const SwitchVariableLinear& p) { return gtsam::Vector1(p.value()); } + + private: + double d_; + }; +} + +namespace gtsam { +// Define Key to be Testable by specializing gtsam::traits +template struct traits; +template<> struct traits { + static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") { + key.print(str); + } + static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) { + return key1.equals(key2, tol); + } + static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();} + + typedef OptionalJacobian<3, 3> ChartJacobian; + typedef gtsam::Vector TangentVector; + static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + return origin.localCoordinates(other); + } + static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + return g.retract(v); + } +}; +} + + + +#endif /* SWITCHVARIABLELINEAR_H_ */ diff --git a/corelib/src/vertigo/gtsam/switchVariableSigmoid.h b/corelib/src/vertigo/gtsam/switchVariableSigmoid.h new file mode 100644 index 00000000..3a86790b --- /dev/null +++ b/corelib/src/vertigo/gtsam/switchVariableSigmoid.h @@ -0,0 +1,129 @@ +/* + * switchVariableSigmoid.h + * + * Created on: 08.08.2012 + * Author: niko + */ + +#ifndef SWITCHVARIABLESIGMOID_H_ +#define SWITCHVARIABLESIGMOID_H_ + +#pragma once + +#include +#include + +namespace vertigo { + + /** + * SwitchVariableSigmoid is a wrapper around double to allow it to be a Lie type + */ + struct SwitchVariableSigmoid : public gtsam::DerivedValue { + + /** default constructor */ + SwitchVariableSigmoid() : d_(10.0) {}; + + /** wrap a double */ + SwitchVariableSigmoid(double d) : d_(d) { + if (d_ < -10.0) d_=-10.0; + else if(d_>10.0) d_=10.0; + }; + + /** access the underlying value */ + double value() const { return d_; } + + /** print @param s optional string naming the object */ + inline void print(const std::string& name="") const { + std::cout << name << ": " << d_ << std::endl; + } + + /** equality up to tolerance */ + inline bool equals(const SwitchVariableSigmoid& expected, double tol=1e-5) const { + return fabs(expected.d_ - d_) <= tol; + } + + // Manifold requirements + + /** Returns dimensionality of the tangent space */ + inline size_t dim() const { return 1; } + inline static size_t Dim() { return 1; } + + /** Update the SwitchVariableSigmoid with a tangent space update */ + inline SwitchVariableSigmoid retract(const gtsam::Vector& v) const { + double x = value() + v(0); + + if (x>10.0) x=10.0; + else if (x<-10.0) x=-10.0; + + return SwitchVariableSigmoid(x); + } + + /** @return the local coordinates of another object */ + inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); } + + // Group requirements + + /** identity */ + inline static SwitchVariableSigmoid identity() { + return SwitchVariableSigmoid(); + } + + /** compose with another object */ + inline SwitchVariableSigmoid compose(const SwitchVariableSigmoid& p) const { + return SwitchVariableSigmoid(d_ + p.d_); + } + + /** between operation */ + inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const { + if(H1) *H1 = -gtsam::eye(1); + if(H2) *H2 = gtsam::eye(1); + return SwitchVariableSigmoid(l2.value() - value()); + } + + /** invert the object and yield a new one */ + inline SwitchVariableSigmoid inverse() const { + return SwitchVariableSigmoid(-1.0 * value()); + } + + // Lie functions + + /** Expmap around identity */ + static inline SwitchVariableSigmoid Expmap(const gtsam::Vector& v) { return SwitchVariableSigmoid(v(0)); } + + /** Logmap around identity - just returns with default cast back */ + static inline gtsam::Vector Logmap(const SwitchVariableSigmoid& p) { return gtsam::Vector1(p.value()); } + + private: + double d_; + }; +} + + +namespace gtsam { +// Define Key to be Testable by specializing gtsam::traits +template struct traits; +template<> struct traits { + static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") { + key.print(str); + } + static bool Equals(const vertigo::SwitchVariableSigmoid& key1, const vertigo::SwitchVariableSigmoid& key2, double tol = 1e-8) { + return key1.equals(key2, tol); + } + static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();} + + typedef OptionalJacobian<3, 3> ChartJacobian; + typedef gtsam::Vector TangentVector; + static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other, + ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) { + return origin.localCoordinates(other); + } + static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v, + ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) { + return g.retract(v); + } +}; +} + +#endif /* SWITCHVARIABLESIGMOID_H_ */ diff --git a/corelib/src/vertigo/readme.txt b/corelib/src/vertigo/readme.txt new file mode 100644 index 00000000..774a679f --- /dev/null +++ b/corelib/src/vertigo/readme.txt @@ -0,0 +1,8 @@ + +Info: http://openslam.org/vertigo.html +Source: https://github.com/christiankerl/vertigo/tree/master/trunk +Commit: fbd438488a56cdba805fa2f75aa3e394e3eda7ff +License: GPL v3 + +Tested with g2o (ROS Indigo/2014.02.18) +Tested with GTSAM commit c73b835 \ No newline at end of file diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index b3aac23e..3708e832 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -82,6 +82,7 @@ private slots: void extractImages(); void generateLocalGraph(); void generateTOROGraph(); + void generateG2OGraph(); void view3DMap(); void generate3DMap(); void detectMoreLoopClosures(); diff --git a/guilib/include/rtabmap/gui/MainWindow.h b/guilib/include/rtabmap/gui/MainWindow.h index 516740eb..c6b0dc37 100644 --- a/guilib/include/rtabmap/gui/MainWindow.h +++ b/guilib/include/rtabmap/gui/MainWindow.h @@ -132,6 +132,7 @@ private slots: void exportPosesRGBDSLAM(); void exportPosesKITTI(); void exportPosesTORO(); + void exportPosesG2O(); void postProcessing(); void deleteMemory(); void openWorkingDirectory(); diff --git a/guilib/include/rtabmap/gui/PreferencesDialog.h b/guilib/include/rtabmap/gui/PreferencesDialog.h index 18f270a2..8a2bdedc 100644 --- a/guilib/include/rtabmap/gui/PreferencesDialog.h +++ b/guilib/include/rtabmap/gui/PreferencesDialog.h @@ -301,7 +301,7 @@ private: void addParameter(const QObject * object, double value); void addParameter(const QObject * object, const QString & value); void addParameters(const QObjectList & children); - void addParameters(const QStackedWidget * stackedWidget); + void addParameters(const QStackedWidget * stackedWidget, int panel = -1); void addParameters(const QGroupBox * box); QList getGroupBoxes(); void readSettingsBegin(); diff --git a/guilib/src/AboutDialog.cpp b/guilib/src/AboutDialog.cpp index a3ccf431..af2d2927 100644 --- a/guilib/src/AboutDialog.cpp +++ b/guilib/src/AboutDialog.cpp @@ -61,6 +61,7 @@ AboutDialog::AboutDialog(QWidget * parent) : _ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No"); _ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No"); + _ui->label_gtsam->setText(graph::GTSAMOptimizer::available()?"Yes":"No"); _ui->label_cvsba->setText(graph::CVSBAOptimizer::available()?"Yes":"No"); } diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 55e73d57..e41ed67d 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -127,6 +127,19 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : ui_->comboBox_graphOptimizer->setCurrentIndex(0); } } + if(!graph::GTSAMOptimizer::available()) + { + ui_->comboBox_graphOptimizer->setItemData(1, 0, Qt::UserRole - 1); + if(ui_->comboBox_graphOptimizer->currentIndex() == 2) + { + UWARN("GTSAM is not available, setting optimization default to TORO."); + ui_->comboBox_graphOptimizer->setCurrentIndex(0); + } + } + if(!graph::G2OOptimizer::available() && !graph::GTSAMOptimizer::available()) + { + ui_->checkBox_robust->setEnabled(false); + } ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction()); ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction()); @@ -146,6 +159,8 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph())); connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph())); connect(ui_->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this, SLOT(generateTOROGraph())); + connect(ui_->actionGenerate_g2o_graph_g2o, SIGNAL(triggered()), this, SLOT(generateG2OGraph())); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(graph::G2OOptimizer::available()); connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap())); connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap())); connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures())); @@ -168,6 +183,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : ui_->pushButton_reject->setEnabled(false); ui_->actionGenerate_TORO_graph_graph->setEnabled(false); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(false); ui_->horizontalSlider_A->setTracking(false); ui_->horizontalSlider_B->setTracking(false); @@ -198,6 +214,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); + connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); @@ -239,6 +256,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) : // Graph view connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified())); + connect(ui_->checkBox_robust, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(configModified())); connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified())); @@ -358,6 +376,7 @@ void DatabaseViewer::readSettings() settings.beginGroup("optimization"); ui_->spinBox_iterations->setValue(settings.value("iterations", ui_->spinBox_iterations->value()).toInt()); ui_->checkBox_spanAllMaps->setChecked(settings.value("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked()).toBool()); + ui_->checkBox_robust->setChecked(settings.value("robust", ui_->checkBox_robust->isChecked()).toBool()); ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool()); ui_->checkBox_ignorePoseCorrection->setChecked(settings.value("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()).toBool()); ui_->checkBox_ignoreGlobalLoop->setChecked(settings.value("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked()).toBool()); @@ -451,6 +470,7 @@ void DatabaseViewer::writeSettings() settings.beginGroup("optimization"); settings.setValue("iterations", ui_->spinBox_iterations->value()); settings.setValue("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked()); + settings.setValue("robust", ui_->checkBox_robust->isChecked()); settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()); settings.setValue("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()); settings.setValue("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked()); @@ -556,7 +576,9 @@ bool DatabaseViewer::openDatabase(const QString & path) linksRefined_.clear(); linksRemoved_.clear(); localMaps_.clear(); + ui_->graphViewer->clearAll(); ui_->actionGenerate_TORO_graph_graph->setEnabled(false); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(false); ui_->checkBox_showOptimized->setEnabled(false); databaseFileName_.clear(); } @@ -1057,6 +1079,7 @@ void DatabaseViewer::updateIds() } ui_->actionGenerate_TORO_graph_graph->setEnabled(false); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(false); graphes_.clear(); graphLinks_.clear(); neighborLinks_.clear(); @@ -1193,7 +1216,51 @@ void DatabaseViewer::generateTOROGraph() QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)")); if(!path.isEmpty()) { - graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_); + if(ui_->checkBox_ignoreCovariance->isChecked()) + { + std::multimap links = graphLinks_; + for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) + { + iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1)); + } + graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links); + } + else + { + graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_); + } + } + } +} + +void DatabaseViewer::generateG2OGraph() +{ + if(!graphes_.size() || !graphLinks_.size()) + { + QMessageBox::warning(this, tr("Cannot generate a g2o graph"), tr("No poses or no links...")); + return; + } + bool ok = false; + int id = QInputDialog::getInt(this, tr("Which iteration?"), tr("Iteration (0 -> %1)").arg((int)graphes_.size()-1), (int)graphes_.size()-1, 0, (int)graphes_.size()-1, 1, &ok); + + if(ok) + { + QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".g2o", tr("g2o file (*.g2o)")); + if(!path.isEmpty()) + { + if(ui_->checkBox_ignoreCovariance->isChecked()) + { + std::multimap links = graphLinks_; + for(std::multimap::iterator iter=links.begin(); iter!=links.end(); ++iter) + { + iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1)); + } + graph::G2OOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links, ui_->checkBox_robust->isChecked()); + } + else + { + graph::G2OOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), graphLinks_, ui_->checkBox_robust->isChecked()); + } } } } @@ -2604,6 +2671,7 @@ void DatabaseViewer::updateGraphView() graphes_.push_back(poses); ui_->actionGenerate_TORO_graph_graph->setEnabled(true); + ui_->actionGenerate_g2o_graph_g2o->setEnabled(true); std::multimap links = links_; // filter current map if not spanning to all maps @@ -2697,13 +2765,32 @@ void DatabaseViewer::updateGraphView() ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4)").arg(totalGlobal).arg(totalLocalSpace).arg(totalLocalTime).arg(totalUser)); graph::Optimizer * optimizer = 0; - if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O) + if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeGTSAM) { - optimizer = new graph::G2OOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked()); + optimizer = new graph::GTSAMOptimizer( + ui_->spinBox_iterations->value(), + ui_->checkBox_2dslam->isChecked(), + ui_->checkBox_ignoreCovariance->isChecked(), + 0.0, + ui_->checkBox_robust->isChecked()); + } + else if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O) + { + UINFO("ui_->checkBox_robust->isChecked()=%d", ui_->checkBox_robust->isChecked()?1:0); + optimizer = new graph::G2OOptimizer( + ui_->spinBox_iterations->value(), + ui_->checkBox_2dslam->isChecked(), + ui_->checkBox_ignoreCovariance->isChecked(), + 0.0, + ui_->checkBox_robust->isChecked()); } else { - optimizer = new graph::TOROOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked()); + optimizer = new graph::TOROOptimizer( + ui_->spinBox_iterations->value(), + ui_->checkBox_2dslam->isChecked(), + ui_->checkBox_ignoreCovariance->isChecked(), + 0.0); } std::map posesOut; std::multimap linksOut; @@ -2723,6 +2810,10 @@ void DatabaseViewer::updateGraphView() graphLinks_ = linksOut; ui_->label_nodes->setNum((int)finalPoses.size()); delete optimizer; + if(posesOut.size() && finalPoses.empty()) + { + QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors.")); + } } if(graphes_.size()) { @@ -3006,7 +3097,7 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update } if(!updated) { - linksRefined_.insert(std::make_pair(newLink.from(), newLink)); + linksRefined_.insert(std::make_pair(newLink.from(), newLink)); if(updateGraph) { @@ -3129,7 +3220,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo } if(!updated) { - linksRefined_.insert(std::make_pair(newLink.from(), newLink)); + linksRefined_.insert(std::make_pair(newLink.from(), newLink)); if(updateGraph) { diff --git a/guilib/src/MainWindow.cpp b/guilib/src/MainWindow.cpp index d5e953db..a57cf406 100644 --- a/guilib/src/MainWindow.cpp +++ b/guilib/src/MainWindow.cpp @@ -308,6 +308,8 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) : connect(_ui->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM())); connect(_ui->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI())); connect(_ui->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO())); + connect(_ui->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O())); + _ui->actionG2o_g2o->setVisible(graph::G2OOptimizer::available()); connect(_ui->actionDelete_memory, SIGNAL(triggered()), this , SLOT(deleteMemory())); connect(_ui->actionDownload_all_clouds, SIGNAL(triggered()), this , SLOT(downloadAllClouds())); connect(_ui->actionDownload_graph, SIGNAL(triggered()), this , SLOT(downloadPoseGraph())); @@ -3178,6 +3180,10 @@ void MainWindow::exportPosesTORO() { exportPoses(3); } +void MainWindow::exportPosesG2O() +{ + exportPoses(4); +} void MainWindow::exportPoses(int format) { @@ -3215,14 +3221,14 @@ void MainWindow::exportPoses(int format) if(_exportPosesFileName[format].isEmpty()) { - _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":"poses.txt"); + _exportPosesFileName[format] = _preferencesDialog->getWorkingDirectory() + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt"); } QString path = QFileDialog::getSaveFileName( this, tr("Save File"), _exportPosesFileName[format], - format == 3?tr("TORO file (*.graph)"):tr("Text file (*.txt)")); + format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)")); if(!path.isEmpty()) { @@ -3233,7 +3239,7 @@ void MainWindow::exportPoses(int format) _ui->dockWidget_console->show(); _ui->widget_console->appendMsg( QString("%1 saved (global=%2, optimized=%3)... %4") - .arg(format == 3?"TORO graph":"Poses") + .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses") .arg(global?"true":"false") .arg(optimized?"true":"false") .arg(_exportPosesFileName[format])); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 5d211b04..83bb2e72 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -176,6 +176,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : { _ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1); } + if(!graph::GTSAMOptimizer::available()) + { + _ui->graphOptimization_type->setItemData(2, 0, Qt::UserRole - 1); + } + if(!graph::G2OOptimizer::available() && !graph::GTSAMOptimizer::available()) + { + _ui->graphOptimization_robust->setEnabled(false); + } if(!CameraOpenni::available()) { _ui->comboBox_cameraRGBD->setItemData(0, 0, Qt::UserRole - 1); @@ -571,6 +579,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kRGBDOptimizeVarianceIgnored().c_str()); _ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str()); _ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kRGBDOptimizeEpsilon().c_str()); + _ui->graphOptimization_robust->setObjectName(Parameters::kRGBDOptimizeRobust().c_str()); _ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str()); _ui->graphPlan_planWithNearNodesLinked->setObjectName(Parameters::kRGBDPlanVirtualLinks().c_str()); @@ -1854,7 +1863,7 @@ bool PreferencesDialog::validateForm() // optimization strategy if(!graph::G2OOptimizer::available()) { - if(_ui->graphOptimization_type->currentIndex() > 0) + if(_ui->graphOptimization_type->currentIndex() == 1) { QMessageBox::warning(this, tr("Parameter warning"), tr("Selected graph optimization strategy (g2o) is not available. RTAB-Map is not built " @@ -1862,6 +1871,16 @@ bool PreferencesDialog::validateForm() _ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO); } } + if(!graph::GTSAMOptimizer::available()) + { + if(_ui->graphOptimization_type->currentIndex() == 2) + { + QMessageBox::warning(this, tr("Parameter warning"), + tr("Selected graph optimization strategy (GTSAM) is not available. RTAB-Map is not built " + "with GTSAM. TORO is set instead for graph optimization strategy.")); + _ui->graphOptimization_type->setCurrentIndex(graph::Optimizer::kTypeTORO); + } + } //verify binary features and nearest neighbor // BOW dictionary type @@ -2487,6 +2506,17 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string ok = false; } } + if(!graph::GTSAMOptimizer::available()) + { + if(valueInt==2 && combo->objectName().toStdString().compare(Parameters::kRGBDOptimizeStrategy()) == 0) + { + UWARN("Trying to set \"%s\" to GTSAM but RTAB-Map isn't built " + "with GTSAM. Keeping default combo value: %s.", + combo->objectName().toStdString().c_str(), + combo->currentText().toStdString().c_str()); + ok = false; + } + } if(ok) { combo->setCurrentIndex(valueInt); @@ -2650,6 +2680,22 @@ void PreferencesDialog::addParameter(const QObject * object, int value) this->addParameters(_ui->groupBox_loopClosure_icp2); } } + else if(comboBox == _ui->loopClosure_estimationType) + { + this->addParameters(_ui->stackedWidget_loopClosureEstimation, _ui->loopClosure_estimationType->currentIndex()); + } + else if(comboBox == _ui->odom_estimationType) + { + this->addParameters(_ui->stackedWidget_odomEstimation, _ui->stackedWidget_odomEstimation->currentIndex()); + } + else if(comboBox == _ui->graphOptimization_type) + { + this->addParameter(_ui->graphOptimization_iterations, _ui->graphOptimization_iterations->value()); + this->addParameter(_ui->graphOptimization_covarianceIgnored, _ui->graphOptimization_covarianceIgnored->isChecked()); + this->addParameter(_ui->graphOptimization_slam2d, _ui->graphOptimization_slam2d->isChecked()); + this->addParameter(_ui->graphOptimization_stopEpsilon, _ui->graphOptimization_stopEpsilon->value()); + this->addParameter(_ui->graphOptimization_robust, _ui->graphOptimization_robust->isChecked()); + } } // Add parameter _parameters.insert(rtabmap::ParametersPair(object->objectName().toStdString(), QString::number(value).toStdString())); @@ -2816,13 +2862,22 @@ void PreferencesDialog::addParameters(const QObjectList & children) } } -void PreferencesDialog::addParameters(const QStackedWidget * stackedWidget) +void PreferencesDialog::addParameters(const QStackedWidget * stackedWidget, int panel) { if(stackedWidget) { - for(int i=0; icount(); ++i) + if(panel == -1) { - const QObjectList & children = stackedWidget->widget(i)->children(); + for(int i=0; icount(); ++i) + { + const QObjectList & children = stackedWidget->widget(i)->children(); + addParameters(children); + } + } + else + { + UASSERT(panelcount()); + const QObjectList & children = stackedWidget->widget(panel)->children(); addParameters(children); } } diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 64a22a90..879260a4 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -50,8 +50,8 @@ 0 0 - 154 - 184 + 175 + 173 @@ -236,8 +236,8 @@ 0 0 - 154 - 184 + 174 + 173 @@ -418,7 +418,7 @@ 0 0 1285 - 22 + 25 @@ -441,6 +441,7 @@ + @@ -828,15 +829,15 @@ - 1 + 2 0 0 - 312 - 314 + 314 + 303 @@ -1055,8 +1056,8 @@ 0 0 - 366 - 361 + 351 + 347 @@ -1344,9 +1345,9 @@ 0 - 0 - 330 - 304 + -26 + 333 + 333 @@ -1363,14 +1364,14 @@ - + Ignore global loop closures - + @@ -1419,14 +1420,14 @@ - + Ignore pose correction - + @@ -1436,7 +1437,7 @@ - + @@ -1446,7 +1447,7 @@ - + @@ -1456,6 +1457,19 @@ + + + + Qt::Vertical + + + + 0 + 40 + + + + @@ -1471,6 +1485,11 @@ g2o + + + GTSAM + + @@ -1480,34 +1499,21 @@ - + Ignore user loop closures - + Ignore local loop closures (space) - - - - Qt::Vertical - - - - 0 - 40 - - - - - + @@ -1517,7 +1523,7 @@ - + Ignore local loop closures (time) @@ -1531,7 +1537,7 @@ - + @@ -1548,13 +1554,30 @@ - + Ignore covariance + + + + Robust optimization + + + + + + + + + + false + + + @@ -1562,8 +1585,8 @@ 0 0 - 248 - 319 + 243 + 284 @@ -1757,7 +1780,7 @@ 0 0 201 - 126 + 117 @@ -1856,8 +1879,8 @@ 0 0 - 283 - 322 + 285 + 309 @@ -2301,6 +2324,11 @@ Reset all changes + + + Generate g2o graph (*.g2o)... + + diff --git a/guilib/src/ui/aboutDialog.ui b/guilib/src/ui/aboutDialog.ui index 7fe1f390..b9937a35 100644 --- a/guilib/src/ui/aboutDialog.ui +++ b/guilib/src/ui/aboutDialog.ui @@ -82,6 +82,13 @@ p, li { white-space: pre-wrap; } + + + + With Freenect : + + + @@ -205,13 +212,6 @@ p, li { white-space: pre-wrap; } - - - - With Freenect : - - - @@ -293,14 +293,14 @@ p, li { white-space: pre-wrap; } - + With cvsba : - + @@ -310,6 +310,23 @@ p, li { white-space: pre-wrap; } + + + + With GTSAM : + + + + + + + + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + diff --git a/guilib/src/ui/mainWindow.ui b/guilib/src/ui/mainWindow.ui index a3cd3a9a..0222ab10 100644 --- a/guilib/src/ui/mainWindow.ui +++ b/guilib/src/ui/mainWindow.ui @@ -27,7 +27,7 @@ 0 0 1012 - 22 + 25 @@ -64,6 +64,7 @@ + @@ -1259,6 +1260,11 @@ Raw format (*.txt) + + + g2o (*.g2o) + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 652e1aba..14972472 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -64,8 +64,8 @@ 0 -392 - 755 - 1591 + 760 + 1570 @@ -86,7 +86,7 @@ QFrame::Raised - 3 + 19 @@ -6553,6 +6553,11 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag g2o + + + GTSAM + + @@ -6575,14 +6580,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - + Optimize graph from the newest node. @@ -6595,7 +6600,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + Qt::Horizontal @@ -6632,7 +6637,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + -If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation). @@ -6645,7 +6650,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + -If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures. @@ -6710,6 +6715,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + + + + + + + + Robust graph optimization using Vertigo (only for g2o and GTSAM optimization strategies). This approach can filter wrong loop closure detections. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + +