ch9/project

project/0.1/CMakeLists.txt

@@ -0,0 +1,30 @@
+cmake_minimum_required( VERSION 2.8 )
+project ( myslam )
+
+# set( CMAKE_CXX_COMPILER "clang++" )
+set( CMAKE_CXX_COMPILER "g++" )
+set( CMAKE_BUILD_TYPE "Debug" )
+set( CMAKE_CXX_FLAGS "-std=c++11 -march=native" )
+
+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} )
+
+set( THIRD_PARTY_LIBS 
+ ${OpenCV_LIBS}
+ ${Sophus_LIBRARIES}
+)
+############### dependencies ######################
+include_directories( ${PROJECT_SOURCE_DIR}/include )
+add_subdirectory( src )
+add_subdirectory( test )

project/0.1/config/default.yaml

@@ -0,0 +1,24 @@
+%YAML:1.0
+# data
+# the tum dataset directory, change it to yours! 
+dataset_dir: /home/xiang/dataset/rgbd_dataset_freiburg1_desk
+
+# camera intrinsics
+# fr1
+camera.fx: 517.3
+camera.fy: 516.5
+camera.cx: 325.1
+camera.cy: 249.7
+
+# fr2
+#camera.fx: 520.9
+#camera.fy: 521.0
+#camera.cx: 325.1
+#camera.cy: 249.7
+
+camera.depth_scale: 5000
+
+# VO Parameters
+num_of_features: 500
+scale_factor: 1.2
+level_pyramid: 4

project/0.1/include/myslam/camera.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 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_;
+ SE3 T_c_w_;
+
+ Camera();
+ Camera ( float fx, float fy, float cx, float cy, float depth_scale=0, SE3 T_c_w=SE3() ) :
+ fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale ), T_c_w_ ( T_c_w ) 
+ {}
+
+ // coordinate transform: world, camera, pixel
+ Vector3d world2camera( const Vector3d& p_w );
+ Vector3d camera2world( const Vector3d& p_c );
+ Vector2d camera2pixel( const Vector3d& p_c );
+ Vector3d pixel2camera( const Vector2d& p_p, double depth=1 ); 
+ Vector3d pixel2world ( const Vector2d& p_p, double depth=1 );
+ Vector2d world2pixel ( const Vector3d& p_w );
+
+};
+
+}
+#endif // CAMERA_H

project/0.1/include/myslam/common_include.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 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>
+using Sophus::SE3;
+
+// 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>
+
+using namespace std; 
+#endif

project/0.1/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

project/0.1/include/myslam/frame.h

@@ -0,0 +1,65 @@
+/*
+ * <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 FRAME_H
+#define FRAME_H
+
+#include "myslam/common_include.h"
+#include "myslam/camera.h"
+
+namespace myslam 
+{
+
+// forward declare 
+class MapPoint;
+class Frame
+{
+public:
+ typedef std::shared_ptr<Frame> Ptr;
+ long id_; // id of this frame
+ double time_stamp_; // when it is recorded
+ SE3 T_c_w_; // transform from frame to world
+ Camera::Ptr camera_; // Pinhole RGBD Camera model 
+ Mat color_, depth_; // color and depth image 
+ std::vector<cv::KeyPoint> keypoints_; // key points in image
+ std::vector<MapPoint*> map_points_; // associated map points
+ bool is_key_frame_; // whether a key-frame
+
+public: // data members 
+ Frame();
+ Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera=nullptr, Mat color=Mat(), Mat depth=Mat() );
+ ~Frame();
+
+ static Frame::Ptr createFrame(); 
+
+ // find the depth in depth map
+ double findDepth( const cv::KeyPoint& kp );
+
+ // Get Camera Center
+ Vector3d getCamCenter() const;
+
+ void setPose( const SE3& T_c_w );
+
+ // check if a point is in this frame 
+ bool isInFrame( const Vector3d& pt_world );
+};
+
+}
+
+#endif // FRAME_H

project/0.1/include/myslam/map.h

@@ -0,0 +1,43 @@
+/*
+ * <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 MAP_H
+#define MAP_H
+
+#include "myslam/common_include.h"
+#include "myslam/frame.h"
+#include "myslam/mappoint.h"
+
+namespace myslam
+{
+class Map
+{
+public:
+ typedef shared_ptr<Map> Ptr;
+ unordered_map<long, MapPoint* > map_points_; // all landmarks
+ unordered_map<long, Frame*> keyframes_; // all key-frames
+
+ Map() {}
+
+ void insertKeyFrame( Frame::Ptr frame );
+ void insertMapPoint( MapPoint::Ptr map_point );
+};
+}
+
+#endif // MAP_H

project/0.1/include/myslam/mappoint.h

@@ -0,0 +1,46 @@
+/*
+ * <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 MAPPOINT_H
+#define MAPPOINT_H
+
+namespace myslam
+{
+
+class Frame;
+class MapPoint
+{
+public:
+ typedef shared_ptr<MapPoint> Ptr;
+ long id_; // ID
+ bool good_; // wheter a good point 
+ Vector3d pos_; // Position in world
+ Vector3d norm_; // Normal of viewing direction 
+ Mat descriptor_; // Descriptor for matching 
+
+ list<Frame*> observed_frames_; // frames that can observe this point 
+
+ MapPoint();
+ MapPoint( long id, Vector3d position, Vector3d norm );
+
+ static MapPoint::Ptr createMapPoint();
+};
+}
+
+#endif // MAPPOINT_H