project 0.2

project/0.2/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required( VERSION 2.8 )
+project ( myslam )
+
+set( CMAKE_CXX_COMPILER "g++" )
+set( CMAKE_BUILD_TYPE "Release" )
+set( CMAKE_CXX_FLAGS "-std=c++11 -march=native -O3" )
+
+list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
+set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )
+set( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib )
+
+############### dependencies ######################
+# Eigen
+include_directories( "/usr/include/eigen3" )
+# OpenCV
+find_package( OpenCV 3.1 REQUIRED )
+include_directories( ${OpenCV_INCLUDE_DIRS} )
+# Sophus 
+find_package( Sophus REQUIRED )
+include_directories( ${Sophus_INCLUDE_DIRS} )
+# G2O
+find_package( G2O REQUIRED )
+include_directories( ${G2O_INCLUDE_DIRS} )
+
+set( THIRD_PARTY_LIBS 
+ ${OpenCV_LIBS}
+ ${Sophus_LIBRARIES}
+ g2o_core g2o_stuff g2o_types_sba
+)
+############### source and test ######################
+include_directories( ${PROJECT_SOURCE_DIR}/include )
+add_subdirectory( src )
+add_subdirectory( test )

project/0.2/cmake_modules/FindCSparse.cmake

@@ -0,0 +1,25 @@
+# Look for csparse; note the difference in the directory specifications!
+FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
+ PATHS
+ /usr/include/suitesparse
+ /usr/include
+ /opt/local/include
+ /usr/local/include
+ /sw/include
+ /usr/include/ufsparse
+ /opt/local/include/ufsparse
+ /usr/local/include/ufsparse
+ /sw/include/ufsparse
+ )
+
+FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /opt/local/lib
+ /sw/lib
+ )
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CSPARSE DEFAULT_MSG
+ CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)

project/0.2/cmake_modules/FindG2O.cmake

@@ -0,0 +1,113 @@
+# Find the header files
+
+FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
+ ${G2O_ROOT}/include
+ $ENV{G2O_ROOT}/include
+ $ENV{G2O_ROOT}
+ /usr/local/include
+ /usr/include
+ /opt/local/include
+ /sw/local/include
+ /sw/include
+ NO_DEFAULT_PATH
+ )
+
+# Macro to unify finding both the debug and release versions of the
+# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
+# macro.
+
+MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
+
+ FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+ NAMES "g2o_${MYLIBRARYNAME}_d"
+ PATHS
+ ${G2O_ROOT}/lib/Debug
+ ${G2O_ROOT}/lib
+ $ENV{G2O_ROOT}/lib/Debug
+ $ENV{G2O_ROOT}/lib
+ NO_DEFAULT_PATH
+ )
+
+ FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+ NAMES "g2o_${MYLIBRARYNAME}_d"
+ PATHS
+ ~/Library/Frameworks
+ /Library/Frameworks
+ /usr/local/lib
+ /usr/local/lib64
+ /usr/lib
+ /usr/lib64
+ /opt/local/lib
+ /sw/local/lib
+ /sw/lib
+ )
+
+ FIND_LIBRARY(${MYLIBRARY}
+ NAMES "g2o_${MYLIBRARYNAME}"
+ PATHS
+ ${G2O_ROOT}/lib/Release
+ ${G2O_ROOT}/lib
+ $ENV{G2O_ROOT}/lib/Release
+ $ENV{G2O_ROOT}/lib
+ NO_DEFAULT_PATH
+ )
+
+ FIND_LIBRARY(${MYLIBRARY}
+ NAMES "g2o_${MYLIBRARYNAME}"
+ PATHS
+ ~/Library/Frameworks
+ /Library/Frameworks
+ /usr/local/lib
+ /usr/local/lib64
+ /usr/lib
+ /usr/lib64
+ /opt/local/lib
+ /sw/local/lib
+ /sw/lib
+ )
+
+ IF(NOT ${MYLIBRARY}_DEBUG)
+ IF(MYLIBRARY)
+ SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
+ ENDIF(MYLIBRARY)
+ ENDIF( NOT ${MYLIBRARY}_DEBUG)
+
+ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
+
+# Find the core elements
+FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
+FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
+
+# Find the CLI library
+FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
+
+# Find the pluggable solvers
+FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
+FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
+FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
+FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
+FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
+FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
+
+# Find the predefined types
+FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
+FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
+FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
+FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
+
+# G2O solvers declared found if we found at least one solver
+SET(G2O_SOLVERS_FOUND "NO")
+IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+ SET(G2O_SOLVERS_FOUND "YES")
+ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+
+# G2O itself declared found if we found the core libraries and at least one solver
+SET(G2O_FOUND "NO")
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+ SET(G2O_FOUND "YES")
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)

