segmentatio falt errors

GPU_version/vio/CMakeLists.txt

@@ -26,7 +26,6 @@ SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -
 # Add plain cmake packages 
 FIND_PACKAGE(OpenCV 4 REQUIRED)
 FIND_PACKAGE(Eigen REQUIRED)
-FIND_PACKAGE(Sophus REQUIRED)
 FIND_PACKAGE(Boost REQUIRED COMPONENTS thread system)
 FIND_PACKAGE(OpenCL REQUIRED)
 
@@ -50,7 +49,6 @@ INCLUDE_DIRECTORIES(
         kernel
         ${Eigen_INCLUDE_DIRS}
         ${OpenCV_INCLUDE_DIRS}
-        ${Sophus_INCLUDE_DIRS}
         ${Boost_INCLUDE_DIRS}
         ${OpenCL_INCLUDE_DIRS}
         ${catkin_INCLUDE_DIRS}
@@ -60,13 +58,12 @@ INCLUDE_DIRECTORIES(
 LIST(APPEND LINK_LIBS
   #gtsam
   ${OpenCV_LIBS}
-  ${Sophus_LIBRARIES}
   ${Boost_LIBRARIES}
   ${OpenCL_LIBRARIES}
   ${catkin_LIBRARIES}
 )
 
-file(GLOB SRC  CONFIGURE_DEPENDS "src/*.cpp")
+file(GLOB SRC  CONFIGURE_DEPENDS "src/*.cpp" "include/sophus/*.cpp")
 
 
 

GPU_version/vio/include/sophus/scso3.cpp

@@ -0,0 +1,256 @@
+// This file is part of Sophus.
+//
+// Copyright 2012 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.
+
+#include <iostream>
+#include "scso3.h"
+
+namespace Sophus
+{
+
+ScSO3::ScSO3()
+{
+  quaternion_.setIdentity();
+}
+
+ScSO3
+::ScSO3(const ScSO3 & other) : quaternion_(other.quaternion_) {}
+
+ScSO3
+::ScSO3(const Matrix3d & scale_times_R)
+{
+  Matrix3d squaredScaleMat = scale_times_R*scale_times_R.transpose();
+  double squaredScale
+      =( 1./3.)*(squaredScaleMat(0,0)
+                 +squaredScaleMat(1,1)
+                 +squaredScaleMat(2,2));
+  assert(squaredScale>0);
+  double scale = sqrt(squaredScale);
+
+  quaternion_ = scale_times_R/scale;
+  quaternion_.coeffs() *= scale;
+}
+
+
+ScSO3
+::ScSO3(double scale,
+        const Matrix3d & R)
+{
+  assert(scale>0);
+  quaternion_ = R;
+  quaternion_.normalize();
+  quaternion_.coeffs() *= scale;
+}
+
+ScSO3
+::ScSO3(double scale, const SO3 & so3)
+{
+  assert(scale>0);
+  quaternion_ = so3.unit_quaternion();
+  quaternion_.coeffs() *= scale;
+}
+
+ScSO3
+::ScSO3(const Quaterniond & quat) : quaternion_(quat) {}
+
+void ScSO3
+::operator=(const ScSO3 & other)
+{
+  this->quaternion_ = other.quaternion_;
+}
+
+ScSO3 ScSO3
+::operator*(const ScSO3& other) const
+{
+  ScSO3 result(*this);
+  result.quaternion_ *= other.quaternion_;
+  return result;
+}
+
+void ScSO3
+::operator*=(const ScSO3& other)
+{
+  quaternion_ *= other.quaternion_;
+}
+
+Vector3d ScSO3
+::operator*(const Vector3d & xyz) const
+{
+  //ToDO: implement this directly!
+  double scale = quaternion().norm();
+  Quaterniond norm_quad = quaternion_;
+  norm_quad.coeffs() /= scale;
+  return scale*norm_quad._transformVector(xyz);
+}
+
+ScSO3 ScSO3
+::inverse() const
+{
+  assert(quaternion_.squaredNorm()>0.);
+  return ScSO3(quaternion().inverse());
+}
+
+Matrix3d ScSO3
+::matrix() const
+{
+  //ToDO: implement this directly!
+  double scale = quaternion().norm();
+  Quaterniond norm_quad = quaternion_;
+  norm_quad.coeffs() /= scale;
+  return scale*norm_quad.toRotationMatrix();
+}
+
+Matrix3d ScSO3
+::rotationMatrix() const
+{
+  double scale = quaternion().norm();
+  Quaterniond norm_quad = quaternion_;
+  norm_quad.coeffs() /= scale;
+  return norm_quad.toRotationMatrix();
+}
+
+double ScSO3
+::scale() const
+{
+  return quaternion_.norm();
+}
+
+void ScSO3
+::setRotationMatrix(const Matrix3d & R)
+{
+  double scale = quaternion_.norm();
+  quaternion_ = R;
+  quaternion_.normalize(); //is this step necesarry??
+  quaternion_.coeffs() *= scale;
+}
+
+void ScSO3
+::setScale(double scale)
+{
+  quaternion_.normalize();
+  quaternion_.coeffs() *= scale;
+}
+
+Matrix4d ScSO3
+::Adj() const
+{
+  Matrix4d res;
+  res.setIdentity();
+  res.topLeftCorner<3,3>() = rotationMatrix();
+  return res;
+}
+
+Matrix3d ScSO3
+::generator(int i)
+{
+  assert(i>=0 && i<4);
+  Vector4d e;
+  e.setZero();
+  e[i] = 1.f;
+  return hat(e);
+}
+
+Vector4d ScSO3
+::log() const
+{
+  return ScSO3::log(*this);
+}
+
+Vector4d ScSO3
+::log(const ScSO3 & other)
+{
+  double theta;
+  return logAndTheta(other, &theta);
+}
+
+Vector4d ScSO3
+::logAndTheta(const ScSO3 & other, double * theta)
+{
+  double scale = other.quaternion().norm();
+  Vector4d omega_sigma;
+  omega_sigma[3] = std::log(scale);
+  omega_sigma.head<3>() = SO3::logAndTheta(SO3(other.quaternion()), theta);
+  return omega_sigma;
+}
+
+ScSO3 ScSO3
+::exp(const Vector4d & omega_sigma)
+{
+  double theta;
+  return expAndTheta(omega_sigma, &theta);
+}
+
+ScSO3 ScSO3
+::expAndTheta(const Vector4d & omega_sigma, double * theta)
+{
+  const Vector3d & omega = omega_sigma.head<3>();
+  double sigma = omega_sigma[3];
+  double scale = std::exp(sigma);
+
+  Quaterniond quat = SO3::expAndTheta(omega, theta).unit_quaternion();
+  quat.coeffs() *= scale;
+
+  return ScSO3(quat);
+}
+
+Matrix3d ScSO3
+::hat(const Vector4d & v)
+{
+  Matrix3d Omega;
+  Omega <<  v(3), -v(2),  v(1)
+      ,  v(2),     v(3), -v(0)
+      , -v(1),  v(0),     v(3);
+  return Omega;
+}
+
+Vector4d ScSO3
+::vee(const Matrix3d & Omega)
+{
+  assert(fabs(Omega(2,1)+Omega(1,2))<SMALL_EPS);
+  assert(fabs(Omega(0,2)+Omega(2,0))<SMALL_EPS);
+  assert(fabs(Omega(1,0)+Omega(0,1))<SMALL_EPS);
+  assert(fabs(Omega(0,0)-Omega(1,1))<SMALL_EPS);
+  assert(fabs(Omega(0,0)-Omega(2,2))<SMALL_EPS);
+  return Vector4d(Omega(2,1), Omega(0,2), Omega(1,0), Omega(0,0));
+}
+
+Vector4d ScSO3
+::lieBracket(const Vector4d & omega_sigma1, const Vector4d & omega_sigma2)
+{
+  Vector3d omega1 = omega_sigma1.head<3>();
+  Vector3d omega2 = omega_sigma2.head<3>();
+  Vector4d res;
+  res.head<3>() = omega1.cross(omega2);
+  res[3] = 0.;
+  return res;
+}
+
+Matrix4d ScSO3
+::d_lieBracketab_by_d_a(const Vector4d & b)
+{
+  Matrix4d res;
+  res.setZero();
+  res.topLeftCorner<3,3>() = -SO3::hat(b.head<3>());
+  return res;
+}
+
+
+}

GPU_version/vio/include/sophus/scso3.h

@@ -0,0 +1,151 @@
+// This file is part of Sophus.
+//
+// Copyright 2012 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_ScSO3_H
+#define SOPHUS_ScSO3_H
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+#include <Eigen/Geometry>
+
+#include "so3.h"
+
+
+namespace Sophus
+{
+using namespace Eigen;
+
+class ScSO3
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  ScSO3                      ();
+
+  ScSO3                      (const ScSO3 & other);
+
+  explicit
+  ScSO3                      (const Matrix3d & scale_times_R);
+
+  ScSO3                      (double scale,
+                              const SO3 & so3);
+
+  ScSO3                      (double scale,
+                              const Matrix3d & R);
+
+  explicit
+  ScSO3                      (const Quaterniond & quaternion);
+
+  void
+  operator=                  (const ScSO3 & ScSO3);
+
+  ScSO3
+  operator*                  (const ScSO3 & ScSO3) const;
+
+  void
+  operator*=                 (const ScSO3 & ScSO3);
+
+  Vector3d
+  operator*                  (const Vector3d & xyz) const;
+
+  ScSO3
+  inverse                    () const;
+
+  Matrix3d
+  matrix                     () const;
+
+  Matrix3d
+  rotationMatrix             () const;
+
+  double
+  scale                      () const;
+
+  void
+  setRotationMatrix          (const Matrix3d & R);
+
+  void
+  setScale                   (double scale);
+
+  Matrix4d
+  Adj                        () const;
+
+  Matrix3d
+  generator                  (int i);
+
+  Vector4d
+  log                        () const;
+
+  static ScSO3
+  exp                        (const Vector4d & omega);
+
+  static ScSO3
+  expAndTheta                (const Vector4d & omega,
+                              double * theta);
+
+  static Vector4d
+  log                        (const ScSO3 & ScSO3);
+
+  static Vector4d
+  logAndTheta                (const ScSO3 & ScSO3,
+                              double * theta);
+
+  static Matrix3d
+  hat                        (const Vector4d & omega);
+
+  static Vector4d
+  vee                        (const Matrix3d & Omega);
+
+  static Vector4d
+  lieBracket                 (const Vector4d & omega1,
+                              const Vector4d & omega2);
+
+  static Matrix4d
+  d_lieBracketab_by_d_a      (const Vector4d & b);
+
+
+  const Quaterniond & quaternion() const
+  {
+    return quaternion_;
+  }
+  Quaterniond& quaternion()
+  {
+    return quaternion_;
+  }
+
+  static const int DoF = 4;
+
+protected:
+  Quaterniond quaternion_;
+};
+
+inline std::ostream& operator <<(std::ostream & out_str,
+                                 const ScSO3& scso3)
+{
+  out_str << scso3.scale() << " * " <<
+             scso3.log().head<3>().transpose() << std::endl;
+  return out_str;
+}
+
+} // end namespace
+
+
+#endif

GPU_version/vio/include/sophus/se2.cpp

@@ -0,0 +1,228 @@
+// 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.
+
+#include <iostream>
+#include "se2.h"
+#include "so3.h"
+
+
+namespace Sophus
+{
+SE2
+::SE2()
+{
+  translation_.setZero();
+}
+
+SE2
+::SE2(const SO2 & so2, const Vector2d & translation)
+  : so2_(so2), translation_(translation) {}
+
+SE2
+::SE2(const Matrix2d & rotation_matrix, const Vector2d & translation)
+  : so2_(rotation_matrix), translation_(translation){}
+
+SE2
+::SE2(double theta, const Vector2d & translation)
+  : so2_(theta), translation_(translation) {}
+
+SE2
+::SE2(const SE2 & se2) : so2_(se2.so2_),translation_(se2.translation_){}
+
+
+SE2 & SE2
+::operator = (const SE2 & other)
+{
+  so2_ = other.so2_;
+  translation_ = other.translation_;
+  return *this;
+}
+
+SE2 SE2
+::operator*(const SE2 & other) const
+{
+  SE2 result(*this);
+  result.translation_ += so2_*(other.translation_);
+  result.so2_*=other.so2_;
+  return result;
+}
+
+SE2& SE2
+::operator *= (const SE2 & other)
+{
+  translation_+= so2_*(other.translation_);
+  so2_*=other.so2_;
+  return *this;
+}
+
+SE2 SE2
+::inverse() const
+{
+  SE2 ret;
+  ret.so2_= so2_.inverse();
+  ret.translation_ = ret.so2_*(translation_*-1.);
+  return ret;
+}
+
+Vector3d SE2
+::log() const
+{
+  return log(*this);
+}
+
+Vector2d SE2
+::operator *(const Vector2d & xyz) const
+{
+  return so2_*xyz + translation_;
+}
+
+Matrix3d SE2
+::matrix() const
+{
+  Matrix<double,3,3> homogenious_matrix;
+  homogenious_matrix.setIdentity();
+  homogenious_matrix.block(0,0,2,2) = rotation_matrix();
+  homogenious_matrix.col(2).head(2) = translation_;
+  return homogenious_matrix;
+}
+
+
+Matrix<double, 3, 3> SE2
+::Adj() const
+{
+  const Matrix2d & R = so2_.matrix();
+  Matrix3d res;
+  res.setIdentity();
+  res.topLeftCorner<2,2>() = R;
+  res(0,2) = translation_[1];
+  res(1,2) = -translation_[0];
+  return res;
+}
+
+Matrix3d SE2
+::hat(const Vector3d & v)
+{
+  Matrix3d Omega;
+  Omega.setZero();
+  Omega.topLeftCorner<2,2>() = SO2::hat(v[2]);
+  Omega.col(2).head<2>() = v.head<2>();
+  return Omega;
+}
+
+Vector3d SE2
+::vee(const Matrix3d & Omega)
+{
+  Vector3d upsilon_omega;
+  upsilon_omega.head<2>() = Omega.col(2).head<2>();
+  upsilon_omega[2] = SO2::vee(Omega.topLeftCorner<2,2>());
+  return upsilon_omega;
+}
+
+Vector3d SE2
+::lieBracket(const Vector3d & v1, const Vector3d & v2)
+{
+  Vector2d upsilon1 = v1.head<2>();
+  Vector2d upsilon2 = v2.head<2>();
+  double theta1 = v1[2];
+  double theta2 = v2[2];
+
+  return Vector3d(-theta1*upsilon2[1] + theta2*upsilon1[1],
+                  theta1*upsilon2[0] - theta2*upsilon1[0],
+                  0.);
+}
+
+Matrix3d SE2
+::d_lieBracketab_by_d_a(const Vector3d & v2)
+{
+  Vector2d upsilon2 = v2.head<2>();
+  double theta2 = v2[2];
+
+  Matrix3d res;
+  res <<      0., theta2, -upsilon2[1]
+      ,  -theta2,     0.,  upsilon2[0]
+      ,       0.,     0.,           0.;
+  return res;
+}
+
+SE2 SE2
+::exp(const Vector3d & update)
+{
+  Vector2d upsilon = update.head<2>();
+  double theta = update[2];
+
+  SO2 so2 = SO2::exp(theta);
+
+
+  double sin_theta_by_theta;
+  double one_minus_cos_theta_by_theta;
+  if(abs(theta)<SMALL_EPS)
+  {
+    double theta_sq = theta*theta;
+
+    sin_theta_by_theta
+        = 1.-(1./6.)*theta_sq;
+    one_minus_cos_theta_by_theta
+        = 0.5*theta
+        - (1./24.)*theta*theta_sq;
+  }
+  else
+  {
+    sin_theta_by_theta
+        = so2.unit_complex().imag()/theta;
+    one_minus_cos_theta_by_theta
+        = (1.-so2.unit_complex().real())/theta;
+  }
+  Matrix2d V;
+  V(0,0) = sin_theta_by_theta; V(0,1) = -one_minus_cos_theta_by_theta;
+  V(1,0) = one_minus_cos_theta_by_theta; V(1,1) = sin_theta_by_theta;
+  return SE2(so2,V*upsilon);
+}
+
+Vector3d SE2
+::log(const SE2 & se2)
+{
+  Vector3d upsilon_theta;
+  const SO2 & so2 = se2.so2();
+  double theta = SO2::log(so2);
+  upsilon_theta[2] = theta;
+
+  double halftheta = 0.5*theta;
+
+  double halftheta_by_tan_of_halftheta;
+  if (abs(theta)<SMALL_EPS)
+  {
+    halftheta_by_tan_of_halftheta = 1. - (1./12)*theta*theta;
+  }
+  else
+  {
+    const SO2::Complexd & z = so2.unit_complex();
+    halftheta_by_tan_of_halftheta = -(halftheta*z.imag())/(z.real()-1.);
+  }
+  Matrix2d V_inv;
+  V_inv(0,0) = halftheta_by_tan_of_halftheta; V_inv(1,0) = -halftheta;
+  V_inv(0,1) = halftheta; V_inv(1,1) = halftheta_by_tan_of_halftheta;
+  upsilon_theta.head<2>() = V_inv*se2.translation_;
+  return upsilon_theta;
+}
+
+}
+

GPU_version/vio/include/sophus/se2.h

@@ -0,0 +1,142 @@
+// 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_SE2_H
+#define SOPHUS_SE2_H
+
+#include "so2.h"
+
+namespace Sophus
+{
+using namespace Eigen;
+using namespace std;
+
+class SE2
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  SE2                        ();
+
+  SE2                        (const SO2 & so2,
+                              const Vector2d & translation);
+
+  SE2                        (const Matrix2d & rotation_matrix,
+                              const Vector2d & translation);
+
+  SE2                        (double theta,
+                              const Vector2d & translation_);
+
+  SE2                        (const SE2 & other);
+
+  SE2 &
+  operator=                  (const SE2 & other);
+
+  SE2
+  operator*                  (const SE2& other) const;
+
+  SE2&
+  operator*=                 (const SE2& other);
+
+  SE2
+  inverse                    () const;
+
+  Vector3d
+  log                        () const;
+
+  Vector2d
+  operator*                  (const Vector2d & xy) const;
+
+  static SE2
+  exp                        (const Vector3d & update);
+
+  static Vector3d
+  log                        (const SE2 & SE2);
+
+  Matrix<double,3,3>
+  matrix                     () const;
+
+  Matrix<double, 3, 3>
+  Adj                        () const;
+
+  static Matrix3d
+  hat                        (const Vector3d & omega);
+
+  static Vector3d
+  vee                        (const Matrix3d & Omega);
+
+  static Vector3d
+  lieBracket                 (const Vector3d & v1,
+                              const Vector3d & v2);
+
+  static Matrix3d
+  d_lieBracketab_by_d_a      (const Vector3d & b);
+
+  const Vector2d & translation() const
+  {
+    return translation_;
+  }
+
+  Vector2d& translation()
+  {
+    return translation_;
+  }
+
+  Matrix2d rotation_matrix() const
+  {
+    return so2_.matrix();
+  }
+
+  void setRotationMatrix(const Matrix2d & rotation_matrix)
+  {
+    so2_ = SO2(rotation_matrix);
+  }
+
+  const SO2& so2() const
+  {
+    return so2_;
+  }
+
+  SO2& so2()
+  {
+    return so2_;
+  }
+
+  static const int DoF = 3;
+
+private:
+  SO2 so2_;
+  Vector2d translation_;
+
+};
+
+inline std::ostream& operator <<(std::ostream & out_str,
+                                 const SE2 &  SE2)
+{
+  out_str << SE2.so2() << SE2.translation() << std::endl;
+  return out_str;
+}
+
+} // end namespace
+
+
+#endif

