diff --git a/include/types_six_dof_expmap_ours.h b/include/types_six_dof_expmap_ours.h new file mode 100644 index 0000000000000000000000000000000000000000..8c425dcc5a3761fd86a07c05d3a8a68a28172b50 --- /dev/null +++ b/include/types_six_dof_expmap_ours.h @@ -0,0 +1,214 @@ +// 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 +#include + +#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 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 update(update_); + std::cout<<"VertexSE3Expmap"<{ +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 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(_vertices[1]); + SE3Quat T(vj->estimate()); + VertexSO3Expmap *vi = static_cast(_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(_vertices[0]); + SE3Quat T(vj->estimate()); + //VertexSO3Expmap *vi = static_cast(_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