diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h index 8608d894d5f5536c28adef9c23dc40eeeba249c6..04332ec28e2e3168db8c0475d7de03fc6932b12f 100644 --- a/include/core/ceres_wrapper/qr_manager.h +++ b/include/core/ceres_wrapper/qr_manager.h @@ -14,14 +14,19 @@ namespace wolf { +WOLF_PTR_TYPEDEFS(QRManager); + + class QRManager : public SolverManager { protected: Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_; Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_; - Eigen::SparseMatrixd A_; + Eigen::SparseMatrix<double, Eigen::RowMajor> A_; Eigen::VectorXd b_; + // Stores the first column number in the Jacobian matrix of the block associated to a state block std::map<StateBlockPtr, unsigned int> sb_2_col_; + // Stores the first row number in the Jacobian matrix of the block associated to a factor std::map<FactorBasePtr, unsigned int> fac_2_row_; bool any_state_block_removed_; unsigned int new_state_blocks_; @@ -39,6 +44,8 @@ class QRManager : public SolverManager virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override; virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list) override; + + Eigen::SparseMatrixd getHessian(); private: diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp index 5ee0b91d829af1a495b57d935c6ae14ce94d6702..a3e6ab52a5c07f80097d3eff09fc6afea5a2071c 100644 --- a/src/ceres_wrapper/qr_manager.cpp +++ b/src/ceres_wrapper/qr_manager.cpp @@ -42,7 +42,22 @@ std::string QRManager::solve(const unsigned int& _report_level) // update state blocks for (auto sb_pair : sb_2_col_) - sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false); // FIXME use LocalParam->Plus() + { + StateBlockPtr sb = sb; + Eigen::VectorXd sb_x_incr = x_incr.segment(sb_pair.second, sb->getLocalSize()); + if (sb->hasLocalParametrization()){ + Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize()); + Eigen::Map<const Eigen::VectorXd> sb_x_incr_map(sb_x_incr.data(), sb->getLocalSize()); + Eigen::VectorXd new_sb_state(sb->getSize()); + Eigen::Map<Eigen::VectorXd> new_sb_state_map(new_sb_state.data(), sb->getSize()); + + sb->getLocalParametrization()->plus(sb_state_map, sb_x_incr_map, new_sb_state_map); + sb->setState(new_sb_state , false); + } + else { + sb->setState(sb->getState() + sb_x_incr, false); + } + } if (_report_level == 1) return std::string("Success!\n"); @@ -52,6 +67,10 @@ std::string QRManager::solve(const unsigned int& _report_level) return std::string(); } +Eigen::SparseMatrixd QRManager::getHessian(){ + return A_.transpose() * A_; +} + void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) { // TODO @@ -102,14 +121,16 @@ bool QRManager::computeDecomposition() for (auto sb_pair : sb_2_col_) { sb_2_col_[sb_pair.first] = state_size; - state_size += sb_pair.first->getSize(); // FIXME use getLocalSize() + // column size is the same dimension as the state block tangent space + state_size += sb_pair.first->getLocalSize(); } unsigned int meas_size = 0; for (auto fac_pair : fac_2_row_) { fac_2_row_[fac_pair.first] = meas_size; - meas_size += fac_pair.first->getSize(); // FIXME use getLocalSize() + // factor residual are so far all euclidean quantities + meas_size += fac_pair.first->getSize(); } // resize and setZero A, b @@ -204,51 +225,46 @@ void QRManager::updateStateBlockStatus(const StateBlockPtr& _st_ptr) void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr) // FIXME consider the Jacobian 'M' of the local param of each SB in the factor { - /* Things to do: - * - * 1. For each SB, obtain or create, then store in a vector, this data: - * - state vector - * - size, and local size - * - if local param, get Jacobian M - * - else, M = I, or set a convenient flag sb_has_jac = false - * 2. Evaluate factor, obtain residual and Jacobians - * - evaluate(fac_states_ptr,residual,jacobians) - * - for each SB i - * - if sb_has_jac[i] then J[i] = M[i] * jacobians[i] - * - else - * - J[i] = jacobians[i] - * 3. Fill A and b - * - for each SB i - * - insert J[i] in matrix A, in row A_block_row - * - insert residual in vector b - */ - - - - // evaluate factor std::vector<const double*> fac_states_ptr; for (auto sb : _fac_ptr->getStateBlockPtrVector()){ fac_states_ptr.push_back(sb->getStateData()); } + // retrieve residual value Eigen::VectorXd residual(_fac_ptr->getSize()); - std::vector<Eigen::MatrixXd> jacobians; - _fac_ptr->evaluate(fac_states_ptr,residual,jacobians); - // Fill jacobians + // retrieve jacobian in group size, not local size + std::vector<Eigen::MatrixXd> jacobians; + + _fac_ptr->evaluate(fac_states_ptr, residual, jacobians); + // Fill the factor block jacobians Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols()); for (auto i = 0; i < jacobians.size(); i++) { - if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed()) + StateBlockPtr sb = _fac_ptr->getStateBlockPtrVector()[i]; + if (!sb->isFixed()) { - assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added"); - assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols"); + assert(sb_2_col_.find(sb) != sb_2_col_.end() && "factor involving a state block not added"); + assert(A_.cols() >= sb_2_col_[sb] + jacobians[i].cols() - 1 && "bad A number of cols"); + // insert since A_block_row has just been created so it's empty for sure - insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]); + if (sb->hasLocalParametrization()){ + // if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian + Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize()); + Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize()); + Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize()); + + sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map); + insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb_2_col_[sb]); // (to_insert, matrix_to_fill, row, col) + } + else { + insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[sb]); + } } } + // void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen::SparseMatrix<double, Eigen::RowMajor>& ins, const unsigned int& _row) assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]); // Fill residual