GPU_version/vio/include/sophus/se3.cpp

@@ -0,0 +1,229 @@
+// 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.
+
+#include <iostream>
+#include "se3.h"
+
+
+namespace Sophus
+{
+SE3
+::SE3()
+{
+  translation_.setZero();
+}
+
+SE3
+::SE3(const SO3 & so3, const Vector3d & translation)
+  : so3_(so3), translation_(translation) {}
+
+SE3
+::SE3(const Matrix3d & rotation_matrix, const Vector3d & translation)
+  : so3_(rotation_matrix), translation_(translation){}
+
+SE3
+::SE3(const Quaterniond & quaternion, const Vector3d & translation)
+  : so3_(quaternion), translation_(translation) {}
+
+SE3
+::SE3(const SE3 & se3) : so3_(se3.so3_),translation_(se3.translation_){}
+
+
+SE3 & SE3
+::operator = (const SE3 & other)
+{
+  so3_ = other.so3_;
+  translation_ = other.translation_;
+  return *this;
+}
+
+SE3 SE3
+::operator*(const SE3 & other) const
+{
+  SE3 result(*this);
+  result.translation_ += so3_*(other.translation_);
+  result.so3_*=other.so3_;
+  return result;
+}
+
+SE3& SE3
+::operator *= (const SE3 & other)
+{
+  translation_+= so3_*(other.translation_);
+  so3_*=other.so3_;
+  return *this;
+}
+
+SE3 SE3
+::inverse() const
+{
+  SE3 ret;
+  ret.so3_= so3_.inverse();
+  ret.translation_= ret.so3_*(translation_*-1.);
+  return ret;
+}
+
+Vector6d SE3
+::log() const
+{
+  return log(*this);
+}
+
+Vector3d SE3
+::operator *(const Vector3d & xyz) const
+{
+  return so3_*xyz + translation_;
+}
+
+Matrix4d SE3
+::matrix() const
+{
+  Matrix<double,4,4> homogenious_matrix;
+  homogenious_matrix.setIdentity();
+  homogenious_matrix.block(0,0,3,3) = rotation_matrix();
+  homogenious_matrix.col(3).head(3) = translation_;
+  return homogenious_matrix;
+}
+
+
+Matrix<double, 6, 6> SE3
+::Adj() const
+{
+  Matrix3d R = so3_.matrix();
+  Matrix<double, 6, 6> res;
+  res.block(0,0,3,3) = R;
+  res.block(3,3,3,3) = R;
+  res.block(0,3,3,3) = SO3::hat(translation_)*R;
+  res.block(3,0,3,3) = Matrix3d::Zero(3,3);
+  return res;
+}
+
+Matrix4d SE3
+::hat(const Vector6d & v)
+{
+  Matrix4d Omega;
+  Omega.setZero();
+  Omega.topLeftCorner<3,3>() = SO3::hat(v.tail<3>());
+  Omega.col(3).head<3>() = v.head<3>();
+  return Omega;
+}
+
+Vector6d SE3
+::vee(const Matrix4d & Omega)
+{
+  Vector6d upsilon_omega;
+  upsilon_omega.head<3>() = Omega.col(3).head<3>();
+  upsilon_omega.tail<3>() = SO3::vee(Omega.topLeftCorner<3,3>());
+  return upsilon_omega;
+}
+
+Vector6d SE3
+::lieBracket(const Vector6d & v1, const Vector6d & v2)
+{
+  Vector3d upsilon1 = v1.head<3>();
+  Vector3d upsilon2 = v2.head<3>();
+  Vector3d omega1 = v1.tail<3>();
+  Vector3d omega2 = v2.tail<3>();
+
+  Vector6d res;
+  res.head<3>() = omega1.cross(upsilon2) + upsilon1.cross(omega2);
+  res.tail<3>() = omega1.cross(omega2);
+
+  return res;
+}
+
+Matrix6d SE3
+::d_lieBracketab_by_d_a(const Vector6d & b)
+{
+  Matrix6d res;
+  res.setZero();
+
+  Vector3d upsilon2 = b.head<3>();
+  Vector3d omega2 = b.tail<3>();
+
+  res.topLeftCorner<3,3>() = -SO3::hat(omega2);
+  res.topRightCorner<3,3>() = -SO3::hat(upsilon2);
+
+  res.bottomRightCorner<3,3>() = -SO3::hat(omega2);
+  return res;
+}
+
+SE3 SE3
+::exp(const Vector6d & update)
+{
+  Vector3d upsilon = update.head<3>();
+  Vector3d omega = update.tail<3>();
+
+  double theta;
+  SO3 so3 = SO3::expAndTheta(omega, &theta);
+
+  Matrix3d Omega = SO3::hat(omega);
+  Matrix3d Omega_sq = Omega*Omega;
+  Matrix3d V;
+
+  if(theta<SMALL_EPS)
+  {
+    V = so3.matrix();
+    //Note: That is an accurate expansion!
+  }
+  else
+  {
+    double theta_sq = theta*theta;
+    V = (Matrix3d::Identity()
+         + (1-cos(theta))/(theta_sq)*Omega
+         + (theta-sin(theta))/(theta_sq*theta)*Omega_sq);
+  }
+  return SE3(so3,V*upsilon);
+}
+
+Vector6d SE3
+::log(const SE3 & se3)
+{
+  Vector6d upsilon_omega;
+  double theta;
+  upsilon_omega.tail<3>() = SO3::logAndTheta(se3.so3_, &theta);
+
+  if (theta<SMALL_EPS)
+  {
+    Matrix3d Omega = SO3::hat(upsilon_omega.tail<3>());
+    Matrix3d V_inv = Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);
+
+    upsilon_omega.head<3>() = V_inv*se3.translation_;
+  }
+  else
+  {
+    Matrix3d Omega = SO3::hat(upsilon_omega.tail<3>());
+    Matrix3d V_inv = ( Matrix3d::Identity() - 0.5*Omega
+              + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) );
+    upsilon_omega.head<3>() = V_inv*se3.translation_;
+  }
+  return upsilon_omega;
+}
+
+void SE3::
+setQuaternion(const Quaterniond& quat)
+{
+  return so3_.setQuaternion(quat);
+}
+
+}
+

