上传新文件

src/g2o_opt_type.cpp

+/*create by user 18/06/2021 Jacobian error*/
+#include "mylib/g2o_opt_type.h"
+const double SMALL_EPS = 1e-5;
+
+/**
+ * \brief Plane vertex, omg(XYZ)
+ */
+namespace g2o
+{
+//    virtual bool read(std::istream &is);
+
+//    virtual bool write(std::ostream &os) const;
+
+    void VertexSBAPlaneomg::setToOriginImpl() {
+        _estimate.fill(0.1);
+    }
+
+    void VertexSBAPlaneomg::oplusImpl(const double *update) {
+
+        //Eigen::Map<const Vector3d> omg(update);
+        Eigen::Vector3d omg;
+        omg << update[0],update[1],update[2];
+       /* double theta, half_theta;
+        theta = omg.norm();//M_PI / 6;
+        omg = omg/theta;
+        half_theta = 0.5 * theta;
+        EIGEN_USING_STD(sin)
+        EIGEN_USING_STD(cos)*/
+        // if(theta<SMALL_EPS) TODO
+        Eigen::Quaterniond res;
+        Eigen::Quaterniond q(1,0,0,0),q_tmp;
+        q_tmp = QuatPlane::ExpUpDateQuat(_estimate,q);
+        res = QuatPlane::ExpUpDateQuat(omg, q_tmp);
+        Eigen::Vector3d v = QuatPlane::logQuat(res);
+        _estimate += v;
+
+
+    }
+
+    void EdgeSE3ProjectPlane::computeError()
+    {
+        const VertexSE3Expmap* vj = static_cast<const VertexSE3Expmap*>(_vertices[1]);
+        g2o::SE3Quat T(vj->estimate());
+        Eigen::Matrix4d  T_w_c= T.to_homogeneous_matrix().transpose();
+        const VertexSBAPlaneomg* vi = static_cast<const VertexSBAPlaneomg*>(_vertices[0]);
+        Eigen::Vector3d opt_plane = vi->estimate();
+
+        Eigen::Quaterniond  q(1,0,0,0),qt;
+        double dism = _measurement.norm();
+        Eigen::Vector3d normalm = _measurement/dism;
+        Eigen::Vector4d vec4;
+        QuatPlane::HesseToHomogenous(normalm,dism,vec4);
+        Eigen::Vector4d vec4_c;
+        vec4_c = T_w_c*vec4;
+        qt.vec() = vec4_c.head(3);
+        qt.w() = vec4_c(3);
+        qt.normalize();
+        Eigen::Vector3d obs_plane_w =QuatPlane::logQuat(qt);
+        Eigen::Quaterniond  structure_err = QuatPlane::ExpUpDateQuat(obs_plane_w.normalized(),q)*QuatPlane::ExpUpDateQuat(opt_plane.normalized(),q).inverse();
+        structure_err.normalize();
+        _error = QuatPlane::logQuat(structure_err);
+        std::cout<<"error is : "<<_error.transpose()<<std::endl;
+        /*****************************************************************************/
+
+    }
+
+    void EdgeSE3ProjectPlane::linearizeOplus()
+    {
+        VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
+        SE3Quat T(vj->estimate());
+        VertexSBAPlaneomg *vi = static_cast<VertexSBAPlaneomg *>(_vertices[0]);
+        Eigen::Vector3d plane = vi->estimate();
+        Eigen::Quaterniond q(1,0,0,0);
+        Eigen::Quaterniond plane_obs_c = QuatPlane::ExpUpDateQuat(_measurement,q).normalized();  // the observation
+        Eigen::Vector4d homo_c_4d;
+        homo_c_4d << plane_obs_c.x() , plane_obs_c.y() , plane_obs_c.z() , plane_obs_c.w();
+        Eigen::Vector4d homo_w_4d = T.to_homogeneous_matrix().transpose() * homo_c_4d;
+        Eigen::Vector4d q_y = homo_w_4d / homo_w_4d.norm();
+        Eigen::Quaterniond plane_obs_w(q_y[3] , q_y[0] , q_y[1] , q_y[2]);
+        Eigen::Quaterniond plane_opt_w = QuatPlane::ExpUpDateQuat(plane,q);
+        Eigen::Quaterniond structure_error = plane_obs_w * plane_opt_w.inverse();
+        Eigen::Vector3d u = QuatPlane::logQuat(structure_error);
+        Eigen::Matrix3d jacobian_E_u = QuatPlane::JacobianLInv(u);
+        Eigen::Matrix<double, 3, 4> jacobian_u_phi;
+        double q_n = plane_obs_w.norm();
+        double q_n_3 = q_n*q_n*q_n;
+        jacobian_u_phi.block<3, 3>(0, 0) = 2 * acos(plane_obs_w.w()) * (Eigen::Matrix3d::Identity() / plane_obs_w.vec().norm() -
+                plane_obs_w.vec() * plane_obs_w.vec().transpose() / sqrt(q_n_3));
+        jacobian_u_phi.block<3, 1>(0, 3) = -1 * 2.0 / (plane_obs_w.vec().norm() * sqrt(1.0 - plane_obs_w.w() * plane_obs_w.w() + 0.0001)) * plane_obs_w.vec();
+        double q_y_n = q_y.norm();
+        double q_y_n3 = q_y_n*q_y_n*q_y_n;
+        Eigen::Matrix4d jacobian_phi_y = Eigen::Matrix4d::Identity()/q_y.norm()-q_y*q_y.transpose()/ sqrt(q_y_n3);
+        Eigen::Matrix<double, 4, 6> jacobian_y_pose = Eigen::Matrix<double, 4, 6>::Zero();
+        jacobian_y_pose.block<3, 3>(0, 0) = T.rotation().toRotationMatrix().transpose() * g2o::hat(plane_obs_w.vec());
+        jacobian_y_pose.block<1, 3>(3, 0) = T.translation().transpose() * g2o::hat(plane_obs_w.vec());
+        jacobian_y_pose.block<1, 3>(3, 3) = plane_obs_w.vec().transpose();
+
+        _jacobianOplusXi = QuatPlane::JacobianRInv(u);
+        _jacobianOplusXj = jacobian_E_u * jacobian_u_phi * jacobian_phi_y * jacobian_y_pose; // 3 x 6 matrix
+    }
+    Eigen::Matrix3d hat(const Eigen::Vector3d &v) {
+        Eigen::Matrix3d m;
+        m << 0.0, -v[2], v[1],
+                v[2], 0.0, -v[0],
+                -v[1], v[0], 0.0;
+        return m;
+    }
+};
\ No newline at end of file