Written code for 3D lines vertex representation. Testing phase.

g2o/examples/CMakeLists.txt

@@ -17,6 +17,7 @@ IF(CSPARSE_FOUND)
   ADD_SUBDIRECTORY(calibration_odom_laser)
   ADD_SUBDIRECTORY(simple_optimize)
   ADD_SUBDIRECTORY(plane_slam)
+  ADD_SUBDIRECTORY(line_slam)
 ENDIF()
 
 IF(CSPARSE_FOUND OR CHOLMOD_FOUND)

g2o/examples/line_slam/CMakeLists.txt

@@ -0,0 +1,11 @@
+INCLUDE_DIRECTORIES(${CSPARSE_INCLUDE_DIR})
+
+ADD_EXECUTABLE(line_test line_test.cpp)
+TARGET_LINK_LIBRARIES(line_test types_slam3d_addons types_slam3d)
+SET_TARGET_PROPERTIES(line_test PROPERTIES OUTPUT_NAME line_test${EXE_POSTFIX})
+
+# ADD_EXECUTABLE(simulator_3d_plane
+#   simulator_3d_plane.cpp
+# )
+# TARGET_LINK_LIBRARIES(simulator_3d_plane solver_csparse types_slam3d_addons)
+# SET_TARGET_PROPERTIES(simulator_3d_plane PROPERTIES OUTPUT_NAME simulator_3d_plane${EXE_POSTFIX})

g2o/examples/line_slam/line_test.cpp

@@ -0,0 +1,58 @@
+#include <iostream>
+
+#include "g2o/types/slam3d/isometry3d_mappings.h"
+#include "g2o/types/slam3d/isometry3d_gradients.h"
+#include "g2o/types/slam3d_addons/types_slam3d_addons.h"
+
+using namespace std;
+using namespace Eigen;
+using namespace g2o;
+using namespace g2o::internal;
+
+template <typename T>
+ostream& printVector(ostream& os, const T& t) {
+  for(int i = 0; i < t.rows(); ++i) {
+    os << t(i) << " ";
+  }
+  return os;
+}
+
+// TODO Jacopo
+
+int main(int /*argc*/, char** /*argv*/) {
+  // Vector6d t;
+  // t <<  -3.0, -2.0, -4.0, 0.2, 0.1, 0.3;
+  // Isometry3D T = fromVectorMQT(t);
+  // std::cout << "Transform" << std::endl;
+  // std::cout << T.matrix() << std::endl;
+
+  // Vector6d cl1;
+  // cl1 << 20.0, 50.0, -70.0, 0.1, 0.2, 0.3;
+  // cl1 = normalizeCartesianLine(cl1);
+  // std::cout << "Cartesian line L1: ";
+  // printVector(std::cout, cl1);
+  // std::cout << endl;
+
+  // Line3D pl1 = Line3D::fromCartesian(cl1);
+  // std::cout << "Pluecker line L1: ";
+  // printVector(std::cout, pl1);
+  // std::cout << std::endl;
+  // std::cout << "Cartesian line L1, reconstructed from pluecker: ";
+  // printVector(std::cout, pl1.toCartesian());
+  // std::cout << std::endl;
+
+  // Vector6d cl2 = transformCartesianLine(T, cl1);
+  // Line3D pl2 = T*pl1;
+
+  // std::cout << "Transformed line L2: ";
+  // printVector(std::cout, cl2);
+  // std::cout << std::endl;
+  // std::cout << "Transformed pline L2: ";
+  // printVector(std::cout, pl2);
+  // std::cout << std::endl;
+  // std::cout << "Error of cartesian line L2, reconstructed from puecker: ";
+  // printVector(std::cout, cl2 - pl2.toCartesian());
+  // std::cout << endl;
+
+  return 0;
+}

g2o/types/slam3d_addons/CMakeLists.txt

@@ -25,9 +25,6 @@ IF(G2O_HAVE_OPENGL)
   TARGET_LINK_LIBRARIES(types_slam3d_addons ${OPENGL_gl_LIBRARY} )
 ENDIF()
 