GPU_version/vio/include/sophus/se3.h

@@ -0,0 +1,168 @@
+// 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_SE3_H
+#define SOPHUS_SE3_H
+
+#include "so3.h"
+
+namespace Sophus
+{
+using namespace Eigen;
+using namespace std;
+
+typedef Matrix< double, 6, 1 > Vector6d;
+typedef Matrix< double, 6, 6 > Matrix6d;
+
+class SE3
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  SE3                        ();
+
+  SE3                        (const SO3 & so3,
+                              const Vector3d & translation);
+  SE3                        (const Matrix3d & rotation_matrix,
+                              const Vector3d & translation);
+  SE3                        (const Quaterniond & unit_quaternion,
+                              const Vector3d & translation_);
+  SE3                        (const SE3 & other);
+
+  SE3 &
+  operator=                  (const SE3 & other);
+
+  SE3
+  operator*                  (const SE3& other) const;
+
+  SE3&
+  operator*=                 (const SE3& other);
+
+  SE3
+  inverse                    () const;
+
+  Vector6d
+  log                        () const;
+
+  Vector3d
+  operator*                  (const Vector3d & xyz) const;
+
+  static SE3
+  exp                        (const Vector6d & update);
+
+  static Vector6d
+  log                        (const SE3 & se3);
+
+  Matrix<double,4,4>
+  matrix                     () const;
+
+  Matrix<double, 6, 6>
+  Adj                        () const;
+
+  static Matrix4d
+  hat                        (const Vector6d & omega);
+
+  static Vector6d
+  vee                        (const Matrix4d & Omega);
+
+  static Vector6d
+  lieBracket                 (const Vector6d & v1,
+                              const Vector6d & v2);
+
+  static Matrix6d
+  d_lieBracketab_by_d_a      (const Vector6d & b);
+
+  void
+  setQuaternion              (const Quaterniond& quaternion);
+
+  //TODO: remove later
+  static SE3 from_SE3(const SE3 & se3)
+  {
+    return se3;
+  }
+
+  //TODO: remove later
+  static SE3 to_SE3(const SE3 & se3)
+  {
+    return se3.to_SE3();
+  }
+
+  //TODO: remove later
+  SE3 to_SE3() const
+  {
+    return *this;
+  }
+
+  const Vector3d & translation() const
+  {
+    return translation_;
+  }
+
+  Vector3d& translation()
+  {
+    return translation_;
+  }
+
+  const Quaterniond & unit_quaternion() const
+  {
+    return so3_.unit_quaternion();
+  }
+
+  Matrix3d rotation_matrix() const
+  {
+    return so3_.matrix();
+  }
+
+  void setRotationMatrix(const Matrix3d & rotation_matrix)
+  {
+    so3_.setQuaternion(Quaterniond(rotation_matrix));
+  }
+
+  const SO3& so3() const
+  {
+    return so3_;
+  }
+
+  SO3& so3()
+  {
+    return so3_;
+  }
+
+  static const int DoF = 6;
+
+private:
+  SO3 so3_;
+  Vector3d translation_;
+
+};
+
+inline std::ostream& operator <<(std::ostream & out_str,
+                                 const SE3 &  se3)
+{
+  out_str << se3.so3() << se3.translation().transpose() << std::endl;
+  return out_str;
+}
+
+} // end namespace
+
+
+#endif

