diff --git a/README.md b/README.md index b43ea5c..4c6dfa8 100644 --- a/README.md +++ b/README.md @@ -57,11 +57,11 @@ Alternatively, you can install them by your own: sudo apt-get install libeigen3-dev ``` -+ **Ceres** (>= 2.0) and its dependencies ++ **Ceres** (>= 2.1.0) and its dependencies ```bash sudo apt-get install libgoogle-glog-dev - sudo apt-get libgflags-dev + sudo apt-get install libgflags-dev sudo apt-get install libatlas-base-dev sudo apt-get install libsuitesparse-dev @@ -85,7 +85,7 @@ Alternatively, you can install them by your own: + **GeographicLib** ```bash - sudo apt-get install libgeographic-dev + sudo apt-get install libgeographiclib-dev ``` The library and its applications can be build following this instructions: diff --git a/cmake/FindGeographicLib.cmake b/cmake/FindGeographicLib.cmake index 58932cc..26497a8 100644 --- a/cmake/FindGeographicLib.cmake +++ b/cmake/FindGeographicLib.cmake @@ -1,12 +1,12 @@ # Look for GeographicLib # # Set -# GeographicLib_FOUND = GEOGRAPHICLIB_FOUND = TRUE +# GeographicLib_FOUND = TRUE # GeographicLib_INCLUDE_DIRS = /usr/local/include -# GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so +# GeographicLib_LIBRARIES = /usr/local/lib/libGeographicLib.so # GeographicLib_LIBRARY_DIRS = /usr/local/lib -find_library (GeographicLib_LIBRARIES Geographic +find_library (GeographicLib_LIBRARIES GeographicLib PATHS "${CMAKE_INSTALL_PREFIX}/../GeographicLib/lib") if (GeographicLib_LIBRARIES) @@ -17,7 +17,7 @@ if (GeographicLib_LIBRARIES) set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin") if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h") # On Debian systems the library is in e.g., - # /usr/lib/x86_64-linux-gnu/libGeographic.so + # /usr/lib/x86_64-linux-gnu/libGeographicLib.so # so try stripping another element off _ROOT_DIR get_filename_component (_ROOT_DIR "${_ROOT_DIR}" PATH) set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include") @@ -32,6 +32,7 @@ if (GeographicLib_LIBRARIES) unset (_ROOT_DIR) endif () +# This sets GeographicLib_FOUND (and GEOGRAPHICLIB_FOUND, deprecated) to TRUE include (FindPackageHandleStandardArgs) find_package_handle_standard_args (GeographicLib DEFAULT_MSG GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES diff --git a/externals/ceres-solver b/externals/ceres-solver index 399cda7..0c70ed3 160000 --- a/externals/ceres-solver +++ b/externals/ceres-solver @@ -1 +1 @@ -Subproject commit 399cda773035d99eaf1f4a129a666b3c4df9d1b1 +Subproject commit 0c70ed3a1a2d6ba47c06c7e8b3b040880bc474db diff --git a/include/FactorGraph.h b/include/FactorGraph.h index b994c4c..0b068e6 100644 --- a/include/FactorGraph.h +++ b/include/FactorGraph.h @@ -439,7 +439,7 @@ namespace libRSF /** default settings for new ceres::Problems, especially enable_fast_removal = true */ const ceres::Problem::Options DefaultProblemOptions_ = {ceres::Ownership::TAKE_OWNERSHIP, // cost_function_ownership ceres::Ownership::TAKE_OWNERSHIP, // loss_function_ownership - ceres::Ownership::TAKE_OWNERSHIP, // local_parameterization_ownership + ceres::Ownership::TAKE_OWNERSHIP, // manifold_ownership true, // enable_fast_removal false, // disable_all_safety_checks nullptr, // context diff --git a/include/LocalParametrization.h b/include/LocalParametrization.h index 659709f..e74bc96 100644 --- a/include/LocalParametrization.h +++ b/include/LocalParametrization.h @@ -36,7 +36,8 @@ #include "VectorMath.h" #include "Geometry.h" -#include +#include +#include namespace libRSF { @@ -46,15 +47,22 @@ namespace libRSF public: template - bool operator()(const T* Angle, const T* DeltaAngle, T* AnglePlusDelta) const + bool Plus(const T* Angle, const T* DeltaAngle, T* AnglePlusDelta) const { *AnglePlusDelta = NormalizeAngle(*Angle + *DeltaAngle); return true; } - static ceres::LocalParameterization* Create() + template + bool Minus(const T* y, const T* x, T* y_minus_x) const + { + *y_minus_x = NormalizeAngle(*y) - NormalizeAngle(*x); + return true; + } + + static ceres::Manifold* Create() { - return (new ceres::AutoDiffLocalParameterization); + return (new ceres::AutoDiffManifold); } }; @@ -64,7 +72,7 @@ namespace libRSF public: template - bool operator()(const T* Circle, const T* DeltaCircle, T* CirclePlusDelta) const + bool Plus(const T* Circle, const T* DeltaCircle, T* CirclePlusDelta) const { /** real part --> cos() */ CirclePlusDelta[0] = Circle[0] * cos(DeltaCircle[0]) - Circle[1] * sin(DeltaCircle[0]); @@ -75,9 +83,17 @@ namespace libRSF return true; } - static ceres::LocalParameterization* Create() + template + bool Minus(const T* y, const T* x, T* y_minus_x) const { - return (new ceres::AutoDiffLocalParameterization); + /** extract angle from unit circle representation [cos(θ), sin(θ)] and compute normalized difference */ + *y_minus_x = NormalizeAngle(ceres::atan2(y[1], y[0]) - ceres::atan2(x[1], x[0])); + return true; + } + + static ceres::Manifold* Create() + { + return (new ceres::AutoDiffManifold); } }; @@ -87,7 +103,7 @@ namespace libRSF public: template - bool operator()(const T* Quaternion, const T* Delta, T* QuaternionPlusDelta) const + bool Plus(const T* Quaternion, const T* Delta, T* QuaternionPlusDelta) const { QuaternionRefConst Q(Quaternion); VectorRefConst DeltaVec(Delta); @@ -114,9 +130,22 @@ namespace libRSF return true; } - static ceres::LocalParameterization* Create() + template + bool Minus(const T* y, const T* x, T* y_minus_x) const + { + QuaternionRefConst Qy(y); + QuaternionRefConst Qx(x); + VectorRef DeltaVec(y_minus_x); + + /** compute quaternion difference in tangent space using log map */ + DeltaVec = QuaternionError(Qy, Qx); + + return true; + } + + static ceres::Manifold* Create() { - return (new ceres::AutoDiffLocalParameterization); + return (new ceres::AutoDiffManifold); } }; } diff --git a/include/libRSF.h b/include/libRSF.h index cea5796..0083ddd 100644 --- a/include/libRSF.h +++ b/include/libRSF.h @@ -32,6 +32,8 @@ #ifndef LIBRSF_H_INCLUDED #define LIBRSF_H_INCLUDED +#include + /** most important functions */ #include "FactorGraph.h" #include "FactorGraphConfig.h" diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index dcbb0d7..b705a2f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -105,7 +105,7 @@ target_include_directories(libRSF ) # set target (libRSF) dependencies -target_link_libraries(libRSF PUBLIC Threads::Threads Eigen3::Eigen Ceres::ceres yaml-cpp ${GeographicLib_LIBRARIES}) +target_link_libraries(libRSF PUBLIC Threads::Threads Eigen3::Eigen Ceres::ceres yaml-cpp ${GeographicLib_LIBRARIES} glog) # enable all warnings for libRSF (this is just enabled from time to time to check the code quality) #target_compile_options(libRSF PUBLIC -Wextra -Wpedantic -Wall -fmax-errors=100 -Wno-unused-parameter) diff --git a/src/FactorGraph.cpp b/src/FactorGraph.cpp index b0e5856..25244ce 100644 --- a/src/FactorGraph.cpp +++ b/src/FactorGraph.cpp @@ -118,7 +118,7 @@ namespace libRSF case DataType::Pose2: { - ceres::LocalParameterization *LocalParamPose2 = new ceres::ProductParameterization(new ceres::IdentityParameterization(2), AngleLocalParameterization::Create()); + ceres::Manifold *LocalParamPose2 = new ceres::ProductManifold(new ceres::EuclideanManifold<2>(), AngleLocalParameterization::Create()); Graph_.AddParameterBlock(StatePointer, StateSize, LocalParamPose2); break; } @@ -130,7 +130,7 @@ namespace libRSF Pose3 << 0,0,0, 0,0,0,1; StateData_.getElement(Name, Timestamp, StateNumber).setMean(Pose3); - ceres::LocalParameterization *LocalParamPose3 = new ceres::ProductParameterization(new ceres::IdentityParameterization(3), QuaternionLocalParameterization::Create()); + ceres::Manifold *LocalParamPose3 = new ceres::ProductManifold(new ceres::EuclideanManifold<3>(), QuaternionLocalParameterization::Create()); Graph_.AddParameterBlock(StatePointer, StateSize, LocalParamPose3); break; } @@ -211,9 +211,9 @@ namespace libRSF void FactorGraph::setSubsetConstant(const string& Name, const double Timestamp, const int Number, const std::vector &ConstantIndex) { - Graph_.SetParameterization( + Graph_.SetManifold( StateData_.getElement(Name, Timestamp, Number).getMeanPointer(), - new ceres::SubsetParameterization( + new ceres::SubsetManifold( StateData_.getElement(Name, Timestamp, Number).getMean().size(), ConstantIndex)); } @@ -283,7 +283,7 @@ namespace libRSF StateData_.getElement(State.ID, State.Timestamp, State.Number).getMeanPointer()); /** compute size of the marginalized system */ - MarginalSize += Graph_.ParameterBlockLocalSize(MarginalStates.back()); + MarginalSize += Graph_.ParameterBlockTangentSize(MarginalStates.back()); } /** get connected states */ diff --git a/src/FactorGraphStructure.cpp b/src/FactorGraphStructure.cpp index 7588ae6..0f438d6 100644 --- a/src/FactorGraphStructure.cpp +++ b/src/FactorGraphStructure.cpp @@ -97,7 +97,7 @@ namespace libRSF { ConnectedStates.emplace_back(State); StateDims.emplace_back(Graph_->ParameterBlockSize(State)); - StateDimsLocal.emplace_back(Graph_->ParameterBlockLocalSize(State)); + StateDimsLocal.emplace_back(Graph_->ParameterBlockTangentSize(State)); /** find state info */ StateInfo Info = States_.at(State);