Repository: prime-slam/mrob (language: python)

Issue:
merge from EFs and general FGraph common updates
This branch has grown too much and need to get back. Most of its functionalities are there


Discussion / hints:

Fix patch:
diff --git a/.gitignore b/.gitignore
index 81176ee..67817b8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -13,6 +13,7 @@
 build/
 lib/
 bin/
+cmake-build*
 *.so
 *.idea
 *.png
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5bb5bc9..e1d8698 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -57,9 +57,6 @@ INCLUDE_DIRECTORIES(./src/PCRegistration)
 ADD_SUBDIRECTORY(./src/PCRegistration)
 
 
-#INCLUDE_DIRECTORIES(./src/EigenFactors)
-#ADD_SUBDIRECTORY(./src/EigenFactors)
-
 
 # ===================================================================
 # New modules should be included here
diff --git a/external/Catch2 b/external/Catch2
index 7727c15..c4e3767 160000
--- a/external/Catch2
+++ b/external/Catch2
@@ -1,1 +1,1 @@
-Subproject commit 7727c15290ce2d289c1809d0eab3cceb88384ad6
+Subproject commit c4e3767e265808590986d5db6ca1b5532a7f3d13
diff --git a/mrobpy/FGraphPy.cpp b/mrobpy/FGraphPy.cpp
index e6bfa4e..7819dd5 100644
--- a/mrobpy/FGraphPy.cpp
+++ b/mrobpy/FGraphPy.cpp
@@ -41,8 +41,11 @@
 #include "mrob/factors/factor1Pose1Plane4d.hpp"
 
 #include "mrob/factors/factor1PosePoint2Plane.hpp"
+#include "mrob/factors/factor1PosePoint2Point.hpp"
 
-#include <Eigen/Geometry>
+#include "mrob/factors/EigenFactorPlane.hpp"
+
+//#include <Eigen/Geometry>
 
 namespace py = pybind11;
 using namespace mrob;
@@ -187,7 +190,7 @@ class FGraphPy : public FGraphSolve
         return f->get_id();
     }
 
-    // point to plane and p2p optimizations. Variants of weighted ICP
+    // point to plane iterative optimizations. Variants of weighted ICP using Fgraph
     // ----------------------------------------------------
     // point to Plane factor
     factor_id_t add_factor_1pose_point2plane(const py::EigenDRef<const Mat31> z_point_x, const py::EigenDRef<const Mat31> z_point_y,
@@ -198,6 +201,37 @@ class FGraphPy : public FGraphSolve
         this->add_factor(f);
         return f->get_id();
     }
+    // point to point iterative optimizations. Variants of weighted ICP using Fgraph
+    // ----------------------------------------------------
+    // point to point factor
+    factor_id_t add_factor_1pose_point2point(const py::EigenDRef<const Mat31> z_point_x, const py::EigenDRef<const Mat31> z_point_y,
+                                             factor_id_t nodePoseId, const py::EigenDRef<const Mat3> obsInf)
+    {
+        auto n1 = this->get_node(nodePoseId);
+        std::shared_ptr<mrob::Factor> f(new mrob::Factor1PosePoint2Point(z_point_x,z_point_y, n1,obsInf, robust_type_));
+        this->add_factor(f);
+        return f->get_id();
+    }
+
+
+    // Eigen factors
+    // --------------------------------------------------
+    // Eigen factor plane, it requires adding an empty structure and then each point will increase
+    // the set of points, at the given pose.
+    // NOTE: there is no need to specify pose.
+    factor_id_t add_eigen_factor_plane() //TODO add robust factor when created
+    {
+        std::shared_ptr<mrob::EigenFactor> f(new mrob::EigenFactorPlane(robust_type_));
+        this->add_eigen_factor(f);
+        return f->get_id();
+    }
+    void eigen_factor_plane_add_point(factor_id_t planeEigenId, factor_id_t nodePoseId, const py::EigenDRef<const Mat31> point, matData_t &W)
+    {
+        auto ef = this->get_eigen_factor(planeEigenId);
+        auto n = this->get_node(nodePoseId);//XXX should check if this is a 3D pose node...
+        ef->add_point(point, n, W);
+    }
+
 private:
     mrob::Factor::robustFactorType robust_type_;
 };
@@ -207,6 +241,7 @@ void init_FGraph(py::module &m)
     py::enum_<FGraphSolve::optimMethod>(m, "FGraph.optimMethod")
         .value("GN", FGraphSolve::optimMethod::GN)
         .value("LM", FGraphSolve::optimMethod::LM)
+        .value("LM_ELLIPS", FGraphSolve::optimMethod::LM_ELLIPS)
         .export_values()
         ;
     py::enum_<Factor::robustFactorType>(m, "FGraph.robustFactorType")
@@ -353,6 +388,22 @@ void init_FGraph(py::module &m)
                             py::arg("z_normal_y"),
                             py::arg("nodePoseId"),
                             py::arg("obsInf"))
+                            // point to plane registration
+            .def("add_factor_1pose_point2point", &FGraphPy::add_factor_1pose_point2point,
+                 "Factor measuring point to point distance in a registration problem",
+                 py::arg("z_point_x"),
+                 py::arg("z_point_y"),
+                 py::arg("nodePoseId"),
+                 py::arg("obsInf"))
+            // -----------------------------------------------------------
+            // Eigen Factors
+            .def("add_eigen_factor_plane", &FGraphPy::add_eigen_factor_plane)
+            .def("eigen_factor_plane_add_point", &FGraphPy::eigen_factor_plane_add_point,
+                    "Adds a point given a node id and the EF it belongs to.",
+                    py::arg("planeEigenId"),
+                    py::arg("nodePoseId"),
+                    py::arg("point"),
+                    py::arg("W"))
             ;
 
 }
diff --git a/mrobpy/PCPlanesPy.cpp b/mrobpy/PCPlanesPy.cpp
index 4682519..ffff1f7 100644
--- a/mrobpy/PCPlanesPy.cpp
+++ b/mrobpy/PCPlanesPy.cpp
@@ -24,7 +24,7 @@
 
 
 /**
- * Submodule dedicated to Point Clouds Plane aligment.
+ * Submodule dedicated to Point Clouds Plane alignment.
  */
 
 #include <pybind11/pybind11.h>
@@ -61,7 +61,12 @@ void init_PCPlanes(py::module &m)
         ;
 	// This class creates a synthetic testing
     py::class_<CreatePoints>(m,"CreatePoints")
-            .def(py::init<uint_t, uint_t, uint_t, double, double>())
+            .def(py::init<uint_t, uint_t, uint_t, double, double>(),
+                 py::arg("N") = 10,
+                 py::arg("numerbPlanes") = 4,
+                 py::arg("numberPoses") = 2,
+                 py::arg("noisePerPoint") = 0.01,
+                 py::arg("noiseBias") = 0.1)
             .def("get_point_cloud", &CreatePoints::get_point_cloud,
             		"Input time index and outputs all points at that instant in time")
             .def("get_point_plane_ids", &CreatePoints::get_point_plane_ids,
@@ -70,9 +75,12 @@ void init_PCPlanes(py::module &m)
             		"This fills in the structure for the class plane registration, ready to optimized with the synthetically created points. TODO this is a reference, not a deep copy!")
             .def("get_ground_truth_last_pose", &CreatePoints::get_ground_truth_last_pose,
                     "return SE3 of the last pose")
-            .def("get_trajectory", &CreatePoints::get_ground_truth_pose)
+            .def("get_trajectory", &CreatePoints::get_ground_truth_trajectory)
+            .def("get_number_poses", &CreatePoints::get_number_poses)
+            .def("get_number_planes", &CreatePoints::get_number_planes)
             ;
     // This class is a data structure, containing all points and calculating plane registration
+    // TODO this should be deprecated, is just maintainted for comparison with the Fgraph version
     py::class_<PlaneRegistration>(m,"PlaneRegistration")
             .def(py::init<>(),
                     "Constructor, by default empty structure")
diff --git a/python_examples/FGraph_point_plane_align.py b/python_examples/FGraph_point_plane_align.py
index 581b79e..6eb3b38 100644
--- a/python_examples/FGraph_point_plane_align.py
+++ b/python_examples/FGraph_point_plane_align.py
@@ -30,13 +30,10 @@ def pcd_1(X, color, T = []):
 	pcd.paint_uniform_color(color)
 	return pcd
 
-def vis_her(X, Y, T = []):
+def vis_her(X, Y, T = np.identity(4)):
     blue = np.array([0,0,1], dtype='float64')
     red = np.array([1,0,0], dtype='float64')
-    if T!=[]:
-        open3d.visualization.draw_geometries([pcd_1(X,red,T), pcd_1(Y,blue)])
-    else:
-        open3d.visualization.draw_geometries([pcd_1(X,red), pcd_1(Y,blue)])
+    open3d.visualization.draw_geometries([pcd_1(X,red,T), pcd_1(Y,blue)])
 
 def vis_arr(X):
 	pcd = open3d.PointCloud()
@@ -70,6 +67,18 @@ def vis_arr(X):
                                        nodePoseId = n1,
                                        obsInf = W)
 
+# Solve the problem for the potint to point
+graph_p2p = mrob.FGraph()
+W = np.identity(3)
+print('W point2point = ', W)
+n1 = graph_p2p.add_node_pose_3d(mrob.geometry.SE3())
+for i in range(N_points):
+    graph_p2p.add_factor_1pose_point2point(z_point_x = X[i],
+                                       z_point_y = centroids[ids[i]], #Y[i],
+                                       nodePoseId = n1,
+                                       obsInf = W)
+
+
 #graph.print(True)
 print('Current state of the graph: chi2 = ' , graph.chi2() )
 start = time.time()
@@ -86,10 +95,28 @@ def vis_arr(X):
 vis_her(X,Y,Testimated.T())
 
 
+print('Current state of the graph point2point: chi2 = ' , graph_p2p.chi2() )
+start = time.time()
+#XXX initial ideal lambda is around 1e-1, vs the default 1e-5, so many more iterations are needed.
+graph_p2p.solve(mrob.LM, 30)
+end = time.time()
+print(', chi2 = ', graph_p2p.chi2() , ', time on calculation [s] = ', 1e0*(end - start))
+results = graph_p2p.get_estimated_state()
+
+print(results)
+Testimated = mrob.geometry.SE3(results[0])
+# initial visualization
+print('Error in poses rotation= ', T.distance_rotation(Testimated), ', distance trans = ', T.distance_trans(Testimated))
+vis_her(X,Y,Testimated.T())
+
+
+
+
+
 
-L = graph.get_information_matrix().todense()
-D, U = np.linalg.eig(L)
-print(D)
-print(U)
+#L = graph.get_information_matrix().todense()
+#D, U = np.linalg.eig(L)
+#print(D)
+#print(U)
 
 
diff --git a/python_examples/PC_EFPlanes.py b/python_examples/PC_EFPlanes.py
new file mode 100644
index 0000000..9ba0826
--- /dev/null
+++ b/python_examples/PC_EFPlanes.py
@@ -0,0 +1,67 @@
+import mrob
+import numpy as np
+import open3d
+
+
+def draw_planes(synthetic,traj=[]):
+    pcds = []
+    poses = synthetic.get_number_poses()
+    for i in range(poses):
+        pc = open3d.geometry.PointCloud()
+        pc.points = open3d.utility.Vector3dVector(np.array(synthetic.get_point_cloud(i)))
+        s = i/poses
+        pc.paint_uniform_color([0.5,1-s, (1-s)/2])
+        if traj!=[]:
+            if type(traj[i]) == mrob.geometry.SE3:
+                pc.transform(traj[i].T()) #traj is a list of SE3
+            else:
+                pc.transform(traj[i]) #traj is a list of 4x4 np arrays
+        pcds.append(pc)
+    open3d.visualization.draw_geometries(pcds)
+
+N_points = 500
+N_planes = 4
+N_poses = 2
+
+synthetic = mrob.registration.CreatePoints(N_points,N_planes,N_poses, 0.05, 0.1) #point noise, bias noise
+T_gt = synthetic.get_ground_truth_last_pose()
+T_gt.print()
+draw_planes(synthetic)
+
+graph = mrob.FGraph()
+
+for t in range(N_planes):
+    ef1 = graph.add_eigen_factor_plane()
+    # It is an ordered progression 0:N-1, no need for dict
+    print('EFactor id = ', ef1)
+
+for t in range(N_poses):
+    n1 = graph.add_node_pose_3d(mrob.geometry.SE3())
+    # It is an ordered progression 0:N-1, no need for dict
+    print('Pose node id = ', n1)
+
+graph.add_factor_1pose_3d(mrob.geometry.SE3(),0,1e3*np.identity(6))
+
+for t in range(N_poses):
+    print('Processing pose ', t)
+    points = synthetic.get_point_cloud(t)
+    indexes = synthetic.get_point_plane_ids(t)
+    for p,i in zip(points,indexes):     
+        #print('point ', p, 'index ', i)
+        graph.eigen_factor_plane_add_point(planeEigenId = i,
+                                   nodePoseId = t,
+                                   point = p,
+                                   W = 1.0)
+
+print('Initial error = ', graph.chi2(True))
+# Does it require a better initialization?? with median?
+graph.solve(mrob.LM_ELLIPS,10)
+if 0:
+    import matplotlib.pyplot as plt
+    L = graph.get_information_matrix()
+    plt.spy(L, marker='o', markersize=5)
+    plt.title('Information matrix $\Lambda$')
+    print(L)
+    plt.show()
+draw_planes(synthetic, graph.get_estimated_state())
+#graph.print(True)
diff --git a/python_examples/PC_align.py b/python_examples/PC_align.py
index 34a1f60..0cb851f 100644
--- a/python_examples/PC_align.py
+++ b/python_examples/PC_align.py
@@ -15,21 +15,17 @@
 
 
 
-def pcd_1(X, color, T = []):
-	pcd = open3d.geometry.PointCloud()
-	pcd.points = open3d.utility.Vector3dVector(X)
-	if T!=[]:
-	    pcd.transform(T)
-	pcd.paint_uniform_color(color)
-	return pcd
-
-def vis_her(X, Y, T = []):
+def pcd_1(X, color, T = np.identity(4)):
+    pcd = open3d.geometry.PointCloud()
+    pcd.points = open3d.utility.Vector3dVector(X)
+    pcd.transform(T)
+    pcd.paint_uniform_color(color)
+    return pcd
+
+def vis_her(X, Y, T = np.identity(4)):
     blue = np.array([0,0,1], dtype='float64')
     red = np.array([1,0,0], dtype='float64')
-    if T!=[]:
-        open3d.visualization.draw_geometries([pcd_1(X,red), pcd_1(Y,blue, T)])
-    else:
-        open3d.visualization.draw_geometries([pcd_1(X,red), pcd_1(Y,blue)])
+    open3d.visualization.draw_geometries([pcd_1(X,red), pcd_1(Y,blue, T)])
 
 def vis_arr(X):
 	pcd = open3d.PointCloud()