GPU_version/vio/include/sophus/sim3.cpp

@@ -0,0 +1,281 @@
+// 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.
+
+#include "sim3.h"
+
+#include <iostream>
+
+namespace Sophus
+{
+Sim3
+::Sim3()
+{
+  translation_.setZero();
+}
+
+Sim3
+::Sim3(const ScSO3& scso3, const Vector3d & t)
+  :scso3_(scso3),translation_(t)
+{
+}
+
+Sim3
+::Sim3(const Sim3 & sim3)
+  : scso3_(sim3.scso3_),translation_(sim3.translation_)
+{
+}
+
+Sim3 Sim3
+::from_SE3(const SE3 & se3)
+{
+  return Sim3(ScSO3(1.,se3.so3()),se3.translation());
+}
+
+SE3 Sim3
+::to_SE3(const Sim3 & sim3)
+{
+  return sim3.to_SE3();
+}
+
+SE3 Sim3
+::to_SE3() const
+{
+  return to_SE3(*this);
+}
+
+Sim3 Sim3
+::inverse() const
+{
+  const ScSO3 sRinv = scso3_.inverse();
+  return Sim3(sRinv, -(sRinv*translation_));
+}
+
+Sim3& Sim3
+::operator*=(const Sim3& sim3)
+{
+  translation_ += (scso3_ * sim3.translation());
+  scso3_ *= sim3.scso3();
+  return *this;
+}
+
+Sim3 Sim3
+::operator*(const Sim3& sim3) const
+{
+  return Sim3(scso3_*sim3.scso3(),
+              (scso3_*sim3.translation()) + translation_);
+}
+
+Matrix4d Sim3
+::matrix() const
+{
+  Matrix4d homogenious_matrix;
+  homogenious_matrix.setIdentity();
+  homogenious_matrix.block(0,0,3,3) = scale()*rotation_matrix();
+  homogenious_matrix.col(3).head(3) = translation();
+  homogenious_matrix(3,3) = 1;
+  return homogenious_matrix;
+}
+
+Vector3d Sim3
+::operator*(const Vector3d & xyz) const
+{
+  return (scso3_*xyz) + translation_;
+}
+
+Matrix7d Sim3::
+Adj() const
+{
+  Matrix3d R = scso3_.rotationMatrix();
+  Matrix7d res;
+  res.setZero();
+
+  res.block(0,0,3,3) = scale()*R;
+  res.block(0,3,3,3) = SO3::hat(translation_)*R;
+  res.block(0,6,3,1) = -translation_;
+  res.block(3,3,3,3) = R;
+  res(6,6) = 1;
+
+  return res;
+}
+
+
+Matrix3d Sim3
+::calcW(double theta,
+        double sigma,
+        double scale,
+        const Matrix3d & Omega)
+{
+  Matrix3d Omega2 = Omega*Omega;
+
+  static const Matrix3d I = Matrix3d::Identity();
+
+  double A,B,C;
+  if (fabs(sigma)<SMALL_EPS)
+  {
+    C = 1;
+    if (theta<SMALL_EPS)
+    {
+      A = 1./2.;
+      B = 1./6.;
+      //ToDO: Use more accurate expansion
+    }
+    else
+    {
+      double theta_sq = theta*theta;
+      A = (1-cos(theta))/theta_sq;
+      B = (theta-sin(theta))/(theta_sq*theta);
+      //ToDO: Use more accurate expansion
+    }
+  }
+  else
+  {
+    C=(scale-1)/sigma;
+    if (theta<SMALL_EPS)
+    {
+      double sigma_sq = sigma*sigma;
+      A = ((sigma-1)*scale+1)/sigma_sq;
+      B = ((0.5*sigma*sigma-sigma+1)*scale)/(sigma_sq*sigma);
+     //ToDO: Use more accurate expansion
+    }
+    else
+    {
+      double theta_sq = theta*theta;
+
+      double a = scale*sin(theta);
+      double b = scale*cos(theta);
+      double c = theta_sq+sigma*sigma;
+      A = (a*sigma+ (1-b)*theta)/(theta*c);
+      B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta_sq);
+
+    }
+  }
+  return A*Omega + B*Omega2 + C*I;
+}
+
+Sim3 Sim3
+::exp(const Matrix<double,7,1>& vect)
+{
+  Vector3d upsilon = vect.segment(0,3);
+  Vector3d omega = vect.segment(3,3);
+  double sigma = vect[6];
+
+  double theta;
+  ScSO3 scso3 = ScSO3::expAndTheta(vect.tail<4>(), &theta);
+
+  Matrix3d Omega = SO3::hat(omega);
+  Matrix3d W = calcW(theta, sigma, scso3.scale(), Omega);
+  Vector3d t = W*upsilon;
+  return Sim3(scso3, t);
+}
+
+Matrix<double,7,1> Sim3
+::log(const Sim3& sim3)
+{
+  Vector7d res;
+  double scale = sim3.scale();
+
+  Vector3d t = sim3.translation_;
+
+  double theta;
+
+  Vector4d omega_sigma = ScSO3::logAndTheta(sim3.scso3_, &theta);
+  Vector3d omega = omega_sigma.head<3>();
+  double sigma = omega_sigma[3];
+  Matrix3d Omega = SO3::hat(omega);
+
+  Matrix3d W = calcW(theta, sigma, scale, Omega);
+
+  //Vector3d upsilon = W.jacobiSvd(ComputeFullU | ComputeFullV).solve(t);
+  Vector3d upsilon = W.partialPivLu().solve(t);
+
+  res.segment(0,3) = upsilon;
+  res.segment(3,3) = omega;
+  res[6] = sigma;
+  return res;
+}
+
+Matrix4d Sim3::
+hat(const Vector7d & v)
+{
+  Matrix4d Omega;
+  Omega.topLeftCorner<3,3>() = ScSO3::hat(v.tail<4>());
+  Omega.col(3).head<3>() = v.head<3>();
+  Omega.row(3) = Vector4d(0., 0., 0., 0.);
+  return Omega;
+}
+
+Vector7d Sim3::
+vee(const Matrix4d & Omega)
+{
+  Vector7d upsilon_omega_sigma;
+  upsilon_omega_sigma.head<3>() = Omega.col(3).head<3>();
+  upsilon_omega_sigma.tail<4>() = ScSO3::vee(Omega.topLeftCorner<3,3>());
+  return upsilon_omega_sigma;
+}
+
+Vector7d Sim3
+::lieBracket(const Vector7d & v1, const Vector7d & v2)
+{
+  Vector3d upsilon1 = v1.head<3>();
+  Vector3d upsilon2 = v2.head<3>();
+  Vector3d omega1 = v1.segment<3>(3);
+  Vector3d omega2 = v2.segment<3>(3);
+  double sigma1 = v1[6];
+  double sigma2 = v2[6];
+
+  Vector7d res;
+  res.head<3>() =
+      SO3::hat(omega1)*upsilon2 + SO3::hat(upsilon1)*omega2
+      + sigma1*upsilon2 - sigma2*upsilon1;
+  res.segment<3>(3) = omega1.cross(omega2);
+  res[6] = 0.;
+
+  return res;
+}
+
+Matrix7d Sim3
+::d_lieBracketab_by_d_a(const Vector7d & b)
+{
+  Matrix7d res;
+  res.setZero();
+
+  Vector3d upsilon2 = b.head<3>();
+  Vector3d omega2 = b.segment<3>(3);
+  double sigma2 = b[6];
+
+  res.topLeftCorner<3,3>() = -SO3::hat(omega2)-sigma2*Matrix3d::Identity();
+  res.block<3,3>(0,3) = -SO3::hat(upsilon2);
+  res.topRightCorner<3,1>() = upsilon2;
+
+  res.block<3,3>(3,3) = -SO3::hat(omega2);
+  return res;
+}
+
+SO3 Sim3
+::so3() const
+{
+  return SO3(quaternion());
+}
+
+}
+
+

