adding some functionalities, results not totally working, needs review. WIP

Author: g-ferrer

src/geometry/SE3tc.cpp

@@ -164,6 +164,28 @@ Mat61 SE3tc::Ln_position() const
     return result;
 }
 
+Mat6 SE3tc::adj() const
+{
+    Mat6 res(Mat6::Zero());
+    Mat3 tx = hat3( this->p() );
+    res.topLeftCorner<3,3>() << R();
+    res.bottomRightCorner<3,3>() << R();
+    res.bottomLeftCorner<3,3>() << tx*R();// what is this? TODO
+    return res;
+}
+
+SE3tc SE3tc::inv(void) const
+{
+    Mat5 inv;
+    Mat3 R = this->R();
+    R.transposeInPlace();
+    inv << R, -R * (this->p() - this->t()*this->v()), -R*this->v(),
+           0,0,0,1,0, 
+           0,0,0,-this->t(),0;
+    return SE3tc(inv);
+
+}
+
 void SE3tc::regenerate()
 {
     Mat61 xi = this->Ln_position();

src/geometry/mrob/SE3tc.hpp

@@ -69,7 +69,9 @@ class SE3tc{
         Mat<3,5> T_compact() const;
 
 
-        //Mat6 adj() const;//TODO necesary here?
+        Mat6 adj() const;
+
+        SE3tc inv(void) const;
 
         void Exp(const Mat61& xi, const matData_t &t);
         Mat61 Ln_position(void) const;

src/pybind/FGraphPy.cpp

@@ -477,7 +477,11 @@ void init_FGraph(py::module &m)
                     "Input are poses in 3D, as Lie Algebra of RBT around the Identity",
                     py::arg("x"),
                     py::arg("mode") = Node::nodeMode::STANDARD)
-            .def("add_factor_1pose_3d", &FGraphPy::add_factor_1pose_3d)
+            .def("add_factor_1pose_3d", &FGraphPy::add_factor_1pose_3d,
+                    "Adds a factor observing one pose, a GPS-like factor",
+                    py::arg("obs"),
+                    py::arg("nodePoseId"),
+                    py::arg("obsInvCov"))
             .def("add_factor_2poses_3d", &FGraphPy::add_factor_2poses_3d,
                             "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"),

src/pybind/FGraphTimePy.cpp

@@ -167,7 +167,12 @@ void init_FGraphTime(py::module &m)
                     "Ladmarks are 3D points, in [x,y,z]",
                     py::arg("x"),
                     py::arg("mode") = Node::nodeMode::STANDARD)
-            .def("add_factor_1pose_3d", &FGraphTimePy::add_factor_1pose_3d)
+            .def("add_factor_1pose_3d", &FGraphTimePy::add_factor_1pose_3d,
+                    "Adds a factor observing one pose, a GPS-like factor",
+                    py::arg("obs"),
+                    py::arg("nodeId"),
+                    py::arg("time_stamp"),
+                    py::arg("obsInvCov"))
             .def("add_factor_camera_proj_3d_point", &FGraphTimePy::add_factor_camera_proj_3d_point,
                     "\n Factor for the reprojection error from a point in the image plane",
                     py::arg("obs"),