上传新文件

include/so3.h

+// This file is part of Sophus.
+//
+// Copyright 2011 Hauke Strasdat (Imperial College London)
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights  to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+// IN THE SOFTWARE.
+
+#ifndef SOPHUS_SO3_H
+#define SOPHUS_SO3_H
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <Eigen/Geometry>
+
+
+namespace g2o
+{
+using namespace Eigen;
+
+const double SMALL_EPS = 1e-5;
+
+class SO3
+{
+
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+public:
+  
+  // Jr, right jacobian of SO(3)
+  static Matrix3d JacobianR(const Vector3d& w);
+  // Jr^(-1)
+  static Matrix3d JacobianRInv(const Vector3d& w);
+  // Jl, left jacobian of SO(3), Jl(x) = Jr(-x)
+  static Matrix3d JacobianL(const Vector3d& w);
+  // Jl^(-1)
+  static Matrix3d JacobianLInv(const Vector3d& w);
+
+  // ----------------------------------------
+
+  SO3                        ();
+
+  SO3                        (const SO3 & other);
+
+  explicit
+  SO3                        (const Matrix3d & _R);
+
+  explicit
+  SO3                        (const Quaterniond & unit_quaternion);
+
+  SO3                        (double rot_x,
+                              double rot_y,
+                              double rot_z);
+  /*SO3                        (double homo1,
+                              double homo2,
+                              double homo3,
+                              double homo4);*/
+  void
+  operator=                  (const SO3 & so3);
+
+  SO3
+  operator*                  (const SO3 & so3) const;
+
+  void
+  operator*=                 (const SO3 & so3);
+
+  Vector3d
+  operator*                  (const Vector3d & xyz) const;
+
+  SO3
+  inverse                    () const;
+
+  Matrix3d
+  matrix                     () const;
+
+  Matrix3d
+  Adj                        () const;
+
+  Matrix3d
+  generator                  (int i);
+
+  Vector3d
+  log                        () const;
+
+  static SO3
+  exp                        (const Vector3d & omega);
+
+  static SO3
+  expAndTheta                (const Vector3d & omega,
+                              double * theta);
+  static Vector3d
+  log                        (const SO3 & so3);
+
+  static Vector3d
+  logAndTheta                (const SO3 & so3,
+                              double * theta);
+
+  static Matrix3d
+  hat                        (const Vector3d & omega);
+
+  static Vector3d
+  vee                        (const Matrix3d & Omega);
+
+  static Vector3d
+  lieBracket                 (const Vector3d & omega1,
+                              const Vector3d & omega2);
+
+  static Matrix3d
+  d_lieBracketab_by_d_a      (const Vector3d & b);
+
+  void
+  setQuaternion              (const Quaterniond& quaternion);
+
+  static Quaterniond
+  HomoToQuad(double homo1,double homo2,double homo3,double homo4);
+
+
+  const Quaterniond & unit_quaternion() const
+  {
+    return unit_quaternion_;
+  }
+
+  static const int DoF = 3;
+
+public:
+  static Eigen::Matrix4d rightR(const Eigen::Quaterniond q);
+  static Eigen::Matrix4d leftR(const Eigen::Quaterniond q);
+
+
+protected:
+  Quaterniond unit_quaternion_;
+};
+
+inline std::ostream& operator <<(std::ostream & out_str,
+                                 const SO3 & so3)
+{
+
+  out_str << so3.log().transpose() << std::endl;
+  return out_str;
+}
+
+} // end namespace
+
+
+#endif