GPU_version/vio/include/sophus/sim3.h

@@ -0,0 +1,181 @@
+// 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_SIMILARITY_H
+#define SOPHUS_SIMILARITY_H
+
+#include "scso3.h"
+#include "se3.h"
+
+namespace Sophus
+{
+typedef Matrix< double, 7, 1 > Vector7d;
+typedef Matrix< double, 7, 7 > Matrix7d;
+
+/**
+   * This class implements the Lie group and the corresponding Lie
+   * algebra of 3D similarity transformationns Sim3 as described in:
+   *
+   * > H. Strasdat, J.M.M. Montiel, A.J. Davison:
+   *   "Scale Drift-Aware Large Scale Monocular SLAM",
+   *   Proc. of Robotics: Science and Systems (RSS),
+   *   Zaragoza, Spain, 2010.
+   *   http://www.roboticsproceedings.org/rss06/p10.html
+   */
+class Sim3
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Sim3                       ();
+
+  Sim3                       (const ScSO3& scso3,
+                              const Vector3d & t);
+  Sim3                       (double s,
+                              const Matrix3d & R,
+                              const Vector3d & t);
+  Sim3                       (const Quaterniond& q,
+                              const Vector3d & t);
+  Sim3                       (const Sim3 & sim3);
+
+  static Sim3
+  from_SE3                   (const SE3 & se3);
+
+  static SE3
+  to_SE3                     (const Sim3 & sim3);
+
+  SE3
+  to_SE3                     () const;
+
+  Matrix7d
+  Adj                        () const;
+
+  static Sim3
+  exp                        (const Matrix<double,7,1> & vect);
+
+  static Vector7d
+  log                        (const Sim3& sim3);
+
+  Sim3
+  inverse                    () const;
+
+  Sim3 &
+  operator*=                 (const Sim3 & sim3);
+
+  Sim3
+  operator*                  (const Sim3 & sim3) const;
+
+  Matrix4d
+  matrix                     () const;
+
+  Vector3d
+  operator*                  (const Vector3d & xyz) const;
+
+  static Matrix4d
+  hat                        (const Vector7d & omega);
+
+  static Vector7d
+  vee                        (const Matrix4d & Omega);
+
+  static Vector7d
+  lieBracket                 (const Vector7d & omega1,
+                              const Vector7d & omega2);
+
+  static Matrix7d
+  d_lieBracketab_by_d_a      (const Vector7d & b);
+
+  SO3
+  so3                        () const;
+
+  const Vector3d& translation() const
+  {
+    return translation_;
+  }
+
+  Vector3d& translation()
+  {
+    return translation_;
+  }
+
+  const Quaterniond & quaternion() const
+  {
+    return scso3_.quaternion();
+  }
+
+  Quaterniond& quaternion()
+  {
+    return scso3_.quaternion();
+  }
+
+  const ScSO3 & scso3() const
+  {
+    return scso3_;
+  }
+
+  ScSO3& scso3()
+  {
+    return scso3_;
+  }
+
+  Matrix3d rotation_matrix() const
+  {
+    return scso3_.rotationMatrix();
+  }
+
+
+  double scale() const
+  {
+    return scso3_.scale();
+  }
+
+  Matrix<double,7,1> log() const
+  {
+    return Sim3::log(*this);
+  }
+
+  static const int DoF = 7;
+
+protected:
+  static Matrix3d
+  calcW                      (double theta,
+                              double sigma,
+                              double scale,
+                              const Matrix3d & Omega);
+
+  ScSO3 scso3_;
+  Vector3d translation_;
+};
+
+
+inline std::ostream& operator <<(std::ostream& out_str,
+                                 const Sim3 &  sim3)
+{
+  out_str << sim3.scso3() <<
+             sim3.translation().transpose() << std::endl;
+  return out_str;
+}
+
+
+}
+
+
+#endif

