上传新文件

include/mylib/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 "g2o/g2o/core/base_vertex.h"
+#include "g2o/g2o/core/base_binary_edge.h"
+#include "g2o/g2o/core/base_unary_edge.h"
+//#include "se3_ops.h"
+//#include "se3quat.h"
+#include "g2o/g2o/types/types_sba.h"
+#include "so3.h"
+#include <Eigen/Geometry>
+#include <Eigen/StdVector>
+
+#include "g2o/g2o/core/factory.h"
+#include "g2o/g2o/stuff/macros.h"
+
+
+#include "g2o/g2o/types/types_sba.h"
+#include "g2o/g2o/types/types_six_dof_expmap.h"
+#include "g2o/g2o/core/base_edge.h"
+
+#include "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_w(vi->estimate());
+        Eigen::Matrix3d R = T.to_homogeneous_matrix().block(0,0,3,3);
+        Eigen::Vector3d t = T.to_homogeneous_matrix().block(0,3,3,1);
+        Eigen::Quaterniond q_m =_measurement_plane_c.unit_quaternion();
+        Eigen::Quaterniond q;
+        q.vec() = R.transpose()*q_m.vec();
+        q.w() = t.transpose()*q_m.vec()+q_m.w();
+        SO3 obs_w(q);//T_w_c*_measurement;
+        SO3 structure_error = obs_w*plane_w.inverse();
+        Eigen::Vector3d u = structure_error.log();
+        _error = u;
+        int i=0;
+/*
+      Quaterniond q_m =_measurement_plane_c.unit_quaternion();      // the observation
+      Vector4d vec_4;
+      vec_4 << q_m.x() , q_m.y() , q_m.z() , q_m.w();
+      
+      Vector4d _measurement_plane_w = T.to_homogeneous_matrix().transpose().inverse() * vec_4;
+      Vector4d q_y = _measurement_plane_w / _measurement_plane_w.norm();
+      Quaterniond q_mw(q_y[3] , q_y[0] , q_y[1] , q_y[2]);
+      SO3 plane_w_obs(q_mw);
+      SO3 structure_error = plane_w_obs * plane_w.inverse();
+      Vector3d u = structure_error.log();
+      _error = u;
+*/
+    }
+
+    virtual void linearizeOplus();
+
+    void setParams(const SO3& plane_);
+
+    SO3 _measurement_plane_c;     // the plane coordinates in the current camera frame C
+};
+
+
+
+
+
+
+
+} // end namespace
+
+#endif