From 073127eca184889755d2fedc5ae77172adf4bc94 Mon Sep 17 00:00:00 2001
From: jvallve <jvallve@224674b8-e365-4e73-a4a8-558dbbfec58c>
Date: Tue, 23 Jun 2015 16:25:11 +0000
Subject: [PATCH] I'm working on solver, uooo!
---
src/constraint_base.h | 14 +
src/constraint_corner_2D_theta.h | 14 +-
src/constraint_gps_2D.h | 12 +-
src/constraint_odom_2D_complex_angle.h | 12 +-
src/constraint_odom_2D_theta.h | 12 +-
src/constraint_sparse.h | 141 +++-
src/examples/CMakeLists.txt | 12 +
src/examples/test_ceres_2_lasers.cpp | 8 +-
src/examples/test_iQR_wolf.cpp | 564 ++++++++++++++
src/examples/test_iQR_wolf2.cpp | 989 +++++++++++++++++++++++++
src/solver/CMakeLists.txt | 4 +
src/solver/solver_QR.h | 33 +
src/solver/solver_manager.cpp | 228 ++++++
src/solver/solver_manager.h | 49 ++
14 files changed, 2023 insertions(+), 69 deletions(-)
create mode 100644 src/examples/test_iQR_wolf.cpp
create mode 100644 src/examples/test_iQR_wolf2.cpp
create mode 100644 src/solver/CMakeLists.txt
create mode 100644 src/solver/solver_QR.h
create mode 100644 src/solver/solver_manager.cpp
create mode 100644 src/solver/solver_manager.h
diff --git a/src/constraint_base.h b/src/constraint_base.h
index 4bf7666b5..fc12182e0 100644
--- a/src/constraint_base.h
+++ b/src/constraint_base.h
@@ -52,6 +52,13 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
**/
virtual const std::vector<WolfScalar*> getStateBlockPtrVector() = 0;
+ /** \brief Returns a vector of pointers to the states
+ *
+ * Returns a vector of pointers to the state in which this constraint depends
+ *
+ **/
+ virtual const std::vector<StateBase*> getStatePtrVector() const = 0;
+
/** \brief Returns a pointer to the feature measurement
*
* Returns a pointer to the feature measurement
@@ -73,5 +80,12 @@ class ConstraintBase : public NodeLinked<FeatureBase, NodeTerminus>
**/
CaptureBase* getCapturePtr() const;
+ /** \brief Returns the constraint residual size
+ *
+ * Returns the constraint residual size
+ *
+ **/
+ virtual unsigned int getSize() const = 0;
+
};
#endif
diff --git a/src/constraint_corner_2D_theta.h b/src/constraint_corner_2D_theta.h
index d3a0e6bbf..47e4babe0 100644
--- a/src/constraint_corner_2D_theta.h
+++ b/src/constraint_corner_2D_theta.h
@@ -14,15 +14,15 @@ class ConstraintCorner2DTheta: public ConstraintSparse<3,2,1,2,1>
public:
static const unsigned int N_BLOCKS = 4;
- ConstraintCorner2DTheta(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, WolfScalar* _robotPPtr, WolfScalar* _robotOPtr, WolfScalar* _landmarkPPtr, WolfScalar* _landmarkOPtr) :
- ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CORNER_2D_THETA, _robotPPtr, _robotOPtr, _landmarkPPtr, _landmarkOPtr),
- lmk_ptr_(_lmk_ptr)
- {
- //
- }
+// ConstraintCorner2DTheta(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, WolfScalar* _robotPPtr, WolfScalar* _robotOPtr, WolfScalar* _landmarkPPtr, WolfScalar* _landmarkOPtr) :
+// ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CORNER_2D_THETA, _robotPPtr, _robotOPtr, _landmarkPPtr, _landmarkOPtr),
+// lmk_ptr_(_lmk_ptr)
+// {
+// //
+// }
ConstraintCorner2DTheta(FeatureBase* _ftr_ptr, LandmarkCorner2D* _lmk_ptr, StateBase* _robotPPtr, StateBase* _robotOPtr, StateBase* _landmarkPPtr, StateBase* _landmarkOPtr) :
- ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CORNER_2D_THETA, _robotPPtr->getPtr(), _robotOPtr->getPtr(),_landmarkPPtr->getPtr(), _landmarkOPtr->getPtr()),
+ ConstraintSparse<3,2,1,2,1>(_ftr_ptr,CTR_CORNER_2D_THETA, _robotPPtr, _robotOPtr,_landmarkPPtr, _landmarkOPtr),
lmk_ptr_(_lmk_ptr)
{
//
diff --git a/src/constraint_gps_2D.h b/src/constraint_gps_2D.h
index 9b94e4a09..ad57d9ce4 100644
--- a/src/constraint_gps_2D.h
+++ b/src/constraint_gps_2D.h
@@ -11,14 +11,14 @@ class ConstraintGPS2D: public ConstraintSparse<2,2>
public:
static const unsigned int N_BLOCKS = 1;
- ConstraintGPS2D(FeatureBase* _ftr_ptr, WolfScalar* _statePtr) :
- ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _statePtr)
- {
- //
- }
+// ConstraintGPS2D(FeatureBase* _ftr_ptr, WolfScalar* _statePtr) :
+// ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _statePtr)
+// {
+// //
+// }
ConstraintGPS2D(FeatureBase* _ftr_ptr, StateBase* _statePtr):
- ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _statePtr->getPtr())
+ ConstraintSparse<2,2>(_ftr_ptr,CTR_GPS_FIX_2D, _statePtr)
{
//
}
diff --git a/src/constraint_odom_2D_complex_angle.h b/src/constraint_odom_2D_complex_angle.h
index 4597709e0..a2ed5d503 100644
--- a/src/constraint_odom_2D_complex_angle.h
+++ b/src/constraint_odom_2D_complex_angle.h
@@ -10,14 +10,14 @@ class ConstraintOdom2DComplexAngle : public ConstraintSparse<3, 2, 2, 2, 2>
public:
static const unsigned int N_BLOCKS = 4;
- ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- ConstraintSparse<3, 2, 2, 2, 2>(_ftr_ptr, CTR_ODOM_2D_COMPLEX_ANGLE, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
- {
- //
- }
+// ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
+// ConstraintSparse<3, 2, 2, 2, 2>(_ftr_ptr, CTR_ODOM_2D_COMPLEX_ANGLE, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
+// {
+// //
+// }
ConstraintOdom2DComplexAngle(FeatureBase* _ftr_ptr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
- ConstraintSparse<3, 2, 2, 2, 2>(_ftr_ptr, CTR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr->getPtr(), _state1Ptr->getPtr(), _state2Ptr->getPtr(), _state3Ptr->getPtr())
+ ConstraintSparse<3, 2, 2, 2, 2>(_ftr_ptr, CTR_ODOM_2D_COMPLEX_ANGLE, _state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr)
{
//
}
diff --git a/src/constraint_odom_2D_theta.h b/src/constraint_odom_2D_theta.h
index 9e2024207..c4d8d91ce 100644
--- a/src/constraint_odom_2D_theta.h
+++ b/src/constraint_odom_2D_theta.h
@@ -10,14 +10,14 @@ class ConstraintOdom2DTheta : public ConstraintSparse<3, 2, 1, 2, 1>
public:
static const unsigned int N_BLOCKS = 4;
- ConstraintOdom2DTheta(FeatureBase*_ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
- ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D_THETA, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
- {
- //
- }
+// ConstraintOdom2DTheta(FeatureBase*_ftr_ptr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) :
+// ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D_THETA, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr)
+// {
+// //
+// }
ConstraintOdom2DTheta(FeatureBase* _ftr_ptr, StateBase* _state0Ptr, StateBase* _state1Ptr, StateBase* _state2Ptr, StateBase* _state3Ptr) :
- ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D_THETA, _state0Ptr->getPtr(), _state1Ptr->getPtr(), _state2Ptr->getPtr(), _state3Ptr->getPtr())
+ ConstraintSparse<3, 2, 1, 2, 1>(_ftr_ptr, CTR_ODOM_2D_THETA, _state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr)
{
//
}
diff --git a/src/constraint_sparse.h b/src/constraint_sparse.h
index 9ee533e3b..94bc5fe41 100644
--- a/src/constraint_sparse.h
+++ b/src/constraint_sparse.h
@@ -28,6 +28,7 @@ template <const unsigned int MEASUREMENT_SIZE,
class ConstraintSparse: public ConstraintBase
{
protected:
+ std::vector<StateBase*> state_ptr_vector_;
std::vector<WolfScalar*> state_block_ptr_vector_;
std::vector<unsigned int> state_block_sizes_vector_;
@@ -50,59 +51,99 @@ class ConstraintSparse: public ConstraintBase
* JVN: Potser aquest constructor no l'utilitzarem mai.. no?
*
**/
- ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, WolfScalar** _blockPtrArray) :
- ConstraintBase(_ftr_ptr,_tp),
- state_block_ptr_vector_(10),
- state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
- {
- for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
- {
- if (state_block_sizes_vector_.at(ii) != 0)
- {
- state_block_ptr_vector_.at(ii) = _blockPtrArray[ii];
- }
- else //at the end the vector is cropped to just relevant components
- {
- state_block_ptr_vector_.resize(ii);
- break;
- }
- }
- }
+// ConstraintSparse(FeatureBase* _ftr_ptr, ConstraintType _tp, WolfScalar** _blockPtrArray) :
+// ConstraintBase(_ftr_ptr,_tp),
+// state_block_ptr_vector_(10),
+// state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
+// {
+// for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
+// {
+// if (state_block_sizes_vector_.at(ii) != 0)
+// {
+// state_block_ptr_vector_.at(ii) = _blockPtrArray[ii];
+// }
+// else //at the end the vector is cropped to just relevant components
+// {
+// state_block_ptr_vector_.resize(ii);
+// break;
+// }
+// }
+// }
+//
+// /** \brief Contructor with state pointer separated
+// *
+// * Constructor with state pointers separated
+// *
+// **/
+// ConstraintSparse(FeatureBase* _ftr_ptr,
+// ConstraintType _tp,
+// WolfScalar* _state0Ptr,
+// WolfScalar* _state1Ptr = nullptr,
+// WolfScalar* _state2Ptr = nullptr,
+// WolfScalar* _state3Ptr = nullptr,
+// WolfScalar* _state4Ptr = nullptr,
+// WolfScalar* _state5Ptr = nullptr,
+// WolfScalar* _state6Ptr = nullptr,
+// WolfScalar* _state7Ptr = nullptr,
+// WolfScalar* _state8Ptr = nullptr,
+// WolfScalar* _state9Ptr = nullptr ) :
+// ConstraintBase(_ftr_ptr,_tp),
+// state_block_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+// state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
+// {
+// for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
+// {
+// if ( (state_block_ptr_vector_.at(ii) == nullptr) && (state_block_sizes_vector_.at(ii) == 0) )
+// {
+// state_block_sizes_vector_.resize(ii);
+// state_block_ptr_vector_.resize(ii);
+// break;
+// }
+// else // check error cases
+// {
+// assert(state_block_ptr_vector_.at(ii) != nullptr);
+// assert(state_block_sizes_vector_.at(ii) != 0);
+// }
+// }
+// }
/** \brief Contructor with state pointer separated
- *
+ *
* Constructor with state pointers separated
- *
- **/
+ *
+ **/
ConstraintSparse(FeatureBase* _ftr_ptr,
- ConstraintType _tp,
- WolfScalar* _state0Ptr,
- WolfScalar* _state1Ptr = nullptr,
- WolfScalar* _state2Ptr = nullptr,
- WolfScalar* _state3Ptr = nullptr,
- WolfScalar* _state4Ptr = nullptr,
- WolfScalar* _state5Ptr = nullptr,
- WolfScalar* _state6Ptr = nullptr,
- WolfScalar* _state7Ptr = nullptr,
- WolfScalar* _state8Ptr = nullptr,
- WolfScalar* _state9Ptr = nullptr ) :
+ ConstraintType _tp,
+ StateBase* _state0Ptr,
+ StateBase* _state1Ptr = nullptr,
+ StateBase* _state2Ptr = nullptr,
+ StateBase* _state3Ptr = nullptr,
+ StateBase* _state4Ptr = nullptr,
+ StateBase* _state5Ptr = nullptr,
+ StateBase* _state6Ptr = nullptr,
+ StateBase* _state7Ptr = nullptr,
+ StateBase* _state8Ptr = nullptr,
+ StateBase* _state9Ptr = nullptr ) :
ConstraintBase(_ftr_ptr,_tp),
- state_block_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ state_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
+ state_block_ptr_vector_({_state0Ptr->getPtr(), nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr, nullptr}),
state_block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE})
{
- for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
+ for (unsigned int ii = 1; ii<state_block_sizes_vector_.size(); ii++)
{
- if ( (state_block_ptr_vector_.at(ii) == nullptr) && (state_block_sizes_vector_.at(ii) == 0) )
+ if (state_ptr_vector_.at(ii) != nullptr)
+ {
+ assert(state_block_sizes_vector_.at(ii) != 0 && "Too many non-null state pointers in ConstraintSparse constructor");
+ state_block_ptr_vector_.at(ii) = state_ptr_vector_.at(ii)->getPtr();
+ }
+ else
{
+ assert(state_block_sizes_vector_.at(ii) == 0 && "No non-null state pointers enough in ConstraintSparse constructor");
+ state_ptr_vector_.resize(ii);
state_block_sizes_vector_.resize(ii);
state_block_ptr_vector_.resize(ii);
break;
}
- else // check error cases
- {
- assert(state_block_ptr_vector_.at(ii) != nullptr);
- assert(state_block_sizes_vector_.at(ii) != 0);
- }
}
}
@@ -126,6 +167,26 @@ class ConstraintSparse: public ConstraintBase
return state_block_ptr_vector_;
}
+ /** \brief Returns a vector of pointers to the states
+ *
+ * Returns a vector of pointers to the state in which this constraint depends
+ *
+ **/
+ virtual const std::vector<StateBase*> getStatePtrVector() const
+ {
+ return state_ptr_vector_;
+ }
+
+ /** \brief Returns the constraint residual size
+ *
+ * Returns the constraint residual size
+ *
+ **/
+ virtual unsigned int getSize() const
+ {
+ return MEASUREMENT_SIZE;
+ }
+
virtual void print(unsigned int _ntabs = 0, std::ostream& _ost = std::cout) const
{
NodeLinked::printSelf(_ntabs, _ost);
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index adb268674..cc05783d0 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -52,6 +52,10 @@ IF(Suitesparse_FOUND)
ADD_EXECUTABLE(test_iQR test_iQR.cpp)
TARGET_LINK_LIBRARIES(test_iQR ${PROJECT_NAME})
+ # Testing QR solver test tending to wolf
+ ADD_EXECUTABLE(test_iQR_wolf test_iQR_wolf.cpp)
+ TARGET_LINK_LIBRARIES(test_iQR_wolf ${PROJECT_NAME})
+
# Testing SPQR simple test
ADD_EXECUTABLE(test_SPQR test_SPQR.cpp)
TARGET_LINK_LIBRARIES(test_SPQR ${PROJECT_NAME})
@@ -81,6 +85,14 @@ IF(faramotics_FOUND)
${pose_state_time_LIBRARIES}
${faramotics_LIBRARIES}
${PROJECT_NAME})
+
+ IF(Suitesparse_FOUND)
+ ADD_EXECUTABLE(test_iQR_wolf2 test_iQR_wolf2.cpp)
+ TARGET_LINK_LIBRARIES(test_iQR_wolf2
+ ${pose_state_time_LIBRARIES}
+ ${faramotics_LIBRARIES}
+ ${PROJECT_NAME})
+ ENDIF(Suitesparse_FOUND)
ENDIF (laser_scan_utils_FOUND)
ENDIF(faramotics_FOUND)
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 4acd945f3..92c06eb36 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -135,8 +135,8 @@ int main(int argc, char** argv)
string modelFileName;
//model and initial view point
- //modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
- modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
+ modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
+ //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
//modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
devicePose.setPose(2, 8, 0.2, 0, 0, 0);
viewPoint.setPose(devicePose);
@@ -168,8 +168,8 @@ int main(int argc, char** argv)
Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
- SensorOdom2D odom_sensor(new StatePoint3D(odom_pose.data()), new StateTheta(&odom_pose(3)), odom_std_factor, odom_std_factor);
- SensorGPSFix gps_sensor(new StatePoint3D(gps_pose.data()), new StateTheta(&gps_pose(3)), gps_std);
+ SensorOdom2D odom_sensor(new StatePoint3D(odom_pose.data()), new StateTheta(&odom_pose(2)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StatePoint3D(gps_pose.data()), new StateTheta(&gps_pose(2)), gps_std);
SensorLaser2D laser_1_sensor(new StatePoint3D(laser_1_pose.data()), new StateTheta(&laser_1_pose(3)));
SensorLaser2D laser_2_sensor(new StatePoint3D(laser_2_pose.data()), new StateTheta(&laser_2_pose(3)));
diff --git a/src/examples/test_iQR_wolf.cpp b/src/examples/test_iQR_wolf.cpp
new file mode 100644
index 000000000..4b06f370a
--- /dev/null
+++ b/src/examples/test_iQR_wolf.cpp
@@ -0,0 +1,564 @@
+/*
+ * test_iQR_wolf.cpp
+ *
+ * Created on: Jun 17, 2015
+ * Author: jvallve
+ */
+
+
+//std includes
+#include <cstdlib>
+#include <string>
+#include <iostream>
+#include <fstream>
+#include <memory>
+#include <random>
+#include <typeinfo>
+#include <ctime>
+#include <queue>
+
+// eigen includes
+#include <eigen3/Eigen/OrderingMethods>
+#include <eigen3/Eigen/SparseQR>
+#include <Eigen/SPQRSupport>
+
+// ccolamd
+#include "solver/ccolamd_ordering.h"
+
+using namespace Eigen;
+
+class block_pruning
+{
+ public:
+ int col, row, Nrows, Ncols;
+ block_pruning(int _col, int _row, int _Nrows, int _Ncols) :
+ col(_col),
+ row(_row),
+ Nrows(_Nrows),
+ Ncols(_Ncols)
+ {
+ //
+ }
+ bool operator()(int i, int j, double) const
+ {
+ return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
+ }
+};
+
+void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
+{
+ // prune all non-zero elements that not satisfy the 'keep' operand
+ // elements that are not in the block rows or are not in the block columns should be kept
+ //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); });
+
+ block_pruning bp(row, col, Nrows, Ncols);
+ original.prune(bp);
+
+// for (uint i = row; i < row + Nrows; i++)
+// for (uint j = col; j < row + Ncols; j++)
+// original.coeffRef(i,j) = 0.0;
+//
+// original.prune(0);
+}
+
+void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
+{
+ for (uint r=0; r<ins.rows(); ++r)
+ for (uint c = 0; c < ins.cols(); c++)
+ if (ins(r,c) != 0)
+ original.coeffRef(r + row, c + col) += ins(r,c);
+}
+
+struct node
+{
+ public:
+ int id;
+ int dim;
+ int location;
+ int order;
+
+ node(const int _id, const int _dim, const int _location, const int _order) :
+ id(_id),
+ dim(_dim),
+ location(_location),
+ order(_order)
+ {
+
+ }
+};
+
+struct measurement
+{
+ public:
+ std::vector<MatrixXd> jacobians;
+ std::vector<int> nodes_idx;
+ VectorXd error;
+ int dim;
+ bool odometry_type;
+ int location;
+
+ measurement(const MatrixXd & _jacobian1, const int _idx1, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) :
+ jacobians({_jacobian1}),
+ nodes_idx({_idx1}),
+ error(_error),
+ dim(_meas_dim),
+ odometry_type(_odometry_type),
+ location(0)
+ {
+ //jacobians.push_back(_jacobian1);
+ }
+
+ measurement(const MatrixXd & _jacobian1, const int _idx1, const MatrixXd & _jacobian2, const int _idx2, const VectorXd &_error, const int _meas_dim, bool _odometry_type=false) :
+ jacobians({_jacobian1, _jacobian2}),
+ nodes_idx({_idx1, _idx2}),
+ error(_error),
+ dim(_meas_dim),
+ odometry_type(_odometry_type),
+ location(0)
+ {
+
+ }
+};
+
+class SolverQR
+{
+ protected:
+ std::string name_;
+ SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_;
+ SparseMatrix<double> A_, R_;
+ VectorXd b_, x_incr_;
+ int n_measurements;
+ int n_nodes_;
+ std::vector<node> nodes_;
+ std::vector<measurement> measurements_;
+
+ // ordering
+ SparseMatrix<int> A_nodes_;
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_;
+
+ CCOLAMDOrdering<int> ordering_;
+ VectorXi nodes_ordering_constraints_;
+ int first_ordered_node_;
+
+ // time
+ clock_t t_ordering_, t_solving_, t_managing_;
+ double time_ordering_, time_solving_, time_managing_;
+
+ public:
+ SolverQR(const std::string &_name) :
+ name_(_name),
+ A_(0,0),
+ R_(0,0),
+// b_(0),
+// x_(0),
+ n_measurements(0),
+ n_nodes_(0),
+ A_nodes_(0,0),
+ acc_permutation_nodes_(0),
+// nodes_(0),
+// measurements_(0),
+ time_ordering_(0),
+ time_solving_(0),
+ time_managing_(0)
+ {
+ //
+ }
+
+ virtual ~SolverQR()
+ {
+
+ }
+
+ void add_state_unit(const int node_dim, const int node_idx)
+ {
+ t_managing_ = clock();
+
+ n_nodes_++;
+ nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1));
+
+ // Resize accumulated permutations
+ augment_permutation(acc_permutation_nodes_, n_nodes_);
+
+ // Resize state
+ x_incr_.conservativeResize(x_incr_.size() + node_dim);
+
+ // Resize problem
+ A_.conservativeResize(A_.rows(), A_.cols() + node_dim);
+ R_.conservativeResize(R_.cols() + node_dim, R_.cols() + node_dim);
+ //A_nodes_.conservativeResize(n_measurements, n_nodes); // not necessary
+
+ time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
+ }
+
+ void addConstraint(const measurement& _meas)
+ {
+ t_managing_ = clock();
+
+ assert(_meas.jacobians.size() == _meas.nodes_idx.size());
+ assert(_meas.error.size() == _meas.dim);
+
+ n_measurements++;
+ measurements_.push_back(_meas);
+ measurements_.back().location = A_.rows();
+
+ // Resize problem
+ A_.conservativeResize(A_.rows() + _meas.dim, A_.cols());
+ b_.conservativeResize(b_.size() + _meas.dim);
+ A_nodes_.conservativeResize(n_measurements,n_nodes_);
+
+ // ADD MEASUREMENTS
+ first_ordered_node_ = n_nodes_;
+ for (unsigned int j = 0; j < _meas.nodes_idx.size(); j++)
+ {
+ assert(acc_permutation_nodes_.indices()(_meas.nodes_idx.at(j)) == nodes_.at(_meas.nodes_idx.at(j)).order);
+
+ int ordered_node = nodes_.at(_meas.nodes_idx.at(j)).order;//acc_permutation_nodes_.indices()(_nodes_idx.at(j));
+
+ add_sparse_block(_meas.jacobians.at(j), A_, A_.rows()-_meas.dim, nodes_.at(_meas.nodes_idx.at(j)).location);
+
+ A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
+
+
+ assert(_meas.jacobians.at(j).cols() == nodes_.at(_meas.nodes_idx.at(j)).dim);
+ assert(_meas.jacobians.at(j).rows() == _meas.dim);
+
+ // store minimum ordered node
+ if (first_ordered_node_ > ordered_node)
+ first_ordered_node_ = ordered_node;
+ }
+
+ // error
+ b_.tail(_meas.dim) = _meas.error;
+
+ time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
+ }
+
+ void ordering(const int & _first_ordered_node)
+ {
+ t_ordering_ = clock();
+
+ // full problem ordering
+ if (_first_ordered_node == 0)
+ {
+ // ordering ordering constraints
+ nodes_ordering_constraints_.resize(n_nodes_);
+ nodes_ordering_constraints_ = A_nodes_.bottomRows(1).transpose();
+
+ // computing nodes partial ordering_
+ A_nodes_.makeCompressed();
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_);
+ ordering_(A_nodes_, incr_permutation_nodes, nodes_ordering_constraints_.data());
+
+ // node ordering to variable ordering
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols());
+ permutation_2_block_permutation(incr_permutation_nodes, incr_permutation);
+
+ // apply partial_ordering orderings
+ A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
+ A_ = (A_ * incr_permutation.transpose()).sparseView();
+
+ // ACCUMULATING PERMUTATIONS
+ accumulate_permutation(incr_permutation_nodes);
+ }
+
+ // partial ordering
+ else
+ {
+ int ordered_nodes = n_nodes_ - _first_ordered_node;
+ int unordered_nodes = n_nodes_ - ordered_nodes;
+ if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
+ {
+ // SUBPROBLEM ORDERING (from first node variable to last one)
+ //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
+ SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
+
+ // _partial_ordering ordering_ constraints
+ nodes_ordering_constraints_.resize(ordered_nodes);
+ nodes_ordering_constraints_ = sub_A_nodes_.bottomRows(1).transpose();
+
+ // computing nodes partial ordering_
+ sub_A_nodes_.makeCompressed();
+ PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes);
+ ordering_(sub_A_nodes_, partial_permutation_nodes, nodes_ordering_constraints_.data());
+
+ // node ordering to variable ordering
+ PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(A_.cols());
+ permutation_2_block_permutation(partial_permutation_nodes, partial_permutation);
+
+ // apply partial_ordering orderings
+ int ordered_variables = A_.cols() - nodes_.at(_first_ordered_node).location;
+ A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView();
+ A_.rightCols(ordered_variables) = (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
+ R_.rightCols(ordered_variables) = (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
+
+ // ACCUMULATING PERMUTATIONS
+ accumulate_permutation(partial_permutation_nodes);
+ }
+ }
+ time_ordering_ += ((double) clock() - t_ordering_) / CLOCKS_PER_SEC;
+ }
+
+ bool solve(const int mode)
+ {
+ bool batch = (mode !=2 || first_ordered_node_ == 0);
+ bool order = (mode !=0 && n_nodes_ > 1);
+
+ // BATCH
+ if (batch)
+ {
+ // REORDER
+ if (order)
+ ordering(0);
+
+ //print_problem();
+
+ // SOLVE
+ t_solving_ = clock();
+ A_.makeCompressed();
+ solver_.compute(A_);
+ if (solver_.info() != Success)
+ {
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
+ }
+ x_incr_ = solver_.solve(b_);
+ R_ = solver_.matrixR();
+ //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
+ time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
+ }
+ // INCREMENTAL
+ else
+ {
+ // REORDER SUBPROBLEM
+ ordering(first_ordered_node_);
+ //print_problem();
+
+ // SOLVE ORDERED SUBPROBLEM
+ t_solving_= clock();
+ A_nodes_.makeCompressed();
+ A_.makeCompressed();
+
+ // finding measurements block
+ SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
+ // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
+ // std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
+ int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
+ int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
+ int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
+ int unordered_variables = nodes_.at(first_ordered_node_).location;
+
+ SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
+ solver_.compute(A_partial);
+ if (solver_.info() != Success)
+ {
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
+ }
+ //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
+ x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
+
+ // store new part of R
+ erase_sparse_block(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
+ //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
+ add_sparse_block(solver_.matrixR(), R_, unordered_variables, unordered_variables);
+ //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
+ R_.makeCompressed();
+
+ // solving not ordered subproblem
+ if (unordered_variables > 0)
+ {
+ //std::cout << "--------------------- solving unordered part" << std::endl;
+ SparseMatrix<double> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
+ //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
+ SparseMatrix<double> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
+ //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
+ solver_.compute(R1);
+ if (solver_.info() != Success)
+ {
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
+ }
+ x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
+ }
+
+ }
+ // UNDO ORDERING FOR RESULT
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
+ permutation_2_block_permutation(acc_permutation_nodes_, acc_permutation); // TODO via pointers
+ x_incr_ = acc_permutation.inverse() * x_incr_;
+
+ time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
+
+ return 1;
+ }
+
+
+ void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
+ {
+ ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
+
+ int last_idx = 0;
+ for (unsigned int i = 0; i<locations.size(); i++)
+ {
+ perm_variables.indices().segment(last_idx, nodes_.at(i).dim) = VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i)+nodes_.at(i).dim-1);
+ last_idx += nodes_.at(i).dim;
+ }
+ }
+
+ ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes)
+ {
+ ArrayXi indices = _perm_nodes.indices().array();
+
+ for (unsigned int i = 0; i<indices.size(); i++)
+ indices = (indices > indices(i)).select(indices + nodes_.at(i).dim-1, indices);
+
+ return indices;
+ }
+
+ void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
+ {
+ int old_size = perm.indices().size();
+ int dim = new_size - old_size;
+ VectorXi new_indices(new_size);
+ new_indices.head(old_size)= perm.indices();
+ new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
+ perm.resize(new_size);
+ perm.indices() = new_indices;
+ }
+
+ void accumulate_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm)
+ {
+ print_name();
+ //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
+ //std::cout << "incr perm " << perm.indices().transpose() << std::endl;
+
+ // acumulate permutation
+ if (perm.size() == acc_permutation_nodes_.size()) //full permutation
+ acc_permutation_nodes_ = perm * acc_permutation_nodes_;
+ else //partial permutation
+ {
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation
+ incr_permutation_nodes.indices().tail(perm.size()) = perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size());
+ //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
+ acc_permutation_nodes_ = incr_permutation_nodes * acc_permutation_nodes_;
+ }
+ //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
+
+ // update nodes orders and locations
+ ArrayXi locations = perm_nodes_2_locations(acc_permutation_nodes_);
+ for (int i = 0; i < nodes_.size(); i++)
+ {
+ nodes_.at(i).order = acc_permutation_nodes_.indices()(i);
+ nodes_.at(i).location = locations(i);
+ }
+ }
+
+ void print_name()
+ {
+ std::cout << name_;
+ }
+
+ void print_results()
+ {
+ print_name();
+ std::cout << " solved in " << time_solving_*1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"<< std::endl;
+ std::cout << "x = " << x_incr_.transpose() << std::endl;
+ }
+
+ void print_problem()
+ {
+ print_name();
+ std::cout << std::endl << "A_nodes_: " << std::endl << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl;
+ std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
+ std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
+ }
+};
+
+//main
+int main(int argc, char *argv[])
+{
+ if (argc != 3 || atoi(argv[1]) < 1|| atoi(argv[2]) < 1)
+ {
+ std::cout << "Please call me with: [./test_iQR SIZE DIM], where:" << std::endl;
+ std::cout << " - SIZE: integer size of the problem" << std::endl;
+ std::cout << " - DIM: integer dimension of the nodes" << std::endl;
+ std::cout << "EXIT due to bad user input" << std::endl << std::endl;
+ return -1;
+ }
+ int size = atoi(argv[1]);
+ int dim = atoi(argv[2]);
+
+ // Problems
+ SolverQR solver_ordered("FULL ORDERED");
+ SolverQR solver_unordered("UNORDERED");
+ SolverQR solver_ordered_partial("PARTIALLY ORDERED");
+
+ MatrixXd omega = MatrixXd::Constant(dim, dim, 0.1) + MatrixXd::Identity(dim, dim);
+
+ // results variables
+ clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4;
+ double time_ordering=0, time_solving_unordered=0, time_solving_ordered=0, time_solving_ordered_partial=0;
+
+ std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
+
+ // GENERATING MEASUREMENTS
+ std::vector<measurement> measurements;
+ for (unsigned int i = 0; i < size; i++)
+ {
+ std::vector<int> meas(0);
+ if (i == 0) //prior
+ measurements.push_back(measurement(omega, 0, VectorXd::LinSpaced(dim, 0, dim-1), dim));
+
+ else //odometry
+ measurements.push_back(measurement(2*omega, i-1, 2*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim, true));
+
+ if (i > size / 2) //loop closures
+ measurements.push_back(measurement(4*omega, 0, 4*omega, i, VectorXd::LinSpaced(dim, dim * i, dim * (i+1)-1), dim));
+ }
+
+ // INCREMENTAL LOOP
+ for (unsigned int i = 0; i < measurements.size(); i++)
+ {
+ std::cout << "========================= MEASUREMENT " << i << ":" << std::endl;
+
+ // AUGMENT THE PROBLEM ----------------------------
+ if (measurements.at(i).odometry_type || i == 0) // if odometry, augment the problem
+ {
+ solver_unordered.add_state_unit(dim, i);
+ solver_ordered.add_state_unit(dim, i);
+ solver_ordered_partial.add_state_unit(dim,i);
+ }
+
+ // ADD MEASUREMENTS
+ solver_unordered.addConstraint(measurements.at(i));
+ solver_ordered.addConstraint(measurements.at(i));
+ solver_ordered_partial.addConstraint(measurements.at(i));
+
+ // PRINT PROBLEM
+ solver_unordered.print_problem();
+ solver_ordered.print_problem();
+ solver_ordered_partial.print_problem();
+
+ // SOLVING
+ solver_unordered.solve(0);
+ solver_ordered.solve(1);
+ solver_ordered_partial.solve(2);
+
+ // RESULTS ------------------------------------
+ std::cout << "========================= RESULTS " << i << ":" << std::endl;
+ solver_unordered.print_results();
+ solver_ordered.print_results();
+ solver_ordered_partial.print_results();
+
+// if ((x_ordered_partial-x_ordered).maxCoeff() < 1e-10)
+// std::cout << "Both solutions are equals (tolerance " << (x_ordered_partial-x_ordered).maxCoeff() << ")" << std::endl;
+// else
+// std::cout << "DIFFERENT SOLUTIONS!!!!!!!! max difference " << (x_ordered_partial-x_ordered).maxCoeff() << std::endl;
+ }
+}
+
+
+
+
+
+
diff --git a/src/examples/test_iQR_wolf2.cpp b/src/examples/test_iQR_wolf2.cpp
new file mode 100644
index 000000000..d2d9bfdd9
--- /dev/null
+++ b/src/examples/test_iQR_wolf2.cpp
@@ -0,0 +1,989 @@
+/*
+ * test_iQR_wolf.cpp
+ *
+ * Created on: Jun 17, 2015
+ * Author: jvallve
+ */
+
+//std includes
+#include <cstdlib>
+#include <string>
+#include <iostream>
+#include <fstream>
+#include <memory>
+#include <random>
+#include <typeinfo>
+#include <ctime>
+#include <queue>
+
+// eigen includes
+#include <eigen3/Eigen/OrderingMethods>
+#include <eigen3/Eigen/SparseQR>
+#include <Eigen/SPQRSupport>
+
+//Wolf includes
+#include "state_base.h"
+#include "constraint_base.h"
+#include "wolf_manager.h"
+
+// ccolamd
+#include "solver/ccolamd_ordering.h"
+
+//C includes for sleep, time and main args
+#include "unistd.h"
+
+//faramotics includes
+#include "faramotics/dynamicSceneRender.h"
+#include "faramotics/rangeScan2D.h"
+#include "btr-headers/pose3d.h"
+
+//laser_scan_utils
+#include "iri-algorithms/laser_scan_utils/corner_detector.h"
+#include "iri-algorithms/laser_scan_utils/entities.h"
+
+using namespace Eigen;
+
+//function travel around
+void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
+{
+ if (ii <= 120)
+ {
+ displacement_ = 0.1;
+ rotation_ = 0;
+ }
+ else if ((ii > 120) && (ii <= 170))
+ {
+ displacement_ = 0.2;
+ rotation_ = 1.8 * M_PI / 180;
+ }
+ else if ((ii > 170) && (ii <= 220))
+ {
+ displacement_ = 0;
+ rotation_ = -1.8 * M_PI / 180;
+ }
+ else if ((ii > 220) && (ii <= 310))
+ {
+ displacement_ = 0.1;
+ rotation_ = 0;
+ }
+ else if ((ii > 310) && (ii <= 487))
+ {
+ displacement_ = 0.1;
+ rotation_ = -1. * M_PI / 180;
+ }
+ else if ((ii > 487) && (ii <= 600))
+ {
+ displacement_ = 0.2;
+ rotation_ = 0;
+ }
+ else if ((ii > 600) && (ii <= 700))
+ {
+ displacement_ = 0.1;
+ rotation_ = -1. * M_PI / 180;
+ }
+ else if ((ii > 700) && (ii <= 780))
+ {
+ displacement_ = 0;
+ rotation_ = -1. * M_PI / 180;
+ }
+ else
+ {
+ displacement_ = 0.3;
+ rotation_ = 0.0 * M_PI / 180;
+ }
+
+ pose.moveForward(displacement_);
+ pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
+}
+
+class block_pruning
+{
+ public:
+ int col, row, Nrows, Ncols;
+ block_pruning(int _col, int _row, int _Nrows, int _Ncols) :
+ col(_col),
+ row(_row),
+ Nrows(_Nrows),
+ Ncols(_Ncols)
+ {
+ //
+ }
+ bool operator()(int i, int j, double) const
+ {
+ return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1);
+ }
+};
+
+void erase_sparse_block(SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col, const unsigned int& Nrows, const unsigned int& Ncols)
+{
+ // prune all non-zero elements that not satisfy the 'keep' operand
+ // elements that are not in the block rows or are not in the block columns should be kept
+ //original.prune([](int i, int j, double) { return (i < row || i > row + Nrows-1) || (j < col || j > col + Ncols-1); });
+
+ block_pruning bp(row, col, Nrows, Ncols);
+ original.prune(bp);
+}
+
+void add_sparse_block(const MatrixXd& ins, SparseMatrix<double>& original, const unsigned int& row, const unsigned int& col)
+{
+ for (uint r=0; r<ins.rows(); ++r)
+ for (uint c = 0; c < ins.cols(); c++)
+ if (ins(r,c) != 0)
+ original.coeffRef(r + row, c + col) += ins(r,c);
+}
+
+struct node
+{
+ public:
+ int id;
+ int dim;
+ int location;
+ int order;
+
+ node(const int _id, const int _dim, const int _location, const int _order) :
+ id(_id),
+ dim(_dim),
+ location(_location),
+ order(_order)
+ {
+
+ }
+};
+
+struct measurement
+{
+ public:
+ std::vector<int> nodes_idx;
+ VectorXd error;
+ int dim;
+ int location;
+
+ measurement(const int _idx1, const VectorXd &_error, const int _meas_dim) :
+ nodes_idx({_idx1}),
+ error(_error),
+ dim(_meas_dim),
+ location(0)
+ {
+ //jacobians.push_back(_jacobian1);
+ }
+
+ measurement(const int _idx1, const int _idx2, const VectorXd &_error, const int _meas_dim) :
+ nodes_idx({_idx1, _idx2}),
+ error(_error),
+ dim(_meas_dim),
+ location(0)
+ {
+
+ }
+
+ measurement(const std::vector<int> & _idxs, const VectorXd &_error, const int _meas_dim) :
+ nodes_idx(_idxs),
+ error(_error),
+ dim(_meas_dim),
+ location(0)
+ {
+
+ }
+};
+
+class SolverQR
+{
+ protected:
+ std::string name_;
+ SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_;
+ SparseMatrix<double> A_, R_;
+ VectorXd b_, x_incr_;
+ int n_measurements;
+ int n_nodes_;
+ std::vector<node> nodes_;
+ std::vector<StateBase*> states_;
+ std::vector<measurement> measurements_;
+ std::vector<ConstraintBase*> constraints_;
+
+ // ordering
+ SparseMatrix<int> A_nodes_;
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_;
+ std::map<int, int> id_2_idx;
+
+ CCOLAMDOrdering<int> ordering_;
+ VectorXi nodes_ordering_constraints_;
+ int n_new_measurements_;
+
+ // time
+ clock_t t_ordering_, t_solving_, t_managing_;
+ double time_ordering_, time_solving_, time_managing_;
+
+
+
+ public:
+ SolverQR(const std::string &_name) :
+ name_(_name),
+ A_(0,0),
+ R_(0,0),
+ n_measurements(0),
+ n_nodes_(0),
+ A_nodes_(0,0),
+ acc_permutation_nodes_(0),
+ time_ordering_(0),
+ time_solving_(0),
+ time_managing_(0)
+ {
+ //
+ }
+
+ virtual ~SolverQR()
+ {
+
+ }
+
+ void update(WolfProblem* _problem_ptr)
+ {
+ // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
+ if (_problem_ptr->isReallocated())
+ {
+ // TODO
+// // Remove all parameter blocks (residual blocks will be also removed)
+// removeAllStateUnits();
+//
+// // Add all parameter blocks
+// for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
+// addStateUnit(*state_unit_it);
+//
+// // Add all residual blocks
+// ConstraintBaseList ctr_list;
+// _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
+// for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
+// addConstraint(*ctr_it);
+//
+// // set the wolf problem reallocation flag to false
+// _problem_ptr->reallocationDone();
+ }
+ else
+ {
+ // ADD/UPDATE STATE UNITS
+ for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
+ {
+ if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
+ addStateUnit(*state_unit_it);
+
+ else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
+ updateStateUnitStatus(*state_unit_it);
+ }
+ //std::cout << "state units updated!" << std::endl;
+
+ // REMOVE STATE UNITS
+ while (!_problem_ptr->getRemovedStateListPtr()->empty())
+ {
+ // TODO
+ _problem_ptr->getRemovedStateListPtr()->pop_front();
+ }
+ //std::cout << "state units removed!" << std::endl;
+
+ // ADD CONSTRAINTS
+ ConstraintBaseList ctr_list;
+ _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
+ //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
+ for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
+ if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
+ addConstraint(*ctr_it);
+
+ //std::cout << "constraints updated!" << std::endl;
+ }
+ }
+
+ void addStateUnit(StateBase* _state_ptr)
+ {
+ std::cout << "adding state unit " << _state_ptr->nodeId() << std::endl;
+ if (_state_ptr->getStateStatus() == ST_ESTIMATED)
+ {
+ states_.push_back(_state_ptr);
+ id_2_idx[_state_ptr->nodeId()] = states_.size()-1;
+
+ std::cout << "idx " << id_2_idx[_state_ptr->nodeId()] << std::endl;
+
+ t_managing_ = clock();
+ unsigned int node_dim = _state_ptr->getStateSize();
+ int node_idx= _state_ptr->nodeId();
+
+ n_nodes_++;
+ nodes_.push_back(node(node_idx, node_dim, x_incr_.size(), n_nodes_-1));
+
+ // Resize accumulated permutations
+ augment_permutation(acc_permutation_nodes_, n_nodes_);
+
+ // Resize state
+ x_incr_.conservativeResize(x_incr_.size() + node_dim);
+
+ // Resize problem
+ A_.conservativeResize(A_.rows(), A_.cols() + node_dim);
+ R_.conservativeResize(R_.cols() + node_dim, R_.cols() + node_dim);
+ //A_nodes_.conservativeResize(n_measurements, n_nodes); // not necessary
+
+ time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
+ }
+ _state_ptr->setPendingStatus(NOT_PENDING);
+ }
+
+ void updateStateUnitStatus(StateBase* _state_ptr)
+ {
+ //TODO
+ }
+
+ void getResidualJacobians(const ConstraintBase* _constraint_ptr, VectorXs &error, std::vector<MatrixXs>& jacobians)
+ {
+ assert(error.size() == _constraint_ptr->getSize());
+
+ if (!jacobians.empty())
+ jacobians.clear();
+
+ //TODO get the jacobian calling functor operator()
+ for (int i = 0; i < _constraint_ptr->getStatePtrVector().size(); i++)
+ if (_constraint_ptr->getStatePtrVector().at(i)->getStateStatus() == ST_ESTIMATED)
+ jacobians.push_back(MatrixXs::Constant(_constraint_ptr->getSize(), _constraint_ptr->getStatePtrVector().at(i)->getStateSize(), 0.1) + MatrixXs::Identity(_constraint_ptr->getSize(), _constraint_ptr->getStatePtrVector().at(i)->getStateSize()));
+
+ // TODO get the error calling functor operator()
+ error = VectorXs::LinSpaced(_constraint_ptr->getSize(), _constraint_ptr->getSize() * _constraint_ptr->nodeId(), _constraint_ptr->getSize() * _constraint_ptr->nodeId()+_constraint_ptr->getSize()-1);
+ }
+
+ void addConstraint(ConstraintBase* _constraint_ptr)
+ {
+ std::cout << "adding constraint " << _constraint_ptr->nodeId() << std::endl;
+ t_managing_ = clock();
+
+ constraints_.push_back(_constraint_ptr);
+
+ int meas_dim = _constraint_ptr->getSize();
+
+ std::vector<MatrixXs> jacobians(_constraint_ptr->getStatePtrVector().size());
+ VectorXs error(_constraint_ptr->getSize());
+
+ getResidualJacobians(_constraint_ptr, error, jacobians);
+
+ std::vector<int> idxs;
+ for (int i = 0; i < _constraint_ptr->getStatePtrVector().size(); i++)
+ if (_constraint_ptr->getStatePtrVector().at(i)->getStateStatus() == ST_ESTIMATED)
+ idxs.push_back(id_2_idx[_constraint_ptr->getStatePtrVector().at(i)->nodeId()]);
+
+ measurement _meas(idxs, error, meas_dim);
+
+ n_measurements++;
+ n_new_measurements_++;
+ measurements_.push_back(_meas);
+ measurements_.back().location = A_.rows();
+
+ // Resize problem
+ A_.conservativeResize(A_.rows() + meas_dim, A_.cols());
+ b_.conservativeResize(b_.size() + meas_dim);
+ A_nodes_.conservativeResize(n_measurements,n_nodes_);
+
+ // ADD MEASUREMENTS
+ for (unsigned int j = 0; j < idxs.size(); j++)
+ {
+ assert(acc_permutation_nodes_.indices()(idxs.at(j)) == nodes_.at(idxs.at(j)).order);
+
+ int ordered_node = nodes_.at(idxs.at(j)).order;//acc_permutation_nodes_.indices()(_nodes_idx.at(j));
+
+ add_sparse_block(jacobians.at(j), A_, A_.rows()-meas_dim, nodes_.at(idxs.at(j)).location);
+
+ A_nodes_.coeffRef(A_nodes_.rows()-1, ordered_node) = 1;
+
+ assert(jacobians.at(j).cols() == nodes_.at(idxs.at(j)).dim);
+ assert(jacobians.at(j).rows() == meas_dim);
+ }
+
+ // error
+ b_.tail(meas_dim) = error;
+
+ _constraint_ptr->setPendingStatus(NOT_PENDING);
+
+ time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
+ }
+
+ void ordering(const int & _first_ordered_idx)
+ {
+ std::cout << "ordering from idx " << _first_ordered_idx << std::endl;
+ t_ordering_ = clock();
+
+ // full problem ordering
+ if (_first_ordered_idx == -1)
+ {
+ // ordering ordering constraints
+ nodes_ordering_constraints_.resize(n_nodes_);
+ nodes_ordering_constraints_ = A_nodes_.bottomRows(1).transpose();
+
+ // computing nodes partial ordering_
+ A_nodes_.makeCompressed();
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(n_nodes_);
+ ordering_(A_nodes_, incr_permutation_nodes, nodes_ordering_constraints_.data());
+
+ // node ordering to variable ordering
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation(A_.cols());
+ permutation_2_block_permutation(incr_permutation_nodes, incr_permutation);
+
+ // apply partial_ordering orderings
+ A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
+ A_ = (A_ * incr_permutation.transpose()).sparseView();
+
+ // ACCUMULATING PERMUTATIONS
+ accumulate_permutation(incr_permutation_nodes);
+ }
+
+ // partial ordering
+ else
+ {
+ int first_ordered_node = nodes_.at(_first_ordered_idx).order;
+ int ordered_nodes = n_nodes_ - first_ordered_node;
+ int unordered_nodes = n_nodes_ - ordered_nodes;
+ if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
+ {
+ // SUBPROBLEM ORDERING (from first node variable to last one)
+ //std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
+ SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
+
+ // _partial_ordering ordering_ constraints
+ nodes_ordering_constraints_.resize(ordered_nodes);
+ nodes_ordering_constraints_ = sub_A_nodes_.bottomRows(1).transpose();
+
+ // computing nodes partial ordering_
+ sub_A_nodes_.makeCompressed();
+ PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes(ordered_nodes);
+ ordering_(sub_A_nodes_, partial_permutation_nodes, nodes_ordering_constraints_.data());
+
+ // node ordering to variable ordering
+ int ordered_variables = A_.cols() - nodes_.at(_first_ordered_idx).location;
+// std::cout << "first_ordered_node " << first_ordered_node << std::endl;
+// std::cout << "A_.cols() " << A_.cols() << std::endl;
+// std::cout << "nodes_.at(_first_ordered_idx).location " << nodes_.at(_first_ordered_idx).location << std::endl;
+// std::cout << "ordered_variables " << ordered_variables << std::endl;
+ PermutationMatrix<Dynamic, Dynamic, int> partial_permutation(ordered_variables);
+ permutation_2_block_permutation(partial_permutation_nodes, partial_permutation);
+
+ // apply partial_ordering orderings
+ A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes) * partial_permutation_nodes.transpose()).sparseView();
+ A_.rightCols(ordered_variables) = (A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
+ R_.rightCols(ordered_variables) = (R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
+
+ // ACCUMULATING PERMUTATIONS
+ accumulate_permutation(partial_permutation_nodes);
+ }
+ }
+ time_ordering_ += ((double) clock() - t_ordering_) / CLOCKS_PER_SEC;
+ }
+
+ bool solve(const int mode)
+ {
+ if (n_new_measurements_ == 0)
+ return 1;
+
+ std::cout << "solving mode " << mode << std::endl;
+
+ int first_ordered_node = n_nodes_;
+ int first_ordered_idx;
+ for (int i = 0; i < n_new_measurements_; i++)
+ {
+ ConstraintBase* ct_ptr = constraints_.at(constraints_.size()-1-i);
+ for (int j = 0; j < ct_ptr->getStatePtrVector().size(); j++)
+ {
+ int idx = id_2_idx[ct_ptr->getStatePtrVector().at(j)->nodeId()];
+ if (first_ordered_node > nodes_.at(idx).order)
+ {
+ first_ordered_idx = idx;
+ first_ordered_node = nodes_.at(idx).order;
+ }
+ }
+ }
+// std::cout << "first_ordered_node " << first_ordered_node << std::endl;
+// std::cout << "first_ordered_idx " << first_ordered_idx << std::endl;
+
+ bool batch = (mode !=2 || first_ordered_node == 0);
+ bool order = (mode !=0 && n_nodes_ > 1);
+
+ // BATCH
+ if (batch)
+ {
+ // REORDER
+ if (order)
+ ordering(-1);
+
+ //print_problem();
+
+ // SOLVE
+ t_solving_ = clock();
+ A_.makeCompressed();
+ solver_.compute(A_);
+ if (solver_.info() != Success)
+ {
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
+ }
+ x_incr_ = solver_.solve(b_);
+ R_ = solver_.matrixR();
+ //std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
+ time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
+ }
+ // INCREMENTAL
+ else
+ {
+ // REORDER SUBPROBLEM
+ ordering(first_ordered_idx);
+ //print_problem();
+
+ // SOLVE ORDERED SUBPROBLEM
+ t_solving_= clock();
+ A_nodes_.makeCompressed();
+ A_.makeCompressed();
+
+ // finding measurements block
+ SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node);
+ // std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
+ // std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
+ int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
+ int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
+ int ordered_variables = A_.cols() - nodes_.at(first_ordered_idx).location;
+ int unordered_variables = nodes_.at(first_ordered_idx).location;
+
+ SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
+ solver_.compute(A_partial);
+ if (solver_.info() != Success)
+ {
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
+ }
+ //std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
+ x_incr_.tail(ordered_variables) = solver_.solve(b_.tail(ordered_measurements));
+
+ // store new part of R
+ erase_sparse_block(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
+ //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
+ add_sparse_block(solver_.matrixR(), R_, unordered_variables, unordered_variables);
+ //std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
+ R_.makeCompressed();
+
+ // solving not ordered subproblem
+ if (unordered_variables > 0)
+ {
+ //std::cout << "--------------------- solving unordered part" << std::endl;
+ SparseMatrix<double> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
+ //std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
+ SparseMatrix<double> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
+ //std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
+ solver_.compute(R1);
+ if (solver_.info() != Success)
+ {
+ std::cout << "decomposition failed" << std::endl;
+ return 0;
+ }
+ x_incr_.head(unordered_variables) = solver_.solve(b_.head(unordered_variables) - R2 * x_incr_.tail(ordered_variables));
+ }
+
+ }
+ // UNDO ORDERING FOR RESULT
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation(A_.cols());
+ permutation_2_block_permutation(acc_permutation_nodes_, acc_permutation); // TODO via pointers
+ x_incr_ = acc_permutation.inverse() * x_incr_;
+
+ time_solving_ += ((double) clock() - t_solving_) / CLOCKS_PER_SEC;
+ n_new_measurements_ = 0;
+ return 1;
+ }
+
+
+ void permutation_2_block_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes, PermutationMatrix<Dynamic, Dynamic, int> &perm_variables)
+ {
+ //std::cout << "perm_nodes: " << _perm_nodes.indices().transpose() << std::endl;
+ ArrayXi locations = perm_nodes_2_locations(_perm_nodes);
+ //std::cout << "locations: " << locations.transpose() << std::endl;
+ //std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
+
+ int last_idx = 0;
+ for (unsigned int i = 0; i<locations.size(); i++)
+ {
+ perm_variables.indices().segment(last_idx, nodes_.at(i).dim) = VectorXi::LinSpaced(nodes_.at(i).dim, locations(i), locations(i)+nodes_.at(i).dim-1);
+ last_idx += nodes_.at(i).dim;
+ //std::cout << i << " perm_variables: " << perm_variables.indices().transpose() << std::endl;
+ }
+ //std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
+ }
+
+ ArrayXi perm_nodes_2_locations(const PermutationMatrix<Dynamic, Dynamic, int> &_perm_nodes)
+ {
+ ArrayXi indices = _perm_nodes.indices().array();
+
+ for (unsigned int i = 0; i<indices.size(); i++)
+ indices = (indices > indices(i)).select(indices + nodes_.at(i).dim-1, indices);
+
+ return indices;
+ }
+
+ void augment_permutation(PermutationMatrix<Dynamic, Dynamic, int> &perm, const int new_size)
+ {
+ int old_size = perm.indices().size();
+ int dim = new_size - old_size;
+ VectorXi new_indices(new_size);
+ new_indices.head(old_size)= perm.indices();
+ new_indices.tail(dim) = ArrayXi::LinSpaced(dim, old_size, new_size-1);
+ perm.resize(new_size);
+ perm.indices() = new_indices;
+ }
+
+ void accumulate_permutation(const PermutationMatrix<Dynamic, Dynamic, int> &perm)
+ {
+ print_name();
+ //std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
+ //std::cout << "incr perm " << perm.indices().transpose() << std::endl;
+
+ // acumulate permutation
+ if (perm.size() == acc_permutation_nodes_.size()) //full permutation
+ acc_permutation_nodes_ = perm * acc_permutation_nodes_;
+ else //partial permutation
+ {
+ PermutationMatrix<Dynamic, Dynamic, int> incr_permutation_nodes(VectorXi::LinSpaced(n_nodes_, 0, n_nodes_ - 1)); // identity permutation
+ incr_permutation_nodes.indices().tail(perm.size()) = perm.indices() + VectorXi::Constant(perm.size(), n_nodes_ - perm.size());
+ //std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
+ acc_permutation_nodes_ = incr_permutation_nodes * acc_permutation_nodes_;
+ }
+ //std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
+
+ // update nodes orders and locations
+ ArrayXi locations = perm_nodes_2_locations(acc_permutation_nodes_);
+ for (int i = 0; i < nodes_.size(); i++)
+ {
+ nodes_.at(i).order = acc_permutation_nodes_.indices()(i);
+ nodes_.at(i).location = locations(i);
+ }
+ }
+
+ void print_name()
+ {
+ std::cout << name_;
+ }
+
+ void print_results()
+ {
+ print_name();
+ std::cout << " solved in " << time_solving_*1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"<< std::endl;
+ std::cout << "x = " << x_incr_.transpose() << std::endl;
+ }
+
+ void print_problem()
+ {
+ print_name();
+ std::cout << std::endl << "A_nodes_: " << std::endl << MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl;
+ //std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
+ std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
+ }
+};
+
+//main
+int main(int argc, char *argv[])
+{
+ // USER INPUT ============================================================================================
+ if (argc != 2 || atoi(argv[1]) < 1 || atoi(argv[1]) > 1100)
+ {
+ std::cout << "Please call me with: [./test_ceres_manager NI], where:" << std::endl;
+ std::cout << " - NI is the number of iterations (0 < NI < 1100)" << std::endl;
+ std::cout << "EXIT due to bad user input" << std::endl << std::endl;
+ return -1;
+ }
+ bool complex_angle = false;
+ unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution
+
+ // INITIALIZATION ============================================================================================
+ // Problems
+ SolverQR solver_ordered("FULL ORDERED");
+ SolverQR solver_unordered("UNORDERED");
+ SolverQR solver_ordered_partial("PARTIALLY ORDERED");
+
+ //init random generators
+ WolfScalar odom_std_factor = 0.1;
+ WolfScalar gps_std = 1;
+ std::default_random_engine generator(1);
+ std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std_factor); //odometry noise
+ std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
+
+ std::ofstream log_file, landmark_file; //output file
+
+ // Faramotics stuff
+ Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
+ vector < Cpose3d > devicePoses;
+ vector<float> scan1, scan2;
+ string modelFileName;
+
+ //model and initial view point
+ modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
+ //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
+ //modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
+ devicePose.setPose(2, 8, 0.2, 0, 0, 0);
+ viewPoint.setPose(devicePose);
+ viewPoint.moveForward(10);
+ viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
+ viewPoint.moveForward(-15);
+ //glut initialization
+ faramotics::initGLUT(argc, argv);
+
+ //create a viewer for the 3D model and scan points
+ CdynamicSceneRender* myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
+ myRender->loadAssimpModel(modelFileName, true); //with wireframe
+ //create scanner and load 3D model
+ CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
+ myScanner->loadAssimpModel(modelFileName);
+
+ //variables
+ Eigen::Vector3s odom_reading;
+ Eigen::Vector2s gps_fix_reading;
+ Eigen::VectorXs pose_odom(3); //current odometry integred pose
+ Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
+ Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
+ clock_t t1, t2;
+
+ // Wolf manager initialization
+ Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
+ Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
+ Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
+ laser_1_pose << 1.2, 0, 0, 0; //laser 1
+ laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
+ SensorOdom2D odom_sensor(new StatePoint3D(odom_pose.data()), new StateTheta(&odom_pose(2)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StatePoint3D(gps_pose.data()), new StateTheta(&gps_pose(2)), gps_std);
+ SensorLaser2D laser_1_sensor(new StatePoint3D(laser_1_pose.data()), new StateTheta(&laser_1_pose(3)));
+ SensorLaser2D laser_2_sensor(new StatePoint3D(laser_2_pose.data()), new StateTheta(&laser_2_pose(3)));
+
+ // Initial pose
+ pose_odom << 2, 8, 0;
+ ground_truth.head(3) = pose_odom;
+ odom_trajectory.head(3) = pose_odom;
+
+ WolfManager* wolf_manager = new WolfManager(1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10);
+
+ std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
+ std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
+ // START TRAJECTORY ============================================================================================
+ for (unsigned int step = 1; step < n_execution; step++)
+ {
+ //get init time
+ t2 = clock();
+
+ // ROBOT MOVEMENT ---------------------------
+ //std::cout << "ROBOT MOVEMENT..." << std::endl;
+ // moves the device position
+ t1 = clock();
+ motionCampus(step, devicePose, odom_reading(0), odom_reading(2));
+ odom_reading(1) = 0;
+ devicePoses.push_back(devicePose);
+
+ // SENSOR DATA ---------------------------
+ //std::cout << "SENSOR DATA..." << std::endl;
+ // store groundtruth
+ ground_truth.segment(step * 3, 3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
+
+ // compute odometry
+ odom_reading(0) += distribution_odom(generator) * (odom_reading(0) == 0 ? 1e-6 : odom_reading(0));
+ odom_reading(1) += distribution_odom(generator) * 1e-6;
+ odom_reading(2) += distribution_odom(generator) * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2));
+
+ // odometry integration
+ pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)) - odom_reading(1) * sin(pose_odom(2));
+ pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2)) + odom_reading(1) * cos(pose_odom(2));
+ pose_odom(2) = pose_odom(2) + odom_reading(1);
+ odom_trajectory.segment(step * 3, 3) = pose_odom;
+
+ // compute GPS
+ gps_fix_reading << devicePose.pt(0), devicePose.pt(1);
+ gps_fix_reading(0) += distribution_gps(generator);
+ gps_fix_reading(1) += distribution_gps(generator);
+
+ //compute scans
+ scan1.clear();
+ scan2.clear();
+ // scan 1
+ laser1Pose.setPose(devicePose);
+ laser1Pose.moveForward(laser_1_pose(0));
+ myScanner->computeScan(laser1Pose, scan1);
+ // scan 2
+ laser2Pose.setPose(devicePose);
+ laser2Pose.moveForward(laser_2_pose(0));
+ laser2Pose.rt.setEuler(laser2Pose.rt.head() + M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll());
+ myScanner->computeScan(laser2Pose, scan2);
+
+ mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // ADD CAPTURES ---------------------------
+ //std::cout << "ADD CAPTURES..." << std::endl;
+ // adding new sensor captures
+ wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
+// wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
+ //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
+ //wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
+ // updating problem
+ wolf_manager->update();
+
+ // UPDATING SOLVER ---------------------------
+ //std::cout << "UPDATING..." << std::endl;
+ // update state units and constraints in ceres
+ solver_unordered.update(wolf_manager->getProblemPtr());
+
+ // PRINT PROBLEM
+ solver_unordered.print_problem();
+
+ // SOLVE OPTIMIZATION ---------------------------
+ //std::cout << "SOLVING..." << std::endl;
+ solver_unordered.solve(2);
+
+ std::cout << "========================= RESULTS " << step << ":" << std::endl;
+ solver_unordered.print_results();
+
+ // COMPUTE COVARIANCES ---------------------------
+ //std::cout << "COMPUTING COVARIANCES..." << std::endl;
+ // TODO
+
+ // DRAWING STUFF ---------------------------
+ // draw detected corners
+ std::list < laserscanutils::Corner > corner_list;
+ std::vector<double> corner_vector;
+ CaptureLaser2D last_scan(TimeStamp(), &laser_1_sensor, scan1);
+ last_scan.extractCorners(corner_list);
+ for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++)
+ {
+ corner_vector.push_back(corner_it->pt_(0));
+ corner_vector.push_back(corner_it->pt_(1));
+ }
+ myRender->drawCorners(laser1Pose, corner_vector);
+
+ // draw landmarks
+ std::vector<double> landmark_vector;
+ for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ {
+ WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+ landmark_vector.push_back(*position_ptr); //x
+ landmark_vector.push_back(*(position_ptr + 1)); //y
+ landmark_vector.push_back(0.2); //z
+ }
+ myRender->drawLandmarks(landmark_vector);
+
+ // draw localization and sensors
+ estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0), wolf_manager->getVehiclePose()(1), 0.2, wolf_manager->getVehiclePose()(2), 0, 0);
+ estimated_laser_1_pose.setPose(estimated_vehicle_pose);
+ estimated_laser_1_pose.moveForward(laser_1_pose(0));
+ estimated_laser_2_pose.setPose(estimated_vehicle_pose);
+ estimated_laser_2_pose.moveForward(laser_2_pose(0));
+ estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
+ myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
+
+ //Set view point and render the scene
+ //locate visualization view point, somewhere behind the device
+ myRender->setViewPoint(viewPoint);
+ myRender->drawPoseAxis(devicePose);
+ myRender->drawScan(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
+ myRender->render();
+
+ // TIME MANAGEMENT ---------------------------
+ double dt = ((double) clock() - t2) / CLOCKS_PER_SEC;
+ mean_times(6) += dt;
+ if (dt < 0.1)
+ usleep(100000 - 1e6 * dt);
+
+// std::cout << "\nTree after step..." << std::endl;
+// wolf_manager->getProblemPtr()->print();
+ }
+
+ // DISPLAY RESULTS ============================================================================================
+ mean_times /= n_execution;
+ std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl;
+ std::cout << " data generation: " << mean_times(0) << std::endl;
+ std::cout << " wolf managing: " << mean_times(1) << std::endl;
+ std::cout << " ceres managing: " << mean_times(2) << std::endl;
+ std::cout << " ceres optimization: " << mean_times(3) << std::endl;
+ std::cout << " ceres covariance: " << mean_times(4) << std::endl;
+ std::cout << " results drawing: " << mean_times(5) << std::endl;
+ std::cout << " loop time: " << mean_times(6) << std::endl;
+
+// std::cout << "\nTree before deleting..." << std::endl;
+// wolf_manager->getProblemPtr()->print();
+
+ // Draw Final result -------------------------
+ std::vector<double> landmark_vector;
+ for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ {
+ WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+ landmark_vector.push_back(*position_ptr); //x
+ landmark_vector.push_back(*(position_ptr + 1)); //y
+ landmark_vector.push_back(0.2); //z
+ }
+ myRender->drawLandmarks(landmark_vector);
+// viewPoint.setPose(devicePoses.front());
+// viewPoint.moveForward(10);
+// viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
+// viewPoint.moveForward(-10);
+ myRender->setViewPoint(viewPoint);
+ myRender->render();
+
+ // Print Final result in a file -------------------------
+ // Vehicle poses
+ int i = 0;
+ Eigen::VectorXs state_poses(n_execution * 3);
+ for (auto frame_it = wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->begin(); frame_it != wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->end(); frame_it++)
+ {
+ if (complex_angle)
+ state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), atan2(*(*frame_it)->getOPtr()->getPtr(), *((*frame_it)->getOPtr()->getPtr() + 1));
+ else
+ state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), *(*frame_it)->getOPtr()->getPtr();
+ i += 3;
+ }
+
+ // Landmarks
+ i = 0;
+ Eigen::VectorXs landmarks(wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->size() * 2);
+ for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++)
+ {
+ Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr());
+ landmarks.segment(i, 2) = landmark;
+ i += 2;
+ }
+
+ // Print log files
+ std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt"));
+ log_file.open(filepath, std::ofstream::out); //open log file
+
+ if (log_file.is_open())
+ {
+ log_file << 0 << std::endl;
+ for (unsigned int ii = 0; ii < n_execution; ii++)
+ log_file << state_poses.segment(ii * 3, 3).transpose() << "\t" << ground_truth.segment(ii * 3, 3).transpose() << "\t" << (state_poses.segment(ii * 3, 3) - ground_truth.segment(ii * 3, 3)).transpose() << "\t" << odom_trajectory.segment(ii * 3, 3).transpose() << std::endl;
+ log_file.close(); //close log file
+ std::cout << std::endl << "Result file " << filepath << std::endl;
+ }
+ else
+ std::cout << std::endl << "Failed to write the log file " << filepath << std::endl;
+
+ std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt"));
+ landmark_file.open(filepath2, std::ofstream::out); //open log file
+
+ if (landmark_file.is_open())
+ {
+ for (unsigned int ii = 0; ii < landmarks.size(); ii += 2)
+ landmark_file << landmarks.segment(ii, 2).transpose() << std::endl;
+ landmark_file.close(); //close log file
+ std::cout << std::endl << "Landmark file " << filepath << std::endl;
+ }
+ else
+ std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl;
+
+ std::cout << "Press any key for ending... " << std::endl << std::endl;
+ std::getchar();
+
+ delete myRender;
+ delete myScanner;
+ delete wolf_manager;
+ std::cout << "wolf deleted" << std::endl;
+
+ std::cout << " ========= END ===========" << std::endl << std::endl;
+
+ //exit
+ return 0;
+}
+
+
+
+
+
+
diff --git a/src/solver/CMakeLists.txt b/src/solver/CMakeLists.txt
new file mode 100644
index 000000000..32da45f88
--- /dev/null
+++ b/src/solver/CMakeLists.txt
@@ -0,0 +1,4 @@
+SET(HDRS_SOLVER
+ ccolamd_ordering.h )
+SET(HDRS_SOLVER
+ )
\ No newline at end of file
diff --git a/src/solver/solver_QR.h b/src/solver/solver_QR.h
new file mode 100644
index 000000000..2fa0d11d1
--- /dev/null
+++ b/src/solver/solver_QR.h
@@ -0,0 +1,33 @@
+/*
+ * solver_QR.h
+ *
+ * Created on: Jun 22, 2015
+ * Author: jvallve
+ */
+
+#ifndef TRUNK_SRC_SOLVER_SOLVER_QR_H_
+#define TRUNK_SRC_SOLVER_SOLVER_QR_H_
+
+using namespace Eigen;
+
+class SolverQR
+{
+ protected:
+ SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_;
+ SparseMatrix<double> A, A_ordered, R;
+ VectorXd b, x, x_ordered, x_ordered_partial;
+ int n_measurements = 0;
+ int n_nodes = 0;
+
+ // ordering variables
+ SparseMatrix<int> A_nodes_ordered;
+ PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix;
+
+ CCOLAMDOrdering<int> ordering, partial_ordering;
+ VectorXi nodes_ordering_constraints;
+
+ private:
+};
+
+
+#endif /* TRUNK_SRC_SOLVER_SOLVER_QR_H_ */
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
new file mode 100644
index 000000000..16d7ab67a
--- /dev/null
+++ b/src/solver/solver_manager.cpp
@@ -0,0 +1,228 @@
+#include "ceres_manager.h"
+
+SolverManager::SolverManager()
+{
+
+}
+
+SolverManager::~SolverManager()
+{
+ removeAllStateUnits();
+}
+
+void SolverManager::solve()
+{
+
+}
+
+//void SolverManager::computeCovariances(WolfProblem* _problem_ptr)
+//{
+//}
+
+void SolverManager::update(WolfProblem* _problem_ptr)
+{
+ // IF REALLOCATION OF STATE, REMOVE EVERYTHING AND BUILD THE PROBLEM AGAIN
+ if (_problem_ptr->isReallocated())
+ {
+ // todo: reallocate x
+ }
+ else
+ {
+ // ADD/UPDATE STATE UNITS
+ for(auto state_unit_it = _problem_ptr->getStateListPtr()->begin(); state_unit_it!=_problem_ptr->getStateListPtr()->end(); state_unit_it++)
+ {
+ if ((*state_unit_it)->getPendingStatus() == ADD_PENDING)
+ addStateUnit(*state_unit_it);
+
+ else if((*state_unit_it)->getPendingStatus() == UPDATE_PENDING)
+ updateStateUnitStatus(*state_unit_it);
+ }
+ //std::cout << "state units updated!" << std::endl;
+
+ // REMOVE STATE UNITS
+ while (!_problem_ptr->getRemovedStateListPtr()->empty())
+ {
+ // TODO: remove state unit
+ //_problem_ptr->getRemovedStateListPtr()->pop_front();
+ }
+ //std::cout << "state units removed!" << std::endl;
+
+ // ADD CONSTRAINTS
+ ConstraintBaseList ctr_list;
+ _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
+ //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
+ for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
+ if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
+ addConstraint(*ctr_it);
+
+ //std::cout << "constraints updated!" << std::endl;
+ }
+}
+
+void SolverManager::addConstraint(ConstraintBase* _corr_ptr)
+{
+ //TODO MatrixXs J; Vector e;
+ // getResidualsAndJacobian(_corr_ptr, J, e);
+ // solverQR->addConstraint(_corr_ptr, J, e);
+
+// constraint_map_[_corr_ptr->nodeId()] = blockIdx;
+ _corr_ptr->setPendingStatus(NOT_PENDING);
+}
+
+void SolverManager::removeConstraint(const unsigned int& _corr_idx)
+{
+ // TODO
+}
+
+void SolverManager::addStateUnit(StateBase* _st_ptr)
+{
+ //std::cout << "Adding State Unit " << _st_ptr->nodeId() << std::endl;
+ //_st_ptr->print();
+
+ switch (_st_ptr->getStateType())
+ {
+ case ST_COMPLEX_ANGLE:
+ {
+ // TODO
+ //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
+ //ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
+ break;
+ }
+ case ST_THETA:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateTheta*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_1D:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_2D:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint2D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ case ST_POINT_3D:
+ {
+ //std::cout << "No Local Parametrization to be added" << std::endl;
+ ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint3D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown Local Parametrization type!" << std::endl;
+ }
+ if (_st_ptr->getStateStatus() != ST_ESTIMATED)
+ updateStateUnitStatus(_st_ptr);
+
+ _st_ptr->setPendingStatus(NOT_PENDING);
+}
+
+void SolverManager::removeAllStateUnits()
+{
+ std::vector<double*> parameter_blocks;
+
+ ceres_problem_->GetParameterBlocks(¶meter_blocks);
+
+ for (unsigned int i = 0; i< parameter_blocks.size(); i++)
+ ceres_problem_->RemoveParameterBlock(parameter_blocks[i]);
+}
+
+void SolverManager::updateStateUnitStatus(StateBase* _st_ptr)
+{
+ // TODO
+
+// if (_st_ptr->getStateStatus() == ST_ESTIMATED)
+// ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
+// else if (_st_ptr->getStateStatus() == ST_FIXED)
+// ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
+// else
+// printf("\nERROR: Update state unit status with unknown status");
+//
+// _st_ptr->setPendingStatus(NOT_PENDING);
+}
+
+ceres::CostFunction* SolverManager::createCostFunction(ConstraintBase* _corrPtr)
+{
+ //std::cout << "adding ctr " << _corrPtr->nodeId() << std::endl;
+ //_corrPtr->print();
+
+ switch (_corrPtr->getConstraintType())
+ {
+ case CTR_GPS_FIX_2D:
+ {
+ ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintGPS2D,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_ODOM_2D_COMPLEX_ANGLE:
+ {
+ ConstraintOdom2DComplexAngle* specific_ptr = (ConstraintOdom2DComplexAngle*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintOdom2DComplexAngle,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_ODOM_2D_THETA:
+ {
+ ConstraintOdom2DTheta* specific_ptr = (ConstraintOdom2DTheta*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintOdom2DTheta,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ case CTR_CORNER_2D_THETA:
+ {
+ ConstraintCorner2DTheta* specific_ptr = (ConstraintCorner2DTheta*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<ConstraintCorner2DTheta,
+ specific_ptr->measurementSize,
+ specific_ptr->block0Size,
+ specific_ptr->block1Size,
+ specific_ptr->block2Size,
+ specific_ptr->block3Size,
+ specific_ptr->block4Size,
+ specific_ptr->block5Size,
+ specific_ptr->block6Size,
+ specific_ptr->block7Size,
+ specific_ptr->block8Size,
+ specific_ptr->block9Size>(specific_ptr);
+ break;
+ }
+ default:
+ std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
+
+ return nullptr;
+ }
+}
diff --git a/src/solver/solver_manager.h b/src/solver/solver_manager.h
new file mode 100644
index 000000000..32604ed2a
--- /dev/null
+++ b/src/solver/solver_manager.h
@@ -0,0 +1,49 @@
+#ifndef CERES_MANAGER_H_
+#define CERES_MANAGER_H_
+
+//wolf includes
+#include "../wolf.h"
+#include "../state_base.h"
+#include "../state_point.h"
+#include "../state_complex_angle.h"
+#include "../state_theta.h"
+#include "../constraint_sparse.h"
+#include "../constraint_gps_2D.h"
+#include "../constraint_odom_2D_theta.h"
+#include "../constraint_odom_2D_complex_angle.h"
+#include "../constraint_corner_2D_theta.h"
+
+/** \brief solver manager for WOLF
+ *
+ */
+
+class SolverManager
+{
+ protected:
+
+
+ public:
+ SolverManager(ceres::Problem::Options _options);
+
+ ~SolverManager();
+
+ ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
+
+ //void computeCovariances(WolfProblem* _problem_ptr);
+
+ void update(const WolfProblemPtr _problem_ptr);
+
+ void addConstraint(ConstraintBase* _corr_ptr);
+
+ void removeConstraint(const unsigned int& _corr_idx);
+
+ void addStateUnit(StateBase* _st_ptr);
+
+ void removeAllStateUnits();
+
+ void updateStateUnitStatus(StateBase* _st_ptr);
+
+ ceres::CostFunction* createCostFunction(ConstraintBase* _corrPtr);
+};
+
+#endif
--
GitLab