From b9dc68dcc4eb9c57b8f97acb0ec4a1e8f808f7cd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Mon, 30 Mar 2020 23:59:27 +0200
Subject: [PATCH] Began updating the qr manager related files, WIP
---
CMakeLists.txt | 4 ++--
include/core/ceres_wrapper/qr_manager.h | 4 ++--
include/core/state_block/state_block.h | 10 ++++++++++
src/ceres_wrapper/qr_manager.cpp | 15 ++++++++++-----
4 files changed, 24 insertions(+), 9 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index fdc81d7a6..f51379091 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -376,7 +376,7 @@ SET(SRCS_YAML
#optional HDRS and SRCS
IF (Ceres_FOUND)
SET(HDRS_WRAPPER
- #ceres_wrapper/qr_manager.h
+ include/core/ceres_wrapper/qr_manager.h
include/core/ceres_wrapper/ceres_manager.h
include/core/ceres_wrapper/cost_function_wrapper.h
include/core/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -385,7 +385,7 @@ IF (Ceres_FOUND)
include/core/solver_suitesparse/sparse_utils.h
)
SET(SRCS_WRAPPER
- #ceres_wrapper/qr_manager.cpp
+ src/ceres_wrapper/qr_manager.cpp
src/ceres_wrapper/ceres_manager.cpp
src/ceres_wrapper/local_parametrization_wrapper.cpp
src/solver/solver_manager.cpp
diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h
index 691c44166..855b5dab5 100644
--- a/include/core/ceres_wrapper/qr_manager.h
+++ b/include/core/ceres_wrapper/qr_manager.h
@@ -9,7 +9,7 @@
#define SRC_CERES_WRAPPER_QR_MANAGER_H_
#include "core/solver/solver_manager.h"
-#include "core/solver_suitesparse/sparse_utils.h"
+#include "core/ceres_wrapper/sparse_utils.h"
namespace wolf
{
@@ -36,7 +36,7 @@ class QRManager : public SolverManager
virtual std::string solve(const unsigned int& _report_level);
- virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
+ virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS);
virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list);
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index b68b4ad7d..1e94d116b 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -74,6 +74,10 @@ public:
**/
Eigen::VectorXd getState() const;
+ /** \brief Returns the state vector data pointer
+ **/
+ double* getStateData();
+
/** \brief Sets the state vector
**/
void setState(const Eigen::VectorXd& _state, const bool _notify = true);
@@ -198,6 +202,12 @@ inline Eigen::VectorXd StateBlock::getState() const
return state_;
}
+inline double* StateBlock::getStateData()
+{
+ std::lock_guard<std::mutex> lock(mut_state_);
+ return state_.data();
+}
+
inline SizeEigen StateBlock::getSize() const
{
return state_size_.load();
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 0d7746f92..2a7c21aa3 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -5,7 +5,7 @@
* Author: jvallve
*/
-#include "qr_manager.h"
+#include "core/ceres_wrapper/qr_manager.h"
namespace wolf {
@@ -57,7 +57,7 @@ void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
// TODO
}
-void QRManager::computeCovariances(const StateBlockPtrList& _sb_list)
+void QRManager::computeCovariances(const std::vector<StateBlockPtr>& _sb_list)
{
//std::cout << "computing covariances.." << std::endl;
update();
@@ -206,8 +206,10 @@ void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
{
// evaluate factor
std::vector<const double*> fac_states_ptr;
- for (auto sb : _fac_ptr->getStateBlockPtrVector())
- fac_states_ptr.push_back(sb->get());
+ for (auto sb : _fac_ptr->getStateBlockPtrVector()){
+ fac_states_ptr.push_back(sb->getStateData());
+ }
+
Eigen::VectorXd residual(_fac_ptr->getSize());
std::vector<Eigen::MatrixXd> jacobians;
_fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
@@ -215,13 +217,16 @@ void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
// Fill 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())
+ {
+ if (!_fac_ptr->getStateBlockPtrVector()[i]->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");
// 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]]);
}
+ }
+
assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
// Fill residual
--
GitLab