project/0.2/config/default.yaml

@@ -0,0 +1,23 @@
+%YAML:1.0
+# data
+# the tum dataset directory, change it to yours! 
+dataset_dir: /home/xiang/dataset/rgbd_dataset_freiburg1_xyz
+
+# camera intrinsics
+# fr1
+camera.fx: 517.3
+camera.fy: 516.5
+camera.cx: 325.1
+camera.cy: 249.7
+
+camera.depth_scale: 5000
+
+# VO paras
+number_of_features: 800
+scale_factor: 1.2
+level_pyramid: 8
+match_ratio: 2.0
+max_num_lost: 10
+min_inliers: 10
+keyframe_rotation: 0.1
+keyframe_translation: 0.1

project/0.2/include/myslam/camera.h

@@ -0,0 +1,51 @@
+/*
+ * <one line to give the program's name and a brief idea of what it does.>
+ * Copyright (C) 2016 <copyright holder> <email>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http:https://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef CAMERA_H
+#define CAMERA_H
+
+#include "myslam/common_include.h"
+
+namespace myslam
+{
+
+// Pinhole RGBD camera model
+class Camera
+{
+public:
+ typedef std::shared_ptr<Camera> Ptr;
+ float fx_, fy_, cx_, cy_, depth_scale_; // Camera intrinsics 
+
+ Camera();
+ Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) :
+ fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale )
+ {}
+
+ // coordinate transform: world, camera, pixel
+ Vector3d world2camera( const Vector3d& p_w, const SE3& T_c_w );
+ Vector3d camera2world( const Vector3d& p_c, const SE3& T_c_w );
+ Vector2d camera2pixel( const Vector3d& p_c );
+ Vector3d pixel2camera( const Vector2d& p_p, double depth=1 ); 
+ Vector3d pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth=1 );
+ Vector2d world2pixel ( const Vector3d& p_w, const SE3& T_c_w );
+
+};
+
+}
+#endif // CAMERA_H

project/0.2/include/myslam/common_include.h

@@ -0,0 +1,52 @@
+/*
+ * <one line to give the program's name and a brief idea of what it does.>
+ * Copyright (C) 2016 <copyright holder> <email>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http:https://www.gnu.org/licenses/>.
+ *
+ */
+
+
+#ifndef COMMON_INCLUDE_H
+#define COMMON_INCLUDE_H
+
+// define the commonly included file to avoid a long include list
+// for Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+using Eigen::Vector2d;
+using Eigen::Vector3d;
+
+// for Sophus
+#include <sophus/se3.h>
+#include <sophus/so3.h>
+using Sophus::SE3;
+using Sophus::SO3;
+
+// for cv
+#include <opencv2/core/core.hpp>
+using cv::Mat;
+
+// std 
+#include <vector>
+#include <list>
+#include <memory>
+#include <string>
+#include <iostream>
+#include <set>
+#include <unordered_map>
+#include <map>
+
+using namespace std; 
+#endif

project/0.2/include/myslam/config.h

@@ -0,0 +1,49 @@
+/*
+ * <one line to give the program's name and a brief idea of what it does.>
+ * Copyright (C) 2016 <copyright holder> <email>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http:https://www.gnu.org/licenses/>.
+ *
+ */
+
+#ifndef CONFIG_H
+#define CONFIG_H
+
+#include "myslam/common_include.h" 
+
+namespace myslam 
+{
+class Config
+{
+private:
+ static std::shared_ptr<Config> config_; 
+ cv::FileStorage file_;
+
+ Config () {} // private constructor makes a singleton
+public:
+ ~Config(); // close the file when deconstructing 
+
+ // set a new config file 
+ static void setParameterFile( const std::string& filename ); 
+
+ // access the parameter values
+ template< typename T >
+ static T get( const std::string& key )
+ {
+ return T( Config::config_->file_[key] );
+ }
+};
+}
+
+#endif // CONFIG_H