diff --git a/.gitignore b/.gitignore index 43e3bd1682..4b39dbb469 100644 --- a/.gitignore +++ b/.gitignore @@ -34,3 +34,4 @@ conductor/ .ccache/ gtsam_unstable/timing/data/ timing/results/bayes_tree_covariance/ +_agent/docs diff --git a/CMakeLists.txt b/CMakeLists.txt index 68b4506fa1..67f7aac9b6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -123,6 +123,7 @@ include(cmake/HandleEigen.cmake) # Eigen3 include(cmake/HandleMetis.cmake) # metis include(cmake/HandleCephes.cmake) # cephes include(cmake/HandleMKL.cmake) # MKL +include(cmake/HandleMOSEK.cmake) # MOSEK SDP solver, optional include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools include(cmake/HandlePython.cmake) # Python options and commands @@ -142,6 +143,9 @@ add_subdirectory(CppUnitLite) # Build GTSAM library add_subdirectory(gtsam) +# Build certifiable examples +add_subdirectory(gtsam/certifiable) + # Build Tests add_subdirectory(tests) @@ -209,4 +213,4 @@ include(cmake/HandlePrintConfiguration.cmake) include(cmake/HandleFinalChecks.cmake) # Include CPack *after* all flags -include(CPack) \ No newline at end of file +include(CPack) diff --git a/cmake/FindMOSEK.cmake b/cmake/FindMOSEK.cmake new file mode 100644 index 0000000000..5fb0491b90 --- /dev/null +++ b/cmake/FindMOSEK.cmake @@ -0,0 +1,141 @@ +# FindMOSEK.cmake - Find MOSEK Fusion API. +# +# Sets: +# MOSEK_FOUND - True if MOSEK was found. +# MOSEK_INCLUDE_DIRS - Include directories for Fusion. +# MOSEK_LIBRARIES - Libraries to link. +# MOSEK_LIBRARY_DIR - Runtime library directory, for rpath. +# MOSEK_FUSION_SOURCES - Optional Fusion C++ sources when the package does +# not ship a prebuilt Fusion runtime library. + +set(MOSEK_ROOT "" CACHE PATH + "Path to MOSEK platform directory (contains h/ and bin/), e.g. .../tools/platform/osxaarch64") + +set(_MOSEK_PLATFORMS) +if(APPLE) + if(CMAKE_SYSTEM_PROCESSOR MATCHES "^(arm64|aarch64)$") + list(APPEND _MOSEK_PLATFORMS osxaarch64) + endif() + list(APPEND _MOSEK_PLATFORMS osx64x86) +elseif(UNIX) + list(APPEND _MOSEK_PLATFORMS linux64x86) +elseif(WIN32) + list(APPEND _MOSEK_PLATFORMS win64x86) +endif() + +set(_MOSEK_BASE_CANDIDATES) +if(MOSEK_ROOT) + list(APPEND _MOSEK_BASE_CANDIDATES "${MOSEK_ROOT}") +endif() +if(DEFINED ENV{MOSEK_ROOT} AND NOT "$ENV{MOSEK_ROOT}" STREQUAL "") + list(APPEND _MOSEK_BASE_CANDIDATES "$ENV{MOSEK_ROOT}") +endif() +if(DEFINED ENV{MOSEK_HOME} AND NOT "$ENV{MOSEK_HOME}" STREQUAL "") + list(APPEND _MOSEK_BASE_CANDIDATES "$ENV{MOSEK_HOME}") +endif() +if(DEFINED ENV{HOME} AND NOT "$ENV{HOME}" STREQUAL "") + list(APPEND _MOSEK_BASE_CANDIDATES + "$ENV{HOME}/Desktop/mosek" + "$ENV{HOME}/Desktop/MOSEK/mosek" + "$ENV{HOME}/mosek") +endif() + +set(_MOSEK_HINT_PATHS) +foreach(_base IN LISTS _MOSEK_BASE_CANDIDATES) + if(NOT _base) + continue() + endif() + + list(APPEND _MOSEK_HINT_PATHS "${_base}") + + file(GLOB _mosek_version_dirs "${_base}/*/tools/platform") + foreach(_platform_root IN LISTS _mosek_version_dirs) + foreach(_platform IN LISTS _MOSEK_PLATFORMS) + list(APPEND _MOSEK_HINT_PATHS "${_platform_root}/${_platform}") + endforeach() + endforeach() + + foreach(_platform IN LISTS _MOSEK_PLATFORMS) + list(APPEND _MOSEK_HINT_PATHS "${_base}/tools/platform/${_platform}") + endforeach() +endforeach() +list(REMOVE_DUPLICATES _MOSEK_HINT_PATHS) + +find_path(MOSEK_INCLUDE_DIR + NAMES fusion.h + HINTS ${_MOSEK_HINT_PATHS} + PATH_SUFFIXES h +) + +find_library(MOSEK_LIBRARY + NAMES mosek64 mosek64.12.0 mosek64.11.1 mosek64.11.0 mosek64.10.2 + HINTS ${_MOSEK_HINT_PATHS} + PATH_SUFFIXES bin +) + +find_library(MOSEK_FUSION_LIBRARY + NAMES fusion64 fusion + HINTS ${_MOSEK_HINT_PATHS} + PATH_SUFFIXES bin +) + +set(MOSEK_FUSION_SOURCES "") +set(MOSEK_FUSION_AVAILABLE FALSE) +set(_MOSEK_FUSION_SRC_DIR "") + +if(MOSEK_FUSION_LIBRARY) + set(MOSEK_FUSION_AVAILABLE TRUE) +else() + set(_MOSEK_FUSION_SRC_CANDIDATES) + foreach(_hint IN LISTS _MOSEK_HINT_PATHS) + list(APPEND _MOSEK_FUSION_SRC_CANDIDATES "${_hint}/src/fusion_cxx") + endforeach() + if(MOSEK_ROOT) + list(APPEND _MOSEK_FUSION_SRC_CANDIDATES "${MOSEK_ROOT}/src/fusion_cxx") + endif() + if(MOSEK_INCLUDE_DIR) + get_filename_component(_MOSEK_PLATFORM_ROOT "${MOSEK_INCLUDE_DIR}" DIRECTORY) + list(APPEND _MOSEK_FUSION_SRC_CANDIDATES + "${_MOSEK_PLATFORM_ROOT}/src/fusion_cxx") + endif() + list(REMOVE_DUPLICATES _MOSEK_FUSION_SRC_CANDIDATES) + + # Some MOSEK packages ship Fusion C++ as source instead of a runtime library. + # Check all resolved platform candidates, not just the user-supplied root. + foreach(_src_dir IN LISTS _MOSEK_FUSION_SRC_CANDIDATES) + if(EXISTS "${_src_dir}/fusion.cc") + set(_MOSEK_FUSION_SRC_DIR "${_src_dir}") + file(GLOB MOSEK_FUSION_SOURCES "${_MOSEK_FUSION_SRC_DIR}/*.cc") + if(MOSEK_FUSION_SOURCES) + set(MOSEK_FUSION_AVAILABLE TRUE) + endif() + break() + endif() + endforeach() +endif() + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(MOSEK DEFAULT_MSG + MOSEK_INCLUDE_DIR MOSEK_LIBRARY MOSEK_FUSION_AVAILABLE) + +if(MOSEK_FOUND) + set(MOSEK_INCLUDE_DIRS ${MOSEK_INCLUDE_DIR}) + if(_MOSEK_FUSION_SRC_DIR) + list(APPEND MOSEK_INCLUDE_DIRS "${_MOSEK_FUSION_SRC_DIR}") + list(REMOVE_DUPLICATES MOSEK_INCLUDE_DIRS) + endif() + if(MOSEK_FUSION_LIBRARY) + set(MOSEK_LIBRARIES ${MOSEK_FUSION_LIBRARY} ${MOSEK_LIBRARY}) + else() + set(MOSEK_LIBRARIES ${MOSEK_LIBRARY}) + endif() + get_filename_component(MOSEK_LIBRARY_DIR ${MOSEK_LIBRARY} DIRECTORY) + + if(NOT MOSEK_ROOT) + get_filename_component(_MOSEK_INCLUDE_PARENT "${MOSEK_INCLUDE_DIR}" DIRECTORY) + set(MOSEK_ROOT "${_MOSEK_INCLUDE_PARENT}" CACHE PATH + "Path to MOSEK platform directory (contains h/ and bin/)" FORCE) + endif() +endif() + +mark_as_advanced(MOSEK_INCLUDE_DIR MOSEK_LIBRARY MOSEK_FUSION_LIBRARY) diff --git a/cmake/HandleMOSEK.cmake b/cmake/HandleMOSEK.cmake new file mode 100644 index 0000000000..1f27b59b85 --- /dev/null +++ b/cmake/HandleMOSEK.cmake @@ -0,0 +1,46 @@ +############################################################################### +# Handle MOSEK Fusion API for SDP solving. +# MOSEK is OFF by default so normal GTSAM builds do not need MOSEK installed. + +option(GTSAM_WITH_MOSEK "Build with MOSEK SDP solver support" OFF) + +if(GTSAM_WITH_MOSEK) + find_package(MOSEK) + if(MOSEK_FOUND) + message(STATUS "Found MOSEK: ${MOSEK_INCLUDE_DIRS}") + message(STATUS "MOSEK libraries: ${MOSEK_LIBRARIES}") + if(MOSEK_FUSION_LIBRARY) + message(STATUS "MOSEK Fusion backend: prebuilt library") + elseif(MOSEK_FUSION_SOURCES) + message(STATUS "MOSEK Fusion backend: source files") + endif() + + set(MOSEK_LICENSE_FILE "$ENV{MOSEKLM_LICENSE_FILE}" CACHE FILEPATH + "Path to MOSEK license file (mosek.lic)") + if(NOT MOSEK_LICENSE_FILE) + message(WARNING "MOSEK_LICENSE_FILE is not set. Tests will fail unless " + "MOSEKLM_LICENSE_FILE is set in the environment. Pass " + "-DMOSEK_LICENSE_FILE=/path/to/mosek.lic to cmake, or set the " + "MOSEKLM_LICENSE_FILE environment variable.") + elseif(NOT EXISTS "${MOSEK_LICENSE_FILE}") + message(WARNING "MOSEK_LICENSE_FILE does not exist: ${MOSEK_LICENSE_FILE}") + endif() + + set(GTSAM_USE_MOSEK 1) + list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MOSEK_LIBRARIES}) + list(APPEND GTSAM_ADDITIONAL_INCLUDE_DIRS ${MOSEK_INCLUDE_DIRS}) + if(MOSEK_FUSION_SOURCES) + list(APPEND GTSAM_ADDITIONAL_SOURCES ${MOSEK_FUSION_SOURCES}) + endif() + + if(NOT WIN32) + set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_RPATH};${MOSEK_LIBRARY_DIR}") + set(CMAKE_BUILD_RPATH "${CMAKE_BUILD_RPATH};${MOSEK_LIBRARY_DIR}") + endif() + else() + message(FATAL_ERROR "GTSAM_WITH_MOSEK is ON but MOSEK was not found. " + "Please set MOSEK_ROOT to your MOSEK installation path.") + endif() +else() + set(GTSAM_USE_MOSEK 0) +endif() diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index a5a6f19afd..eee9589728 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -91,6 +91,15 @@ foreach(subdir ${gtsam_subdirs}) list(APPEND gtsam_srcs ${${subdir}_srcs}) endforeach(subdir) list(APPEND gtsam_srcs ${gtsam_core_headers}) +if(GTSAM_ADDITIONAL_SOURCES) + list(APPEND gtsam_srcs ${GTSAM_ADDITIONAL_SOURCES}) + gtsam_assign_source_folders("${GTSAM_ADDITIONAL_SOURCES}") + if(MSVC) + set_source_files_properties(${GTSAM_ADDITIONAL_SOURCES} PROPERTIES COMPILE_FLAGS "/w") + else() + set_source_files_properties(${GTSAM_ADDITIONAL_SOURCES} PROPERTIES COMPILE_FLAGS "-w") + endif() +endif() IF(MSVC) # Add precompiled header to sources @@ -137,6 +146,9 @@ message(STATUS "Building GTSAM - as a ${GTSAM_LIBRARY_TYPE} library") add_library(gtsam ${GTSAM_LIBRARY_TYPE} ${gtsam_srcs}) target_link_libraries(gtsam PUBLIC ${GTSAM_BOOST_LIBRARIES}) target_link_libraries(gtsam PUBLIC ${GTSAM_ADDITIONAL_LIBRARIES}) +if(GTSAM_ADDITIONAL_INCLUDE_DIRS) + target_include_directories(gtsam SYSTEM PRIVATE ${GTSAM_ADDITIONAL_INCLUDE_DIRS}) +endif() # Apply build flags: gtsam_apply_build_flags(gtsam) diff --git a/gtsam/certifiable/CMakeLists.txt b/gtsam/certifiable/CMakeLists.txt new file mode 100644 index 0000000000..20772241bb --- /dev/null +++ b/gtsam/certifiable/CMakeLists.txt @@ -0,0 +1,44 @@ +# Examples for certifiable optimization experiments. These targets are kept +# outside the core gtsam library so SDP prototyping does not change libgtsam. + +add_library(gtsam_certifiable STATIC LiftedSDPProblem.cpp LiftedSDPProblem.h) +target_link_libraries(gtsam_certifiable PUBLIC gtsam) +gtsam_apply_build_flags(gtsam_certifiable) +set_property(TARGET gtsam_certifiable PROPERTY FOLDER "certifiable") + +if(GTSAM_BUILD_TESTS) + add_executable(testLiftedSDPs tests/testLiftedSDPs.cpp) + target_link_libraries(testLiftedSDPs CppUnitLite gtsam) + gtsam_apply_build_flags(testLiftedSDPs) + add_test(NAME testLiftedSDPs COMMAND testLiftedSDPs) + set_property(TARGET testLiftedSDPs PROPERTY FOLDER "tests") +endif() + +if(GTSAM_USE_MOSEK) + target_link_libraries(gtsam_certifiable PRIVATE ${MOSEK_LIBRARIES}) + target_include_directories(gtsam_certifiable SYSTEM PRIVATE ${MOSEK_INCLUDE_DIRS}) + + add_executable(Rot2RingQcqpToMonolithicSDP examples/Rot2RingQcqpToMonolithicSDP.cpp) + target_link_libraries(Rot2RingQcqpToMonolithicSDP gtsam_certifiable) + gtsam_apply_build_flags(Rot2RingQcqpToMonolithicSDP) + add_dependencies(examples Rot2RingQcqpToMonolithicSDP) + set_property(TARGET Rot2RingQcqpToMonolithicSDP PROPERTY FOLDER "examples") + + if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS) + set_target_properties(Rot2RingQcqpToMonolithicSDP PROPERTIES EXCLUDE_FROM_ALL ON) + endif() + + add_executable(SimpleMosekSDPExample tests/testMOSEKInstall.cpp) + target_link_libraries(SimpleMosekSDPExample gtsam ${MOSEK_LIBRARIES}) + target_include_directories(SimpleMosekSDPExample SYSTEM PRIVATE ${MOSEK_INCLUDE_DIRS}) + set_target_properties(SimpleMosekSDPExample PROPERTIES + BUILD_RPATH "${MOSEK_LIBRARY_DIR}" + INSTALL_RPATH "${MOSEK_LIBRARY_DIR}") + gtsam_apply_build_flags(SimpleMosekSDPExample) + add_dependencies(examples SimpleMosekSDPExample) + set_property(TARGET SimpleMosekSDPExample PROPERTY FOLDER "examples") + + if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS) + set_target_properties(SimpleMosekSDPExample PROPERTIES EXCLUDE_FROM_ALL ON) + endif() +endif() diff --git a/gtsam/certifiable/LiftedSDPProblem.cpp b/gtsam/certifiable/LiftedSDPProblem.cpp new file mode 100644 index 0000000000..58c163b6bf --- /dev/null +++ b/gtsam/certifiable/LiftedSDPProblem.cpp @@ -0,0 +1,337 @@ +#include + +#include +#include +#include + +#ifdef GTSAM_USE_MOSEK +#include +#include + +namespace mf = mosek::fusion; +#endif + +namespace gtsam { + +#ifdef GTSAM_USE_MOSEK + +// Put MOSEK only functions in anonymous namespace. +namespace { + +using LiftedVariableXijToSDPVariableViewMap = std::map, mf::Variable::t>; + +std::map CollectQpCostKeyDims(const QcqpProblem& problem, + KeySet* costKeys) { + std::map keyDims; + for (const auto& factor : problem.costs()) { + if (!factor) { + continue; + } + + const auto* cost = dynamic_cast(factor.get()); + if (!cost) { + throw std::runtime_error( + "CollectQpCostKeyDims: expected objective factors to be QpCost."); + } + + const HessianFactor& H = cost->hessianFactor(); + for (auto it = H.begin(); it != H.end(); ++it) { + const Key key = *it; + if (costKeys) { + costKeys->insert(key); + } + const DenseIndex dim = H.getDim(it); + const auto [entry, inserted] = keyDims.emplace(key, dim); + if (!inserted && entry->second != dim) { + throw std::runtime_error( + "CollectQpCostKeyDims: inconsistent QpCost dimension for key."); + } + } + } + return keyDims; +} + +void DisposeMosekModel(const mf::Model::t& M) { + if (M.get() != nullptr) { + M->dispose(); + } +} + +// Convert Eigen matrix to a Fusion-compatible numeric buffer with one +// allocation and one pass. The buffer is filled in row-major order. +// In principle, this is a performant way of doing things. +// This returns a 1D array buffer that is the row-major form of mat. +std::shared_ptr> convertToMOSEKArray2D( + const Matrix& mat) { + const int rows = static_cast(mat.rows()); + const int cols = static_cast(mat.cols()); + auto buffer = monty::new_array_ptr(monty::shape(rows * cols)); + + // We use Eigen's type conversion which should be performant. + using RowMajorMat = Eigen::Matrix; + // Eigen overloads the operator= to perform element-wise assignment into the memory referenced by view. + Eigen::Map view(buffer->raw(), rows, cols); + view = mat; + + return buffer; +} + +mf::Matrix::t convertToMosekDenseMatrix(const Matrix& mat) { + return mf::Matrix::dense(static_cast(mat.rows()), + static_cast(mat.cols()), + convertToMOSEKArray2D(mat)); +} + +mf::Matrix::t convertToMosekDenseMatrix(const Vector& vec) { + Matrix mat(vec.size(), 1); + mat.col(0) = vec; + return convertToMosekDenseMatrix(mat); +} + +mf::Expression::t BuildQpCostObjectiveTerm( + const QpCost& cost, + const LiftedVariableXijToSDPVariableViewMap& xijMap) { + const HessianFactor& H = cost.hessianFactor(); + if (H.linearTerm().norm() > 0.0 || H.constantTerm() != 0.0) { + throw std::runtime_error( + "BuildQpCostObjectiveTerm: linear/constant QpCost terms are not " + "supported yet."); + } + + // Assemble the local SDP block matrix X_f in the Hessian factor's key order. + std::vector blockRows; + for (Key key_i : H.keys()) { + std::vector rowBlocks; + for (Key key_j : H.keys()) { + rowBlocks.push_back(xijMap.at({key_i, key_j})->asExpr()); + } + blockRows.push_back( + mf::Expr::hstack(monty::new_array_ptr(rowBlocks))); + } + + const auto X_f = + mf::Expr::vstack(monty::new_array_ptr(blockRows)); + const Matrix Q_f = H.information(); + return mf::Expr::dot(convertToMosekDenseMatrix(Q_f), X_f); +} + +mf::Expression::t BuildObjective( + const QcqpProblem& problem, + const LiftedVariableXijToSDPVariableViewMap& xijMap) { + std::vector objectiveTerms; + + for (const auto& factor : problem.costs()) { + if (!factor) { + continue; + } + + const auto* cost = dynamic_cast(factor.get()); + if (!cost) { + throw std::runtime_error("BuildObjective: expected QpCost."); + } + + objectiveTerms.push_back(BuildQpCostObjectiveTerm(*cost, xijMap)); + } + + if (objectiveTerms.empty()) { + return mf::Expr::constTerm(0.0); + } + return mf::Expr::add(monty::new_array_ptr(objectiveTerms)); +} + +void AddQuadraticConstraint( + const mf::Model::t& M, const QuadraticConstraint& constraint, + const LiftedVariableXijToSDPVariableViewMap& xijMap) { + const Key key = constraint.key(); + // Lower trace(X_i' A X_i) ~ b to the affine SDP constraint ~ b. + const auto Xii = xijMap.at({key, key})->asExpr(); + const auto lhs = + mf::Expr::dot(convertToMosekDenseMatrix(constraint.A()), Xii); + + switch (constraint.sense()) { + case QuadraticConstraint::Sense::Equal: + M->constraint(lhs, mf::Domain::equalsTo(constraint.b())); + break; + case QuadraticConstraint::Sense::LessEqual: + M->constraint(lhs, mf::Domain::lessThan(constraint.b())); + break; + case QuadraticConstraint::Sense::GreaterEqual: + M->constraint(lhs, mf::Domain::greaterThan(constraint.b())); + break; + } +} + +void AddLinearEqualityConstraint( + const mf::Model::t& M, const LinearConstraint& constraint, + const LiftedVariableXijToSDPVariableViewMap& xijMap) { + const JacobianFactor& J = constraint.factor(); + if (constraint.sense() == LinearConstraint::Sense::Equal && J.size() == 1) { + auto it = J.begin(); + const Key key = *it; + const auto Xii = xijMap.at({key, key}); + const DenseIndex dim = J.getDim(it); + + auto first = monty::new_array_ptr({0, 0}); + auto last = + monty::new_array_ptr({static_cast(dim), 1}); + const auto xi = Xii->slice(first, last)->asExpr(); + const Matrix A = J.getA(it); + const Vector b = J.getb(); + const auto lhs = mf::Expr::mul(convertToMosekDenseMatrix(A), xi); + M->constraint(lhs, mf::Domain::equalsTo(convertToMosekDenseMatrix(b))); + } else { + throw std::runtime_error( + "MonolithicSDP: only unary linear equality QCQP constraints are " + "supported."); + } +} + +} // namespace + +struct LiftedSDPProblem::Impl { + mf::Model::t M; + KeyVector orderedKeys; + std::map orderedKeyDims; + std::map> orderedKeyToYSlice; + DenseIndex totalMonolithicDimension; + LiftedVariableXijToSDPVariableViewMap liftedVariableXijToSDPVariableViewMap; + + ~Impl() { + liftedVariableXijToSDPVariableViewMap.clear(); + DisposeMosekModel(M); + } + + void collectOrderedKeysAndDims(const QcqpProblem& problem) { + KeySet costKeys; + orderedKeyDims = CollectQpCostKeyDims(problem, &costKeys); + + const KeySet eqKeys = problem.eConstraints().keys(); + const KeySet ineqKeys = problem.iConstraints().keys(); + + KeySet constraintKeys = eqKeys; + constraintKeys.merge(ineqKeys); + + // TODO: Does this always hold? + if (costKeys != constraintKeys) { + throw std::runtime_error( + "MonolithicSDP: QCQP constraint keys do not match objective cost keys."); + } + + orderedKeys.assign(costKeys.begin(), costKeys.end()); + } + + void computeMonolithicLayout() { + DenseIndex cumulativeIndex = 0; + orderedKeyToYSlice.clear(); + for (Key key : orderedKeys) { + const DenseIndex start = cumulativeIndex; + const DenseIndex end = start + orderedKeyDims.at(key); + orderedKeyToYSlice[key] = {start, end}; + std::cout << DefaultKeyFormatter(key) << " Y slice [" << start << ", " + << end << ")" << std::endl; + cumulativeIndex = end; + } + totalMonolithicDimension = cumulativeIndex; + } + + void populateXijMap(const mf::Variable::t& Y) { + liftedVariableXijToSDPVariableViewMap.clear(); + + for (Key key_i : orderedKeys) { + for (Key key_j : orderedKeys) { + const auto [i_start, i_end] = orderedKeyToYSlice.at(key_i); + const auto [j_start, j_end] = orderedKeyToYSlice.at(key_j); + + auto first = monty::new_array_ptr( + {static_cast(i_start), static_cast(j_start)}); + auto last = monty::new_array_ptr( + {static_cast(i_end), static_cast(j_end)}); + + liftedVariableXijToSDPVariableViewMap.emplace( + std::make_pair(key_i, key_j), Y->slice(first, last)); + } + } + } +}; + +LiftedSDPProblem::LiftedSDPProblem( + const QcqpProblem& problem): impl_(std::make_unique()) { + + impl_->collectOrderedKeysAndDims(problem); + impl_->computeMonolithicLayout(); + + impl_->M = new mf::Model("MonolithicSDP_MosekSDPSolver"); + auto Y = impl_->M->variable( + "Y", + mf::Domain::inPSDCone( + static_cast(impl_->totalMonolithicDimension))); + impl_->populateXijMap(Y); + + const auto objective = BuildObjective(problem, impl_->liftedVariableXijToSDPVariableViewMap); + impl_->M->objective(mf::ObjectiveSense::Minimize, + mf::Expr::mul(0.5, objective)); + + // Process QCQP equality constraints: + for (const auto& factor : problem.eConstraints()) { + if (!factor) { + continue; + } + + const auto* quadratic = + dynamic_cast(factor.get()); + if (quadratic) { + AddQuadraticConstraint(impl_->M, quadratic->quadraticConstraint(), + impl_->liftedVariableXijToSDPVariableViewMap); + continue; + } + + const auto* linear = + dynamic_cast(factor.get()); + if (linear) { + AddLinearEqualityConstraint(impl_->M, linear->linearConstraint(), + impl_->liftedVariableXijToSDPVariableViewMap); + continue; + } + + throw std::runtime_error( + "MonolithicSDP: expected quadratic or linear equality constraints."); + } + + // Process QCQP Inequality constraints: + for (const auto& factor : problem.iConstraints()) { + if (!factor) { + continue; + } + + const auto* quadratic = + dynamic_cast(factor.get()); + if (quadratic) { + AddQuadraticConstraint(impl_->M, quadratic->quadraticConstraint(), + impl_->liftedVariableXijToSDPVariableViewMap); + continue; + } + + if (dynamic_cast(factor.get())) { + throw std::runtime_error( + "MonolithicSDP: linear inequality constraints are not supported."); + } + + throw std::runtime_error( + "MonolithicSDP: expected quadratic inequality constraints."); + } +} + +LiftedSDPProblem::~LiftedSDPProblem() = default; + +const KeyVector& +LiftedSDPProblem::orderedKeys() const { + return impl_->orderedKeys; +} + +const std::map& +LiftedSDPProblem::orderedKeyDims() const { + return impl_->orderedKeyDims; +} +#endif + +} // namespace gtsam diff --git a/gtsam/certifiable/LiftedSDPProblem.h b/gtsam/certifiable/LiftedSDPProblem.h new file mode 100644 index 0000000000..7e8f3dd86b --- /dev/null +++ b/gtsam/certifiable/LiftedSDPProblem.h @@ -0,0 +1,38 @@ +#pragma once + +#include +#include +#include + +#include +#include + +namespace gtsam { + +struct MonolithicSDP {}; + +struct MosekSDPSolver {}; + +template +class LiftedSDPProblem; + +#ifdef GTSAM_USE_MOSEK +template <> +class LiftedSDPProblem { + public: + + explicit LiftedSDPProblem(const QcqpProblem& problem); + + ~LiftedSDPProblem(); + + const KeyVector& orderedKeys() const; + + const std::map& orderedKeyDims() const; + + private: + struct Impl; + std::unique_ptr impl_; +}; +#endif + +} // namespace gtsam diff --git a/gtsam/certifiable/examples/Rot2RingQcqpToMonolithicSDP.cpp b/gtsam/certifiable/examples/Rot2RingQcqpToMonolithicSDP.cpp new file mode 100644 index 0000000000..6030d54a34 --- /dev/null +++ b/gtsam/certifiable/examples/Rot2RingQcqpToMonolithicSDP.cpp @@ -0,0 +1,119 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Rot2RingQcqpExample.cpp + * @brief Minimal Rot2 ring SLAM construction ending at a QcqpProblem. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace gtsam; + +namespace { + +constexpr double kPi = 3.141592653589793238462643383279502884; + +/** + * Build the small Rot2 ring used by the QCQP tests. This intentionally uses the + * existing Frobenius factor conversion path, then stops once QcqpProblem exists. + */ +NonlinearFactorGraph MakeRot2RingGraph(size_t numPoses, double delta) { + NonlinearFactorGraph graph; + const auto hardPriorNoise = noiseModel::Constrained::All(4); + graph.emplace_shared>( + Symbol('x', 0), Rot2::Identity().matrix(), hardPriorNoise); + + for (size_t i = 0; i < numPoses; ++i) { + graph.emplace_shared>( + Symbol('x', i), Symbol('x', (i + 1) % numPoses), + Rot2::fromAngle(delta)); + } + return graph; +} + +/** + * Create feasible QCQP matrix values for reporting the constructed problem. + * SDP construction should consume the QcqpProblem itself, not this graph. + */ +Values MakeRot2RingQcqpValues(size_t numPoses, double delta) { + Values values; + for (size_t i = 0; i < numPoses; ++i) { + InsertQcqpValue(Symbol('x', i), Rot2::fromAngle(i * delta), + &values); + } + return values; +} + +void PrintKeys(const std::string& label, const KeySet& keys) { + std::cout << label << ":"; + for (Key key : keys) { + std::cout << " " << DefaultKeyFormatter(key); + } + std::cout << std::endl; +} + +} // namespace + +int main() { + constexpr size_t numPoses = 5; + const double delta = 2.0 * kPi / static_cast(numPoses); + + const NonlinearFactorGraph graph = MakeRot2RingGraph(numPoses, delta); + const QcqpProblem problem(graph); + const Values qcqpValues = MakeRot2RingQcqpValues(numPoses, delta); + + size_t quadraticEqualities = 0; + size_t linearEqualities = 0; + for (const auto& factor : problem.eConstraints()) { + if (dynamic_cast(factor.get())) { + ++quadraticEqualities; + } else if (dynamic_cast( + factor.get())) { + ++linearEqualities; + } + } + + std::cout << "Constructed Rot2 ring QCQP with hard Frobenius prior" + << std::endl; + std::cout << "poses: " << numPoses << std::endl; + std::cout << "cost factors: " << problem.costs().size() << std::endl; + std::cout << "equality constraints: " << problem.eConstraints().size() + << std::endl; + std::cout << "quadratic equality constraints: " << quadraticEqualities + << std::endl; + std::cout << "linear equality constraints: " << linearEqualities << std::endl; + std::cout << "cost at feasible ring values: " + << problem.costs().error(qcqpValues) << std::endl; + //std::cout << "equality violation at feasible ring values: " + // << problem.eConstraints().violationNorm(qcqpValues) << std::endl; + PrintKeys("cost keys", problem.costs().keys()); + PrintKeys("equality constraint keys", problem.eConstraints().keys()); + PrintKeys("inequality constraint keys", problem.iConstraints().keys()); + + const LiftedSDPProblem sdp(problem); + std::cout << "ordered key dims:"; + for (Key key : sdp.orderedKeys()) { + std::cout << " " << DefaultKeyFormatter(key) << "=" + << sdp.orderedKeyDims().at(key); + } + std::cout << std::endl; + return 0; +} diff --git a/gtsam/certifiable/tests/testLiftedSDPs.cpp b/gtsam/certifiable/tests/testLiftedSDPs.cpp new file mode 100644 index 0000000000..c7f29f9db6 --- /dev/null +++ b/gtsam/certifiable/tests/testLiftedSDPs.cpp @@ -0,0 +1,134 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testLiftedSDPs.cpp + * @brief Tests for QCQP-backed lifted SDP objective assembly. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace gtsam; + +namespace { + +constexpr double kPi = 3.141592653589793238462643383279502884; + +NonlinearFactorGraph Rot2RingGraph(size_t numPoses, double delta) { + NonlinearFactorGraph graph; + for (size_t i = 0; i < numPoses; ++i) { + graph.emplace_shared>( + Symbol('x', i), Symbol('x', (i + 1) % numPoses), + Rot2::fromAngle(delta)); + } + return graph; +} + +Values Rot2RingQcqpValues(size_t numPoses, double delta, double perturbation) { + Values values; + for (size_t i = 0; i < numPoses; ++i) { + InsertQcqpValue( + Symbol('x', i), + Rot2::fromAngle(i * delta + perturbation * static_cast(i)), + &values); + } + return values; +} + +Matrix BuildLocalX(const HessianFactor& H, const Values& values) { + std::vector localValues; + DenseIndex totalDim = 0; + for (auto it = H.begin(); it != H.end(); ++it) { + Vector x = values.at(*it).col(0); + if (x.size() != H.getDim(it)) { + throw std::runtime_error( + "BuildLocalX: QCQP value dimension mismatch."); + } + totalDim += x.size(); + localValues.push_back(std::move(x)); + } + + Matrix X_f = Matrix::Zero(totalDim, totalDim); + DenseIndex rowStart = 0; + for (const Vector& x_i : localValues) { + DenseIndex colStart = 0; + for (const Vector& x_j : localValues) { + X_f.block(rowStart, colStart, x_i.size(), x_j.size()) = + x_i * x_j.transpose(); + colStart += x_j.size(); + } + rowStart += x_i.size(); + } + return X_f; +} + +double ComputeLiftedObjective(const QcqpProblem& problem, const Values& values) { + double objective = 0.0; + for (const auto& factor : problem.costs()) { + if (!factor) { + continue; + } + + const auto* cost = dynamic_cast(factor.get()); + if (!cost) { + throw std::runtime_error("ComputeLiftedObjective: expected QpCost."); + } + + const HessianFactor& H = cost->hessianFactor(); + if (H.linearTerm().norm() > 0.0 || H.constantTerm() != 0.0) { + throw std::runtime_error( + "ComputeLiftedObjective: linear/constant terms are not supported."); + } + + const Matrix Q_f = H.information(); + const Matrix X_f = BuildLocalX(H, values); + objective += 0.5 * Q_f.cwiseProduct(X_f).sum(); + } + return objective; +} + +} // namespace + +/* ************************************************************************* */ +TEST(LiftedSDPs, Rot2_QcqpObjectiveMatchesLiftedObjective) { + constexpr size_t N = 5; + const double delta = 2.0 * kPi / static_cast(N); + constexpr double perturbation = 0.03; + + const NonlinearFactorGraph graph = Rot2RingGraph(N, delta); + const QcqpProblem problem(graph); + const Values qcqpValues = Rot2RingQcqpValues(N, delta, perturbation); + + const double qcqpObjective = problem.costs().error(qcqpValues); + const double sdpObjective = ComputeLiftedObjective(problem, qcqpValues); + + std::cout << "qcqpObjective: " << qcqpObjective << std::endl; + std::cout << "sdpObjective: " << sdpObjective << std::endl; + + EXPECT_DOUBLES_EQUAL(qcqpObjective, sdpObjective, 1e-12); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} diff --git a/gtsam/certifiable/tests/testMOSEKInstall.cpp b/gtsam/certifiable/tests/testMOSEKInstall.cpp new file mode 100644 index 0000000000..4201fa3d31 --- /dev/null +++ b/gtsam/certifiable/tests/testMOSEKInstall.cpp @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testMOSEKInstall.cpp + * @brief This tests if MOSEK is installed and running correctly. + */ + +#include + +#include +#include + +using namespace mosek::fusion; +using namespace monty; + +namespace { + +struct SimpleSdpSolution { + double x00 = 0.0; + double x10 = 0.0; + double x01 = 0.0; + double x11 = 0.0; +}; + +/** + * Solve minimize trace(X) subject to X in S_+^2 and X(0,0)=1. The optimum is + * X = [1 0; 0 0], which is enough to verify MOSEK Fusion links and runs. + */ +SimpleSdpSolution SolveSimpleSdp() { + Model::t model = new Model("SimpleSDP"); + auto cleanup = finally([&]() { model->dispose(); }); + + auto X = model->variable("X", Domain::inPSDCone(2)); + model->constraint(X->index(0, 0), Domain::equalsTo(1.0)); + model->objective(ObjectiveSense::Minimize, + Expr::add(X->index(0, 0), X->index(1, 1))); + model->solve(); + + auto level = X->level(); + return {(*level)[0], (*level)[1], (*level)[2], (*level)[3]}; +} + +} // namespace + +int main() { + const SimpleSdpSolution solution = SolveSimpleSdp(); + const double trace = solution.x00 + solution.x11; + + std::cout << "Simple MOSEK SDP solution" << std::endl; + std::cout << "X = [[" << solution.x00 << ", " << solution.x01 << "], [" + << solution.x10 << ", " << solution.x11 << "]]" << std::endl; + std::cout << "trace(X) = " << trace << std::endl; + + constexpr double tol = 1e-6; + if (std::fabs(solution.x00 - 1.0) > tol || + std::fabs(solution.x10) > tol || std::fabs(solution.x01) > tol || + std::fabs(solution.x11) > tol || std::fabs(trace - 1.0) > tol) { + return 1; + } + return 0; +} diff --git a/gtsam/config.h.in b/gtsam/config.h.in index 06033aaf42..1cdae4952e 100644 --- a/gtsam/config.h.in +++ b/gtsam/config.h.in @@ -59,6 +59,9 @@ // Whether to use bounded memory growth for TBB parallel tree traversal #cmakedefine GTSAM_TBB_BOUNDED_MEMORY_GROWTH_FLAG +// Whether MOSEK SDP solver support is enabled +#cmakedefine GTSAM_USE_MOSEK + // Whether we are using system-Eigen or our own patched version #cmakedefine GTSAM_USE_SYSTEM_EIGEN diff --git a/gtsam/constrained/tests/testQcqpProblem.cpp b/gtsam/constrained/tests/testQcqpProblem.cpp index 745e99f751..8714abafb3 100644 --- a/gtsam/constrained/tests/testQcqpProblem.cpp +++ b/gtsam/constrained/tests/testQcqpProblem.cpp @@ -378,6 +378,45 @@ TEST(QcqpProblem, SingleFrobeniusBetweenFactor) { 1e-12); } +// Verifies hard Rot2 Frobenius priors become D=1 lifted linear equalities. +TEST(QcqpProblem, HardFrobeniusPriorRot2D1) { + const Rot2 measured = Rot2::fromAngle(0.25); + const auto hardNoise = noiseModel::Constrained::All(4); + + NonlinearFactorGraph graph; + graph.emplace_shared>(x0, measured.matrix(), hardNoise); + + const QcqpProblem problem(graph); + + bool foundLinearEquality = false; + for (const auto& factor : problem.eConstraints()) { + if (dynamic_cast(factor.get())) { + foundLinearEquality = true; + break; + } + } + + LONGS_EQUAL(0, problem.costs().size()); + EXPECT(foundLinearEquality); + + Values qcqpValues; + InsertQcqpValue(x0, measured, &qcqpValues); + EXPECT_DOUBLES_EQUAL(0.0, problem.eConstraints().violationNorm(qcqpValues), + 1e-12); +} + +// Verifies the deferred non-constrained Frobenius prior cost path rejects. +TEST(QcqpProblem, FrobeniusPriorRot2D1GaussianRejected) { + const Rot2 measured = Rot2::fromAngle(0.25); + const auto gaussianNoise = noiseModel::Isotropic::Sigma(4, 0.1); + + NonlinearFactorGraph graph; + graph.emplace_shared>(x0, measured.matrix(), + gaussianNoise); + + CHECK_EXCEPTION({ QcqpProblem problem(graph); }, std::runtime_error); +} + } // namespace QcqpSingleFactorFixture /* ************************************************************************* */ namespace QcqpRingFixture { diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h index 6876ce0fa5..277ecc0ec3 100644 --- a/gtsam/geometry/Rot2.h +++ b/gtsam/geometry/Rot2.h @@ -256,7 +256,8 @@ struct traits : public internal::MatrixLieGroup { /** * Return a matrix-valued QCQP variable for Rot2. * - * D=1 is vectorized SO(2) as a 4-by-1 matrix. + * D=1 prepends a fixed homogenization coordinate to the existing + * column-major SO(2) vectorization, yielding a 5-by-1 matrix. * D=2 returns R' as a 2-by-2 row-orthonormal matrix. * D>=3 returns [R', 0] as a 2-by-D row-orthonormal matrix. */ @@ -264,7 +265,10 @@ struct traits : public internal::MatrixLieGroup { static Matrix QcqpValue(const Rot2& value) { if constexpr (D == 1) { const Matrix2 R = value.matrix(); - return Eigen::Map(R.data(), 4, 1); + Matrix X(5, 1); + X(0, 0) = 1.0; // Homogenization entry + X.bottomRows(4) = Eigen::Map(R.data(), 4, 1); + return X; } else if constexpr (D == 2) { return value.matrix().transpose(); } else if constexpr (D >= 3) { @@ -279,39 +283,49 @@ struct traits : public internal::MatrixLieGroup { /** * Return row-space QCQP equality constraints A, b such that - * trace(X' A X) = b. For D=1 these are the vec(R) SO(2) constraints, - * including orientation. For D>=2 the same 2-by-2 constraints enforce + * trace(x_i' A x_i) = b[j]. For D=1 these are the lifted SO(2) constraints in + * column-major coordinates. For D>=2 the same 2-by-2 constraints enforce * row orthonormality. */ template static std::vector> QcqpConstraints() { if constexpr (D == 1) { + // The homogenized Rot2 lifted vector is + // x = [1, r00, r10, r01, r11]. std::vector> constraints; - constraints.reserve(4); + constraints.reserve(5); - Matrix A = Matrix::Zero(4, 4); + Matrix A = Matrix::Zero(5, 5); + // Fix the leading-coordinate convention x(0) = 1. A(0, 0) = 1.0; - A(1, 1) = 1.0; constraints.emplace_back(A, 1.0); + // det(R) = r00*r11 - r10*r01 = 1. A.setZero(); - A(2, 2) = 1.0; + A(1, 4) = 0.5; + A(4, 1) = 0.5; + A(2, 3) = -0.5; + A(3, 2) = -0.5; + constraints.emplace_back(A, 1.0); + + // RR^T = I supplies the default, non-redundant row-orthonormality + // Note the reuse of the A variable for multiple constraints. + A.setZero(); + A(1, 1) = 1.0; A(3, 3) = 1.0; constraints.emplace_back(A, 1.0); A.setZero(); - A(0, 2) = 0.5; - A(2, 0) = 0.5; - A(1, 3) = 0.5; - A(3, 1) = 0.5; + A(1, 2) = 0.5; + A(2, 1) = 0.5; + A(3, 4) = 0.5; + A(4, 3) = 0.5; constraints.emplace_back(A, 0.0); A.setZero(); - A(0, 3) = 0.5; - A(3, 0) = 0.5; - A(1, 2) = -0.5; - A(2, 1) = -0.5; + A(2, 2) = 1.0; + A(4, 4) = 1.0; constraints.emplace_back(A, 1.0); return constraints; diff --git a/gtsam/inference/tests/CMakeLists.txt b/gtsam/inference/tests/CMakeLists.txt index aaa01d7750..35cf44de0f 100644 --- a/gtsam/inference/tests/CMakeLists.txt +++ b/gtsam/inference/tests/CMakeLists.txt @@ -1 +1,22 @@ -gtsamAddTestsGlob(inference "test*.cpp" "" "gtsam") +# Add standard inference tests. The MOSEK smoke test is gated separately +# because MOSEK is an optional dependency and requires a license to solve. +gtsamAddTestsGlob(inference "test*.cpp" "testMosekSDP.cpp" "gtsam") + +if(GTSAM_BUILD_TESTS AND GTSAM_USE_MOSEK) + add_executable(testMosekSDP testMosekSDP.cpp) + target_link_libraries(testMosekSDP CppUnitLite gtsam ${MOSEK_LIBRARIES}) + target_include_directories(testMosekSDP SYSTEM PRIVATE ${MOSEK_INCLUDE_DIRS}) + set_target_properties(testMosekSDP PROPERTIES + BUILD_RPATH "${MOSEK_LIBRARY_DIR}" + INSTALL_RPATH "${MOSEK_LIBRARY_DIR}") + + gtsam_apply_build_flags(testMosekSDP) + add_test(NAME testMosekSDP COMMAND testMosekSDP) + if(MOSEK_LICENSE_FILE) + set_tests_properties(testMosekSDP PROPERTIES + ENVIRONMENT "MOSEKLM_LICENSE_FILE=${MOSEK_LICENSE_FILE}") + endif() + add_dependencies(check.inference testMosekSDP) + add_dependencies(check testMosekSDP) + add_dependencies(all.tests testMosekSDP) +endif() diff --git a/gtsam/inference/tests/testMosekSDP.cpp b/gtsam/inference/tests/testMosekSDP.cpp new file mode 100644 index 0000000000..6e286c30ce --- /dev/null +++ b/gtsam/inference/tests/testMosekSDP.cpp @@ -0,0 +1,63 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testMosekSDP.cpp + * @brief Minimal MOSEK Fusion SDP smoke test. + */ + +#include +#include + +#include + +using namespace mosek::fusion; +using namespace monty; + +/* ************************************************************************* */ +// Solves minimize trace(X) subject to X in S_+^2 and X(0,0)=1. This tests only +// optional MOSEK wiring; it is intentionally independent of lifted QCQP SDP code. +TEST(MosekSDP, SimplePSD) { + Model::t model = new Model("SimpleSDP"); + auto cleanup = finally([&]() { model->dispose(); }); + + auto X = model->variable("X", Domain::inPSDCone(2)); + model->constraint(X->index(0, 0), Domain::equalsTo(1.0)); + model->objective(ObjectiveSense::Minimize, + Expr::add(X->index(0, 0), X->index(1, 1))); + model->solve(); + + std::cout << "testMosekSDP problem status: " << model->getProblemStatus() + << std::endl; + std::cout << "testMosekSDP primal solution status: " + << model->getPrimalSolutionStatus() << std::endl; + std::cout << "testMosekSDP dual solution status: " + << model->getDualSolutionStatus() << std::endl; + + auto level = X->level(); + const double X00 = (*level)[0]; + const double X10 = (*level)[1]; + const double X01 = (*level)[2]; + const double X11 = (*level)[3]; + + EXPECT_DOUBLES_EQUAL(1.0, X00, 1e-6); + EXPECT_DOUBLES_EQUAL(0.0, X10, 1e-6); + EXPECT_DOUBLES_EQUAL(0.0, X01, 1e-6); + EXPECT_DOUBLES_EQUAL(0.0, X11, 1e-6); + EXPECT_DOUBLES_EQUAL(1.0, X00 + X11, 1e-6); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/slam/FrobeniusFactor.h b/gtsam/slam/FrobeniusFactor.h index 31baee69b4..0474406c4e 100644 --- a/gtsam/slam/FrobeniusFactor.h +++ b/gtsam/slam/FrobeniusFactor.h @@ -27,6 +27,7 @@ #include #include +#include namespace gtsam { @@ -110,6 +111,75 @@ class FrobeniusPrior : public NoiseModelFactorN { return traits::Vec(g, H) - vecM_; // Jacobian is computed only when needed. } + + /** Add this Frobenius prior to the QCQP graph. + * D=1 uses the lifted vector form; higher column dimensions are kept as a + * separate dispatch point for the future matrix-form prior path. */ + void qcqpFactors(NonlinearFactorGraph* costs, + NonlinearEqualityConstraints* constraints, + size_t columnDimension = 1) const override { + if (columnDimension == 0) { + throw std::invalid_argument( + "FrobeniusPrior::qcqpFactors: columnDimension must be >= 1"); + } + if (columnDimension == 1) { + qcqpFactorsForVec(costs, constraints); + } else { + qcqpFactorsForMatrix(costs, constraints, columnDimension); + } + } + + private: + /// D=1 constrained-noise prior in lifted vector form. + /// The stored measurement is vecM_ = vec(M), where M is the matrix passed to + /// the FrobeniusPrior constructor. The lifted variable for the current value + /// is x = [1, vec(R)]^T, where R is the matrix represented by this key. The + /// matrix B = [-vecM_, I] therefore gives B*x = vec(R) - vec(M). A hard prior + /// asks for that residual to be zero, so we add the linear equality B*x = 0. + void qcqpFactorsForVec(NonlinearFactorGraph* costs, + NonlinearEqualityConstraints* constraints) const { + if constexpr (!internal::HasQcqpVariableTraits::value) { + (void)costs; + (void)constraints; + throw std::runtime_error( + "FrobeniusPrior::qcqpFactors requires QCQP variable traits for this " + "type and column dimension 1."); + } else { + (void)costs; + if (this->noiseModel_->isConstrained()) { + InsertQcqpConstraints(this->key(), constraints); + + constexpr int AmbientDim = N * N; + constexpr int LiftedDim = AmbientDim + 1; + Matrix B = Matrix::Zero(AmbientDim, LiftedDim); + B.col(0) = -vecM_; + B.block(0, 1, AmbientDim, AmbientDim) = + Matrix::Identity(AmbientDim, AmbientDim); + + constraints->push_back(LinearConstraint::Equal( + JacobianFactor(this->key(), B, + Vector::Zero(AmbientDim))) + .createEqualityFactor()); + } else { + throw std::runtime_error( + "FrobeniusPrior::qcqpFactors D=1 non-constrained noise is not yet " + "implemented."); + } + } + } + + /// Matrix-form Frobenius priors are deliberately separate from the D=1 + /// vector path; no QCQP lowering is implemented for this branch yet. + void qcqpFactorsForMatrix(NonlinearFactorGraph* costs, + NonlinearEqualityConstraints* constraints, + size_t columnDimension) const { + (void)costs; + (void)constraints; + (void)columnDimension; + throw std::runtime_error( + "FrobeniusPrior::qcqpFactors with column dimension > 1 is not yet " + "implemented."); + } }; /** @@ -297,8 +367,8 @@ class FrobeniusBetweenFactor : public FrobeniusBetweenFactorNL { return result; } - /// Vec(R) form (K=1): variable is (N*N)x1; accepts any non-robust - /// Gaussian noise model. + /// Vec(R) form (K=1): build the full (N*N)x1 vec(R) cost first, then + /// embed its quadratic matrix in the lifted coordinate layout. void qcqpFactorsForVec(NonlinearFactorGraph* costs, NonlinearEqualityConstraints* constraints) const { if constexpr (!internal::HasQcqpVariableTraits::value) { @@ -319,9 +389,6 @@ class FrobeniusBetweenFactor : public FrobeniusBetweenFactorNL { "non-robust quadratic noise model"); } - InsertQcqpConstraints(this->key1(), constraints); - InsertQcqpConstraints(this->key2(), constraints); - constexpr int AmbientDim = N * N; const Matrix measurement = this->T12_.matrix(); const Matrix A = RightProductMatrix(measurement); @@ -333,10 +400,31 @@ class FrobeniusBetweenFactor : public FrobeniusBetweenFactorNL { const Matrix whitenedB = this->noiseModel_->Whiten(B); const Matrix Q = whitenedB.transpose() * whitenedB; - const SymmetricBlockMatrix blockQ( - std::vector{AmbientDim, AmbientDim}, Q); - costs->push_back(std::make_shared( - KeyVector{this->key1(), this->key2()}, blockQ)); + + // From here on we do homogenization and truncation (which may be Lie-group specific): + if constexpr (std::is_same_v) { + constexpr int LiftedDim = AmbientDim + 1; // First entry is homogenization + Matrix Q_trunc_hom = Matrix::Zero(2 * LiftedDim, 2 * LiftedDim); + Q_trunc_hom.block(1, 1, AmbientDim, AmbientDim) = Q.block(0, 0, AmbientDim, AmbientDim); + Q_trunc_hom.block(1, LiftedDim + 1, AmbientDim, AmbientDim) = Q.block(0, AmbientDim, AmbientDim, AmbientDim); + Q_trunc_hom.block(LiftedDim + 1, 1, AmbientDim, AmbientDim) = Q.block(AmbientDim, 0, AmbientDim, AmbientDim); + Q_trunc_hom.block(LiftedDim + 1, LiftedDim + 1, AmbientDim, AmbientDim) = Q.block(AmbientDim, AmbientDim, AmbientDim, AmbientDim); + + // Keep constraint insertion inside the type-specific branch so + // Q_trunc_hom dimensions agree with the lifted vector, e.g. Rot2.h. + InsertQcqpConstraints(this->key1(), constraints); + InsertQcqpConstraints(this->key2(), constraints); + + const SymmetricBlockMatrix blockQ( + std::vector{LiftedDim, LiftedDim}, Q_trunc_hom); + costs->push_back(std::make_shared( + KeyVector{this->key1(), this->key2()}, blockQ)); + } else { + (void)constraints; + throw std::runtime_error( + "FrobeniusBetweenFactor::qcqpFactors D=1 lifted Q embedding is " + "currently implemented only for Rot2."); + } } }