上传新文件

include/types_six_dof_expmap_ours.h

+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+//   notice, this list of conditions and the following disclaimer in the
+//   documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Modified by Raúl Mur Artal (2014)
+// Added EdgeSE3ProjectXYZ (project using focal_length in x,y directions)
+// Modified by Raúl Mur Artal (2016)
+// Added EdgeStereoSE3ProjectXYZ (project using focal_length in x,y directions)
+// Added EdgeSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)
+// Added EdgeStereoSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)
+
+#ifndef G2O_SIX_DOF_TYPES_EXPMAP_our
+#define G2O_SIX_DOF_TYPES_EXPMAP_our
+
+#include "../Thirdparty/g2o/g2o/core/base_vertex.h"
+#include "../Thirdparty/g2o/g2o/core/base_binary_edge.h"
+#include "../Thirdparty/g2o/g2o/core/base_unary_edge.h"
+//#include "se3_ops.h"
+//#include "se3quat.h"
+#include "../Thirdparty/g2o/g2o/types/types_sba.h"
+#include "so3.h"
+#include <Eigen/Geometry>
+#include <Eigen/StdVector>
+
+#include "Thirdparty/g2o/g2o/core/factory.h"
+#include "Thirdparty/g2o/g2o/stuff/macros.h"
+
+
+#include "Thirdparty/g2o/g2o/types/types_sba.h"
+#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
+#include "Thirdparty/g2o/g2o/core/base_edge.h"
+
+#include "Thirdparty/g2o/g2o/core/block_solver.h"
+
+namespace g2o {
+namespace types_six_dof_expmap {
+void init();
+}
+
+using namespace Eigen;
+
+typedef Matrix<double, 6, 6> Matrix6d;
+
+
+/**
+ * \brief SE3 Vertex parameterized internally with a transformation matrix
+ and externally with its exponential map
+ */
+
+    class  VertexMyExpmap : public BaseVertex<6, SE3Quat>{
+    public:
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+        VertexMyExpmap();
+
+        bool read(std::istream& is);
+
+        bool write(std::ostream& os) const;
+
+        virtual void setToOriginImpl() {
+            _estimate = SE3Quat();
+        }
+
+        virtual void oplusImpl(const double* update_)  {
+            Eigen::Map<const Vector6d> update(update_);
+            std::cout<<"VertexSE3Expmap"<<update.transpose()<<std::endl;
+            setEstimate(SE3Quat::exp(update)*estimate());
+        }
+    };
+
+
+/**
+ * \brief SO3 Vertex parameterized internally with a rotation matrix and externally with its exponential map
+ */
+
+class VertexSO3Expmap : public BaseVertex<3, SO3>{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+public:
+
+  VertexSO3Expmap();
+  bool read(std::istream& is);
+  bool write(std::ostream& os) const;
+
+  virtual void setToOriginImpl(){
+    _estimate = SO3();
+  }
+
+  virtual void oplusImpl(const double* update_){
+    Eigen::Map<const Vector3d> update(update_);
+    SO3 tmp(SO3::exp(update));
+    setEstimate(tmp*estimate());      // exp(w + delta_w ) = exp(delta_w ) * exp(w); left multipline
+  }
+};
+
+
+
+class EdgeSE3ProjectPlaneSO3 : public BaseBinaryEdge<3, Vector3d , VertexSO3Expmap , VertexMyExpmap> {
+
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+public:
+
+    EdgeSE3ProjectPlaneSO3();
+
+    bool read(std::istream& is);
+    bool write(std::ostream& os) const;
+
+    void computeError(){
+
+        VertexMyExpmap * vj = static_cast<VertexMyExpmap *>(_vertices[1]);
+      SE3Quat T(vj->estimate());
+      VertexSO3Expmap *vi = static_cast<VertexSO3Expmap *>(_vertices[0]);
+      SO3 plane(vi->estimate());
+
+      Quaterniond pi_x = plane_c.unit_quaternion();      // the observation
+      Vector4d pi_x_4d;
+      pi_x_4d << pi_x.x() , pi_x.y() , pi_x.z() , pi_x.w();
+      
+      Vector4d y = T.to_homogeneous_matrix().transpose() * pi_x_4d;
+      Vector4d q_y = y / y.norm();
+      Quaterniond n_q_y(q_y[3] , q_y[0] , q_y[1] , q_y[2]);
+      SO3 q_T(n_q_y);
+      SO3 q_u = q_T * plane.inverse();
+      Vector3d u = q_u.log();
+
+      Vector3d obs;
+      obs = u;
+      _error = obs;
+
+    }
+
+    virtual void linearizeOplus();
+
+    void setParams(const SO3& plane_);
+
+    SO3 plane_c;     // the plane coordinates in the current camera frame C     
+};
+
+class EdgeSE3ProjectPlaneSO3OnlyPose : public BaseUnaryEdge<3, Vector3d , VertexSE3Expmap> {
+
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+public:
+
+    EdgeSE3ProjectPlaneSO3OnlyPose();
+
+    bool read(std::istream& is);
+    bool write(std::ostream& os) const;
+
+    void computeError(){
+
+      VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[0]);
+      SE3Quat T(vj->estimate());
+      //VertexSO3Expmap *vi = static_cast<VertexSO3Expmap *>(_vertices[0]);
+      SO3 plane = plane_g; //(vi->estimate());
+
+      Quaterniond pi_x = plane_c.unit_quaternion();      // the observation
+      Vector4d pi_x_4d;
+      pi_x_4d << pi_x.x() , pi_x.y() , pi_x.z() , pi_x.w();
+      
+      Vector4d y = T.to_homogeneous_matrix().transpose() * pi_x_4d;
+      Vector4d q_y = y / y.norm();
+      Quaterniond n_q_y(q_y[3] , q_y[0] , q_y[1] , q_y[2]);
+      SO3 q_T(n_q_y);
+      SO3 q_u = q_T * plane.inverse();
+      Vector3d u = q_u.log();
+
+      Vector3d obs;
+      obs = u;
+      _error = obs;
+
+    }
+
+    virtual void linearizeOplus();
+
+    void setParams(const SO3& plane_, const SO3& _plane_g);
+
+    SO3 plane_c;     // the plane coordinates in the current camera frame C    
+    SO3 plane_g; 
+};
+
+
+
+
+
+} // end namespace
+
+#endif