GPU_version/vio/include/sophus/so2.cpp

@@ -0,0 +1,171 @@
+// This file is part of Sophus.
+//
+// Copyright 2012 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.
+
+#include <iostream>
+#include "so2.h"
+#include "so3.h"
+
+namespace Sophus
+{
+
+SO2::SO2()
+{
+  unit_complex_ = std::complex<double>(1,0);
+}
+
+SO2
+::SO2(const SO2 & other) : unit_complex_(other.unit_complex_) {}
+
+SO2
+::SO2(const Matrix2d & R) : unit_complex_(0.5*(R(0,0)+R(1,1)),
+                                          0.5*(R(1,0)-R(0,1))) {}
+
+SO2
+::SO2(const Complexd & complex) : unit_complex_(complex)
+{
+  assert(abs(unit_complex_)!=0);
+  normalize();
+}
+
+SO2
+::SO2(double theta)
+{
+  unit_complex_ = SO2::exp(theta).unit_complex_;
+}
+
+void SO2
+::operator=(const SO2 & other)
+{
+  this->unit_complex_ = other.unit_complex_;
+}
+
+SO2 SO2
+::operator*(const SO2& other) const
+{
+  assert(this!= nullptr);
+  SO2 result(*this);
+  result.unit_complex_ *= other.unit_complex_;
+  result.normalize();
+  return result;
+}
+
+void SO2
+::operator*=(const SO2& other)
+{
+  unit_complex_ *= other.unit_complex_;
+  normalize();
+}
+
+Vector2d SO2
+::operator*(const Vector2d & xy) const
+{
+  const double & real = unit_complex_.real();
+  const double & imag = unit_complex_.imag();
+  return Vector2d(real*xy[0] - imag*xy[1], imag*xy[0] + real*xy[1]);
+}
+
+SO2 SO2
+::inverse() const
+{
+  return SO2(conj(unit_complex_));
+}
+
+Matrix2d SO2
+::matrix() const
+{
+  const double & real = unit_complex_.real();
+  const double & imag = unit_complex_.imag();
+  Matrix2d R;
+  R(0,0) = real;  R(0,1) = -imag;
+  R(1,0) = imag; R(1,1) = real;
+  return R;
+}
+
+double SO2
+::Adj() const
+{
+  return 1.;
+}
+
+Matrix2d SO2
+::generator(int i)
+{
+  assert(i==0);
+  return hat(1.);
+}
+
+double SO2
+::log() const
+{
+  return SO2::log(*this);
+}
+
+double SO2
+::log(const SO2 & other)
+{
+  return atan2(other.unit_complex_.imag(), other.unit_complex().real());
+}
+
+SO2 SO2
+::exp(double theta)
+{
+  return SO2(Complexd(cos(theta), sin(theta)));
+}
+
+Matrix2d SO2
+::hat(double v)
+{
+  Matrix2d Omega;
+  Omega <<  0, -v
+      ,     v,  0;
+  return Omega;
+}
+
+double SO2
+::vee(const Matrix2d & Omega)
+{
+  assert(fabs(Omega(1,0)+Omega(0,1))<SMALL_EPS);
+  return Omega(1,0);
+}
+
+double SO2
+::lieBracket(double omega1, double omega2)
+{
+  return 0.;
+}
+
+void SO2::
+setComplex(const Complexd& complex)
+{
+  assert(abs(complex)!=0);
+  unit_complex_ = complex;
+  normalize();
+}
+
+void SO2::
+normalize()
+{
+  unit_complex_ /= abs(unit_complex_);
+}
+
+
+}

GPU_version/vio/include/sophus/so2.h

@@ -0,0 +1,124 @@
+// 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_SO2_H
+#define SOPHUS_SO2_H
+
+#include <complex>
+
+#include <Eigen/Core>
+
+
+namespace Sophus
+{
+using namespace Eigen;
+
+
+class SO2
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  typedef std::complex<double> Complexd;
+
+  SO2                        ();
+
+  SO2                        (const SO2 & other);
+
+  explicit
+  SO2                        (const Matrix2d & R);
+
+  explicit
+  SO2                        (const Complexd & unit_complex);
+
+  SO2                        (double theta);
+  void
+  operator=                  (const SO2 & SO2);
+
+  SO2
+  operator*                  (const SO2 & SO2) const;
+
+  void
+  operator*=                 (const SO2 & SO2);
+
+  Vector2d
+  operator*                  (const Vector2d & xy) const;
+
+  SO2
+  inverse                    () const;
+
+  Matrix2d
+  matrix                     () const;
+
+  double
+  Adj                        () const;
+
+  Matrix2d
+  generator                  (int i);
+
+  double
+  log                        () const;
+
+  static SO2
+  exp                        (double theta);
+
+  static double
+  log                        (const SO2 & SO2);
+
+  static Matrix2d
+  hat                        (double omega);
+
+  static double
+  vee                        (const Matrix2d & Omega);
+
+  static double
+  lieBracket                 (double omega1,
+                              double omega2);
+  void
+  setComplex                 (const Complexd& z);
+
+  const Complexd & unit_complex() const
+  {
+    return unit_complex_;
+  }
+
+  static const int DoF = 2;
+
+protected:
+  void
+  normalize                  ();
+
+  Complexd unit_complex_;
+};
+
+inline std::ostream& operator <<(std::ostream & out_str,
+                                 const SO2 & SO2)
+{
+
+  out_str << SO2.log() << std::endl;
+  return out_str;
+}
+
+} // end namespace
+
+
+#endif

GPU_version/vio/include/sophus/so3.cpp

