working on covariance matrix

mxgrey · May 22, 2014 · 45cba6a · 45cba6a
1 parent b37b72a
commit 45cba6a
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 3 deletions.
diff --git a/ccrrt/Chomper.h b/ccrrt/Chomper.h
@@ -1,13 +1,23 @@
 #ifndef CHOMPER_H
 #define CHOMPER_H
 
+//#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
+
 #include "../ccrrt/Constraint.h"
+#include <Eigen/Sparse>
 
 namespace ccrrt {
 
 class Chomper
 {
 public:
+
+    typedef enum {
+
+        VELOCITY = 0,
+        ACCELERATION
+
+    } path_optimization_t;
 
     Chomper();
 
@@ -16,6 +26,8 @@ class Chomper
 
     const Trajectory& getTrajectory() const;
     const Constraint* const getConstraint() const; // Do we need so many consts?
+
+    path_optimization_t opt_type;
 
 protected:
 
@@ -25,11 +37,14 @@ class Chomper
     Eigen::VectorXd Df;
     Eigen::VectorXd h;
 
-    Eigen::MatrixXd A; // TODO: Make sparse matrix
+    Eigen::SparseMatrix<double> A; // TODO: Make sparse matrix
     Eigen::MatrixXd Ainv;
+    Eigen::SimplicialLLT<Eigen::SparseMatrix<double>, Eigen::Lower > llt;
     Eigen::MatrixXd H;
     Eigen::MatrixXd HAinvHt_inv;
     Eigen::MatrixXd Ainv_Ht_HAinvHt_inv;
+
+    void _generate_A(size_t state_space, size_t waypoints);
 };
 
 } // namespace ccrrt

diff --git a/src/Chomper.cpp b/src/Chomper.cpp
@@ -7,6 +7,7 @@ using namespace Eigen;
 Chomper::Chomper()
 {
     _constraint = NULL;
+    opt_type = VELOCITY;
 }
 
 const Trajectory& Chomper::getTrajectory() const
@@ -29,14 +30,25 @@ void Chomper::initialize(const Trajectory& trajectory, Constraint* constraint)
 
     Df.resize(nm);
     h.resize(_constraint->constraintDimension());
-    
-    A.resize(nm,nm);
+
+    _generate_A(trajectory.state_space, trajectory.waypoints);
     Ainv.resize(nm,nm);
     H.resize(c,nm);
     HAinvHt_inv.resize(c,c);
     Ainv_Ht_HAinvHt_inv.resize(nm,c);
 }
 
+void Chomper::_generate_A(size_t state_space, size_t waypoints)
+{
+    size_t nm = state_space*waypoints;
+    A.resize(nm,nm);
+
+    if(VELOCITY == opt_type)
+    {
+//        A.reserve();
+    }
+}
+
 Constraint::validity_t Chomper::iterate()
 {
     return Constraint::INVALID;

diff --git a/test/constraint_test.cpp b/test/constraint_test.cpp
@@ -27,6 +27,10 @@ void get_cost(Constraint& constraint, Trajectory& traj)
 
 int main(int argc, char* argv[])
 {
+    std::cout << "Eigen version: world " << EIGEN_WORLD_VERSION
+              << ", major " << EIGEN_MAJOR_VERSION << ", minor " << EIGEN_MINOR_VERSION
+              << std::endl;
+
     CircleConstraint circle(Vector2d(0,0), 2, 0.1);
     Trajectory traj;
     traj.state_space = 2;