上传新文件

src/types_six_dof_expmap_ours.cpp

+// 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.
+
+#include "types_six_dof_expmap_ours.h"
+
+
+namespace g2o {
+
+using namespace std;
+/*************************/
+    VertexMyExpmap::VertexMyExpmap() : BaseVertex<6, SE3Quat>() {
+    }
+
+    bool VertexMyExpmap::read(std::istream& is) {
+        Vector7d est;
+        for (int i=0; i<7; i++)
+            is  >> est[i];
+        SE3Quat cam2world;
+        cam2world.fromVector(est);
+        setEstimate(cam2world.inverse());
+        return true;
+    }
+
+    bool VertexMyExpmap::write(std::ostream& os) const {
+        SE3Quat cam2world(estimate().inverse());
+        for (int i=0; i<7; i++)
+            os << cam2world[i] << " ";
+        return os.good();
+    }
+
+/**************************/
+
+
+
+Vector2d project2d(const Vector3d& v)  {
+  Vector2d res;
+  res(0) = v(0)/v(2);
+  res(1) = v(1)/v(2);
+  return res;
+}
+
+Vector3d unproject2d(const Vector2d& v)  {
+  Vector3d res;
+  res(0) = v(0);
+  res(1) = v(1);
+  res(2) = 1;
+  return res;
+}
+
+
+VertexSO3Expmap::VertexSO3Expmap() : BaseVertex<3, SO3>(){
+
+}
+
+bool VertexSO3Expmap::read(std::istream& is){
+  Vector3d est;
+  for(int i = 0 ;i < 3 ; i ++){
+    is >> est[i];
+
+    SO3 plane(SO3::exp(est));
+    setEstimate(plane);
+    return true;
+  }
+}
+
+bool VertexSO3Expmap::write(std::ostream& os) const{
+  SO3 plane(estimate());
+  Vector3d angle = plane.log();
+  for(int i= 0; i < 3; i ++){
+    os << angle[i] << " ";
+  }
+  return os.good();
+}
+
+EdgeSE3ProjectPlaneSO3::EdgeSE3ProjectPlaneSO3() : BaseBinaryEdge<3, Vector3d , VertexSO3Expmap , VertexMyExpmap>(){
+
+}
+
+bool EdgeSE3ProjectPlaneSO3::read(std::istream& is){
+  for(int i=0; i <= 2; i ++){
+    is >> _measurement[i];
+  }
+  for (int i=0; i<=2; i++)
+    for (int j=i; j<=2; j++) {
+      is >> information()(i,j);
+      if (i!=j)
+        information()(j,i)=information()(i,j);
+    }
+
+  return true;  
+}
+
+bool EdgeSE3ProjectPlaneSO3::write(std::ostream& os) const{
+    for (int i=0; i<=2; i++){
+    os << measurement()[i] << " ";
+  }
+
+  for (int i=0; i<=2; i++)
+    for (int j=i; j<=2; j++){
+      os << " " <<  information()(i,j);
+    }
+  return os.good();
+}
+
+void EdgeSE3ProjectPlaneSO3::linearizeOplus(){
+
+    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 q_y_v = n_q_y.vec();
+  double q_y_v_norm = q_y_v.norm();
+  double q_y_v_norm_3 = q_y_v_norm * q_y_v_norm * q_y_v_norm;
+  double q4 = n_q_y.w();
+
+  Eigen::Matrix3d J_l_u_inv = SO3::JacobianLInv(u);
+
+  Eigen::Matrix<double ,3,4> M;
+  M.block<3,3>(0,0) = 2 * acos(q4) * ( Eigen::Matrix3d::Identity() / q_y_v_norm - q_y_v * q_y_v.transpose()/q_y_v_norm_3);
+  M.block<3,1>(0,3) = -2.0 * q_y_v / (q_y_v_norm * sqrt(1.0 - q4 * q4 + 0.0001))/q_y_v_norm;
+
+  Eigen::Matrix<double, 4,4> Q;
+  double y_norm = y.norm();
+  double y_norm_3 = y_norm * y_norm * y_norm;
+  Q = Eigen::Matrix4d::Identity() / y_norm - y * y.transpose() / y_norm_3;
+
+  Vector3d pi_x_v;
+  pi_x_v << pi_x.x() , pi_x.y() , pi_x.z();
+  Eigen::Matrix<double, 4,6> H = Eigen::Matrix<double, 4,6>::Zero();
+  H.block<3,3>(0,0) = T.rotation().toRotationMatrix().transpose() * SO3::hat(pi_x_v);
+  H.block<1,3>(3,0) = T.translation().transpose() * SO3::hat(pi_x_v);
+  H.block<1,3>(3,3) = pi_x_v.transpose();
+  //////////////////////////////////////////////////////////////////////////////////////////
+
+  // ok, finish
+  _jacobianOplusXi = SO3::JacobianRInv(u); // Jr(u)^(-1)
+  _jacobianOplusXj = J_l_u_inv * M * Q * H; // 3 x 6 matrix
+  //////////////////////////////////////////////////////////////////////////////////////////
+
+}
+void EdgeSE3ProjectPlaneSO3::setParams(const SO3& plane_){
+  plane_c = plane_;
+}
+
+EdgeSE3ProjectPlaneSO3OnlyPose::EdgeSE3ProjectPlaneSO3OnlyPose() : BaseUnaryEdge<3, Vector3d , VertexSE3Expmap>(){
+
+}
+
+bool EdgeSE3ProjectPlaneSO3OnlyPose::read(std::istream& is){
+  for(int i=0; i <= 2; i ++){
+    is >> _measurement[i];
+  }
+  for (int i=0; i<=2; i++)
+    for (int j=i; j<=2; j++) {
+      is >> information()(i,j);
+      if (i!=j)
+        information()(j,i)=information()(i,j);
+    }
+
+  return true;  
+}
+
+bool EdgeSE3ProjectPlaneSO3OnlyPose::write(std::ostream& os) const{
+    for (int i=0; i<=2; i++){
+    os << measurement()[i] << " ";
+  }
+
+  for (int i=0; i<=2; i++)
+    for (int j=i; j<=2; j++){
+      os << " " <<  information()(i,j);
+    }
+  return os.good();
+}
+
+void EdgeSE3ProjectPlaneSO3OnlyPose::linearizeOplus(){
+
+  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 q_y_v = n_q_y.vec();
+  double q_y_v_norm = q_y_v.norm();
+  double q_y_v_norm_3 = q_y_v_norm * q_y_v_norm * q_y_v_norm;
+  double q4 = n_q_y.w();
+
+  Eigen::Matrix3d J_l_u_inv = SO3::JacobianLInv(u);
+
+  Eigen::Matrix<double ,3,4> M;
+  M.block<3,3>(0,0) = 2 * acos(q4) * ( Eigen::Matrix3d::Identity() / q_y_v_norm - q_y_v * q_y_v.transpose()/q_y_v_norm_3);
+  M.block<3,1>(0,3) = 2.0 * q_y_v / (q_y_v_norm * sqrt(1.0 - q4 * q4 + 0.0001));
+
+  Eigen::Matrix<double, 4,4> Q;
+  double y_norm = y.norm();
+  double y_norm_3 = y_norm * y_norm * y_norm;
+  Q = Eigen::Matrix4d::Identity() / y_norm - y * y.transpose() / y_norm_3;
+
+  Vector3d pi_x_v;
+  pi_x_v << pi_x.x() , pi_x.y() , pi_x.z();
+  Eigen::Matrix<double, 4,6> H = Eigen::Matrix<double, 4,6>::Zero();
+  H.block<3,3>(0,0) = T.rotation().toRotationMatrix().transpose() * SO3::hat(pi_x_v);
+  H.block<1,3>(3,0) = T.translation().transpose() * SO3::hat(pi_x_v);
+  H.block<1,3>(3,3) = pi_x_v.transpose();
+  //////////////////////////////////////////////////////////////////////////////////////////
+
+  // ok, finish
+  //_jacobianOplusXi = SO3::JacobianRInv(u); // Jr(u)^(-1)
+  _jacobianOplusXi = J_l_u_inv * M * Q * H; // 3 x 6 matrix
+  //////////////////////////////////////////////////////////////////////////////////////////
+
+}
+void EdgeSE3ProjectPlaneSO3OnlyPose::setParams(const SO3& plane_, const SO3& _plane_g){
+  plane_c = plane_;
+  plane_g = _plane_g;
+}
+
+
+
+
+} // end namespace