diff --git a/src/EigenFactors/CMakeLists.txt b/src/EigenFactors/CMakeLists.txt
deleted file mode 100644
index af69d19..0000000
--- a/src/EigenFactors/CMakeLists.txt
+++ /dev/null
@@ -1,19 +0,0 @@
-# locate the necessary dependencies, if any
-
-# extra header files
-SET(headers
-    mrob/plane_factor.hpp
-)
-
-# extra source files
-SET(sources
-    plane_factor.cpp
-)
-
-# create the shared library
-ADD_LIBRARY(EigenFactors SHARED  ${sources})
-TARGET_LINK_LIBRARIES(EigenFactors FGraph SE3)
-
-
-ADD_SUBDIRECTORY(examples)
-#ADD_SUBDIRECTORY(tests)
diff --git a/src/EigenFactors/README.md b/src/EigenFactors/README.md
deleted file mode 100644
index 330ab4b..0000000
--- a/src/EigenFactors/README.md
+++ /dev/null
@@ -1,2 +0,0 @@
-# Eigen-Factors
-Directory that *will* containt the EF implementation. Right now, the working version is part of the PCRegistration for limited trajectories.
diff --git a/src/EigenFactors/examples/CMakeLists.txt b/src/EigenFactors/examples/CMakeLists.txt
deleted file mode 100644
index 33ac891..0000000
--- a/src/EigenFactors/examples/CMakeLists.txt
+++ /dev/null
@@ -1,4 +0,0 @@
-ADD_EXECUTABLE(test_EF_Hessian  test_EF_Hessian.cpp)
-TARGET_LINK_LIBRARIES(test_EF_Hessian SE3)
-ADD_EXECUTABLE(example_EF_1_pose  example_solve_1_poses.cpp)
-TARGET_LINK_LIBRARIES(example_EF_1_pose EigenFactors)
\ No newline at end of file
diff --git a/src/FGraph/CMakeLists.txt b/src/FGraph/CMakeLists.txt
index ea4d6aa..d04b4e5 100644
--- a/src/FGraph/CMakeLists.txt
+++ b/src/FGraph/CMakeLists.txt
@@ -6,7 +6,6 @@ SET(headers
     mrob/factor.hpp
     mrob/factor_graph.hpp
     mrob/factor_graph_solve.hpp
-    mrob/factor_graph_solve_dense.hpp
 )
 
 # extra source files
@@ -15,7 +14,6 @@ SET(sources
     factor.cpp
     factor_graph.cpp
     factor_graph_solve.cpp
-    factor_graph_solve_dense.cpp
 )
 
 SET(factors_headers
@@ -29,8 +27,6 @@ SET(factors_headers
     mrob/factors/nodeLandmark3d.hpp
     mrob/factors/factor1Pose1Landmark2d.hpp
     mrob/factors/factor1Pose1Landmark3d.hpp
-    mrob/factors/nodePlane4d.hpp
-    mrob/factors/factor1Pose1plane4d.hpp
 )
 
 SET(factors_sources
@@ -44,8 +40,6 @@ SET(factors_sources
     factors/nodeLandmark3d.cpp
     factors/factor1Pose1Landmark2d.cpp
     factors/factor1Pose1Landmark3d.cpp
-    factors/nodePlane4d.cpp
-    factors/factor1Pose1Plane4d.cpp
 )
 
 # create the shared library
diff --git a/src/FGraph/examples/CMakeLists.txt b/src/FGraph/examples/CMakeLists.txt
index 212ffb9..01a5aea 100644
--- a/src/FGraph/examples/CMakeLists.txt
+++ b/src/FGraph/examples/CMakeLists.txt
@@ -7,5 +7,6 @@ TARGET_LINK_LIBRARIES(example_FGraph_3d_example FGraph)
 ADD_EXECUTABLE(example_FGraph_3d_ladmark_example  example_solver_3d_landmarks.cpp)
 TARGET_LINK_LIBRARIES(example_FGraph_3d_ladmark_example FGraph)
 
-ADD_EXECUTABLE(example_FGraph_dense_3d  example_solver_dense_3d.cpp)
-TARGET_LINK_LIBRARIES(example_FGraph_dense_3d FGraph common)
\ No newline at end of file
+# TODO deprecated, remove all the related files
+#ADD_EXECUTABLE(example_FGraph_dense_3d  example_solver_dense_3d.cpp)
+#TARGET_LINK_LIBRARIES(example_FGraph_dense_3d FGraph common)
\ No newline at end of file
diff --git a/src/FGraph/factor.cpp b/src/FGraph/factor.cpp
index 770c1df..20697a1 100644
--- a/src/FGraph/factor.cpp
+++ b/src/FGraph/factor.cpp
@@ -71,3 +71,10 @@ matData_t Factor::evaluate_robust_weight(matData_t u, matData_t params)
     }
     return robust_weight_;
 }
