From 933cfce1daf0321e0f2bd71514b75613d546934b Mon Sep 17 00:00:00 2001 From: gfuse Date: Wed, 1 Sep 2021 14:09:33 +0200 Subject: [PATCH] segmentatio falt errors --- GPU_version/vio/CMakeLists.txt | 5 +- GPU_version/vio/include/sophus/scso3.cpp | 256 ++++++++++++++++ GPU_version/vio/include/sophus/scso3.h | 151 ++++++++++ GPU_version/vio/include/sophus/se2.cpp | 228 ++++++++++++++ GPU_version/vio/include/sophus/se2.h | 142 +++++++++ GPU_version/vio/include/sophus/se3.cpp | 229 ++++++++++++++ GPU_version/vio/include/sophus/se3.h | 168 +++++++++++ GPU_version/vio/include/sophus/sim3.cpp | 281 ++++++++++++++++++ GPU_version/vio/include/sophus/sim3.h | 181 +++++++++++ GPU_version/vio/include/sophus/so2.cpp | 171 +++++++++++ GPU_version/vio/include/sophus/so2.h | 124 ++++++++ GPU_version/vio/include/sophus/so3.cpp | 244 +++++++++++++++ GPU_version/vio/include/sophus/so3.h | 133 +++++++++ GPU_version/vio/include/vio/abstract_camera.h | 14 +- GPU_version/vio/include/vio/frame.h | 4 +- GPU_version/vio/include/vio/global.h | 29 +- GPU_version/vio/include/vio/matcher.h | 5 +- GPU_version/vio/src/frame_handler_mono.cpp | 4 +- GPU_version/vio/src/matcher.cpp | 17 +- GPU_version/vio/src/point.cpp | 4 +- GPU_version/vio/src/reprojector.cpp | 53 ++-- 21 files changed, 2386 insertions(+), 57 deletions(-) create mode 100644 GPU_version/vio/include/sophus/scso3.cpp create mode 100644 GPU_version/vio/include/sophus/scso3.h create mode 100644 GPU_version/vio/include/sophus/se2.cpp create mode 100644 GPU_version/vio/include/sophus/se2.h create mode 100644 GPU_version/vio/include/sophus/se3.cpp create mode 100644 GPU_version/vio/include/sophus/se3.h create mode 100644 GPU_version/vio/include/sophus/sim3.cpp create mode 100644 GPU_version/vio/include/sophus/sim3.h create mode 100644 GPU_version/vio/include/sophus/so2.cpp create mode 100644 GPU_version/vio/include/sophus/so2.h create mode 100644 GPU_version/vio/include/sophus/so3.cpp create mode 100644 GPU_version/vio/include/sophus/so3.h diff --git a/GPU_version/vio/CMakeLists.txt b/GPU_version/vio/CMakeLists.txt index dcc017d..395e8c9 100755 --- a/GPU_version/vio/CMakeLists.txt +++ b/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") diff --git a/GPU_version/vio/include/sophus/scso3.cpp b/GPU_version/vio/include/sophus/scso3.cpp new file mode 100644 index 0000000..f6c3660 --- /dev/null +++ b/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 +#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))(); + 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; +} + + +} diff --git a/GPU_version/vio/include/sophus/scso3.h b/GPU_version/vio/include/sophus/scso3.h new file mode 100644 index 0000000..834f03c --- /dev/null +++ b/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 +#include +#include + +#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 diff --git a/GPU_version/vio/include/sophus/se2.cpp b/GPU_version/vio/include/sophus/se2.cpp new file mode 100644 index 0000000..2d61e0c --- /dev/null +++ b/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 +#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 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 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)() = V_inv*se2.translation_; + return upsilon_theta; +} + +} + diff --git a/GPU_version/vio/include/sophus/se2.h b/GPU_version/vio/include/sophus/se2.h new file mode 100644 index 0000000..3774a05 --- /dev/null +++ b/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 + matrix () const; + + Matrix + 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 diff --git a/GPU_version/vio/include/sophus/se3.cpp b/GPU_version/vio/include/sophus/se3.cpp new file mode 100644 index 0000000..ba230d3 --- /dev/null +++ b/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 +#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 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 SE3 +::Adj() const +{ + Matrix3d R = so3_.matrix(); + Matrix 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() = SO3::logAndTheta(se3.so3_, &theta); + + if (theta()); + 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); +} + +} + diff --git a/GPU_version/vio/include/sophus/se3.h b/GPU_version/vio/include/sophus/se3.h new file mode 100644 index 0000000..f703ad3 --- /dev/null +++ b/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 + matrix () const; + + Matrix + 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 diff --git a/GPU_version/vio/include/sophus/sim3.cpp b/GPU_version/vio/include/sophus/sim3.cpp new file mode 100644 index 0000000..432fb49 --- /dev/null +++ b/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 + +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)& 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 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()); +} + +} + + diff --git a/GPU_version/vio/include/sophus/sim3.h b/GPU_version/vio/include/sophus/sim3.h new file mode 100644 index 0000000..f4a9691 --- /dev/null +++ b/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 & 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 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 diff --git a/GPU_version/vio/include/sophus/so2.cpp b/GPU_version/vio/include/sophus/so2.cpp new file mode 100644 index 0000000..2485b6c --- /dev/null +++ b/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 +#include "so2.h" +#include "so3.h" + +namespace Sophus +{ + +SO2::SO2() +{ + unit_complex_ = std::complex(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)) + +#include + + +namespace Sophus +{ +using namespace Eigen; + + +class SO2 +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + typedef std::complex 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 diff --git a/GPU_version/vio/include/sophus/so3.cpp b/GPU_version/vio/include/sophus/so3.cpp new file mode 100644 index 0000000..c3ab232 --- /dev/null +++ b/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 +#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)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) +#include +#include + + +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 diff --git a/GPU_version/vio/include/vio/abstract_camera.h b/GPU_version/vio/include/vio/abstract_camera.h index 626bad2..cd81fef 100644 --- a/GPU_version/vio/include/vio/abstract_camera.h +++ b/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<= boundary && (int)obs.y() 0){ + if(obs.x() < boundary)return false; + if(obs.x() > (int)(width()/(1< (int)(height()/(1< (int)(width()-boundary))return false; + if(obs.y() < boundary)return false; + if(obs.y() > (int)(height()-boundary))return false; + } return true; - return false; + } }; diff --git a/GPU_version/vio/include/vio/frame.h b/GPU_version/vio/include/vio/frame.h index 53fb1bb..98e7b35 100644 --- a/GPU_version/vio/include/vio/frame.h +++ b/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(); diff --git a/GPU_version/vio/include/vio/global.h b/GPU_version/vio/include/vio/global.h index f115137..3aab729 100644 --- a/GPU_version/vio/include/vio/global.h +++ b/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 R; R< R; R< R; R<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; diff --git a/GPU_version/vio/include/vio/matcher.h b/GPU_version/vio/include/vio/matcher.h index 0a79204..00dc47f 100644 --- a/GPU_version/vio/include/vio/matcher.h +++ b/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. diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index d40723b..6cc9696 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/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; diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index ba9d300..fbef1e8 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/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<level)).cast(), 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(); + 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(), + 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 diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index 5b17804..ccbab25 100644 --- a/GPU_version/vio/src/point.cpp +++ b/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) { diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index c8f0231..fe4e67c 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/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 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;