@@ -0,0 +1,244 @@
+// 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.
+
+#include <iostream>
+#include "so3.h"
+
+//ToDo: Think completely through when to normalize Quaternion
+
+namespace Sophus
+{
+
+SO3::SO3()
+{
+  unit_quaternion_.setIdentity();
+}
+
+SO3
+::SO3(const SO3 & other) : unit_quaternion_(other.unit_quaternion_) {}
+
+SO3
+::SO3(const Matrix3d & R) : unit_quaternion_(R) {}
+
+SO3
+::SO3(const Quaterniond & quat) : unit_quaternion_(quat)
+{
+  assert(unit_quaternion_.squaredNorm() > SMALL_EPS);
+  unit_quaternion_.normalize();
+}
+
+SO3
+::SO3(double rot_x, double rot_y, double rot_z)
+{
+  unit_quaternion_
+      = (SO3::exp(Vector3d(rot_x, 0.f, 0.f))
+         *SO3::exp(Vector3d(0.f, rot_y, 0.f))
+         *SO3::exp(Vector3d(0.f, 0.f, rot_z))).unit_quaternion_;
+}
+
+void SO3
+::operator=(const SO3 & other)
+{
+  this->unit_quaternion_ = other.unit_quaternion_;
+}
+
+SO3 SO3
+::operator*(const SO3& other) const
+{
+  SO3 result(*this);
+  result.unit_quaternion_ *= other.unit_quaternion_;
+  result.unit_quaternion_.normalize();
+  return result;
+}
+
+void SO3
+::operator*=(const SO3& other)
+{
+  unit_quaternion_ *= other.unit_quaternion_;
+  unit_quaternion_.normalize();
+}
+
+Vector3d SO3
+::operator*(const Vector3d & xyz) const
+{
+  return unit_quaternion_._transformVector(xyz);
+}
+
+SO3 SO3
+::inverse() const
+{
+  return SO3(unit_quaternion_.conjugate());
+}
+
+Matrix3d SO3
+::matrix() const
+{
+  return unit_quaternion_.toRotationMatrix();
+}
+
+Matrix3d SO3
+::Adj() const
+{
+  return matrix();
+}
+
+Matrix3d SO3
+::generator(int i)
+{
+  assert(i>=0 && i<3);
+  Vector3d e;
+  e.setZero();
+  e[i] = 1.f;
+  return hat(e);
+}
+
+Vector3d SO3
+::log() const
+{
+  return SO3::log(*this);
+}
+
+Vector3d SO3
+::log(const SO3 & other)
+{
+  double theta;
+  return logAndTheta(other, &theta);
+}
+
+Vector3d SO3
+::logAndTheta(const SO3 & other, double * theta)
+{
+
+    double n = other.unit_quaternion_.vec().norm();
+    double w = other.unit_quaternion_.w();
+    double squared_w = w*w;
+
+    double two_atan_nbyw_by_n;
+    // Atan-based log thanks to
+    //
+    // C. Hertzberg et al.:
+    // "Integrating Generic Sensor Fusion Algorithms with Sound State
+    // Representation through Encapsulation of Manifolds"
+    // Information Fusion, 2011
+
+    if (n < SMALL_EPS)
+    {
+      // If quaternion is normalized and n=1, then w should be 1;
+      // w=0 should never happen here!
+      assert(fabs(w)>SMALL_EPS);
+
+      two_atan_nbyw_by_n = 2./w - 2.*(n*n)/(w*squared_w);
+    }
+    else
+    {
+      if (fabs(w)<SMALL_EPS)
+      {
+        if (w>0)
+        {
+          two_atan_nbyw_by_n = M_PI/n;
+        }
+        else
+        {
+          two_atan_nbyw_by_n = -M_PI/n;
+        }
+      }
+      two_atan_nbyw_by_n = 2*atan(n/w)/n;
+    }
+
+    *theta = two_atan_nbyw_by_n*n;
+    return two_atan_nbyw_by_n * other.unit_quaternion_.vec();
+}
+
+SO3 SO3
+::exp(const Vector3d & omega)
+{
+  double theta;
+  return expAndTheta(omega, &theta);
+}
+
+SO3 SO3
+::expAndTheta(const Vector3d & omega, double * theta)
+{
+  *theta = omega.norm();
+  double half_theta = 0.5*(*theta);
+
+  double imag_factor;
+  double real_factor = cos(half_theta);
+  if((*theta)<SMALL_EPS)
+  {
+    double theta_sq = (*theta)*(*theta);
+    double theta_po4 = theta_sq*theta_sq;
+    imag_factor = 0.5-0.0208333*theta_sq+0.000260417*theta_po4;
+  }
+  else
+  {
+    double sin_half_theta = sin(half_theta);
+    imag_factor = sin_half_theta/(*theta);
+  }
+
+  return SO3(Quaterniond(real_factor,
+                         imag_factor*omega.x(),
+                         imag_factor*omega.y(),
+                         imag_factor*omega.z()));
+}
+
+Matrix3d SO3
+::hat(const Vector3d & v)
+{
+  Matrix3d Omega;
+  Omega <<  0, -v(2),  v(1)
+      ,  v(2),     0, -v(0)
+      , -v(1),  v(0),     0;
+  return Omega;
+}
+
+Vector3d SO3
+::vee(const Matrix3d & Omega)
+{
+  assert(fabs(Omega(2,1)+Omega(1,2))<SMALL_EPS);
+  assert(fabs(Omega(0,2)+Omega(2,0))<SMALL_EPS);
+  assert(fabs(Omega(1,0)+Omega(0,1))<SMALL_EPS);
+  return Vector3d(Omega(2,1), Omega(0,2), Omega(1,0));
+}
+
+Vector3d SO3
+::lieBracket(const Vector3d & omega1, const Vector3d & omega2)
+{
+  return omega1.cross(omega2);
+}
+
+Matrix3d SO3
+::d_lieBracketab_by_d_a(const Vector3d & b)
+{
+  return -hat(b);
+}
+
+void SO3::
+setQuaternion(const Quaterniond& quaternion)
+{
+  assert(quaternion.norm()!=0);
+  unit_quaternion_ = quaternion;
+  unit_quaternion_.normalize();
+}
+
+
+}

GPU_version/vio/include/sophus/so3.h

@@ -0,0 +1,133 @@
+// 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 Sophus
+{
+using namespace Eigen;
+
+const double SMALL_EPS = 1e-10;
+
+class SO3
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  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);
+  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);
+
+  const Quaterniond & unit_quaternion() const
+  {
+    return unit_quaternion_;
+  }
+
+  static const int DoF = 3;
+
+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

GPU_version/vio/include/vio/abstract_camera.h

@@ -67,9 +67,19 @@ public:
 
   inline bool isInFrame(const Vector2i &obs, int boundary, int level) const
   {
-    if((int)obs[0] >= boundary && (int)obs.x() < width()/(1<<level)-boundary && (int)obs[1] >= boundary && (int)obs.y() <height()/(1<<level)-boundary)
+      if(level>0){
+          if(obs.x() < boundary)return false;
+          if(obs.x() > (int)(width()/(1<<level)-boundary))return false;
+          if(obs.y() < boundary)return false;
+          if(obs.y() > (int)(height()/(1<<level)-boundary))return false;
+      }else{
+          if(obs.x() < boundary)return false;
+          if(obs.x() > (int)(width()-boundary))return false;
+          if(obs.y() < boundary)return false;
+          if(obs.y() > (int)(height()-boundary))return false;
+      }
       return true;
-    return false;
+
   }
 };
 

GPU_version/vio/include/vio/frame.h