-ADD_EXECUTABLE(test_line3d line3d_test.cpp)
-TARGET_LINK_LIBRARIES(test_line3d types_slam3d_addons types_slam3d)
-
 INSTALL(TARGETS types_slam3d_addons
   RUNTIME DESTINATION ${CMAKE_INSTALL_PREFIX}/bin
   LIBRARY DESTINATION ${CMAKE_INSTALL_PREFIX}/lib

g2o/types/slam3d_addons/edge_line3d.cpp

@@ -65,8 +65,8 @@ namespace g2o
 #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES
   void EdgeLine3D::linearizeOplus()
   {
-    _jacobianOplusXi=-Matrix6d::Identity();
-    _jacobianOplusXj= Matrix6d::Identity();
+    _jacobianOplusXi=-Matrix6x4d::Identity();
+    _jacobianOplusXj= Matrix6x4d::Identity();
   }
 #endif
 

g2o/types/slam3d_addons/edge_line3d.h

@@ -32,6 +32,8 @@
 #include "g2o/config.h"
 #include "g2o/core/base_binary_edge.h"
 
+// TODO Jacopo
+
 namespace g2o
 {
 

g2o/types/slam3d_addons/edge_se3_line.h

@@ -35,13 +35,12 @@
 #include "g2o/types/slam3d/vertex_se3.h"
 #include "g2o/types/slam3d/parameter_se3_offset.h"
 
+// TODO Jacopo
+
 namespace g2o {
 
   typedef Eigen::Matrix<double, 7, 1, Eigen::ColMajor> Vector7d;
 
-  /**
-   * TODO
-   */
   class G2O_TYPES_SLAM3D_ADDONS_API EdgeSE3Line3D : public BaseBinaryEdge<7, Vector7d, VertexSE3, VertexLine3D> {
     public:
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

g2o/types/slam3d_addons/line3d.cpp

@@ -24,104 +24,70 @@
 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
-#include "line3d.h"
 #include <iostream>
 
+#include "line3d.h"
+
 namespace g2o {
+
   using namespace std;
   using namespace Eigen;
 
-  static inline void _skew(Matrix3D& S, const Vector3D& t){
+  static inline void _skew(Matrix3d& S, const Vector3d& t) {
     S <<
-      0,  -t.z(),   t.y(),
-      t.z(),     0,     -t.x(),
-      -t.y()     ,t.x(),   0;
+           0, -t.z(),  t.y(),
+       t.z(),      0, -t.x(),
+      -t.y(),  t.x(),      0;
   }
 
-  static inline Matrix3D _skew(const Vector3D& t){
-    Matrix3D S;
+  static inline Matrix3d _skew(const Vector3d& t) {
+    Matrix3d S;
     S <<
-      0,  -t.z(),   t.y(),
-      t.z(),     0,     -t.x(),
-      -t.y(),     t.x(),   0;
+           0, -t.z(),  t.y(),
+       t.z(),      0, -t.x(),
+      -t.y(),  t.x(),      0;
     return S;
   }
 
 
-  Vector6d Line3D::toCartesian() const{
+  Vector6d Line3D::toCartesian() const {
     Vector6d cartesian;
     cartesian.tail<3>() = d()/d().norm();
-    Matrix3D W=-_skew(d());
+    Eigen::Matrix3d W = -_skew(d());
     double damping = 1e-9;
-    Matrix3D A = W.transpose()*W+(Matrix3D::Identity()*damping);
+    Eigen::Matrix3d A = W.transpose()*W + (Eigen::Matrix3d::Identity()*damping);
     cartesian.head<3>() = A.ldlt().solve(W.transpose()*w());
     return cartesian;
   }
 
-  void Line3D::jacobian(Matrix7x6d& Jp, Matrix7x6d& Jl, const Isometry3D& x, const Line3D& l){
-    Jp.setZero();
-    Jl.setZero();
-    Matrix6d A=Matrix6d::Zero();
-    A.block<3,3>(0,0)=x.linear();
-    A.block<3,3>(0,3)= _skew(x.translation())*x.linear();
-    A.block<3,3>(3,3)=x.linear();
-    Vector6d v=(Vector6d)l;
-    Line3D lRemapped(v);
-
-    Matrix6d D = Matrix6d::Zero();
-    D.block<3,3>(0,0) = -_skew(l.d());
-    D.block<3,3>(0,3) = -2*_skew(l.w());
-    D.block<3,3>(3,3) = -2*_skew(l.d());
-    Jp.block<6,6>(0,0) = A*D;
-
-    Vector3D d = l.d();
-    Vector3D w = l.w();
-    double ln = d.norm();
-    double iln = 1./ln;
-    double iln3 = std::pow(iln,3);
-    Matrix6d Jll;
-    Jll <<
-      iln,0,0,-(w.x()*d.x())*iln3,-(w.x()*d.y())*iln3,-(w.x()*d.z())*iln3,
-      0,iln,0,-(w.y()*d.x())*iln3,-(w.y()*d.y())*iln3,-(w.y()*d.z())*iln3,
-      0,0,iln,-(w.z()*d.x())*iln3,-(w.z()*d.y())*iln3,-(w.z()*d.z())*iln3,
-      0,0,0,-(d.x()*d.x()-ln*ln)*iln3,-(d.x()*d.y())*iln3,-(d.x()*d.z())*iln3,
-      0,0,0,-(d.x()*d.y())*iln3,-(d.y()*d.y()-ln*ln)*iln3,-(d.y()*d.z())*iln3,
-      0,0,0,-(d.x()*d.z())*iln3,-(d.y()*d.z())*iln3,-(d.z()*d.z()-ln*ln)*iln3;
-    Jl.block<6,6>(0,0) = A*Jll;
-    Jl.block<1,6>(6,0) << 2*d.x(),2*d.y(),2*d.z();
-  }
-
-  Line3D operator*(const Isometry3D& t, const Line3D& line){
-    Matrix6d A=Matrix6d::Zero();
-    A.block<3,3>(0,0)=t.linear();
-    A.block<3,3>(0,3)= _skew(t.translation())*t.linear();
-    A.block<3,3>(3,3)=t.linear();
+  Line3D operator*(const Eigen::Isometry3d& t, const Line3D& line){
+    Matrix6d A = Matrix6d::Zero();
+    A.block<3, 3>(0, 0) = t.linear();
+    A.block<3, 3>(0, 3) = _skew(t.translation())*t.linear();
+    A.block<3, 3>(3, 3) = t.linear();
     Vector6d v = (Vector6d)line;
-    // cout << "v" << endl << v << endl;
-    // cout << "A" << endl << A << endl;
-    // cout << "Av" << endl << A*v << endl;
     return Line3D(A*v);
   }
-
-  namespace internal
-  {
-    Vector6d transformCartesianLine(const Isometry3D& t, const Vector6d& line){
+
+  namespace internal {
+    Vector6d transformCartesianLine(const Eigen::Isometry3d& t, const Vector6d& line) {
       Vector6d l;
       l.head<3>() = t*line.head<3>();
       l.tail<3>() = t.linear()*line.tail<3>();
       return normalizeCartesianLine(l);
     }
 
     Vector6d normalizeCartesianLine(const Vector6d& line) {
-      Vector3D p0 = line.head<3>();
-      Vector3D d0 = line.tail<3>();
+      Vector3d p0 = line.head<3>();
+      Vector3d d0 = line.tail<3>();
       d0.normalize();
-      p0-=d0*(d0.dot(p0));
+      p0 -= d0*(d0.dot(p0));
       Vector6d nl;
-      nl.head<3>()=p0;
-      nl.tail<3>()=d0;
+      nl.head<3>() = p0;
+      nl.tail<3>() = d0;
       return nl;
     }
-  } // end namespace internal
+
+  }
 
-} // end namespace g2o
+}