Fix/2d factor landmark and odometry (#66)

* Minor fix on anchor 2D factor

* update on landmarks 2d, now working and example updated

* working landmark 2d.

mrobpy/FGraphPy.cpp

@@ -230,11 +230,18 @@ void init_FGraph(py::module &m)
             .def("add_factor_2poses_2d", &FGraphPy::add_factor_2poses_2d,
                     "Factors connecting 2 poses. If last input set to true (by default false), also updates the value of the target Node according to the new obs + origin node",
                     py::arg("obs"),
-                    py::arg("nodeOridingId"),
+                    py::arg("nodeOriginId"),
                     py::arg("nodeTargetId"),
                     py::arg("obsInvCov"),
                     py::arg("updateNodeTarget") = false)
-            .def("add_factor_2poses_2d_odom", &FGraphPy::add_factor_2poses_2d_odom)
+            .def("add_factor_2poses_2d_odom", &FGraphPy::add_factor_2poses_2d_odom,
+                    "add_factor_2poses_2d_odom(obs, nodeOriginId, nodeTargetId, W)"
+                    "\nFactor connecting 2 poses, following an odometry model."
+                    "\nArguments are obs, nodeOriginId, nodeTargetId and obsInvCov",
+                    py::arg("obs"),
+                    py::arg("nodeOriginId"),
+                    py::arg("nodeTargetId"),
+                    py::arg("obsInvCov"))
             // 2d Landmkarks
             .def("add_node_landmark_2d", &FGraphPy::add_node_landmark_2d,
                     "Ladmarks are 2D points, in [x,y]")

python_examples/FGraph_2d.py

@@ -16,10 +16,10 @@
 
 
 invCov = np.identity(3)
-graph.add_factor_1pose_2d(np.zeros(3),n1,1e6*invCov)
+graph.add_factor_1pose_2d(np.array([0,0,np.pi/4]),n1,1e6*invCov)
 graph.add_factor_2poses_2d(np.ones(3),n1,n2,invCov)
 
-graph.solve(mrob.GN)
+graph.solve(mrob.LM)
 graph.print(True)
 
 

python_examples/FGraph_landmark_2d_example.py

@@ -7,10 +7,11 @@
 graph = mrob.FGraph()
 
 #initial point at [0,0,0] with some noise
-n1 = graph.add_node_pose_2d(np.random.randn(3)*0.05)
-# non-initialized landmarks, but they could be initialiazed
+n1 = graph.add_node_pose_2d(np.random.randn(3)*0.0005)
+# non-initialized landmarks, but they could be initialiazed when adding factors
+# In case of 2d landmarks, they might be sensitive to IC, around 0 of initiakl pose and landmark On those cases might (rearely) GN fail to converge.
 l1 = graph.add_node_landmark_2d(np.array([0,0]))
-l2 = graph.add_node_landmark_2d(np.zeros(2))
+l2 = graph.add_node_landmark_2d(np.array([0,0]))
 print('node pose id = ', n1, ' , node landmark 1 id = ', l1 , ' , node landmark 2 id = ', l2)
 
 # anchor factor
@@ -23,14 +24,17 @@
 graph.add_factor_1pose_1landmark_2d(obs,n1,l1,W)
 obs = np.array([1,np.pi/2])
 graph.add_factor_1pose_1landmark_2d(obs,n1,l2,W)
-graph.print(True)
 
 
 
 print('\n\n\n Solving Fgraph:\n')
-#graph.solve(mrob.fgraph.GN) #1 iteration of Gauss-Newton
-graph.solve(mrob.LM) #as many iterations until convergence for Levenberg Marquardt
-graph.print(True)
+graph.solve(mrob.GN) #1 iteration of Gauss-Newton
+graph.solve(mrob.GN)
+graph.solve(mrob.GN)
+graph.solve(mrob.GN)
+#graph.solve(mrob.LM)
+#graph.print(True)
+print(graph.get_estimated_state())
 print('Current chi2 = ', graph.chi2() ) # re-evaluates the error, on print it is only the error on evalation before update
 
 

python_examples/FGraph_point_plane_align.py

@@ -85,3 +85,11 @@ def vis_arr(X):
 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)
+
+

src/FGraph/factors/factor1Pose1Landmark2d.cpp

@@ -52,7 +52,11 @@ Factor1Pose1Landmark2d::Factor1Pose1Landmark2d(const Mat21 &observation, std::sh
 
     if (initializeLandmark)
     {
-        // TODO Initialize landmark value to whatever observation we see from current pose
+        Mat31 x = nodePose->get_state();
+        Mat21 dl;
+        dl << std::cos(obs_(1)+x(2)), std::sin(obs_(1)+x(2));
+        Mat21 land = x.head(2) + obs_(0)*dl;
+        nodeLandmark->set_state(land);
     }
 }
 
@@ -71,17 +75,16 @@ void Factor1Pose1Landmark2d::evaluate_residuals()
         poseIndex = 1;
     }
     // From the local frame we observe the landmark
-    // TODO LM needs too many iterations, check once again gradients
     state_ = get_neighbour_nodes()->at(poseIndex)->get_state();
     landmark_ = get_neighbour_nodes()->at(landmarkIndex)->get_state();
     dx_ = landmark_(0) - state_(0);
     dy_ = landmark_(1) - state_(1);
     q_ = dx_*dx_ + dy_*dy_;
+    // r = [range, bearing]
     r_ << std::sqrt(q_),
          std::atan2(dy_,dx_) - state_(2);