@@ -118,7 +118,9 @@ namespace vio {
         inline Vector2d f2c(const Vector3d& f) const { return cam_->world2cam( f ); }
 
         /// Return the pose of the frame in the (w)orld coordinate frame.
-        inline Vector2d pos() const { return T_f_w_.se2().translation(); }
+        inline Vector2d pos() const {
+            return T_f_w_.se2().translation();
+        }
 
         inline SE3 se3() const{
             return T_f_w_.se3();

GPU_version/vio/include/vio/global.h

@@ -52,9 +52,9 @@ namespace vio
     const double PI = 3.14159265;
     class SE2_5: public SE2{
     public:
-        SE2_5(SE2&& se2):T2_(se2){
+        SE2_5(SE2&& se2):T2_(new SE2(se2)){
         }
-        SE2_5(SE2& se2):T2_(se2){
+        SE2_5(SE2& se2):T2_(new SE2(se2)){
         }
         SE2_5(SE3&& se3){
             Quaterniond q=se3.unit_quaternion().normalized();
@@ -62,7 +62,7 @@ namespace vio
             Eigen::Matrix<double, 2,2> R;
             R<<cos(euler(0)),sin(euler(0)),
                     -sin(euler(0)),cos(euler(0));
-            T2_ = SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
+            T2_ = new SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
         }
         SE2_5(SE3& se3){
             Quaterniond q=se3.unit_quaternion().normalized();
@@ -70,24 +70,25 @@ namespace vio
             Eigen::Matrix<double, 2,2> R;
             R<<cos(euler(0)),sin(euler(0)),
                     -sin(euler(0)),cos(euler(0));
-            T2_=SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
+            T2_=new SE2(R,Vector2d(se3.translation().x(),se3.translation().z()));
         }
         SE2_5(double y,double z,double roll){
             Eigen::Matrix<double, 2,2> R;
             R<<cos(roll),sin(roll),
                     -sin(roll),cos(roll);
-            T2_=SE2(R,Vector2d(y,z));
+            T2_=new SE2(R,Vector2d(y,z));
         };
         SE2 se2() const{
-            return T2_;
+            assert(T2_!= nullptr);
+            return *T2_;
         }
         SE2 inverse() const{
-            double roll=atan2(T2_.so2().unit_complex().imag(),T2_.so2().unit_complex().real());
-            return SE2(roll+M_PI,-1.0*T2_.translation());
+            double roll=atan2(T2_->so2().unit_complex().imag(),T2_->so2().unit_complex().real());
+            return SE2(roll+M_PI,-1.0*T2_->translation());
         }
-        // Rotation around x
+        // Rotation around y
         double pitch()const{
-            return atan2(T2_.rotation_matrix()(0,1),T2_.rotation_matrix()(0,0));
+            return atan2(T2_->rotation_matrix()(0,1),T2_->rotation_matrix()(0,0));
         }
         SE3 se3() const{
             //Todo add 15 roll orientation
@@ -95,12 +96,14 @@ namespace vio
             q = AngleAxisd(0.261799, Vector3d::UnitX())
                 * AngleAxisd(pitch(), Vector3d::UnitY())
                 * AngleAxisd(0.0, Vector3d::UnitZ());
-            return SE3(q.toRotationMatrix(),Vector3d(T2_.translation()(0), 0.0,T2_.translation()(1)));
+            return SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
+        }
+        ~SE2_5(){
+           // delete T2_;
         }
-        ~SE2_5(){}
 
     private:
-        SE2 T2_;
+        SE2* T2_= nullptr;
     };
 
     class Frame;

GPU_version/vio/include/vio/matcher.h

@@ -99,7 +99,6 @@ public:
   double h_inv_;                //!< hessian of 1d image alignment along epipolar line
   int search_level_;
   bool reject_;
-  Feature* ref_ftr_;
   Vector2d px_cur_;
 
   Matcher() = default;
@@ -108,8 +107,8 @@ public:
   /// Find a match by directly applying subpix refinement.
   /// IMPORTANT! This function assumes that px_cur is already set to an estimate that is within ~2-3 pixel of the final result!
   bool findMatchDirect(
-      const Point& pt,
-      const Frame& frame,
+       const Point& pt,
+       const Frame& frame,
       Vector2d& px_cur);
 
   /// Find a match by searching along the epipolar line without using any features.

GPU_version/vio/src/frame_handler_mono.cpp

@@ -170,9 +170,7 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame()
   //assert(new_frame_.get()== nullptr);
   SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_);
   if(img_align.run(last_frame_, new_frame_)==0)return  RESULT_FAILURE;
-  //reprojector_.reprojectMap(new_frame_, overlap_kfs_);
-   // reprojector_.reprojectMap1(new_frame_, last_frame_, overlap_kfs_);
-    reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
+  reprojector_.reprojectMap2(new_frame_, last_frame_,overlap_kfs_);
   //assert (false);
   int n_point=0;
   for(auto i:overlap_kfs_)n_point+=i.second;

GPU_version/vio/src/matcher.cpp

@@ -140,10 +140,21 @@ bool Matcher::findMatchDirect(
     const Frame& cur_frame,
     Vector2d& px_cur)
 {
-  ref_ftr_= nullptr;
+  Feature* ref_ftr_= nullptr;
   if(!pt.getCloseViewObs(cur_frame.pos(), ref_ftr_))return false;
-  if(!ref_ftr_->frame->cam_->isInFrame(Vector2d(ref_ftr_->px/(1<<ref_ftr_->level)).cast<int>(), halfpatch_size_+2, ref_ftr_->level))
-    return false;
+  //if(ref_ftr_->frame->cam_== nullptr)return false;
+  assert(ref_ftr_->frame!= nullptr);
+  if(ref_ftr_->frame->cam_== nullptr)return false;
+  assert(ref_ftr_->frame->cam_!= nullptr);
+  Vector2i pxi=ref_ftr_->px.cast<int>();
+  if(ref_ftr_->level==NULL){
+      if(!ref_ftr_->frame->cam_->isInFrame(pxi, 6))return false;
+      ref_ftr_->level=0;
+  }else {
+      if (!ref_ftr_->frame->cam_->isInFrame(Vector2d(ref_ftr_->px / (1 << ref_ftr_->level)).cast<int>(),
+                                            halfpatch_size_ + 2, ref_ftr_->level))
+          return false;
+  }
   if(ref_ftr_->frame->img_pyr_.empty())return false;
   if(ref_ftr_->frame->img_pyr_[ref_ftr_->level].empty())return false;
   // warp affine

GPU_version/vio/src/point.cpp

@@ -102,7 +102,9 @@ bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr) const
   double max_cos_angle = 1.0;
   if(obs_.empty())return false;
   for(auto&& ob:obs_){
-      Vector3d dir(Vector3d(ob->frame->pos()(0),1e-9,ob->frame->pos()(1)) + pos_);
+      assert(ob->frame!= nullptr);
+      Vector2d t=ob->frame->pos();
+      Vector3d dir(Vector3d(t.x(),1e-9,t.y()) + pos_);
       double cos_angle = obs_dir.dot(dir.normalized());
       if(cos_angle < max_cos_angle)
       {

GPU_version/vio/src/reprojector.cpp

@@ -169,11 +169,14 @@ namespace vio {
                 }
             }
         }
-        for (size_t i = 0; i < grid_.cells.size(); ++i) {
-            if (reprojectCell(*grid_.cells.at(grid_.cell_order[i]), frame))
-                ++n_matches_;
-            if (n_matches_ > (size_t) Config::maxFts())
-                break;
+        {
+            boost::unique_lock<boost::mutex> lock(map_.point_candidates_.mut_);
+            for (auto&& cell : grid_.cells) {
+                if (reprojectCell(*cell, frame))
+                    ++n_matches_;
+                if (n_matches_ > (size_t) Config::maxFts())
+                    break;
+            }
         }
     }
 
@@ -186,45 +189,41 @@ namespace vio {
 
     bool Reprojector::reprojectCell(Cell &cell, FramePtr frame) {
         cell.sort(boost::bind(&Reprojector::pointQualityComparator, _1, _2));
-        Cell::iterator it = cell.begin();
-        while (it != cell.end()) {
+        for(auto&& it : cell) {
             ++n_trials_;
-            if (it->pt->type_ == Point::TYPE_DELETED) {
-                it = cell.erase(it);
+            if (it.pt->type_ == Point::TYPE_DELETED) {
+               // cell.erase(it.index);
                 continue;
             }
-            bool found_match = true;
-            if (options_.find_match_direct)
-                found_match = matcher_.findMatchDirect(*it->pt, *frame, it->px);
-            if (!found_match) {
-                it->pt->n_failed_reproj_++;
-                if (it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_failed_reproj_ > 15)
-                    map_.safeDeletePoint(it->pt);
-                if (it->pt->type_ == Point::TYPE_CANDIDATE && it->pt->n_failed_reproj_ > 30)
-                    map_.point_candidates_.deleteCandidatePoint(it->pt);
-                it = cell.erase(it);
+            if (!matcher_.findMatchDirect(*it.pt, *frame, it.px)) {
+                it.pt->n_failed_reproj_++;
+                if (it.pt->type_ == Point::TYPE_UNKNOWN && it.pt->n_failed_reproj_ > 15)
+                    map_.safeDeletePoint(it.pt);
+                if (it.pt->type_ == Point::TYPE_CANDIDATE && it.pt->n_failed_reproj_ > 30)
+                    map_.point_candidates_.deleteCandidatePoint(it.pt);
+                //it = cell.erase(it);
                 continue;
             }
-            it->pt->n_succeeded_reproj_++;
-            if (it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
-                it->pt->type_ = Point::TYPE_GOOD;
+            it.pt->n_succeeded_reproj_++;
+            if (it.pt->type_ == Point::TYPE_UNKNOWN && it.pt->n_succeeded_reproj_ > 10)
+                it.pt->type_ = Point::TYPE_GOOD;
 
-            Feature *new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
+            Feature *new_feature = new Feature(frame.get(), it.px, matcher_.search_level_);
             frame->addFeature(new_feature);
 
             // Here we add a reference in the feature to the 3D point, the other way
             // round is only done if this frame is selected as keyframe.
-            new_feature->point = it->pt;
-
+            new_feature->point = it.pt;
+/*
             if (matcher_.ref_ftr_->type == Feature::EDGELET) {
                 new_feature->type = Feature::EDGELET;
                 new_feature->grad = matcher_.A_cur_ref_ * matcher_.ref_ftr_->grad;
                 new_feature->grad.normalize();
             }
-
+*/
             // If the keyframe is selected and we reproject the rest, we don't have to
             // check this point anymore.
-            it = cell.erase(it);
+            //it = cell.erase(it);
 
             // Maximum one point per cell.
             return true;