+
+EigenFactor::EigenFactor(robustFactorType factor_type, uint_t potNumberNodes):
+        Factor(0,0,factor_type, potNumberNodes)
+{
+    //Dimension zero since this is a non-parametric factor.
+    //Also we don't known how many nodes will connect, so we set the second param to 0 (not-used)
+}
diff --git a/src/FGraph/factor_graph.cpp b/src/FGraph/factor_graph.cpp
index 6f591e8..cde662c 100644
--- a/src/FGraph/factor_graph.cpp
+++ b/src/FGraph/factor_graph.cpp
@@ -33,9 +33,6 @@ FGraph::FGraph() :
 }
 FGraph::~FGraph()
 {
-    // clear every node's list of neigbours factors
-    for (auto &&n: nodes_)
-        n->clear();
     factors_.clear();
     nodes_.clear();
     eigen_factors_.clear();
@@ -43,27 +40,16 @@ FGraph::~FGraph()
 
 factor_id_t FGraph::add_factor(std::shared_ptr<Factor> &factor)
 {
-	factor->set_id(factors_.size());
-	factors_.push_back(factor);
-    auto *list = factor->get_neighbour_nodes();
-    for( auto &&n: *list)
-    {
-        n->add_factor(factor);
-    }
+    factor->set_id(factors_.size());
+    factors_.emplace_back(factor);
     obsDim_ += factor->get_dim();
     return factor->get_id();
 }
 
-factor_id_t FGraph::add_eigen_factor(std::shared_ptr<Factor> &factor)
+factor_id_t FGraph::add_eigen_factor(std::shared_ptr<EigenFactor> &factor)
 {
     factor->set_id(eigen_factors_.size());
-    eigen_factors_.push_back(factor);
-    auto *list = factor->get_neighbour_nodes();
-    for( auto &&n: *list)
-    {
-        n->add_factor(factor);
-    }
-    obsDim_ += factor->get_dim();
+    eigen_factors_.emplace_back(factor);
     return factor->get_id();
 }
 
@@ -71,7 +57,7 @@ factor_id_t FGraph::add_eigen_factor(std::shared_ptr<Factor> &factor)
 factor_id_t FGraph::add_node(std::shared_ptr<Node> &node)
 {
 	node->set_id(nodes_.size());
-	nodes_.push_back(node);
+	nodes_.emplace_back(node);
 	switch(node->get_node_mode())
 	{
 	    case Node::nodeMode::STANDARD:
@@ -89,24 +75,29 @@ factor_id_t FGraph::add_node(std::shared_ptr<Node> &node)
 
 std::shared_ptr<Node>& FGraph::get_node(factor_id_t key)
 {
-    // TODO key on a set or map?
     assert(key < nodes_.size() && "FGraph::get_node: incorrect key");
-    return nodes_[key];// XXX test key  again
+    return nodes_[key];
 }
 
 std::shared_ptr<Factor>& FGraph::get_factor(factor_id_t key)
 {
     // TODO key on a set or map?
-    assert(key < factors_.size() && "FGraph::get_node: incorrect key");
+    assert(key < factors_.size() && "FGraph::get_factor: incorrect key");
     return factors_[key];
 }
 
+std::shared_ptr<EigenFactor>& FGraph::get_eigen_factor(factor_id_t key)
+{
+    assert(key < eigen_factors_.size() && "FGraph::get_eigen_factor: incorrect key");
+    return eigen_factors_[key];
+}
 
 void FGraph::print(bool completePrint) const
 {
     std::cout << "Status of graph: " <<
-            nodes_.size()  << "Nodes and " <<
-            factors_.size() << "Factors." << std::endl;
+            " Nodes = " << nodes_.size()  <<
+            ", Factors = " << factors_.size() <<
+            ", Eigen Factors = " << eigen_factors_.size() << std::endl;
 
     if(completePrint)
     {
@@ -114,5 +105,7 @@ void FGraph::print(bool completePrint) const
             n->print();
         for (auto &&f : factors_)
             f->print();
+        for (auto &&f : eigen_factors_)
+            f->print();
     }
 }
diff --git a/src/FGraph/factor_graph_solve.cpp b/src/FGraph/factor_graph_solve.cpp
index e63c4ca..9eebcd7 100644
--- a/src/FGraph/factor_graph_solve.cpp
+++ b/src/FGraph/factor_graph_solve.cpp
@@ -61,6 +61,7 @@ void FGraphSolve::solve(optimMethod method, uint_t maxIters, matData_t lambda, m
     lambda_ = lambda;
     solutionTolerance_ = solutionTolerance;
     time_profiles_.reset();
+    optimMethod_ = method;
 
     assert(stateDim_ > 0 && "FGraphSolve::solve: empty node state");
 
@@ -72,6 +73,7 @@ void FGraphSolve::solve(optimMethod method, uint_t maxIters, matData_t lambda, m
         this->update_nodes();
         break;
       case LM:
+      case LM_ELLIPS:
         this->optimize_levenberg_marquardt(maxIters);
         break;
       default:
@@ -80,8 +82,8 @@ void FGraphSolve::solve(optimMethod method, uint_t maxIters, matData_t lambda, m
 
 
 
-
-    if (0)
+    // TODO add variable verbose to output times
+    if (1)
         time_profiles_.print();
 }
 
@@ -94,6 +96,13 @@ void FGraphSolve::build_problem(bool useLambda)
     this->build_adjacency();
     time_profiles_.stop("Adjacency");
 
+    if (eigen_factors_.size()>0)
+    {
+        time_profiles_.start();
+        this->build_info_EF();
+        time_profiles_.stop("EFs Jacobian and Hessian");
+    }
+
     // 1.2) builds specifically the information
     switch(matrixMethod_)
     {
@@ -107,6 +116,8 @@ void FGraphSolve::build_problem(bool useLambda)
         assert(0 && "FGraphSolve: method not implemented");
     }
 
+    // 1.3) (Optional) Eigen Factors
+
     // Structure for LM and dampening GN-based methods
     if (useLambda)
     {
@@ -126,8 +137,12 @@ void FGraphSolve::optimize_gauss_newton(bool useLambda)
     if (useLambda)
     {
         for (uint_t n = 0 ; n < N_; ++n)
-            L_.coeffRef(n,n) = lambda_ + diagL_(n);//Circunference =>  diagL_(n) + lambda_
-            //L_.coeffRef(n,n) = (1.0 + lambda_)*diagL_(n);//Elipsoid, for circunference =>  diagL_(n) + lambda.
+        {
+            if (optimMethod_ == LM)
+                L_.coeffRef(n,n) = lambda_ + diagL_(n);//Spherical
+            else
+                L_.coeffRef(n,n) = (1.0 + lambda_)*diagL_(n);//Elipsoid
+        }
     }
     cholesky.compute(L_);
     time_profiles_.stop("Gauss Newton create Cholesky");
@@ -212,10 +227,14 @@ uint_t FGraphSolve::optimize_levenberg_marquardt(uint_t maxIters)
 
 void FGraphSolve::build_adjacency()
 {
-    assert(obsDim_ > 0 && "FGraphSolve::build_adjacency: empty observations vector");
+    // Check for consistency. With 0 observations the problem cannot be built.
+    assert(obsDim_ >0 && "FGraphSolve::build_adjacency: Zeros observations, at least add one anchor factor");
+    if (obsDim_ == 0)
+        return;
+
     // 1) resize properly matrices (if needed)
     r_.resize(obsDim_,1);//dense vector TODO is it better to reserve and push_back??
-    A_.resize(obsDim_, stateDim_);//Sparse matrix clear data
+    A_.resize(obsDim_, stateDim_);//Sparse matrix clears data, but keeps the prev reserved space
     W_.resize(obsDim_, obsDim_);//TODO should we reinitialize this all the time? an incremental should be fairly easy
 
 
@@ -226,6 +245,8 @@ void FGraphSolve::build_adjacency()
     std::unordered_map<factor_id_t, factor_id_t > indNodesMatrix;
     //indNodesMatrix.reserve(nodes->size());
 
+    // XXX: this structure ONLY works because we use all nodes. If we were using a subset of them (l227)
+    // Then this ordered approach for ids {0,..,N} would not work!
     N_ = 0;
     for (size_t i = 0; i < active_nodes_.size(); ++i)
     {
@@ -234,7 +255,6 @@ void FGraphSolve::build_adjacency()
         factor_id_t id = active_nodes_[i]->get_id();
         indNodesMatrix.emplace(id,N_);
         N_ += dim;
-
     }
     assert(N_ == stateDim_ && "FGraphSolve::buildAdjacency: State Dimensions are not coincident\n");
 
@@ -310,7 +330,7 @@ void FGraphSolve::build_adjacency()
 
 
         // 5) Get information matrix for every factor
-        // TODO for robust factors, here is where the robust weights should be applied
+        // For robust factors, here is where the robust weights should be applied
         matData_t robust_weight = 1.0;
         for (uint_t l = 0; l < f->get_dim(); ++l)
         {
@@ -340,6 +360,58 @@ void FGraphSolve::build_info_adjacency()
      */
     L_ = (A_.transpose() * W_.selfadjointView<Eigen::Upper>() * A_);
     b_ = A_.transpose() * W_.selfadjointView<Eigen::Upper>() * r_;
+
+    // If any EF, we should combine both solutions
+    if (eigen_factors_.size() > 0 )
+    {
+        L_ += hessianEF_.selfadjointView<Eigen::Upper>();
+        b_ += gradientEF_;
+    }
+}
+
+
+void FGraphSolve::build_info_EF()
+{
+    gradientEF_.resize(stateDim_,1);
+    gradientEF_.setZero();
+    std::vector<Triplet> hessianData;
+    // XXX if EF ever connected a node that is not 6D, then this will not hold.
+    hessianData.reserve(eigen_factors_.size()*21);//For each EF we reserve the uppder triangular view => 6+5+..+1  = 21
+    // It assumes L_ has been created, and requires at least 1 observation (achnor)
+    for (size_t id = 0; id < eigen_factors_.size(); ++id)
+    {
+        auto f = eigen_factors_[id];
+        f->evaluate_residuals();
+        f->evaluate_jacobians();//and Hessian
+        f->evaluate_chi2();
+        auto neighNodes = f->get_neighbour_nodes();
+        for (auto node : *neighNodes)
+        {
+            uint_t indNode = node->get_id();
+            // Updating Jacobian, b should has been previously calculated
+            Mat61 J = f->get_jacobian(indNode);
+            gradientEF_.block<6,1>(indNodesMatrix_[indNode],0) += J;//TODO robust weight would go here
+            //std::cout << "Jacobian = " << J.transpose() << std::endl;
+
+            // Updating the Hessian
+            Mat6 H = f->get_hessian(indNode);
+            //std::cout << "Hessian = " << H << std::endl;
+            uint_t startingIndex = indNodesMatrix_[indNode];//XXX this should be change (someday) to a proper table for any ordering
+            // XXX if EF ever connected a node that is not 6D, then this will not hold.
+            for (uint_t i = 0; i < 6; i++)
+            {
+                for (uint_t j = i; j<6; j++)
+                {
+                    // convert the hessian to triplets
+                    hessianData.emplace_back(Triplet(startingIndex+ i, startingIndex+ j, H(i,j)));
+                }
+            }
+        }
+    }
+
+    // create a Upper-view sparse matrix from the triplets:
+    hessianEF_.resize(stateDim_,stateDim_);
+    hessianEF_.setFromTriplets(hessianData.begin(), hessianData.end());
 }
 
 matData_t FGraphSolve::chi2(bool evaluateResidualsFlag)
@@ -355,6 +427,16 @@ matData_t FGraphSolve::chi2(bool evaluateResidualsFlag)
         }
         totalChi2 += f->get_chi2();
     }
+
+    for (auto &ef : eigen_factors_)
+    {
+        if (evaluateResidualsFlag)
+        {
+            ef->evaluate_residuals();
+            ef->evaluate_chi2();
+        }
+        totalChi2 += ef->get_chi2();
+    }
     return totalChi2;
 }
 
diff --git a/src/FGraph/factors/nodeLandmark2d.cpp b/src/FGraph/factors/nodeLandmark2d.cpp
index 5469116..71538cb 100644
--- a/src/FGraph/factors/nodeLandmark2d.cpp
+++ b/src/FGraph/factors/nodeLandmark2d.cpp
@@ -37,22 +37,22 @@ NodeLandmark2d::NodeLandmark2d(const Mat21 &initial_x, Node::nodeMode mode) :
 }
 
 
-void NodeLandmark2d::update(const Eigen::Ref<const MatX1> &dx)
+void NodeLandmark2d::update(VectRefConst &dx)
 {
     state_ += dx;
 }
 
-void NodeLandmark2d::update_from_auxiliary(const Eigen::Ref<const MatX1> &dx)
+void NodeLandmark2d::update_from_auxiliary(VectRefConst &dx)
 {
     state_ = auxiliaryState_ + dx;
 }
 
-void NodeLandmark2d::set_state(const Eigen::Ref<const MatX> &x)
+void NodeLandmark2d::set_state(MatRefConst &x)
 {
     state_ = x;
 }
 
-void NodeLandmark2d::set_auxiliary_state(const Eigen::Ref<const MatX> &x)
+void NodeLandmark2d::set_auxiliary_state(MatRefConst &x)
 {
     auxiliaryState_ = x;
 }
@@ -61,6 +61,4 @@ void NodeLandmark2d::print() const
 {
     std::cout << "Printing NodeLandmark2d: " << id_
         << ", state = \n" << state_;
-    std::cout  <<  "\nand neighbour factors " << neighbourFactors_.size()
-        << std::endl;
 }
diff --git a/src/FGraph/factors/nodeLandmark3d.cpp b/src/FGraph/factors/nodeLandmark3d.cpp
index ece885b..05fe2f0 100644
--- a/src/FGraph/factors/nodeLandmark3d.cpp
+++ b/src/FGraph/factors/nodeLandmark3d.cpp
@@ -36,25 +36,25 @@ NodeLandmark3d::NodeLandmark3d(const Mat31 &initial_x, Node::nodeMode mode) :
 }
 
 
-void NodeLandmark3d::update(const Eigen::Ref<const MatX1> &dx)
+void NodeLandmark3d::update(VectRefConst &dx)
 {
     Mat31 dxf = dx;//XXX cast is necessary?
     state_ += dxf;
 }
 
-void NodeLandmark3d::update_from_auxiliary(const Eigen::Ref<const MatX1> &dx)
+void NodeLandmark3d::update_from_auxiliary(VectRefConst &dx)
 {
     Mat31 dxf = dx;
     state_ = auxiliaryState_ + dxf;
 }
 
-void NodeLandmark3d::set_state(const Eigen::Ref<const MatX> &x)
+void NodeLandmark3d::set_state(MatRefConst &x)
 {
 	// cast is done by Eigen
     state_ = x;
 }
 
-void NodeLandmark3d::set_auxiliary_state(const Eigen::Ref<const MatX> &x)
+void NodeLandmark3d::set_auxiliary_state(MatRefConst &x)
 {
     auxiliaryState_ = x;
 }
@@ -63,6 +63,4 @@ void NodeLandmark3d::print() const
 {
     std::cout << "Printing NodeLandmark3d: " << id_
         << ", state = \n" << state_;
-    std::cout  <<  "\nand neighbour factors " << neighbourFactors_.size()
-        << std::endl;
 }
diff --git a/src/FGraph/factors/nodePose2d.cpp b/src/FGraph/factors/nodePose2d.cpp
index 8cf0b74..214884a 100644
--- a/src/FGraph/factors/nodePose2d.cpp
+++ b/src/FGraph/factors/nodePose2d.cpp
@@ -32,14 +32,14 @@ NodePose2d::NodePose2d(const Mat31 &initial_x, Node::nodeMode mode) : Node(3, mo
     assert(initial_x.cols() == 1 && "NodePose2d:: Incorrect dimension on initial state cols");
 }
 
-void NodePose2d::update(const Eigen::Ref<const MatX1> &dx)
+void NodePose2d::update(VectRefConst &dx)
 {
     state_ += dx;
     state_(2) = wrap_angle(state_(2));
 
 }
 
-void NodePose2d::update_from_auxiliary(const Eigen::Ref<const MatX1> &dx)
+void NodePose2d::update_from_auxiliary(VectRefConst &dx)
 {
     state_ = auxiliaryState_ + dx;
     state_(2) = wrap_angle(state_(2));
@@ -47,13 +47,13 @@ void NodePose2d::update_from_auxiliary(const Eigen::Ref<const MatX1> &dx)
 }
 
 
-void NodePose2d::set_state(const Eigen::Ref<const MatX> &x)
+void NodePose2d::set_state(MatRefConst &x)
 {
     state_ = x;
     state_(2) = wrap_angle(state_(2));
 }
 
-void NodePose2d::set_auxiliary_state(const Eigen::Ref<const MatX> &x)
+void NodePose2d::set_auxiliary_state(MatRefConst &x)
 {
     auxiliaryState_ = x;
     auxiliaryState_(2) = wrap_angle(auxiliaryState_(2));
@@ -63,6 +63,5 @@ void NodePose2d::print() const
 {
     std::cout << "Printing NodePose2d: " << id_
               << ", state = \n" << state_
-              <<  "\nand neighbour factors " << neighbourFactors_.size()
               << std::endl;
 }
diff --git a/src/FGraph/factors/nodePose3d.cpp b/src/FGraph/factors/nodePose3d.cpp
index e086b1a..5279565 100644
--- a/src/FGraph/factors/nodePose3d.cpp
+++ b/src/FGraph/factors/nodePose3d.cpp
@@ -44,7 +44,7 @@ NodePose3d::NodePose3d(const SE3 &initial_x, Node::nodeMode mode) :
 }
 
 
-void NodePose3d::update(const Eigen::Ref<const MatX1> &dx)
+void NodePose3d::update(VectRefConst &dx)
 {
     Mat61 dxf = dx;
 
@@ -55,21 +55,21 @@ void NodePose3d::update(const Eigen::Ref<const MatX1> &dx)
     state_.regenerate();
 }
 
-void NodePose3d::update_from_auxiliary(const Eigen::Ref<const MatX1> &dx)
+void NodePose3d::update_from_auxiliary(VectRefConst &dx)
 {
     Mat61 dxf = dx;
     state_ = auxiliaryState_;//we update from the auxiliary state
     state_.update_lhs(dxf);
 }
 
-void NodePose3d::set_state(const Eigen::Ref<const MatX> &x)
+void NodePose3d::set_state(MatRefConst &x)
 {
 	// casting is necessary for SE3 constructor, it does not handle a ref TODO
 	Mat4 newState = x;
     state_ = SE3(newState);
 }
 
-void NodePose3d::set_auxiliary_state(const Eigen::Ref<const MatX> &x)
+void NodePose3d::set_auxiliary_state(MatRefConst &x)
 {
 	Mat4 newState = x;
     auxiliaryState_ = SE3(newState);
@@ -80,6 +80,4 @@ void NodePose3d::print() const
     std::cout << "Printing NodePose3d: " << id_
         << ", state = \n" << state_.ln_vee() << ",\n SE3 matrix: \n";
     state_.print();
-    std::cout  <<  "\nand neighbour factors " << neighbourFactors_.size()
-        << std::endl;
 }
diff --git a/src/FGraph/mrob/factor.hpp b/src/FGraph/mrob/factor.hpp
index a293f8d..958d367 100644
--- a/src/FGraph/mrob/factor.hpp
+++ b/src/FGraph/mrob/factor.hpp
@@ -77,6 +77,7 @@ class Node;
  *                      d                                    0
  */
 
+
 class Factor{
 public:
     /**
@@ -119,18 +120,22 @@ class Factor{
      * doing the conversion with minimal temporary artifacts
      * Observation can be a 3d point, a 3d pose (transformation 4x4), etc.
      */
-    virtual const Eigen::Ref<const MatX> get_obs() const = 0;
+    virtual MatRefConst get_obs() const = 0;
     /**
      * Residual will always be a block vector
      */
-    virtual const Eigen::Ref<const MatX1> get_residual() const = 0;
-    virtual const Eigen::Ref<const MatX> get_information_matrix() const = 0;
+    virtual VectRefConst get_residual() const = 0;
+    virtual MatRefConst get_information_matrix() const = 0;
     /**
      * get_jacobian returns a block matrices stacking all the Jacobians on the factor.
-     * The convention is that Jacobians corresponding to
+     * The convention is that Jacobians corresponding to.
+     *
+     * The input value is in case that Jacobian supports accessing a particular Jacobian
+     * of a node. For most factors (include 1-2 nodes) this option is not available
+     * Mostly only for EigenFactors whose number of connected nodes is unbounded
      *
      */
-    virtual const Eigen::Ref<const MatX> get_jacobian() const = 0;
+    virtual MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const = 0;
 
 
     factor_id_t get_id() const {return id_;}
@@ -187,6 +192,40 @@ class Factor{
 
 };
 
+/**
+ * Abstract class EigenFactor. This is a factor with extra methods than Factor
+ * which requires a new base abstract class.
+ *
+ * The Eigen factor connects different poses that have observed the same geometric entity.
+ * It is not required an explicit parametrization of the current state, which is a geometric entity
+ * estimated a priory on each iteration, e.g. a plane. The resultant topology
+ * is N nodes connecting to the eigen factor.
+ *
+ * Hence, the new method get state, for instance a plane, but this state is outside the FGraph optimization, that
+ * is why we can consider this approach non-parametric
+ *  - get_state()
+ *
+ * NOTE: due to its nature, multiple observation can be added to the same EF,
+ * meaning we need to create a constructor PLUS an additional method
+ *  - add_point()
+ *
+ * In order to build the problem we would follow the interface specifications by FGraph
+ * but we need extra methods and variables to keep track of the neighbours. For instance, we need
+ * to get Jacobians and Hessian matrices
+ *
+ * This class assumes that matrices S = sum p*p' are calculated before since they are directly inputs
+ * XXX should we store all points?
+ */
+class EigenFactor : public Factor
+{
+public:
+    EigenFactor(robustFactorType factor_type = QUADRATIC, uint_t potNumberNodes = 5);
+    virtual ~EigenFactor() = default;
+    virtual VectRefConst get_state() const = 0;
+    virtual void add_point(const Mat31& p, std::shared_ptr<Node> &node, mrob::matData_t &W) = 0;
+    virtual MatRefConst get_hessian(mrob::factor_id_t id = 0) const = 0;
+
+};
 
 }
 
diff --git a/src/FGraph/mrob/factor_graph.hpp b/src/FGraph/mrob/factor_graph.hpp
index 96859e4..27d4895 100644
--- a/src/FGraph/mrob/factor_graph.hpp
+++ b/src/FGraph/mrob/factor_graph.hpp
@@ -78,7 +78,7 @@ class FGraph{
      * and therefore it requires a different processing, apart from the
      * standard residual factors from above.
      */
-    factor_id_t add_eigen_factor(std::shared_ptr<Factor> &factor);
+    factor_id_t add_eigen_factor(std::shared_ptr<EigenFactor> &factor);
     /**
       * Adds a node if it was not already on the set.
       */
@@ -93,6 +93,10 @@ class FGraph{
      * get_node returns the node given the node id key, now a position on the data structure
      */
     std::shared_ptr<Factor>& get_factor(factor_id_t key);
+    /**
+    * get_node returns the Eigen factor given the node id key, now a position on the data structure
+    */
+    std::shared_ptr<EigenFactor>& get_eigen_factor(factor_id_t key);
     void print(bool complete = false) const;
 
 
@@ -104,7 +108,7 @@ class FGraph{
     uint_t get_dimension_state() {return stateDim_;};
     uint_t get_dimension_obs() {return obsDim_;};
 
-    //TODO #46
+    //TODO serialization
     void save_graph() const;
     void load_graph();
 
@@ -127,8 +131,8 @@ class FGraph{
     //std::unordered_set<std::shared_ptr<Factor> > factors_;
     std::deque<std::shared_ptr<Factor> > factors_; // no specific order needed
 
-    // This requires a special list for the factors. Ids will duplicate for standard factors
-    std::deque<std::shared_ptr<Factor> > eigen_factors_;
+    // This requires a special list for the factors
+    std::deque<std::shared_ptr<EigenFactor> > eigen_factors_;
 
     /**
      * Total accumulated dimensions on both the state (nodes)
diff --git a/src/FGraph/mrob/factor_graph_solve.hpp b/src/FGraph/mrob/factor_graph_solve.hpp
index 7f1185f..0fcc12e 100644
--- a/src/FGraph/mrob/factor_graph_solve.hpp
+++ b/src/FGraph/mrob/factor_graph_solve.hpp
@@ -78,6 +78,7 @@ class FGraphSolve: public FGraph
 public:
     /**
      * This enums all matrix building methods available
+     * For now we only do build adjacency
      */
     enum matrixMethod{ADJ=0, SCHUR};
     /**
@@ -85,7 +86,7 @@ class FGraphSolve: public FGraph
      *  - Gauss Newton
      *  - Levenberg Marquardt (trust-region-like for lambda adjustment) TODO LM elliptical?
      */
-    enum optimMethod{GN=0, LM};
+    enum optimMethod{GN=0, LM, LM_ELLIPS};
 
     FGraphSolve(matrixMethod method = ADJ);
     virtual ~FGraphSolve();
@@ -118,16 +119,20 @@ class FGraphSolve: public FGraph
 
     /**
      * Returns a copy to the information matrix.
+     * pybind does not allow to pass by reference, so there is a copy anyway
+     * CHeck out more here: https://pybind11.readthedocs.io/en/stable/advanced/cast/eigen.html
      * TODO If true, it re-evaluates the problem
      */
     SMatCol get_information_matrix() { return L_;}
     /**
-     * Returns a copy to the information matrix.
+     * Returns a copy to the Adjacency matrix.
+     * There is a conversion (implies copy) from Row to Col-convention (which is what np.array needs)
      * TODO If true, it re-evaluates the problem
      */
     SMatCol get_adjacency_matrix() { return A_;}
     /**
      * Returns a copy to the W matrix.
+     * There is a conversion (implies copy) from Row to Col-convention (which is what np.array needs)
      * TODO If true, it re-evaluates the problem
      */
     SMatCol get_W_matrix() { return W_;}
@@ -165,6 +170,12 @@ class FGraphSolve: public FGraph
      * The residuals are also calculated as b = A^T * W *r
      */
     void build_info_adjacency();
+    /**
+     * Builds the information matrix directly from Eigen Factors.
+     * It follows a different approach than build adjacency, it will only create
+     * a Hessian and Jacobian when at least one EF is present.
+     */
+    void build_info_EF();
     void build_schur(); // TODO
 
     /**
@@ -217,15 +228,19 @@ class FGraphSolve: public FGraph
 
     // Variables for solving the FGraph
     matrixMethod matrixMethod_;
+    optimMethod optimMethod_;
+
+    // Indexes of nodes in the information matrix. NOTE: it requires all nodes (not a subset)
+    std::vector<uint_t> indNodesMatrix_;
 
     factor_id_t N_; // total number of state variables
     factor_id_t M_; // total number of observation variables
 
-    SMatRow A_; //Adjacency matrix, as a Row sparse matrix
+    SMatRow A_; //Adjacency matrix, as a Row sparse matrix. The reason is for filling in row-fashion for each factor
     SMatRow W_; //A block diagonal information matrix. For types Adjacency it calculates its block transposed squared root
     MatX1 r_; // Residuals as given by the factors
 
-    SMatCol L_; //Information matrix
+    SMatCol L_; //Information matrix. For Eigen Cholesky AMD Ordering it is necessary Col convetion for compilation.
     MatX1 b_; // Post-processed residuals, A'*W*r
 
     // Correction deltas
@@ -236,6 +251,11 @@ class FGraphSolve: public FGraph
     matData_t solutionTolerance_;
     MatX1 diagL_; //diagonal matrix (vector) of L to update it efficiently
 
+
+    // Methods for handling Eigen factors. If not used, no problem
+    SMatCol hessianEF_;
+    MatX1 gradientEF_;
+
     // time profiling
     TimeProfiling time_profiles_;
 };
diff --git a/src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp b/src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp
index 995680c..a155dc6 100644
--- a/src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp
+++ b/src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp
@@ -77,10 +77,10 @@ class Factor1Pose1Landmark2d : public Factor
 
     void print() const;
 
-    const Eigen::Ref<const MatX> get_obs() const {return obs_;};
-    const Eigen::Ref<const MatX1> get_residual() const {return r_;};
-    const Eigen::Ref<const MatX> get_information_matrix() const {return W_;};
-    const Eigen::Ref<const MatX> get_jacobian() const {return J_;};
+    MatRefConst get_obs() const {return obs_;};
+    VectRefConst get_residual() const {return r_;};
+    MatRefConst get_information_matrix() const {return W_;};
+    MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const {return J_;};
 
   protected:
     Mat21 obs_, r_, landmark_;
diff --git a/src/FGraph/mrob/factors/factor1Pose1Landmark3d.hpp b/src/FGraph/mrob/factors/factor1Pose1Landmark3d.hpp
index d088003..03a0570 100644
--- a/src/FGraph/mrob/factors/factor1Pose1Landmark3d.hpp
+++ b/src/FGraph/mrob/factors/factor1Pose1Landmark3d.hpp
@@ -81,10 +81,10 @@ class Factor1Pose1Landmark3d : public Factor
 
     void print() const;
 
-    const Eigen::Ref<const MatX> get_obs() const {return obs_;};
-    const Eigen::Ref<const MatX1> get_residual() const {return r_;};
-    const Eigen::Ref<const MatX> get_information_matrix() const {return W_;};
-    const Eigen::Ref<const MatX> get_jacobian() const {return J_;};
+    MatRefConst get_obs() const {return obs_;};
+    VectRefConst get_residual() const {return r_;};
+    MatRefConst get_information_matrix() const {return W_;};
+    MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const {return J_;};
 
   protected:
     Mat31 obs_, r_, landmark_;
diff --git a/src/FGraph/mrob/factors/factor1Pose2d.hpp b/src/FGraph/mrob/factors/factor1Pose2d.hpp
index 7e9f1bc..e57587a 100644
--- a/src/FGraph/mrob/factors/factor1Pose2d.hpp
+++ b/src/FGraph/mrob/factors/factor1Pose2d.hpp
@@ -55,10 +55,10 @@ namespace mrob{
 
         void print() const;
 
-        const Eigen::Ref<const MatX> get_obs() const {return obs_;};
-        const Eigen::Ref<const MatX1> get_residual() const {return r_;};
-        const Eigen::Ref<const MatX> get_information_matrix() const {return W_;};
-        const Eigen::Ref<const MatX> get_jacobian() const {return J_;};
+        MatRefConst get_obs() const {return obs_;};
+        VectRefConst get_residual() const {return r_;};
+        MatRefConst get_information_matrix() const {return W_;};
+        MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const {return J_;};
 
     protected:
         Mat31 obs_, r_; //and residuals
diff --git a/src/FGraph/mrob/factors/factor1Pose3d.hpp b/src/FGraph/mrob/factors/factor1Pose3d.hpp
index 9bd0220..01e21bb 100644
--- a/src/FGraph/mrob/factors/factor1Pose3d.hpp
+++ b/src/FGraph/mrob/factors/factor1Pose3d.hpp
@@ -58,10 +58,10 @@ class Factor1Pose3d : public Factor
 
     void print() const;
 
-    const Eigen::Ref<const MatX> get_obs() const {return Tobs_.T();};
-    const Eigen::Ref<const MatX1> get_residual() const {return r_;};
-    const Eigen::Ref<const MatX> get_information_matrix() const {return W_;};
-    const Eigen::Ref<const MatX> get_jacobian() const {return J_;};
+    MatRefConst get_obs() const {return Tobs_.T();};
+    VectRefConst get_residual() const {return r_;};
+    MatRefConst get_information_matrix() const {return W_;};
+    MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const {return J_;};
 
 
   protected:
diff --git a/src/FGraph/mrob/factors/factor2Poses2d.hpp b/src/FGraph/mrob/factors/factor2Poses2d.hpp
index a2694d3..8b9d294 100644
--- a/src/FGraph/mrob/factors/factor2Poses2d.hpp
+++ b/src/FGraph/mrob/factors/factor2Poses2d.hpp
@@ -61,10 +61,10 @@ namespace mrob{
         void evaluate_jacobians() override;
         void evaluate_chi2() override;
 
-        const Eigen::Ref<const MatX> get_obs() const override {return obs_;};
-        const Eigen::Ref<const MatX1> get_residual() const override {return r_;};
-        const Eigen::Ref<const MatX> get_information_matrix() const override {return W_;};
-        const Eigen::Ref<const MatX> get_jacobian() const override {return J_;};
+        MatRefConst get_obs() const override {return obs_;};
+        VectRefConst get_residual() const override {return r_;};
+        MatRefConst get_information_matrix() const override {return W_;};
+        MatRefConst get_jacobian([[maybe_unused]]mrob::factor_id_t id = 0) const override {return J_;};
         void print() const override;
 
     protected:
diff --git a/src/FGraph/mrob/factors/factor2Poses3d.hpp b/src/FGraph/mrob/factors/factor2Poses3d.hpp
index 068519c..6992074 100644
--- a/src/FGraph/mrob/factors/factor2Poses3d.hpp
+++ b/src/FGraph/mrob/factors/factor2Poses3d.hpp
@@ -83,7 +83,7 @@ class Factor2Poses3d : public Factor
     /**
      * Jacobians are not evaluated, just the residuals
      */
-    virtual void evaluate_residuals() override;
+    void evaluate_residuals() override;
     /**
      * Evaluates residuals and Jacobians
      */
@@ -92,10 +92,10 @@ class Factor2Poses3d : public Factor
 
     virtual void print() const;
 
-    virtual const Eigen::Ref<const MatX> get_obs() const {return Tobs_.T();};
-    virtual const Eigen::Ref<const MatX1> get_residual() const {return r_;};
-    virtual const Eigen::Ref<const MatX> get_information_matrix() const {return W_;};
-    virtual const Eigen::Ref<const MatX> get_jacobian() const {return J_;};
+    MatRefConst get_obs() const override {return Tobs_.T();}
+    VectRefConst get_residual() const override {return r_;}
+    MatRefConst get_information_matrix() const override {return W_;}
+    MatRefConst get_jacobian([[maybe_unused]]factor_id_t id = 0) const override {return J_;}
 
   protected:
     // The Jacobians' correspondant nodes are ordered on the vector<Node>
diff --git a/src/FGraph/mrob/factors/nodeLandmark2d.hpp b/src/FGraph/mrob/factors/nodeLandmark2d.hpp
index 0466a2e..ad1f96d 100644
--- a/src/FGraph/mrob/factors/nodeLandmark2d.hpp
+++ b/src/FGraph/mrob/factors/nodeLandmark2d.hpp
@@ -45,12 +45,12 @@ class NodeLandmark2d : public Node
     NodeLandmark2d(const Mat21 &initial_x, Node::nodeMode mode = STANDARD);
     virtual ~NodeLandmark2d()  override = default;
 
-    virtual void update(const Eigen::Ref<const MatX1> &dx);
-    virtual void update_from_auxiliary(const Eigen::Ref<const MatX1> &dx);
-    virtual void set_state(const Eigen::Ref<const MatX> &x);
-    virtual void set_auxiliary_state(const Eigen::Ref<const MatX> &x);
-    virtual const Eigen::Ref<const MatX> get_state() const {return state_;};
-    virtual const Eigen::Ref<const MatX> get_auxiliary_state() const {return auxiliaryState_;};
+    void update(VectRefConst &dx) override;
+    void update_from_auxiliary(VectRefConst &dx) override;
+    void set_state(MatRefConst &x) override;
+    void set_auxiliary_state(MatRefConst &x) override;
+    MatRefConst get_state() const override {return state_;}
+    MatRefConst get_auxiliary_state() const override {return auxiliaryState_;}
     void print() const;
 
   protected:
diff --git a/src/FGraph/mrob/factors/nodeLandmark3d.hpp b/src/FGraph/mrob/factors/nodeLandmark3d.hpp
index b5c06ca..a80b7f0 100644
--- a/src/FGraph/mrob/factors/nodeLandmark3d.hpp
+++ b/src/FGraph/mrob/factors/nodeLandmark3d.hpp
@@ -40,12 +40,12 @@ class NodeLandmark3d : public Node
     //NodePose3d(const SE3 &initial_T);
     virtual ~NodeLandmark3d()  override = default;
 
-    virtual void update(const Eigen::Ref<const MatX1> &dx);
-    virtual void update_from_auxiliary(const Eigen::Ref<const MatX1> &dx);
-    virtual void set_state(const Eigen::Ref<const MatX> &x);
-    virtual void set_auxiliary_state(const Eigen::Ref<const MatX> &x);
-    virtual const Eigen::Ref<const MatX> get_state() const {return state_;};
-    virtual const Eigen::Ref<const MatX> get_auxiliary_state() const {return auxiliaryState_;};
+    void update(VectRefConst &dx) override;
+    void update_from_auxiliary(VectRefConst &dx) override;
+    void set_state(MatRefConst &x) override;
+    void set_auxiliary_state(MatRefConst &x) override;
+    MatRefConst get_state() const override {return state_;}
+    MatRefConst get_auxiliary_state() const override {return auxiliaryState_;}
     void print() const;
 
   protected:
diff --git a/src/FGraph/mrob/factors/nodePose2d.hpp b/src/FGraph/mrob/factors/nodePose2d.hpp
index 8f437f5..ee0f6c5 100644
--- a/src/FGraph/mrob/factors/nodePose2d.hpp
+++ b/src/FGraph/mrob/factors/nodePose2d.hpp
@@ -42,12 +42,12 @@ namespace mrob{
         explicit NodePose2d(const Mat31 &initial_x, Node::nodeMode mode = STANDARD);
         virtual ~NodePose2d()  override = default;
 
-        virtual void update(const Eigen::Ref<const MatX1> &dx);
-        virtual void update_from_auxiliary(const Eigen::Ref<const MatX1> &dx);
-        virtual void set_state(const Eigen::Ref<const MatX> &x);
-        virtual void set_auxiliary_state(const Eigen::Ref<const MatX> &x);
-        virtual const Eigen::Ref<const MatX> get_state() const {return state_;};
-        virtual const Eigen::Ref<const MatX> get_auxiliary_state() const {return auxiliaryState_;};
+        virtual void update(VectRefConst &dx);
+        virtual void update_from_auxiliary(VectRefConst &dx);
+        virtual void set_state(MatRefConst &x);
+        virtual void set_auxiliary_state(MatRefConst &x);
+        virtual MatRefConst get_state() const {return state_;};
+        virtual MatRefConst get_auxiliary_state() const {return auxiliaryState_;};
         void print() const;
     protected:
         Mat31 state_;
diff --git a/src/FGraph/mrob/factors/nodePose3d.hpp b/src/FGraph/mrob/factors/nodePose3d.hpp
index 0bc9729..8e9cc53 100644
--- a/src/FGraph/mrob/factors/nodePose3d.hpp
+++ b/src/FGraph/mrob/factors/nodePose3d.hpp
@@ -50,12 +50,12 @@ class NodePose3d : public Node
      * T'=exp(dxi^)*T
      * x'=vee(ln(T'))
      */
-    virtual void update(const Eigen::Ref<const MatX1> &dx);
-    virtual void update_from_auxiliary(const Eigen::Ref<const MatX1> &dx);
-    virtual void set_state(const Eigen::Ref<const MatX> &x);
-    virtual void set_auxiliary_state(const Eigen::Ref<const MatX> &x);
-    virtual const Eigen::Ref<const MatX> get_state() const {return state_.T();};
-    virtual const Eigen::Ref<const MatX> get_auxiliary_state() const {return auxiliaryState_.T();};
+    virtual void update(VectRefConst &dx);
+    virtual void update_from_auxiliary(VectRefConst &dx);
+    virtual void set_state(MatRefConst &x);
+    virtual void set_auxiliary_state(MatRefConst &x);
+    virtual MatRefConst get_state() const {return state_.T();};
+    virtual MatRefConst get_auxiliary_state() const {return auxiliaryState_.T();};
     void print() const;
 
   protected:
diff --git a/src/FGraph/mrob/node.hpp b/src/FGraph/mrob/node.hpp
index 6f17f7f..28788c2 100644
--- a/src/FGraph/mrob/node.hpp
+++ b/src/FGraph/mrob/node.hpp
@@ -33,15 +33,15 @@
 
 namespace mrob{
 
-class Factor;
 
 /**
  * Node class is an abstract class for creating future nodes. Pure
  * abstract methods on get and set dimension
  * is just a reminder that this is an Abstract class
  *
- * Node class keeps track of all the neighbouring factors.
- * Destructor takes care of the neighbour factors included.
+ * Node does not track the factors associated to it, it just contains
+ * the state variables and allows to update its value.
+ * (whereas before we kept track of factors in an unnecessary complex solution).
  *
  * States are indicated as references to MatX matrices, since
  * it can be either a block vector or a matrix (transformation matrix)
@@ -73,54 +73,45 @@ class Node{
      * a fixed block matrix, and this virtual function will handle
      * it nicely.
      */
-    virtual void update(const Eigen::Ref<const MatX1> &dx) = 0;
+    virtual void update(VectRefConst &dx) = 0;
     /**
      * Updates *FROM* the auxiliary state the principal state.
      */
-    virtual void update_from_auxiliary(const Eigen::Ref<const MatX1> &dx) = 0;
+    virtual void update_from_auxiliary(VectRefConst &dx) = 0;
     /**
      * At run time sets the new value of the estate and auxiliary to be x
      *
      */
-    virtual void set_state(const Eigen::Ref<const MatX> &x) = 0;
+    virtual void set_state(MatRefConst &x) = 0;
     /**
      * New auxiliary state set
      */
-    virtual void set_auxiliary_state(const Eigen::Ref<const MatX> &x) = 0;
+    virtual void set_auxiliary_state(MatRefConst &x) = 0;
     /**
      * Declared as a dynamic matrix reference to allow any size to be returned.
      * At run time returns a Reference to a fixed size matrix and provide
      * it as an argument for the getState function, no need to be dynamic,
      * as long as the dimension is correctly set
      */
-    virtual const Eigen::Ref<const MatX> get_state() const = 0;
-    /**
-     * returns the state Transformation, equivalent to state
-     * but direcly the matrix representing the rotation or RBT
-     TODO to remove, states could be considered matrices with new api
-     */
-    //virtual const Eigen::Ref<const MatX> get_stateT() const = 0;
+    virtual const MatRefConst get_state() const = 0;
     /**
      * Returns a matrix to the last auxiliary state. This data structure is for the incre-
      * metal implementation, or for error evaluation
      */
-    virtual const Eigen::Ref<const MatX> get_auxiliary_state() const = 0;
+    virtual MatRefConst get_auxiliary_state() const = 0;
     virtual void print() const {}
     factor_id_t get_id() const {return id_;}
     void set_id(factor_id_t id) {id_ = id;}
     factor_id_t get_dim(void) const {return dim_;}
+
     /**
-     * Adds a factor to the list of factors connected to this node.
-     */
-    virtual bool add_factor(std::shared_ptr<Factor> &factor);
-    /**
-     * This function is very inefficient: it is an exhaustive search
-     * so use only when necessary.
+     * Methods for interacting with Eigen factors, a special kind of factors.
+     * This bool variable and its associated methods will make easier the
+     * construction of the matrix
+     * TODO not used right now, but it could to improve efficiency.
      */
-    virtual bool rm_factor(std::shared_ptr<Factor> &factor);
-    void clear() {neighbourFactors_.clear();}
-    const std::vector<std::shared_ptr<Factor> >*
-            get_neighbour_factors(void) const {return &neighbourFactors_;}
+     bool is_connected_to_EF() const {return isConnected2EF_;}
+     void set_connected_to_EF(bool state = true) {isConnected2EF_ = state;}
 
 
     void set_node_mode(nodeMode mode){node_mode_ = mode;}
@@ -128,8 +119,6 @@ class Node{
 
 
   protected:
-    // no oder needed here
-    std::vector<std::shared_ptr<Factor> > neighbourFactors_;
     factor_id_t id_;
     uint_t dim_;
     nodeMode node_mode_;
@@ -143,6 +132,9 @@ class Node{
      *      Mat61 x_;
      * or   SE3 T_; (for transformations)
      */
+     // This variable is to know if there is an Eigen Factor connected to this node. The alternative
+     // is a list of factors connected, but removed this option since it was not really necessary.
+     bool isConnected2EF_;
 };
 
 /**
diff --git a/src/FGraph/node.cpp b/src/FGraph/node.cpp
index 3242bee..6a3510b 100644
--- a/src/FGraph/node.cpp
+++ b/src/FGraph/node.cpp
@@ -26,35 +26,14 @@
 using namespace mrob;
 
 Node::Node(uint_t dim, nodeMode mode):
-		 id_(0), dim_(dim), node_mode_(mode)
+		 id_(0), dim_(dim), node_mode_(mode), isConnected2EF_(false)
 {
 }
 
 Node::~Node()
 {
-    neighbourFactors_.clear();
-}
-bool Node::add_factor(std::shared_ptr<Factor> &factor)
-{
-    neighbourFactors_.push_back(factor);
-    return true;
 }
 
-bool Node::rm_factor(std::shared_ptr<Factor> &factor)
-{
-    // exhaustive line search over the vector, this SHOULD be small, right?
-    // still, it is very ineffcient O(n), but we have preferred using a vector
-    // over a set or a map because we are gonna iterate over this container,
-    // while it is not so clear that we would want to remove factors (although possible)
-    std::vector<std::shared_ptr<Factor> >::iterator f;
-    for (f = neighbourFactors_.begin(); f != neighbourFactors_.end(); ++f)
-    {
-        if (*f == factor)
-            break;
-    }
-    neighbourFactors_.erase(f);
-    return true;
-}
 
 // support function for 2D poses
 double mrob::wrap_angle(double angle)
diff --git a/src/PCRegistration/CMakeLists.txt b/src/PCRegistration/CMakeLists.txt
index bcefa9b..e891012 100644
--- a/src/PCRegistration/CMakeLists.txt
+++ b/src/PCRegistration/CMakeLists.txt
@@ -10,7 +10,11 @@ SET(sources
     create_points.cpp
     weight_point.cpp
     estimate_plane.cpp
+    factors/factor1PosePoint2Point.cpp
     factors/factor1PosePoint2Plane.cpp
+    factors/nodePlane4d.cpp
+    factors/factor1Pose1Plane4d.cpp
+    factors/EigenFactorPlane.cpp
 )
 
 # extra header files
@@ -20,7 +24,11 @@ SET(headers
     mrob/plane_registration.hpp
     mrob/create_points.hpp
     mrob/estimate_plane.hpp
+    mrob/factors/factor1PosePoint2Point.hpp
     mrob/factors/factor1PosePoint2Plane.hpp
+    mrob/factors/nodePlane4d.hpp
+    mrob/factors/factor1Pose1plane4d.hpp
+    mrob/factors/EigenFactorPlane.hpp
 )
 
 # create the shared library
diff --git a/src/PCRegistration/README.md b/src/PCRegistration/README.md
index 2663de9..279c574 100644
--- a/src/PCRegistration/README.md
+++ b/src/PCRegistration/README.md
@@ -1,9 +1,20 @@
 # PCRegistration
-Point Cloud Registration. Different methods implemente for point cloud registration:
+Point Cloud Registration. Different methods implemented for point cloud registration:
 * Arun, and SVD-based method (Arun'1983)
 * GICP
-* NICP?
+* Weighted ICP
+* Point-to-point FGraph (solved iteratively)
+* Point-to-plane FGraph
+* Eigen-factors Fgraph
 
 
+There are also some routines dedicated to generate sinthetic planes
+
 ## Dependencies
 C++'14, Eigen
+
+
+## Files deprecated
+estimate_plane.hpp
+plane_registration.cpp
+plane.hpp
\ No newline at end of file
diff --git a/src/PCRegistration/estimate_plane.cpp b/src/PCRegistration/estimate_plane.cpp
index 77c79d4..1335dae 100644
--- a/src/PCRegistration/estimate_plane.cpp
+++ b/src/PCRegistration/estimate_plane.cpp
@@ -26,9 +26,9 @@
 
 using namespace mrob;
 
-Mat41 estimate_plane_centered(const Eigen::Ref<const MatX> X);
+Mat41 estimate_plane_centered(MatRefConst X);
 
-Mat41 mrob::estimate_plane(const Eigen::Ref<const MatX> X)
+Mat41 mrob::estimate_plane(MatRefConst X)
 {
     // Initialization
     assert(X.cols() == 3  && "Estimate_plane: Incorrect sizing, we expect Nx3");
@@ -39,7 +39,7 @@ Mat41 mrob::estimate_plane(const Eigen::Ref<const MatX> X)
 }
 
 //local function, we also will test the homogeneous plane estimation
-Mat41 estimate_plane_centered(const Eigen::Ref<const MatX> X)
+Mat41 estimate_plane_centered(MatRefConst X)
 {
     uint_t N = X.rows();
     // Calculate center of points:
@@ -67,14 +67,14 @@ Mat41 estimate_plane_centered(const Eigen::Ref<const MatX> X)
 
 
 
-Mat31 mrob::estimate_normal(const Eigen::Ref<const MatX> X)
+Mat31 mrob::estimate_normal(MatRefConst X)
 {
     Mat41 res = estimate_plane(X);
     return res.head(3);
 }
 
 
-Mat31 mrob::estimate_centroid(const Eigen::Ref<const MatX> X)
+Mat31 mrob::estimate_centroid(MatRefConst X)
 {
     // Initialization
     assert(X.cols() == 3  && "Estimate_centroid: Incorrect sizing, we expect Nx3");
diff --git a/src/PCRegistration/examples/CMakeLists.txt b/src/PCRegistration/examples/CMakeLists.txt
index dd19577..b39e610 100644
--- a/src/PCRegistration/examples/CMakeLists.txt
+++ b/src/PCRegistration/examples/CMakeLists.txt
@@ -6,3 +6,6 @@ TARGET_LINK_LIBRARIES(example_synthetic_plane_traj PCRegistration)
 
 ADD_EXECUTABLE(example_fgraph_align  example_fgraph_align.cpp)
 TARGET_LINK_LIBRARIES(example_fgraph_align FGraph PCRegistration)
+
+ADD_EXECUTABLE(test_EF_Hessian  test_EF_Hessian.cpp)
+TARGET_LINK_LIBRARIES(test_EF_Hessian SE3)
\ No newline at end of file
diff --git a/src/PCRegistration/factors/EigenFactorPlane.cpp b/src/PCRegistration/factors/EigenFactorPlane.cpp
new file mode 100644
index 0000000..0e78266
--- /dev/null
+++ b/src/PCRegistration/factors/EigenFactorPlane.cpp
@@ -0,0 +1,211 @@
+/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * plane_factor.cpp
+ *
+ *  Created on: Aug 16, 2019
+ *      Author: Gonzalo Ferrer
+ *              g.ferrer@skoltech.ru
+ *              Mobile Robotics Lab.
+ */
+
+
+#include "mrob/factors/EigenFactorPlane.hpp"
+
+#include <iostream>
+#include <Eigen/Eigenvalues>
+#include "mrob/SE3.hpp"
+
+using namespace mrob;
+
+EigenFactorPlane::EigenFactorPlane(Factor::robustFactorType robust_type):
+        EigenFactor(robust_type),
+        planeEstimation_{Mat41::Zero()},
+        planeError_{0.0},
+        numberPoints_{0}
+{
+}
+
+void EigenFactorPlane::evaluate_residuals()
+{
+    this->estimate_plane();
+}
+
+void EigenFactorPlane::evaluate_jacobians()
+{
+    // Assumes residuals evaluated beforehand
+    J_.clear();
+    H_.clear();
+    for (auto &Qt: Q_)
+    {
+        Mat61 jacobian = Mat61::Zero();
+        Mat6 hessian = Mat6::Zero();
+        Mat4 dQ = Mat4::Zero();
+        for (uint_t i = 0 ; i < 6; i++)
+        {
+            dQ = SE3GenerativeMatrix(i)*Qt + Qt*SE3GenerativeMatrix(i).transpose();
+            jacobian(i) = planeEstimation_.dot(dQ*planeEstimation_);
+
+            //now calculate Hessian here. Upper triangular view
+            Mat4 ddQ; // second derivative of the Q matrix
+            for (uint_t j = i ; j< 6 ; ++j)
+            {
+                ddQ.setZero();
+                ddQ = SE3GenerativeMatrix(i)*SE3GenerativeMatrix(j) + SE3GenerativeMatrix(j)*SE3GenerativeMatrix(i);
+                //compound operator *= as in a*=b (this multiplies on the right: a*=b is equivalent to a = a*b)
+                ddQ *= 0.5 * Qt;
+                ddQ += SE3GenerativeMatrix(j) * dQ;//here indices should be different, later Hessian is symmetric.
+                ddQ += ddQ.transpose().eval();
+                hessian(i,j) = planeEstimation_.dot(ddQ*planeEstimation_);
+            }
+        }
+        J_.push_back(jacobian);
+        H_.push_back(hessian);
+
+    }
+}
+
+void EigenFactorPlane::evaluate_chi2()
+{
+    chi2_ = planeError_;
+}
+
+void EigenFactorPlane::add_point(const Mat31& p, std::shared_ptr<Node> &node, matData_t &W)
+{
+    // Pose has been observed, data has been initialized and we simply add point
+    auto id = node->get_id();
+    if (reverseNodeIds_.count(id) > 0)
+    {
+        uint_t localId = reverseNodeIds_[id];
+        allPlanePoints_.at(localId).push_back(p);
+        allPointsInformation_.at(localId).push_back(W);
+    }
+    // If EF has not observed point from the current Node, it creates:
+    else
+    {
+        allPlanePoints_.emplace_back(std::deque<Mat31, Eigen::aligned_allocator<Mat31>>());
+        allPointsInformation_.emplace_back(std::deque<matData_t>());
+        neighbourNodes_.push_back(node);
+        node->set_connected_to_EF(true);//This function is required to properly build the L matrix
+        nodeIds_.push_back(id);
+        uint_t localId = allPlanePoints_.size()-1;
+        reverseNodeIds_.emplace(id, localId);
+        allPlanePoints_.at(localId).push_back(p);
+        allPointsInformation_.at(localId).push_back(W);
+        // S and Q are built later, no need to create an element.
+    }
+    numberPoints_++;
+
+}
+
+
+double EigenFactorPlane::estimate_plane()
+{
+    calculate_all_matrices_S();
+    calculate_all_matrices_Q();
+    accumulatedQ_ = Mat4::Zero();
+    for (auto &Qt: Q_)
+    {
+        accumulatedQ_ += Qt;
+    }
+
+    // Only needs Lower View from Q (https://eigen.tuxfamily.org/dox/classEigen_1_1SelfAdjointEigenSolver.html)
+    Eigen::SelfAdjointEigenSolver<Mat4> es(accumulatedQ_);
+    planeEstimation_ = es.eigenvectors().col(0);
+    //std::cout << es.eigenvectors() << "\n and solution \n" << planeEstimation_ <<  std::endl;
+    //std::cout << "plane estimation error: " << es.eigenvalues() <<  std::endl;
+    planeError_ = es.eigenvalues()(0);
+
+    return planeError_;
+}
+
+void EigenFactorPlane::calculate_all_matrices_S(bool reset)
+{
+    if (reset)
+        S_.clear();
+    if (S_.empty())
+    {
+        for (auto &vectorPoints: allPlanePoints_)
+        {
+            Mat4 S = Mat4::Zero();
+            for (Mat31 &p : vectorPoints)
+            {
+                Mat41 pHomog;
+                pHomog << p , 1.0;
+                S += pHomog * pHomog.transpose();//TODO robust: add coeficient W here...
+            }
+            S_.push_back(S);
+        }
+    }
+}
+
+void EigenFactorPlane::calculate_all_matrices_Q()
+{
+    Q_.clear();
+    uint_t nodeIdLocal = 0;
+    for (auto &S : S_)
+    {
+        Mat4 T = this->neighbourNodes_[nodeIdLocal]->get_state();
+        // Use the corresponding matrix S
+        Mat4 Q;
+        Q.noalias() =  T * S * T.transpose();
+        Q_.push_back(Q);
+        nodeIdLocal++;
+    }
+}
+
+Mat31 EigenFactorPlane::get_mean_point(factor_id_t id)
+{
+    assert(!S_.empty() && "EigenFactorPlane::get_mean_point: S matrix empty");
+    auto localId = reverseNodeIds_.at(id);
+    return S_[localId].topRightCorner<3,1>()/S_[localId](3,3);
+}
+
+void EigenFactorPlane::print() const
+{
+    std::cout << "Plane Eigen Factor " <<  this->get_id()
+              << " current plane estimated: " << planeEstimation_.transpose() << std::endl;
+    for(auto id : nodeIds_)
+        std::cout << "Node ids = "  << id << ", and its reverse in EF = "
+                  << reverseNodeIds_.at(id) << std::endl;
+    /*for(auto &pc: allPlanePoints_ )
+    {
+        std::cout << "new pose: \n";
+        for (auto &p : pc)
+            std::cout << "point = " << p.transpose() <<std::endl;
+    }*/
+    std::cout << "Plotting S \n";
+    for(auto &S: S_)
+        std::cout << S << std::endl;
+    std::cout << "Plotting Jacobians \n";
+    for(auto &J: J_)
+        std::cout << J.transpose() << std::endl;
+}
+
+
+MatRefConst EigenFactorPlane::get_jacobian(mrob::factor_id_t id) const
+{
+    assert(reverseNodeIds_.count(id)   && "EigenFactorPlane::get_jacobian: element not found");
+    uint_t localId = reverseNodeIds_.at(id);
+    return J_.at(localId);
+}
+
+MatRefConst EigenFactorPlane::get_hessian(mrob::factor_id_t id) const
+{
+    assert(reverseNodeIds_.count(id)   && "EigenFactorPlane::get_hessian: element not found");
+    uint_t localId = reverseNodeIds_.at(id);
+    return H_.at(localId);
+}
+
diff --git a/src/FGraph/factors/factor1Pose1Plane4d.cpp b/src/PCRegistration/factors/factor1Pose1Plane4d.cpp
similarity index 98%
rename from src/FGraph/factors/factor1Pose1Plane4d.cpp
rename to src/PCRegistration/factors/factor1Pose1Plane4d.cpp
index 5a39fc5..3f396dd 100644
--- a/src/FGraph/factors/factor1Pose1Plane4d.cpp
+++ b/src/PCRegistration/factors/factor1Pose1Plane4d.cpp
@@ -72,6 +72,7 @@ void Factor1Pose1Plane4d::evaluate_residuals()
     Tinv_transp_ = SE3(Tx).T().transpose();
     plane_ = get_neighbour_nodes()->at(landmarkIndex)->get_state();
     Mat41 pi_local = Tinv_transp_*plane_;
+    //TODO this residual should be a cross product, dot product or triple product
     r_ = pi_local - obs_;
     // Normals must have the same direction, so we ensure this here. XXX: Temporary solution
     //if (pi_local.head(3).dot(obs_.head(3)) < 0.0)
diff --git a/src/PCRegistration/factors/factor1PosePoint2Plane.cpp b/src/PCRegistration/factors/factor1PosePoint2Plane.cpp
index 7cb9c39..1e2ee1a 100644
--- a/src/PCRegistration/factors/factor1PosePoint2Plane.cpp
+++ b/src/PCRegistration/factors/factor1PosePoint2Plane.cpp
@@ -35,11 +35,6 @@ Factor1PosePoint2Plane::Factor1PosePoint2Plane(const Mat31 &z_point_x, const Mat
 }
 
 
-Factor1PosePoint2Plane::~Factor1PosePoint2Plane()
-{
-}
-
-
 void Factor1PosePoint2Plane::evaluate_residuals()
 {
     // r = <pi, Tp>
diff --git a/src/PCRegistration/factors/factor1PosePoint2Point.cpp b/src/PCRegistration/factors/factor1PosePoint2Point.cpp
index 9142e5f..4a88851 100644
--- a/src/PCRegistration/factors/factor1PosePoint2Point.cpp
+++ b/src/PCRegistration/factors/factor1PosePoint2Point.cpp
@@ -29,18 +29,12 @@
 using namespace mrob;
 
 Factor1PosePoint2Point::Factor1PosePoint2Point(const Mat31 &z_point_x, const Mat31 &z_point_y,  std::shared_ptr<Node> &node,
-            const Mat1 &obsInf, Factor::robustFactorType robust_type):
-        Factor(3,6,robust_type), z_point_x_(z_point_x), z_point_y_(z_point_y_), W_(obsInf)
+            const Mat3 &obsInf, Factor::robustFactorType robust_type):
+        Factor(3,6,robust_type), z_point_x_(z_point_x), z_point_y_(z_point_y), W_(obsInf)
 {
     neighbourNodes_.push_back(node);
 }
 
-
-Factor1PosePoint2Point::~Factor1PosePoint2Point()
-{
-
-}
-
 void Factor1PosePoint2Point::evaluate_residuals()
 {
     // r = Tx - y
diff --git a/src/FGraph/factors/nodePlane4d.cpp b/src/PCRegistration/factors/nodePlane4d.cpp
similarity index 85%
rename from src/FGraph/factors/nodePlane4d.cpp
rename to src/PCRegistration/factors/nodePlane4d.cpp
index 9ac2c21..4826765 100644
--- a/src/FGraph/factors/nodePlane4d.cpp
+++ b/src/PCRegistration/factors/nodePlane4d.cpp
@@ -40,13 +40,13 @@ NodePlane4d::NodePlane4d(const Mat41 &initial_x, Node::nodeMode mode):
 
 
 
-void NodePlane4d::update(const Eigen::Ref<const MatX1> &dx)
+void NodePlane4d::update(VectRefConst &dx)
 {
     state_ += dx;
     state_.head(3).normalize();
 }
 
-void NodePlane4d::update_from_auxiliary(const Eigen::Ref<const MatX1> &dx)
+void NodePlane4d::update_from_auxiliary(VectRefConst &dx)
 {
     state_ = auxiliaryState_ + dx;
     state_.head(3).normalize();
@@ -55,7 +55,5 @@ void NodePlane4d::update_from_auxiliary(const Eigen::Ref<const MatX1> &dx)
 void NodePlane4d::print() const
 {
     std::cout << "Printing NodePlane4d: " << id_
-        << ", state = \n" << state_ << ",\n";
-    std::cout  <<  "and neighbour factors " << neighbourFactors_.size()
-        << std::endl;
+        << ", state = \n" << state_ << std::endl;
 }
diff --git a/src/PCRegistration/gicp.cpp b/src/PCRegistration/gicp.cpp
index 63ddeaf..e8ce1e0 100644
--- a/src/PCRegistration/gicp.cpp
+++ b/src/PCRegistration/gicp.cpp
@@ -30,8 +30,8 @@
 
 using namespace mrob;
 
-int PCRegistration::gicp(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y,
-           const Eigen::Ref<const MatX> covX, const Eigen::Ref<const MatX> covY, SE3 &T, double tol)
+int PCRegistration::gicp(MatRefConst X, MatRefConst Y,
+           MatRefConst covX, MatRefConst covY, SE3 &T, double tol)
 {
     assert(X.cols() == 3  && "PCRegistration::Gicp: Incorrect sizing, we expect Nx3");
     assert(X.rows() >= 3  && "PCRegistration::Gicp: Incorrect sizing, we expect at least 3 correspondences (not aligned)");
diff --git a/src/PCRegistration/mrob/create_points.hpp b/src/PCRegistration/mrob/create_points.hpp
index af22644..3c53dad 100644
--- a/src/PCRegistration/mrob/create_points.hpp
+++ b/src/PCRegistration/mrob/create_points.hpp
@@ -92,6 +92,9 @@ class CreatePoints{
     /**
      * Fill the PlaneRegistration class with planes calculated here (reset)
      * and a new initial trajectory set to I's
+     *
+     * This function should be deprecated after PlaneResgitration is no longer used.
+     * Instead, we directly access point here and create a FGraph
      */
     void create_plane_registration(PlaneRegistration& planeReg);
 
@@ -135,7 +138,7 @@ class CreatePoints{
 
     // Generation of planes
     std::vector<SE3> planePoses_;
-    std::vector<std::pair<uint_t, std::shared_ptr<Plane> >> planes_;
+    std::vector<std::pair<uint_t, std::shared_ptr<Plane> >> planes_;//TODO this should be removed...
 
 };
 
diff --git a/src/PCRegistration/mrob/estimate_plane.hpp b/src/PCRegistration/mrob/estimate_plane.hpp
index 050312d..2cc8077 100644
--- a/src/PCRegistration/mrob/estimate_plane.hpp
+++ b/src/PCRegistration/mrob/estimate_plane.hpp
@@ -37,7 +37,7 @@ namespace mrob{
  * For now this is a unique function, TODO: add more methods here
  */
 
-Mat41 estimate_plane(const Eigen::Ref<const MatX> X);
+Mat41 estimate_plane(MatRefConst X);
 
 
 /**
@@ -47,7 +47,7 @@ Mat41 estimate_plane(const Eigen::Ref<const MatX> X);
  *
  */
 
-Mat31 estimate_normal(const Eigen::Ref<const MatX> X);
+Mat31 estimate_normal(MatRefConst X);
 
 
 /**
@@ -57,7 +57,7 @@ Mat31 estimate_normal(const Eigen::Ref<const MatX> X);
  *
  */
 
-Mat31 estimate_centroid(const Eigen::Ref<const MatX> X);
+Mat31 estimate_centroid(MatRefConst X);
 
 
 }
diff --git a/src/PCRegistration/mrob/factors/EigenFactorPlane.hpp b/src/PCRegistration/mrob/factors/EigenFactorPlane.hpp
new file mode 100644
index 0000000..ad4d1d3
--- /dev/null
+++ b/src/PCRegistration/mrob/factors/EigenFactorPlane.hpp
@@ -0,0 +1,194 @@
+/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ *
+ * EigenFactorPlane.hpp
+ *
+ *  Created on: Aug 16, 2019
+ *  Created on: July 10, 2021 (for real)
+ *      Author: Gonzalo Ferrer
+ *              g.ferrer@skoltech.ru
+ *              Mobile Robotics Lab.
+ */
+
+#ifndef EIGENFACTORPLANE_HPP_
+#define EIGENFACTORPLANE_HPP_
+
+
+#include "mrob/factor.hpp"
+#include <unordered_map>
+#include <deque>
+#include <Eigen/StdVector>
+
+
+namespace mrob{
+
+/**
+ * Eigen factor Plane is a vertex that complies with the Fgraph standards
+ * and inherits from base EigenFactor.hpp
+ *
+ * The Plane factor connects different poses that have observed the same geometric entity.
+ * It is not required an explicit parametrization of the plane, so the resultant topology
+ * is N nodes connecting to the plane factor.
+ *
+ * NOTE: due to its nature, multiple observation can be added to the same EF,
+ * meaning we need to create a constructor PLUS an additional method
+ *  - add_point()
+ *
+ * In order to build the problem we would follow the interface specifications by FGraph
+ * but we need extra methods and variables to keep track of the neighbours
+ *
+ * This class assumes that matrices S = sum p*p' are calculated before since they are directly inputs
+ * XXX should we store all points?
+ */
+class EigenFactorPlane: public EigenFactor{
+public:
+    /**
+     * Creates an Eigen Factor plane. The minimum requirements are 1 pose, which is not required
+     * at this stage, but will be introduced when we add points/Q matrix.
+     */
+    EigenFactorPlane(Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
+    ~EigenFactorPlane() override = default;
+    /**
+     * Jacobians are not evaluated, just the residuals.
+     * This function is calculating the current plane estimation
+     */
+    void evaluate_residuals() override;
+    /**
+     * Evaluates Jacobians, given the residual evaluated
+     * It also evaluated the Hessians
+     */
+    void evaluate_jacobians() override;
+    /**
+     * Chi2 is a scaling of the plane error
+     */
+    void evaluate_chi2() override;
+
+    void print() const;
+
+    MatRefConst get_obs() const
+            {assert(0 && "EigenFactorPlane:get_obs: method should not be called");return Mat31::Zero();}
+    VectRefConst get_residual() const
+            {assert(0 && "EigenFactorPlane::get_resigual: method should not be called");return Mat31::Zero();}
+    MatRefConst get_information_matrix() const
+            {assert(0 && "EigenFactorPlane::get_inform method should not be called");return Mat4::Zero();}
+
+    /**
+     * get jacobian returns the jacobian corresponding to the given node id.
+     * @return
+     */
+    MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const override;
+    /**
+     * get hessian returns the Hassian corresponding to the given node id.
+     * @return
+     */
+    MatRefConst get_hessian(mrob::factor_id_t id = 0) const override;
+
+
+    // NEW functions added to the base class factor.hpp
+    /**
+     * get current state of the Eigen Factor, in this case,
+     * a plane is returned as the current planeEstimation
+     */
+    VectRefConst get_state(void) const override
+                {return planeEstimation_;}
+    /**
+     * Add point: raw points are stored into an unoderred map
+     *
+     * Later, the S matrix should be calculated
+     *
+     * The alternative is adding directly S, but this offers less
+     * flexibility.
+     * XXX adding one by one might be inefficient
+     */
+    void add_point(const Mat31& p, std::shared_ptr<Node> &node, matData_t &W) override;
+    /**
+     * get mean point calculates the mean of the pointcloud observed at pose node id,
+     * given that S = sum p * p' =  sum ([x2 xy xz x
+     *                                    yx y2 yz y
+     *                                    zx zy z2 z
+     *                                    x   y  z 1]
+     * ser we just calculate S and return
+     */
+    Mat31 get_mean_point(factor_id_t id);
+
+
+protected:
+    /**
+     * Estimates the plane parameters: v = [n', d]'_{4x1}, where v is unit, (due to the Eigen solution)
+     * although for standard plane estimation we could enforce unit on the normal vector n.
+     */
+    double estimate_plane();
+    /**
+     * Calculates the matrix S = sum(p*p'), where p = [x,y,z,1]
+     * for all planes, as an aggregation of the outer product of all
+     * homogeneous points
+     * If reset = true, clears all information and recalculates S
+     * If reset = false (default) only calculates S if there is no calculation yet
+     */
+    void calculate_all_matrices_S(bool reset=false);
+    /**
+     *  calculates the matrix Qi = 1^T_i * Si * (1^T_i)^transp
+     *  for all planes. Since this is an iterative process on T's,
+     *  we separate the calculation of the S matrix,
+     *  and the Q matrix which rotates S
+     */
+    void calculate_all_matrices_Q();
+    /**
+     * A deque storing the ids in the FGraph of each of the poses
+     * This is preferred over vector due to the a priori unknown size of
+     * elements. The same applies for other structures containing matrices
+     */
+    std::deque<factor_id_t> nodeIds_;
+    /**
+     * Mapping from the FG nodes IDs to the local indexes in the current EF.
+     * This conversion is necessary to maintain consistency in case of
+     * non-subsequent observations
+     */
+    std::unordered_map<factor_id_t, uint_t> reverseNodeIds_;
+    /**
+     * The Jacobian of the plane error, the poses involved.
+     * Stores the map according to the nodes indexes/identifiers.
+     */
+    std::deque<Mat61, Eigen::aligned_allocator<Mat61>> J_;
+    /**
+     * Hessian matrix, dense since it connects all poses from where plane was observed.
+     * We store the block diagonal terms, according to the indexes of the nodes
+     */
+    std::deque<Mat6, Eigen::aligned_allocator<Mat6>> H_;
+    /**
+     * According to our notation S = sum p*p'
+     * We choose unordered map here since this is a subset of neighbours (small) and we will iterate over them
+     * Iterations may be not in strict order, but we don't care much for now (will we?)
+     *
+     * Q = T *S *T'
+     */
+    std::deque<Mat4, Eigen::aligned_allocator<Mat4>> S_, Q_;
+    Mat4 accumulatedQ_;//Q matrix of accumulated values for the incremental update of the error.
+
+    Mat41 planeEstimation_;
+    matData_t planeError_; //this is chi2 scaled by the covariance of point measurement.
+
+    // subset of pointcloud for the given plane
+    //std::unordered_map<factor_id_t, std::vector<Mat31> > allPlanePoints_;
+    std::deque<std::deque<Mat31, Eigen::aligned_allocator<Mat31>> > allPlanePoints_;
+    std::deque<std::deque<matData_t> > allPointsInformation_;
+    uint_t numberPoints_;
+
+public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen
+};
+
+}
+#endif /* EigenFactorPlane_HPP_ */
diff --git a/src/FGraph/mrob/factors/factor1Pose1Plane4d.hpp b/src/PCRegistration/mrob/factors/factor1Pose1Plane4d.hpp
similarity index 84%
rename from src/FGraph/mrob/factors/factor1Pose1Plane4d.hpp
rename to src/PCRegistration/mrob/factors/factor1Pose1Plane4d.hpp
index d33c9a2..fbeae09 100644
--- a/src/FGraph/mrob/factors/factor1Pose1Plane4d.hpp
+++ b/src/PCRegistration/mrob/factors/factor1Pose1Plane4d.hpp
@@ -50,17 +50,17 @@ class Factor1Pose1Plane4d : public Factor
      */
     virtual void evaluate_residuals() override;
     /**
-     * Evaluates residuals and Jacobians
+     * Evaluates the Jacobians
      */
     virtual void evaluate_jacobians() override;
     virtual void evaluate_chi2() override;
 
     virtual void print() const;
 
-    virtual const Eigen::Ref<const MatX> get_obs() const override {return obs_;};
-    virtual const Eigen::Ref<const MatX1> get_residual() const override {return r_;};
-    virtual const Eigen::Ref<const MatX> get_information_matrix() const override {return W_;};
-    virtual const Eigen::Ref<const MatX> get_jacobian() const override {return J_;};
+    MatRefConst get_obs() const override {return obs_;};
+    VectRefConst get_residual() const override {return r_;};
+    MatRefConst get_information_matrix() const override {return W_;};
+    MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const override {return J_;};
 
 
   private:
diff --git a/src/PCRegistration/mrob/factors/factor1PosePoint2Plane.hpp b/src/PCRegistration/mrob/factors/factor1PosePoint2Plane.hpp
index 2780f2a..7f9661c 100644
--- a/src/PCRegistration/mrob/factors/factor1PosePoint2Plane.hpp
+++ b/src/PCRegistration/mrob/factors/factor1PosePoint2Plane.hpp
@@ -57,7 +57,7 @@ class Factor1PosePoint2Plane: public Factor
     Factor1PosePoint2Plane(const Mat31 &z_point_x, const Mat31 &z_point_y, const Mat31 &z_normal_y,
             std::shared_ptr<Node> &node,  const Mat1 &obsInf,
             Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
-    ~Factor1PosePoint2Plane();
+    ~Factor1PosePoint2Plane() override = default;
     /**
      * Jacobians are not evaluated, just the residuals
      * r = <pi, T p>
@@ -74,10 +74,10 @@ class Factor1PosePoint2Plane: public Factor
 
     virtual void print() const;
 
-    virtual const Eigen::Ref<const MatX> get_obs() const {return r_;};
-    virtual const Eigen::Ref<const MatX1> get_residual() const {return r_;};
-    virtual const Eigen::Ref<const MatX> get_information_matrix() const {return W_;};
-    virtual const Eigen::Ref<const MatX> get_jacobian() const {return J_;};
+    MatRefConst get_obs() const override {return r_;};
+    VectRefConst get_residual() const override {return r_;};
+    MatRefConst get_information_matrix() const override {return W_;};
+    MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const override {return J_;};
 
   protected:
     Mat31 z_point_x_, z_point_y_,  Tx_;
diff --git a/src/PCRegistration/mrob/factors/factor1PosePoint2Point.hpp b/src/PCRegistration/mrob/factors/factor1PosePoint2Point.hpp
index b7e736c..c921500 100644
--- a/src/PCRegistration/mrob/factors/factor1PosePoint2Point.hpp
+++ b/src/PCRegistration/mrob/factors/factor1PosePoint2Point.hpp
@@ -20,8 +20,8 @@
  *              Mobile Robotics Lab.
  */
 
-#ifndef FACTORS_FACTOR1POSEPOINT2POINT_HPP_
-#define FACTORS_FACTOR1POSEPOINT2POINT_HPP_
+#ifndef FACTOR1POSEPOINT2POINT_HPP_
+#define FACTOR1POSEPOINT2POINT_HPP_
 
 #include "mrob/matrix_base.hpp"
 #include "mrob/SE3.hpp" //requires including and linking SE3 library
@@ -53,8 +53,8 @@ class Factor1PosePoint2Point : public Factor
 {
   public:
     Factor1PosePoint2Point(const Mat31 &z_point_x, const Mat31 &z_point_y,  std::shared_ptr<Node> &node,
-            const Mat1 &obsInf, Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
-    ~Factor1PosePoint2Point();
+            const Mat3 &obsInf, Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
+    ~Factor1PosePoint2Point() override = default;
     /**
      * Jacobians are not evaluated, just the residuals
      */
@@ -68,10 +68,10 @@ class Factor1PosePoint2Point : public Factor
 
     virtual void print() const;
 
-    virtual const Eigen::Ref<const MatX> get_obs() const {return r_;};
-    virtual const Eigen::Ref<const MatX1> get_residual() const {return r_;};
-    virtual const Eigen::Ref<const MatX> get_information_matrix() const {return W_;};
-    virtual const Eigen::Ref<const MatX> get_jacobian() const {return J_;};
+    MatRefConst get_obs() const override {return r_;};
+    VectRefConst get_residual() const override {return r_;};
+    MatRefConst get_information_matrix() const override{return W_;};
+    MatRefConst get_jacobian([[maybe_unused]] mrob::factor_id_t id = 0) const override {return J_;};
 
   protected:
     Mat31 z_point_x_, z_point_y_, Tx_;
@@ -84,4 +84,4 @@ class Factor1PosePoint2Point : public Factor
 
 
 
-#endif /* FACTORS_FACTOR1POSEPOINT2POINT_HPP_ */
+#endif /* FACTOR1POSEPOINT2POINT_HPP_ */
diff --git a/src/FGraph/mrob/factors/nodePlane4d.hpp b/src/PCRegistration/mrob/factors/nodePlane4d.hpp
similarity index 77%
rename from src/FGraph/mrob/factors/nodePlane4d.hpp
rename to src/PCRegistration/mrob/factors/nodePlane4d.hpp
index 78d52ef..9b1a4f4 100644
--- a/src/FGraph/mrob/factors/nodePlane4d.hpp
+++ b/src/PCRegistration/mrob/factors/nodePlane4d.hpp
@@ -50,13 +50,13 @@ class NodePlane4d : public Node
      * pi'= clamped(pi + dx)
      *
      */
-    virtual void update(const Eigen::Ref<const MatX1> &dx) override;
-    virtual void update_from_auxiliary(const Eigen::Ref<const MatX1> &dx) override;
-    virtual void set_state(const Eigen::Ref<const MatX> &x) override {state_ = x;};
-    virtual void set_auxiliary_state(const Eigen::Ref<const MatX> &x) override {auxiliaryState_ = x;};
-    virtual const Eigen::Ref<const MatX> get_state() const override {return state_;};
-    virtual const Eigen::Ref<const MatX> get_auxiliary_state() const override {return auxiliaryState_;};
-    virtual void print() const override;
+    void update(VectRefConst &dx) override;
+    void update_from_auxiliary(VectRefConst &dx) override;
+    void set_state(MatRefConst &x) override {state_ = x;};
+    void set_auxiliary_state(MatRefConst &x) override {auxiliaryState_ = x;};
+    MatRefConst get_state() const override {return state_;};
+    MatRefConst get_auxiliary_state() const override {return auxiliaryState_;};
+    void print() const override;
 
   protected:
     Mat41 state_;
diff --git a/src/PCRegistration/mrob/pc_registration.hpp b/src/PCRegistration/mrob/pc_registration.hpp
index 286be90..725fbbc 100644
--- a/src/PCRegistration/mrob/pc_registration.hpp
+++ b/src/PCRegistration/mrob/pc_registration.hpp
@@ -52,7 +52,7 @@ namespace PCRegistration{
  *
  *  Returns 0 if failed and 1 if a correct solution was found
  */
-int arun(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y, SE3 &T);
+int arun(MatRefConst X, MatRefConst Y, SE3 &T);
 
 
 
@@ -73,8 +73,8 @@ int arun(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y, SE3 &T)
  *
  * Returns the number of iterations until convergence
  */
-int gicp(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y,
-        const Eigen::Ref<const MatX> covX, const Eigen::Ref<const MatX> covY, SE3 &T, double tol = 1e-4);
+int gicp(MatRefConst X, MatRefConst Y,
+        MatRefConst covX, MatRefConst covY, SE3 &T, double tol = 1e-4);
 
 
 /**
@@ -92,8 +92,8 @@ int gicp(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y,
  *
  * Returns the number of iterations until convergence
  */
-int weighted_point(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y,
-        const Eigen::Ref<const MatX1> w, SE3 &T, double tol = 1e-4);
+int weighted_point(MatRefConst X, MatRefConst Y,
+                   VectRefConst w, SE3 &T, double tol = 1e-4);
 
 
 }}//namespace
diff --git a/src/PCRegistration/mrob/plane.hpp b/src/PCRegistration/mrob/plane.hpp
index e1a99f5..f2f8e17 100644
--- a/src/PCRegistration/mrob/plane.hpp
+++ b/src/PCRegistration/mrob/plane.hpp
@@ -145,11 +145,12 @@ class Plane{
     std::shared_ptr<const std::vector<SE3>> trajectory_;
 
     // gradient calculation Q
-    std::vector<Mat4> matrixS_, matrixQ_;
+    // This container is aligned since we use cpp17 (o.w. it would not be)
+    std::vector<Mat4, Eigen::aligned_allocator<Mat4> > matrixS_, matrixQ_;
     Mat4 accumulatedQ_;//Q matrix of accumulated values for the incremental update of the error.
 
     // Store last gradients calculated (for Hessian)
-    std::vector<Mat4> gradQ_, lieGenerativeMatrices_;//TODO move this to a common library utility, replicating this every time is terrible.
+    std::vector<Mat4, Eigen::aligned_allocator<Mat4> > gradQ_, lieGenerativeMatrices_;
 
   public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
diff --git a/src/PCRegistration/mrob/plane_registration.hpp b/src/PCRegistration/mrob/plane_registration.hpp
index 991b08f..6a2741c 100644
--- a/src/PCRegistration/mrob/plane_registration.hpp
+++ b/src/PCRegistration/mrob/plane_registration.hpp
@@ -40,6 +40,8 @@ namespace mrob{
 /**
  * class PlaneRegistration introduced a class for the alignment of
  * planes.
+ *
+ * TO BE DEPRECATED
  */
 class PlaneRegistration: public OptimizerDense{
 
@@ -110,9 +112,13 @@ class PlaneRegistration: public OptimizerDense{
     void reset_solution();
     double get_current_error() const;
     /**
-     * Get trajectory returns a SE3 transformations,
+     * Get pose from trajectory, a SE3 transformations,
      */
-    SE3 get_trajectory(uint_t time);
+    SE3 get_pose(uint_t time);
+    /**
+      * Get trajectory returns a SE3 transformations,
+      */
+    std::vector<SE3>& get_trajectory();
 
     SE3 get_last_pose() {return trajectory_->back();}
 
@@ -183,6 +189,7 @@ class PlaneRegistration: public OptimizerDense{
     uint_t time_{};
     std::unordered_map<uint_t, std::shared_ptr<Plane>> planes_;
     std::shared_ptr<std::vector<SE3>> trajectory_;
+    std::vector<SE3> temporary_trajectory_;
     SE3 bookept_trajectory_;//last pose is stored/bookept
     double tau_ {};//variable for weighting the number of poses in traj
     uint_t solveIters_ {};
diff --git a/src/PCRegistration/plane_registration.cpp b/src/PCRegistration/plane_registration.cpp
index 31093bf..83eff77 100644
--- a/src/PCRegistration/plane_registration.cpp
+++ b/src/PCRegistration/plane_registration.cpp
@@ -379,7 +379,7 @@ std::vector<Mat31> PlaneRegistration::get_point_cloud(uint_t time)
 	return aggregated_pc;
 }
 
-SE3 PlaneRegistration::get_trajectory(uint_t time)
+SE3 PlaneRegistration::get_pose(uint_t time)
 {
     assert(time < numberPoses_ && "CreatePoints::getPointCloud: temporal index larger than number of calculated poses\n");
     if (time < numberPoses_ )
@@ -387,6 +387,15 @@ SE3 PlaneRegistration::get_trajectory(uint_t time)
     return SE3();
 }
 
+std::vector<SE3>& PlaneRegistration::get_trajectory()
+{
+    // this implementation is not to break much the current class structure, but having in mind it should disapear
+    temporary_trajectory_.clear();
+    for (SE3 pose : *trajectory_)
+        temporary_trajectory_.push_back(pose);
+    return temporary_trajectory_;
+}
+
 void PlaneRegistration::print(bool plotPlanes) const
 {
     std::cout << "Printing plane registration data :"<< std::endl;
diff --git a/src/PCRegistration/weight_point.cpp b/src/PCRegistration/weight_point.cpp
index 074c418..e275600 100644
--- a/src/PCRegistration/weight_point.cpp
+++ b/src/PCRegistration/weight_point.cpp
@@ -30,8 +30,8 @@
 
 using namespace mrob;
 
-int PCRegistration::weighted_point(const Eigen::Ref<const MatX> X, const Eigen::Ref<const MatX> Y,
-           const Eigen::Ref<const MatX1> weight,  SE3 &T, double tol)
+int PCRegistration::weighted_point(MatRefConst X, MatRefConst Y,
+                                   VectRefConst weight,  SE3 &T, double tol)
 {
     assert(X.cols() == 3  && "PCRegistration::Gicp: Incorrect sizing, we expect Nx3");
     assert(X.rows() >= 3  && "PCRegistration::Gicp: Incorrect sizing, we expect at least 3 correspondences (not aligned)");
diff --git a/src/common/mrob/matrix_base.hpp b/src/common/mrob/matrix_base.hpp
index d78c3b8..045518a 100644
--- a/src/common/mrob/matrix_base.hpp
+++ b/src/common/mrob/matrix_base.hpp
@@ -13,7 +13,7 @@
  * limitations under the License.
  *
  *
- * time_profiling.jpp
+ * matrix_base.hpp
  *
  *  Created on: Aug 14, 2019
  *      Author: Gonzalo Ferrer
@@ -28,47 +28,47 @@
 #include <Eigen/Core>
 //#include <Eigen/LU> // for inverse and determinant
 #include <Eigen/Sparse>
+#include <Eigen/StdVector>
 
 #define EIGEN_DEFAULT_TO_ROW_MAJOR
 
 
-
 namespace mrob{
 
 // data types conventions
-typedef double matData_t;
-typedef unsigned int uint_t;
-typedef std::size_t factor_id_t;
+using matData_t = double;
+using uint_t = unsigned int;
+using factor_id_t = std::size_t;
 
 // Definition of squared matrices, by default column major
-typedef Eigen::Matrix<matData_t, 1,1, Eigen::RowMajor> Mat1;
-typedef Eigen::Matrix<matData_t, 2,2, Eigen::RowMajor> Mat2;
-typedef Eigen::Matrix<matData_t, 3,3, Eigen::RowMajor> Mat3;
-typedef Eigen::Matrix<matData_t, 4,4, Eigen::RowMajor> Mat4;
-typedef Eigen::Matrix<matData_t, 5,5, Eigen::RowMajor> Mat5;
-typedef Eigen::Matrix<matData_t, 6,6, Eigen::RowMajor> Mat6;
-typedef Eigen::Matrix<matData_t, Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor> MatX;
+using Mat1 = Eigen::Matrix<matData_t, 1,1, Eigen::RowMajor>;
+using Mat2 = Eigen::Matrix<matData_t, 2,2, Eigen::RowMajor>;
+using Mat3 = Eigen::Matrix<matData_t, 3,3, Eigen::RowMajor>;
+using Mat4 = Eigen::Matrix<matData_t, 4,4, Eigen::RowMajor>;
+using Mat5 = Eigen::Matrix<matData_t, 5,5, Eigen::RowMajor>;
+using Mat6 = Eigen::Matrix<matData_t, 6,6, Eigen::RowMajor>;
+using MatX = Eigen::Matrix<matData_t, Eigen::Dynamic,Eigen::Dynamic, Eigen::RowMajor>;
 
 //Sparse Matrices
-typedef Eigen::SparseMatrix<matData_t, Eigen::ColMajor> SMatCol;
-typedef Eigen::SparseMatrix<matData_t, Eigen::RowMajor> SMatRow;
-typedef Eigen::Triplet<matData_t> Triplet;
+using SMatCol = Eigen::SparseMatrix<matData_t, Eigen::ColMajor>;
+using SMatRow = Eigen::SparseMatrix<matData_t, Eigen::RowMajor>;
+using Triplet = Eigen::Triplet<matData_t>;
 
 // Definition of column matrices (vectors)
-typedef Eigen::Matrix<matData_t, 2,1> Mat21;
-typedef Eigen::Matrix<matData_t, 3,1> Mat31;
-typedef Eigen::Matrix<matData_t, 4,1> Mat41;
-typedef Eigen::Matrix<matData_t, 5,1> Mat51;
-typedef Eigen::Matrix<matData_t, 6,1> Mat61;
-typedef Eigen::Matrix<matData_t, Eigen::Dynamic,1> MatX1;
+using Mat21 = Eigen::Matrix<matData_t, 2,1>;
+using Mat31 = Eigen::Matrix<matData_t, 3,1>;
+using Mat41 = Eigen::Matrix<matData_t, 4,1>;
+using Mat51 = Eigen::Matrix<matData_t, 5,1>;
+using Mat61 = Eigen::Matrix<matData_t, 6,1>;
+using MatX1 = Eigen::Matrix<matData_t, Eigen::Dynamic,1>;
 
 // Definition of row matrices (vectors)
-typedef Eigen::Matrix<matData_t, 1,2> Mat12;
-typedef Eigen::Matrix<matData_t, 1,3> Mat13;
-typedef Eigen::Matrix<matData_t, 1,4> Mat14;
-typedef Eigen::Matrix<matData_t, 1,5> Mat15;
-typedef Eigen::Matrix<matData_t, 1,6> Mat16;
-typedef Eigen::Matrix<matData_t, 1,Eigen::Dynamic> Mat1X;
+using Mat12 = Eigen::Matrix<matData_t, 1,2>;
+using Mat13 = Eigen::Matrix<matData_t, 1,3>;
+using Mat14 = Eigen::Matrix<matData_t, 1,4>;
+using Mat15 = Eigen::Matrix<matData_t, 1,5>;
+using Mat16 = Eigen::Matrix<matData_t, 1,6>;
+using Mat1X = Eigen::Matrix<matData_t, 1,Eigen::Dynamic>;
 
 
 // Definition of templated-based fixed matrices using c'11 aliases
@@ -77,8 +77,12 @@ using Vect = Eigen::Matrix<matData_t, D,1, Eigen::RowMajor>;
 template<int Rw,int Col>
 using Mat = Eigen::Matrix<matData_t, Rw, Col, Eigen::RowMajor>;
 
-
+// Definition of Ref Eigen
+//we might need this for aligment requirements: Eigen::Ref<const MatX1, Eigen::AlignmentType::Aligned16>
+using VectRefConst = const Eigen::Ref<const MatX1>;
+using MatRefConst = const Eigen::Ref<const MatX>;
 
 }//end of namespace
 
+
 #endif /* MATRIX_BASE_HPP_ */
diff --git a/src/common/mrob/time_profiling.hpp b/src/common/mrob/time_profiling.hpp
index 2994b83..947b773 100644
--- a/src/common/mrob/time_profiling.hpp
+++ b/src/common/mrob/time_profiling.hpp
@@ -28,7 +28,7 @@
 
 
 namespace mrob {
-typedef std::chrono::microseconds Ttim;
+using Ttim = std::chrono::microseconds;
 
 
 /**
diff --git a/src/common/time_profiling.cpp b/src/common/time_profiling.cpp
index ce510c5..82c4098 100644
--- a/src/common/time_profiling.cpp
+++ b/src/common/time_profiling.cpp
@@ -61,7 +61,7 @@ void TimeProfiling::print()
 
     std::cout << "\nTime profile for " << sum/1e3 << " [ms]: ";
     for (auto &&t : time_profiles_)
-        std::cout << t.first << " = " << t.second/sum *100 << "%, ";
+        std::cout << t.first << " = " << t.second/sum *100 << "%,\n";
     std::cout << "\n";
 }
 
diff --git a/src/geometry/CMakeLists.txt b/src/geometry/CMakeLists.txt
index 1309250..dfa3ac1 100644
--- a/src/geometry/CMakeLists.txt
+++ b/src/geometry/CMakeLists.txt
@@ -10,8 +10,8 @@ SET(sources
 
 # extra header files
 SET(headers
-	mrob/SO3.hpp
-	mrob/SE3.hpp
+    mrob/SO3.hpp
+    mrob/SE3.hpp
     mrob/SE3cov.hpp
 )
 
diff --git a/src/geometry/SE3.cpp b/src/geometry/SE3.cpp
index 59fa776..ff10e11 100644
--- a/src/geometry/SE3.cpp
+++ b/src/geometry/SE3.cpp
@@ -43,13 +43,13 @@ SE3::SE3(const Mat61 &xi) : T_(Mat4::Identity())
 SE3::SE3(const SE3 &T): T_(T.T())
 {
 }
-SE3::SE3(const SO3 &R, const Mat31 t)
+SE3::SE3(const SO3 &R, const Mat31 &t)
 {
     T_  << R.R(), t,
            0,0,0,1;
 }
 
-SE3::SE3(const Mat3 &R, const Mat31 t)
+SE3::SE3(const Mat3 &R, const Mat31 &t)
 {
     T_  << R, t,
            0,0,0,1;
@@ -287,7 +287,7 @@ void SE3::regenerate()
     this->exp(xi_hat);
 }
 
-bool mrob::isSE3(Mat4 T)
+bool mrob::isSE3(const Mat4 &T)
 {
     if (!isSO3(T.topLeftCorner<3,3>()) )
         return false;
@@ -297,3 +297,49 @@ bool mrob::isSE3(Mat4 T)
         return false;
     return true;
 }
+
+// Here we deine a global variable inside the file of this class, to be copied
+const std::vector<Mat4> LieGenerative{
+
+};
+
+
+// DEPRECATED?
+Mat4 mrob::SE3GenerativeMatrix(uint_t coordinate)
+{
+    Mat4 G = Mat4::Zero();
+    switch(coordinate)
+    {
+    case 0: //theta1
+        G(1,2) = -1.0;
+        G(2,1) = 1.0;
+        break;
+    case 1: // theta 2
+        G(0,2) = 1.0;
+        G(2,0) = -1.0;
+        break;
+    case 2: // theta 3
+        G(0,1) = -1.0;
+        G(1,0) = 1.0;
+        break;
+    case 3: // rho 1
+        G(0,3) = 1.0;
+        break;
+    case 4: // rho 2
+        G(1,3) = 1.0;
+        break;
+    case 5: // rho 3
+        G(2,3) = 1.0;
+        break;
+    }
+    return G;
+}
+
+
+
+
+
+
+
+
+
diff --git a/src/geometry/SO3.cpp b/src/geometry/SO3.cpp
index 8f39bdd..2aae343 100644
--- a/src/geometry/SO3.cpp
+++ b/src/geometry/SO3.cpp
@@ -277,7 +277,7 @@ void SO3::print_lie(void) const
     std::cout << w << std::endl;
 }
 
-bool mrob::isSO3(Mat3 R)
+bool mrob::isSO3(const Mat3 &R)
 {
     matData_t det = R.determinant();
     if (det  < 0)
diff --git a/src/geometry/mrob/SE3.hpp b/src/geometry/mrob/SE3.hpp
index 38dc28c..7f533f4 100644
--- a/src/geometry/mrob/SE3.hpp
+++ b/src/geometry/mrob/SE3.hpp
@@ -66,11 +66,11 @@ class SE3
     /**
      * Constructor, requires the Rotation in SO3 and translation
      */
-    SE3(const SO3 &R, const Mat31 t);
+    SE3(const SO3 &R, const Mat31 &t);
     /**
      * Constructor, requires the Rotation as a 3x3 matrix and translation
      */
-    SE3(const Mat3 &R, const Mat31 t);
+    SE3(const Mat3 &R, const Mat31 &t);
     /**
      * This constructor allows to construct from Eigen expressions
      * Eigen suggestion: TopicCustomizingEigen.html
@@ -207,7 +207,40 @@ Mat4 hat6(const Mat61 &xi);
  */
 Mat61 vee6(const Mat4 &xi_hat);
 
-bool isSE3(Mat4 T);
+bool isSE3(const Mat4 &T);
+
+/**
+ * Returns the generative matrix given the coordinate,
+ * considering xi(0..5) = [theta(0..2), rho(3..5)]
+ */
+Mat4 SE3GenerativeMatrix(uint_t coordinate);
+/**
+ * Returns the double generative matrix given the coordinates (i,j),
+ * considering xi(0..5) = [theta(0..2), rho(3..5)]
+ * This corresponds to the differential form:
+ *
+ * d Exp / d xi = 0.5 (Gi *Gj + Gj*Gi)
+ */
+//Mat4 SE3DoubleGenerativeMatrix(uint_t i, uint_t j);
+
+/**
+ * Global variables declared here, defined once in the cpp.
+ *
+ * These are common data to access, so we make it accessible the fastest way.
+ *  generative matrix given the coordinate,
+ *  considering xi(0..5) = [theta(0..2), rho(3..5)]
+ */
+extern const std::vector<Mat4> LieGenerative;
+/**
+ * Global variables declared here, defined once in the cpp.
+ * These are common data to access, so we make it accessible the fastest way.
+ * Returns the double generative matrix given the coordinates (i,j),
+ * considering xi(0..5) = [theta(0..2), rho(3..5)]
+ * This corresponds to the differential form:
+ *
+ * d Exp / d xi = 0.5 (Gi *Gj + Gj*Gi)
+ */
+extern const std::vector< Mat4> LieDoubleGenerative;
 
 }// end namespace
 #endif /* SE3_HPP_ */
diff --git a/src/geometry/mrob/SO3.hpp b/src/geometry/mrob/SO3.hpp
index a1d9141..68092c2 100644
--- a/src/geometry/mrob/SO3.hpp
+++ b/src/geometry/mrob/SO3.hpp
@@ -161,7 +161,7 @@ Mat3 hat3(const Mat31 &w);
  */
 Mat31 vee3(const Mat3 &w_hat);
 
-bool isSO3(Mat3 R);
+bool isSO3(const Mat3 &R);
 
 
 /**


Test patch:
diff --git a/src/EigenFactors/examples/test_EF_Hessian.cpp b/src/PCRegistration/examples/test_EF_Hessian.cpp
similarity index 100%
rename from src/EigenFactors/examples/test_EF_Hessian.cpp
rename to src/PCRegistration/examples/test_EF_Hessian.cpp