-    r_ -= obs_;
-    r_(2) = wrap_angle(r_(2));
-
+    r_ = r_-obs_;
+    r_(1) = wrap_angle(r_(1));
 }
 void Factor1Pose1Landmark2d::evaluate_jacobians()
 {

src/FGraph/factors/factor1Pose2d.cpp

@@ -43,6 +43,7 @@ void Factor1Pose2d::evaluate_jacobians()
 void Factor1Pose2d::evaluate_residuals()
 {
     r_ = get_neighbour_nodes()->at(0).get()->get_state() - obs_;
+    r_(2) = wrap_angle(r_(2));
 }
 
 void Factor1Pose2d::evaluate_chi2()

src/FGraph/factors/factor2Poses2d.cpp

@@ -62,8 +62,8 @@ void Factor2Poses2d::evaluate_residuals() {
     // r = h(i,j) - obs = Ri^T * (xj- xi) - obs .  From "i", i.e, at its reference frame, we observe xj
     Mat31 h = nodeTarget - nodeOrigin;
     Mat2 RiT;
-    double c1 = cos(nodeOrigin(2)),
-           s1 = sin(nodeOrigin(2));
+    double c1 = std::cos(nodeOrigin(2)),
+           s1 = std::sin(nodeOrigin(2));
     RiT <<  c1, s1,
            -s1, c1;
     h.head(2) = RiT * h.head(2);
@@ -79,8 +79,8 @@ void Factor2Poses2d::evaluate_jacobians()
     // r =  Ri^T * (xj- xi) - obs
     // J1 = [-R1^T,    J2 = [R1^T 0]
     //      [  ]           [0    1]
-    double c1 = cos(nodeOrigin(2)),
-           s1 = sin(nodeOrigin(2)),
+    double c1 = std::cos(nodeOrigin(2)),
+           s1 = std::sin(nodeOrigin(2)),
            dx = nodeTarget(0) - nodeOrigin(0),
            dy = nodeTarget(1) - nodeOrigin(1);
     J_ <<   -c1, -s1, -s1*dx + c1*dy,     c1, s1, 0,
@@ -125,15 +125,15 @@ void Factor2Poses2dOdom::evaluate_residuals()
     auto prediction = get_odometry_prediction(stateOrigin, obs_);
 
     r_ =  prediction - stateTarget;
-    r_[2] = wrap_angle(r_[2]);
+    r_(2) = wrap_angle(r_(2));
 
 }
 void Factor2Poses2dOdom::evaluate_jacobians()
 {
     // Get the position of node we are traversing from
     Mat31 node1 = get_neighbour_nodes()->at(0).get()->get_state();
 
-    auto s = -obs_[1] * sin(node1[2]), c = obs_[1] * sin(node1[2]);
+    auto s = -obs_(1) * std::sin(node1(2)), c = obs_(1) * std::cos(node1(2));
 
     // Jacobians for odometry model which are: G and -I
     J_ <<   1, 0, s,    -1, 0, 0,
@@ -143,10 +143,10 @@ void Factor2Poses2dOdom::evaluate_jacobians()
 
 
 Mat31 Factor2Poses2dOdom::get_odometry_prediction(Mat31 state, Mat31 motion) {
-    state[2] += motion[0];
-    state[0] += motion[1] * cos(state[2]);
-    state[1] += motion[1] * sin(state[2]);
-    state[2] += motion[2];
+    state(2) += motion(0);
+    state(0) += motion(1) * std::cos(state(2));
+    state(1) += motion(1) * std::sin(state(2));
+    state(2) += motion(2);
 
     return state;
 }

src/FGraph/factors/nodeLandmark2d.cpp

@@ -43,21 +43,16 @@ NodeLandmark2d::~NodeLandmark2d()
 
 void NodeLandmark2d::update(const Eigen::Ref<const MatX1> &dx)
 {
-    //TODO test cast
-    Mat21 dxf = dx;
-    state_ += dxf;
+    state_ += dx;
 }
 
 void NodeLandmark2d::update_from_auxiliary(const Eigen::Ref<const MatX1> &dx)
 {
-    //TODO test cast
-    Mat21 dxf = dx;
-    state_ = auxiliaryState_ + dxf;
+    state_ = auxiliaryState_ + dx;
 }
 
 void NodeLandmark2d::set_state(const Eigen::Ref<const MatX> &x)
 {
-    // cast is done by Eigen
     state_ = x;
 }
 

src/FGraph/mrob/factor.hpp

@@ -101,7 +101,7 @@ class Factor{
      * Return a Ref to a dynamic matrix, while the child matrix should declare
      * all these variables as fixed size matrices, and ref takes care of
      * doing the conversion with minimal temporary artifacts
-     * Observation can be a 3d point, a 3d pose (transfromation 4x4), etc.
+     * Observation can be a 3d point, a 3d pose (transformation 4x4), etc.
      */
     virtual const Eigen::Ref<const MatX> get_obs() const = 0;
     /**

src/FGraph/mrob/factors/factor1Pose1Landmark2d.hpp

@@ -49,9 +49,6 @@ namespace mrob{
  * z = [range,bearing] is a 2d vector, composed of a range and bearing in the local frame.
  * l is a 2d point encoding the landmark position l = [x,y]
  *
- * and the residual is thus:
- *   r = T-1*l - z
- *
  *
  * Constructor functions will be overloaded to include the pointers of the nodes,
  * The convention is 2d pose, we observe node destination,