├── script
    ├── run.sh
    └── run.sh~
├── lib
    └── libfullBA.so
├── src
    ├── .Frame.cpp.swo
    ├── .system.cpp.swo
    ├── main.cpp
    ├── Map.cpp
    ├── MapPoint.cpp
    ├── CMakeLists.txt~
    ├── CMakeLists.txt
    ├── Mapviewer.cpp
    ├── PoseOpt.cpp
    ├── draw.cpp
    ├── utils.cpp
    ├── bal_problem.cc
    ├── Frame.cpp
    ├── system.cpp
    └── optimizer.cpp
├── assets
    └── Images
    │   ├── Seq06.png
    │   └── Seq09.png
├── include
    ├── system.h
    ├── Map.h
    ├── MapPoint.h
    ├── Mapviewer.h
    ├── PoseOpt.h
    ├── draw.hpp
    ├── optimizer.hpp
    ├── random.h
    ├── Frame.h
    ├── ORBextractor.h
    ├── utils.h
    ├── bal_problem.h
    └── snavely_reprojection_error.h
├── CMakeLists.txt
├── visual_odom.yaml
├── cmake_modules
    ├── FindCSparse.cmake
    ├── FindQGLViewer.cmake
    ├── FindCholmod.cmake
    ├── FindSuiteSparse.cmake
    ├── FindEigen3.cmake
    ├── FindG2O.cmake
    ├── FindLAPACK.cmake
    ├── FindBLAS.cmake
    ├── CeresConfig.cmake
    └── CeresConfig.cmake.in
└── README.md


/script/run.sh:
--------------------------------------------------------------------------------
1 | ./ba ~/Desktop/data/08 ../visual_odom.yaml
2 | 


--------------------------------------------------------------------------------
/script/run.sh~:
--------------------------------------------------------------------------------
1 | ./ba ~/Desktop/data/08 ../visual_odom.yaml
2 | 


--------------------------------------------------------------------------------
/lib/libfullBA.so:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akshayapurohit23/Stereo-Visual-Odometry/HEAD/lib/libfullBA.so


--------------------------------------------------------------------------------
/src/.Frame.cpp.swo:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akshayapurohit23/Stereo-Visual-Odometry/HEAD/src/.Frame.cpp.swo


--------------------------------------------------------------------------------
/src/.system.cpp.swo:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akshayapurohit23/Stereo-Visual-Odometry/HEAD/src/.system.cpp.swo


--------------------------------------------------------------------------------
/assets/Images/Seq06.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akshayapurohit23/Stereo-Visual-Odometry/HEAD/assets/Images/Seq06.png


--------------------------------------------------------------------------------
/assets/Images/Seq09.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/akshayapurohit23/Stereo-Visual-Odometry/HEAD/assets/Images/Seq09.png


--------------------------------------------------------------------------------
/src/main.cpp:
--------------------------------------------------------------------------------
 1 | #include <stdio.h>
 2 | #include <iostream>
 3 | #include <string.h>
 4 | #include <fstream>
 5 | 
 6 | #include "system.h"
 7 | 
 8 | int main(int argc, const char* argv[]){
 9 | 	if(argc != 3){
10 | 		cout << "wrong input! " << endl;
11 | 		exit;
12 | 	}
13 | 
14 | 	SLAMsystem((string)argv[1], (string)argv[2]);
15 | 	return 0;
16 | }


--------------------------------------------------------------------------------
/include/system.h:
--------------------------------------------------------------------------------
 1 | #ifndef SYSTEM_H
 2 | #define SYSTEM_H
 3 | 
 4 | #include <stdio.h>
 5 | 
 6 | #include "utils.h"
 7 | #include "Frame.h"
 8 | #include "Map.h"
 9 | #include "Mapviewer.h"
10 | #include "optimizer.hpp"
11 | #include "PoseOpt.h"
12 | #include <pcl/point_cloud.h>
13 | #include <pcl/kdtree/kdtree_flann.h>
14 | #include <pcl/kdtree/kdtree.h>
15 | 
16 | void SLAMsystem(std::string commonPath, std::string yamlPath);
17 | 
18 | #endif
19 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | CMAKE_MINIMUM_REQUIRED( VERSION 2.8 )
 2 | PROJECT(fullBA)
 3 | 
 4 | SET(CMAKE_CXX_COMPILER "g++")
 5 | SET( CMAKE_BUILD_TYPE Release  )
 6 | SET( CMAKE_CXX_FLAGS "-std=c++11 -O3")
 7 | SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/build)
 8 | SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
 9 | 
10 | INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include )
11 | LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib)
12 | 
13 | ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src )
14 | 
15 | 


--------------------------------------------------------------------------------
/include/Map.h:
--------------------------------------------------------------------------------
 1 | #ifndef MAP_H
 2 | #define MAP_H
 3 | 
 4 | #include "MapPoint.h"
 5 | #include "Frame.h"
 6 | #include <set>
 7 | 
 8 | 
 9 | class MapPoint;
10 | class Frame;
11 | 
12 | class Map{
13 | public:
14 | 	Map();
15 | 	void addMapPoint(MapPoint* mappoint);
16 | 	std::vector<cv::Point3f> getAllMapPoints();
17 | 	long unsigned int allMapPointNumber();
18 | 	void getMapPointsFromFrame(Frame* frame);
19 | 
20 | 	std::set<MapPoint*> allMapPoints;
21 | private:
22 | 	long unsigned int pointIdx; 
23 | 
24 | };
25 | 
26 | #endif
27 | 


--------------------------------------------------------------------------------
/src/Map.cpp:
--------------------------------------------------------------------------------
 1 | #include "Map.h"
 2 | 
 3 | Map::Map():pointIdx(0){
 4 | }
 5 | 
 6 | void Map::addMapPoint(MapPoint* mappoint){
 7 | 	allMapPoints.insert(mappoint);
 8 | 	pointIdx++;
 9 | }
10 | 
11 | std::vector<cv::Point3f> Map::getAllMapPoints(){
12 | 	std::vector<cv::Point3f> result;
13 | 
14 | 	for(auto mp : allMapPoints){
15 | 		if(!mp->isBad){ result.push_back(mp->pos); }
16 | 	}
17 | 
18 | 	return result;
19 | }
20 | 
21 | 
22 | long unsigned int Map::allMapPointNumber(){
23 | 	return (long unsigned int)allMapPoints.size();
24 | }
25 | 
26 | 
27 | 
28 | 
29 | 


--------------------------------------------------------------------------------
/visual_odom.yaml:
--------------------------------------------------------------------------------
 1 | %YAML:1.0
 2 | 
 3 | height: 370
 4 | width: 1226
 5 | 
 6 | 
 7 | LEFT.P: !!opencv-matrix
 8 |     rows:3
 9 |     cols:4
10 |     dt:d
11 |     data: [707.0912, 0.0, 601.8873, 0.0, 0.0, 707.0912, 183.1104, 0.0, 0.0, 0.0, 1.0, 0.0]
12 | 
13 | 
14 | RIGHT.P: !!opencv-matrix
15 |     rows:3
16 |     cols:4
17 |     dt:d
18 |     data: [707.0912, 0.0, 601.8873, -379.8145, 0.0, 707.0912, 183.1104, 0.0, 0.0, 0.0, 1.0, 0.0]
19 | 
20 | 
21 | 
22 | start index: 0
23 | data length: 0
24 | local optimization times: 3
25 | 
26 | 
27 | trajectory file: "../pose01.txt" 
28 | trajectory file before: "../tfileBefore.txt"
29 | trajectory file after: "../tfileAfter.txt"
30 | 
31 | 


--------------------------------------------------------------------------------
/cmake_modules/FindCSparse.cmake:
--------------------------------------------------------------------------------
 1 | # Look for csparse; note the difference in the directory specifications!
 2 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
 3 |   PATHS
 4 |   /usr/include/suitesparse
 5 |   /usr/include
 6 |   /opt/local/include
 7 |   /usr/local/include
 8 |   /sw/include
 9 |   /usr/include/ufsparse
10 |   /opt/local/include/ufsparse
11 |   /usr/local/include/ufsparse
12 |   /sw/include/ufsparse
13 |   )
14 | 
15 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
16 |   PATHS
17 |   /usr/lib
18 |   /usr/local/lib
19 |   /opt/local/lib
20 |   /sw/lib
21 |   )
22 | 
23 | include(FindPackageHandleStandardArgs)
24 | find_package_handle_standard_args(CSPARSE DEFAULT_MSG
25 |   CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)
26 | 


--------------------------------------------------------------------------------
/src/MapPoint.cpp:
--------------------------------------------------------------------------------
 1 | #include "MapPoint.h"
 2 | 
 3 | using namespace cv;
 4 | using namespace std;
 5 | 
 6 | MapPoint::MapPoint(Point3f _pos, 
 7 | 				   unsigned int _frameID, 
 8 | 				   unsigned int _keypointIdx):
 9 | 				   pos(_pos), firstVisitFrameID(_frameID),firstVisitKeyPointIdx(_keypointIdx) {}
10 | 
11 | void MapPoint::addObservation(Frame* frame, unsigned int pointIdx) { 
12 | 	observations.insert(pair<Frame*, unsigned int>(frame, pointIdx)); 
13 | }
14 | 
15 | Point3f MapPoint::getPositionInCameraCoordinate(Mat rvec, Mat tvec){
16 | 	Eigen::Affine3d curTrans = vectorToTransformation(rvec, tvec);
17 | 	Eigen::Affine3d curTransInv = curTrans.inverse();
18 | 	return transformPoint(curTransInv, pos);;
19 | }
20 | 


--------------------------------------------------------------------------------
/include/MapPoint.h:
--------------------------------------------------------------------------------
 1 | #ifndef MAPPOINT_H
 2 | #define MAPPOINT_H
 3 | 
 4 | #include <opencv2/core/core.hpp>
 5 | #include <vector>
 6 | #include <map>
 7 | #include <stdio.h>
 8 | 
 9 | #include "Frame.h" 
10 | 
11 | class Frame;
12 | 
13 | class MapPoint{
14 | 
15 | public:
16 | 	MapPoint(cv::Point3f _pos, 
17 | 			 unsigned int _frameID, 
18 | 			 unsigned int _keypointIdx);
19 | 	void addObservation(Frame* frame, unsigned int pointIdx);
20 | 	cv::Point3f getPositionInCameraCoordinate(cv::Mat rvec, cv::Mat tvec);
21 | 	cv::Point3f pos; //pose in the first visited frame, in world coordinate
22 | 	unsigned int firstVisitFrameID;
23 | 	unsigned int firstVisitKeyPointIdx;
24 | 	bool isBad = false;
25 | 	std::map<Frame*, unsigned int> observations;
26 | };
27 | 
28 | #endif
29 | 


--------------------------------------------------------------------------------
/include/Mapviewer.h:
--------------------------------------------------------------------------------
 1 | #ifndef MAPVIEWER_H
 2 | #define MAPVIEWER_H
 3 | 
 4 | #include <pcl/io/pcd_io.h>
 5 | #include <pcl/point_types.h>
 6 | #include <pcl/visualization/cloud_viewer.h>
 7 | #include <pcl/common/transforms.h>
 8 | #include <pcl/filters/voxel_grid.h>
 9 | 
10 | #include <opencv2/core/core.hpp>
11 | 
12 | #include <stdio.h>
13 | #include <iostream>
14 | #include <fstream>
15 | #include <vector>
16 | 
17 | #include <Eigen/Core>
18 | #include <Eigen/Geometry>
19 | 
20 | class Mapviewer{
21 | 
22 | public:
23 | 	std::vector<pcl::PointCloud<pcl::PointXYZRGB> > allCameras;
24 | 	pcl::PointCloud<pcl::PointXYZRGB> templateCamera;
25 | 
26 | 
27 | 	pcl::PointCloud<pcl::PointXYZRGB> entireMap;// (new PointCloud<PointXYZ>);
28 | 	bool initialized = false;
29 | 
30 | 	Mapviewer();
31 | 	void jointToMap(pcl::PointCloud<pcl::PointXYZRGB> frameMap, Eigen::Affine3d& trans);
32 | 	pcl::PointCloud<pcl::PointXYZRGB> pointToPointCloud(std::vector<cv::Point3f> scenePts,
33 | 													 int R=255, int G=255, int B=255);
34 | 	
35 | 	void addMorePoints(pcl::PointCloud<pcl::PointXYZRGB> frameMap, Eigen::Affine3d& trans, bool downsample = false);
36 | 	void addCamera(Eigen::Affine3d& trans);
37 | };
38 | 
39 | #endif
40 | 


--------------------------------------------------------------------------------
/include/PoseOpt.h:
--------------------------------------------------------------------------------
 1 | #ifndef PoseOpt_h
 2 | #define PoseOpt_h
 3 | 
 4 | #include <stdio.h>
 5 | #include <iostream>
 6 | // #include <iomanip>
 7 | #include <time.h>
 8 | #include <string.h>
 9 | // #include <dirent.h>
10 | #include <fstream>
11 | 
12 | #include <g2o/types/slam3d/types_slam3d.h>
13 | #include <g2o/core/sparse_optimizer.h>
14 | #include <g2o/core/block_solver.h>
15 | #include <g2o/core/factory.h>
16 | #include <g2o/core/optimization_algorithm_factory.h>
17 | #include <g2o/core/optimization_algorithm_gauss_newton.h>
18 | #include <g2o/solvers/csparse/linear_solver_csparse.h>
19 | #include <g2o/core/robust_kernel.h>
20 | #include <g2o/core/robust_kernel_factory.h>
21 | #include <g2o/core/optimization_algorithm_levenberg.h>
22 | 
23 | #include "utils.h"
24 | #include "Frame.h"
25 | #include "MapPoint.h"
26 | 
27 | #include <Eigen/Core>
28 | #include <Eigen/Geometry>
29 | 
30 | 
31 | typedef g2o::BlockSolver_6_3 SlamBlockSolver; 
32 | typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 
33 | 
34 | 
35 | class PoseOpt {
36 | public:
37 | 	g2o::SparseOptimizer optimizer;
38 | 	g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Welsch" );
39 | 	PoseOpt();
40 | 	void addNode(const std::vector<Frame*>& frames, int idx);
41 | 	void addEdge(const std::vector<Frame*>& frames, int fromIdx);
42 | 	void solve();	
43 | };
44 | 
45 | 
46 | 
47 | 
48 | 
49 | 
50 | #endif
51 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | ## Synopsis
 2 | System is developed to achieve Visual Odometry for a moving car, using the output of a stereo camera. Rectified stereo images from a moving camera is used to estimate camera pose and reconstruct a 3D map of scene points. Our system is further optimized using bundle adjustment and pose-graph optimization. Developed model was used to estimate the trajectory of the camera for various scenarios in the KITTI dataset.
 3 | 
 4 | ## Results
 5 | 
 6 | Visual Odometry results for two sequences from the KITTI dataset.
 7 | 
 8 | <img src="https://raw.githubusercontent.com/akshayapurohit23/Stereo-Visual-Odometry/master/assets/Images/Seq06.png" width=350 height=275 align="middle" >     <img src="https://raw.githubusercontent.com/akshayapurohit23/Stereo-Visual-Odometry/master/assets/Images/Seq09.png" width=350 height=275 align="middle" >
 9 | 
10 | [![](https://img.youtube.com/vi/EFFH1OTh_IQ/0.jpg)](https://www.youtube.com/watch?v=EFFH1OTh_IQ)
11 | 
12 | ## Dependencies:
13 | ```
14 |   OpenCV2.4
15 |   PCL
16 |   g2o
17 | ```
18 | 
19 | ## Building Project in terminal:
20 | ```
21 |   mkdir build
22 |   cd build
23 |   cmake ..
24 |   make
25 | ```
26 | 
27 | ##  Running project
28 | ```
29 | cd <project_dir>
30 | ./vo PATH_TO_LEFT_IMAGE_SET_DIRECTORY PATH_TO_RIGHT_IMAGE_SET_DIRECTORY PATH_TO_YAML_FILE
31 | ```
32 | 
33 | ## Contributors
34 | ```
35 | Xiaoyu Zhou @ucsdxiaoyuzhou
36 | Akshaya Purohit @akshayapurohit23
37 | Bolun Zhang @zblzcj
38 | Leonard Melvix @lmelvix
39 | ```
40 | 


--------------------------------------------------------------------------------
/include/draw.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef draw_hpp
 2 | #define draw_hpp
 3 | 
 4 | #include <stdio.h>
 5 | #include <iostream>
 6 | #include <opencv2/core/core.hpp>
 7 | #include <opencv2/highgui/highgui.hpp>
 8 | #include <opencv2/imgproc/imgproc.hpp>
 9 | #include <opencv2/video/video.hpp>
10 | #include "opencv2/calib3d/calib3d.hpp"
11 | 
12 | 
13 | void drawFeature(cv::Mat img, std::vector<cv::Point2f> features, char* windowName);
14 | void drawFeature(cv::Mat img, std::vector<cv::KeyPoint> features, char* windowName);
15 | 
16 | void drawFarandCloseFeatures(cv::Mat img, std::vector<cv::Point2f> closePts, 
17 |                              std::vector<cv::Point2f> farPts, const char* windowName);
18 | 
19 | void drawFarandCloseFeatures(cv::Mat img, std::vector<cv::Point2f> pts, 
20 |                              std::vector<int> farIdx, char* windowName);
21 | void drawMatch(cv::Mat img_1,
22 |                std::vector<cv::KeyPoint> keypoints1,
23 |                std::vector<cv::KeyPoint> keypoints2,
24 |                std::vector<cv::DMatch> matches,
25 |                int radius,
26 |                char* windowName);
27 | 
28 | void drawMatch(cv::Mat img_1,
29 |                std::vector<cv::KeyPoint> keypoints1,
30 |                std::vector<cv::KeyPoint> keypoints2,
31 |                int radius,
32 |                char* windowName);
33 | 
34 | void drawMatch(cv::Mat img_1,
35 |                std::vector<cv::Point2f> p_keypoints1,
36 |                std::vector<cv::Point2f> p_keypoints2,
37 |                int radius,
38 |                char* windowName);
39 | 
40 | #endif /* draw_hpp */
41 | 


--------------------------------------------------------------------------------
/src/CMakeLists.txt~:
--------------------------------------------------------------------------------
 1 | FIND_PACKAGE( PCL REQUIRED)
 2 | FIND_PACKAGE( OpenCV 2.4 REQUIRED )
 3 | 
 4 | LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
 5 | 
 6 | #PCL
 7 | ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
 8 | INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
 9 | LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )
10 | 
11 | #g2o
12 | #SET( G2O_ROOT /usr/local/include/g2o )
13 | #FIND_PACKAGE( G2O REQUIRED )
14 | 
15 | #CSparse
16 | #FIND_PACKAGE( CSparse REQUIRED )
17 | #INCLUDE_DIRECTORIES( ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR} )
18 | 
19 | # Eigen
20 | FIND_PACKAGE( Eigen3 REQUIRED )
21 | include_directories( ${EIGEN3_INCLUDE_DIR})
22 | 
23 | ### CERES
24 | FIND_PACKAGE(Ceres REQUIRED)
25 | INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
26 | MESSAGE("ceres found ${Ceres_FOUND} at ${Ceres_DIR}")
27 | 
28 | ### GFLAGS
29 | FIND_PACKAGE(gflags REQUIRED)
30 | INCLUDE_DIRECTORIES(${GFLAGS_INCLUDE_DIRS})
31 | MESSAGE(${GFLAGS_INCLUDE_DIRS})
32 | MESSAGE("gflags found ${Gflags_FOUND} at ${GFLAGS_INCLUDE_DIRS} with namespace: ${GFLAGS_NAMESPACE}")
33 | 
34 | 
35 | #==build library for my own source code=====
36 | add_library (${PROJECT_NAME} SHARED 
37 | draw.cpp
38 | Frame.cpp
39 | Map.cpp
40 | MapPoint.cpp
41 | utils.cpp
42 | system.cpp
43 | Mapviewer.cpp
44 | optimizer.cpp
45 | )
46 | 
47 | target_link_libraries(${PROJECT_NAME}
48 | ${PCL_LIBRARIES} 
49 | ${OpenCV_LIBS}
50 | ${EIGEN3_LIBS}
51 | #g2o_core g2o_types_slam3d g2o_solver_csparse g2o_stuff g2o_csparse_extension ${CSPARSE_LIBRARY}
52 | ${CERES_LIBRARIES} 
53 | ${GFLAGS_LIBRARIES}
54 | )
55 | 
56 | #=======build main=========================
57 | ADD_EXECUTABLE(ba main.cpp )
58 | TARGET_LINK_LIBRARIES( 
59 | ba 
60 | ${PROJECT_NAME} 
61 | ${OpenCV_LIBS}
62 | ${PCL_LIBRARIES}
63 | #${CSPARSE_LIBRARY} 
64 | ${CERES_LIBRARIES} 
65 | ${GFLAGS_LIBRARIES}
66 | )
67 | 


--------------------------------------------------------------------------------
/src/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | FIND_PACKAGE( PCL REQUIRED)
 2 | FIND_PACKAGE( OpenCV 2.4 REQUIRED )
 3 | 
 4 | LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
 5 | 
 6 | #PCL
 7 | ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
 8 | INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
 9 | LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )
10 | 
11 | #g2o
12 | SET( G2O_ROOT /usr/local/include/g2o )
13 | FIND_PACKAGE( G2O REQUIRED )
14 | 
15 | #CSparse
16 | FIND_PACKAGE( CSparse REQUIRED )
17 | INCLUDE_DIRECTORIES( ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR} )
18 | 
19 | # Eigen
20 | FIND_PACKAGE( Eigen3 REQUIRED )
21 | include_directories( ${EIGEN3_INCLUDE_DIR})
22 | 
23 | ### CERES
24 | FIND_PACKAGE(Ceres REQUIRED)
25 | INCLUDE_DIRECTORIES(${CERES_INCLUDE_DIRS})
26 | MESSAGE("ceres found ${Ceres_FOUND} at ${Ceres_DIR}")
27 | 
28 | ### GFLAGS
29 | FIND_PACKAGE(gflags REQUIRED)
30 | INCLUDE_DIRECTORIES(${GFLAGS_INCLUDE_DIRS})
31 | MESSAGE(${GFLAGS_INCLUDE_DIRS})
32 | MESSAGE("gflags found ${Gflags_FOUND} at ${GFLAGS_INCLUDE_DIRS} with namespace: ${GFLAGS_NAMESPACE}")
33 | 
34 | 
35 | #==build library for my own source code=====
36 | add_library (${PROJECT_NAME} SHARED 
37 | PoseOpt.cpp
38 | draw.cpp
39 | Frame.cpp
40 | Map.cpp
41 | MapPoint.cpp
42 | utils.cpp
43 | system.cpp
44 | Mapviewer.cpp
45 | optimizer.cpp
46 | ORBextractor.cc
47 | )
48 | 
49 | target_link_libraries(${PROJECT_NAME}
50 | ${PCL_LIBRARIES} 
51 | ${OpenCV_LIBS}
52 | ${EIGEN3_LIBS}
53 | g2o_core g2o_types_slam3d g2o_solver_csparse g2o_stuff g2o_csparse_extension ${CSPARSE_LIBRARY}
54 | ${CERES_LIBRARIES} 
55 | ${GFLAGS_LIBRARIES}
56 | )
57 | 
58 | #=======build main=========================
59 | ADD_EXECUTABLE(ba main.cpp )
60 | TARGET_LINK_LIBRARIES( 
61 | ba 
62 | ${PROJECT_NAME} 
63 | ${OpenCV_LIBS}
64 | ${PCL_LIBRARIES}
65 | ${CSPARSE_LIBRARY} 
66 | ${CERES_LIBRARIES} 
67 | ${GFLAGS_LIBRARIES}
68 | )
69 | 


--------------------------------------------------------------------------------
/cmake_modules/FindQGLViewer.cmake:
--------------------------------------------------------------------------------
 1 | # Need to find both Qt{4,5} and QGLViewer if the QQL support is to be built
 2 | FIND_PACKAGE(Qt4 COMPONENTS QtCore QtXml QtOpenGL QtGui)
 3 | IF(NOT Qt4_FOUND)
 4 | 	FIND_PACKAGE(Qt5 QUIET COMPONENTS Core Xml OpenGL Gui Widgets)
 5 | 	IF(NOT Qt4_FOUND AND NOT Qt5_FOUND)
 6 | 		MESSAGE("Qt{4,5} not found. Install it and set Qt{4,5}_DIR accordingly")
 7 | 		IF (WIN32)
 8 | 			MESSAGE("  In Windows, Qt5_DIR should be something like C:/Qt/5.4/msvc2013_64_opengl/lib/cmake/Qt5")
 9 | 		ENDIF()
10 | 	ENDIF()
11 | ENDIF()
12 | 
13 | FIND_PATH(QGLVIEWER_INCLUDE_DIR qglviewer.h
14 |     /usr/include/QGLViewer
15 |     /opt/local/include/QGLViewer
16 |     /usr/local/include/QGLViewer
17 |     /sw/include/QGLViewer
18 |     ENV QGLVIEWERROOT
19 |   )
20 | 
21 | find_library(QGLVIEWER_LIBRARY_RELEASE
22 |   NAMES qglviewer-qt4 qglviewer QGLViewer QGLViewer2
23 |   PATHS /usr/lib
24 |         /usr/local/lib
25 |         /opt/local/lib
26 |         /sw/lib
27 |         ENV QGLVIEWERROOT
28 |         ENV LD_LIBRARY_PATH
29 |         ENV LIBRARY_PATH
30 |   PATH_SUFFIXES QGLViewer QGLViewer/release
31 | )
32 | find_library(QGLVIEWER_LIBRARY_DEBUG
33 |   NAMES dqglviewer dQGLViewer dQGLViewer2 QGLViewerd2
34 |   PATHS /usr/lib
35 |         /usr/local/lib
36 |         /opt/local/lib
37 |         /sw/lib
38 |         ENV QGLVIEWERROOT
39 |         ENV LD_LIBRARY_PATH
40 |         ENV LIBRARY_PATH
41 |   PATH_SUFFIXES QGLViewer QGLViewer/release
42 | )
43 | 
44 | if(QGLVIEWER_LIBRARY_RELEASE)
45 |   if(QGLVIEWER_LIBRARY_DEBUG)
46 |     set(QGLVIEWER_LIBRARY optimized ${QGLVIEWER_LIBRARY_RELEASE} debug ${QGLVIEWER_LIBRARY_DEBUG})
47 |   else()
48 |     set(QGLVIEWER_LIBRARY ${QGLVIEWER_LIBRARY_RELEASE})
49 |   endif()
50 | endif()
51 | 
52 | include(FindPackageHandleStandardArgs)
53 | find_package_handle_standard_args(QGLVIEWER DEFAULT_MSG
54 |   QGLVIEWER_INCLUDE_DIR QGLVIEWER_LIBRARY)
55 | 


--------------------------------------------------------------------------------
/include/optimizer.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef optimizer_hpp
 2 | #define optimizer_hpp
 3 | 
 4 | #include <stdio.h>
 5 | #include <iostream>
 6 | #include <iomanip>
 7 | #include <time.h>
 8 | #include <string.h>
 9 | #include <dirent.h>
10 | #include <fstream>
11 | #include <algorithm>
12 | #include <unordered_set>
13 | 
14 | //ceres
15 | #include "ceres/ceres.h"
16 | #include "glog/logging.h"
17 | #include "gflags/gflags.h"
18 | #include <ceres/local_parameterization.h>
19 | #include <ceres/autodiff_local_parameterization.h>
20 | #include <ceres/autodiff_cost_function.h>
21 | #include <ceres/types.h>
22 | #include <ceres/rotation.h>
23 | #include "snavely_reprojection_error.h"
24 | #include "bal_problem.h"
25 | 
26 | using ceres::AutoDiffCostFunction;
27 | using ceres::CostFunction;
28 | using ceres::CauchyLoss;
29 | using ceres::Problem;
30 | using ceres::Solve;
31 | using ceres::Solver;
32 | 
33 | #include <Eigen/Core>
34 | #include <Eigen/Geometry>
35 | 
36 | #include <opencv2/core/core.hpp>
37 | #include "opencv2/calib3d/calib3d.hpp"
38 | 
39 | #include "Frame.h"
40 | #include "Map.h"
41 | #include "MapPoint.h"
42 | #include "draw.hpp"
43 | #include "utils.h"
44 | 
45 | using namespace std;
46 | using namespace ceres;
47 | using namespace examples;
48 | 
49 | class Frame;
50 | class Map;
51 | class MapPoint;
52 | 
53 | class Optimizer{
54 | private:
55 |     Solver::Options options;
56 |     Solver::Summary summary;
57 |     const int pointBlkSize  = 3;
58 |     const int cameraBlkSize = 6;
59 | 
60 | public:
61 |     Problem& globalBAProblem;
62 | 	Optimizer(bool printOut, Problem& prob);
63 | 
64 |     void globalBundleAdjustment(Map* map, vector<Frame*> frames);
65 |     void localBundleAdjustment(vector<Frame*> frames, int startIdx, int length);
66 | 	void poseOptimization(vector<Frame* >& frames);
67 |     void reprojectionOnlyAdjustment(Map* map, vector<Frame*> frames);
68 | 	void solveProblem(Problem& pb);
69 | };
70 | 
71 | 
72 | #endif
73 | 


--------------------------------------------------------------------------------
/src/Mapviewer.cpp:
--------------------------------------------------------------------------------
 1 | #include "Mapviewer.h"
 2 | 
 3 | using namespace pcl;
 4 | using namespace std;
 5 | 
 6 | Mapviewer::Mapviewer(){
 7 | 	float cameraSize = 0.1;
 8 | 	for(float x = -cameraSize; x < cameraSize; x+=(cameraSize/2)){
 9 | 		for(float y = -cameraSize; y < cameraSize; y+=(cameraSize/2)){
10 | 			for(float z = -cameraSize; z < cameraSize; z+=(cameraSize/2)){
11 | 				PointXYZRGB pt;
12 | 				pt.x = x;
13 | 				pt.y = y;
14 | 				pt.z = z;
15 | 				pt.r = 255;
16 | 				pt.g = 0;
17 | 				pt.b = 0;
18 | 				templateCamera.push_back(pt);
19 | 			}}}
20 | }
21 | 
22 | pcl::PointCloud<pcl::PointXYZRGB> Mapviewer::pointToPointCloud(std::vector<cv::Point3f> scenePts, 
23 | 														    int R, int G, int B){
24 | 	int ptsNum = scenePts.size();
25 | 	PointCloud<PointXYZRGB> result;
26 | 	for(int n = 0; n < ptsNum; n++){
27 | 		PointXYZRGB pt;
28 | 		pt.x = scenePts[n].x;
29 | 		pt.y = scenePts[n].y;
30 | 		pt.z = scenePts[n].z;
31 | 		pt.r = R;
32 | 		pt.g = G;
33 | 		pt.b = B;
34 | 		result.points.push_back(pt);
35 | 	}
36 | 
37 | 	result.height = 1;
38 | 	result.width = result.points.size();
39 | 	return result;
40 | }
41 | 
42 | 
43 | void Mapviewer::jointToMap(PointCloud<PointXYZRGB> frameMap, Eigen::Affine3d& trans){
44 | 	if(false == initialized){
45 | 		initialized = true;
46 | 		cout << "map initializing! " << endl;
47 | 		entireMap = frameMap;
48 | 		cout << "map initialized!" << endl;
49 | 	}
50 | 	entireMap = frameMap;
51 | 	for(auto cam : allCameras)
52 | 		entireMap += cam;
53 | }
54 | 
55 | void Mapviewer::addMorePoints(PointCloud<PointXYZRGB> frameMap, Eigen::Affine3d& trans, bool downsample){
56 | 	if(false == initialized){
57 | 		initialized = true;
58 | 		cout << "map initializing! " << endl;
59 | 		entireMap = frameMap;
60 | 		cout << "map initialized!" << endl;
61 | 	}
62 | 	if(downsample == true){
63 | 		PointCloud<PointXYZRGB>::Ptr cloud  (new PointCloud<PointXYZRGB>);
64 | 		*cloud = frameMap;
65 | 		pcl::VoxelGrid<pcl::PointXYZRGB> sor;
66 |         sor.setInputCloud (cloud);
67 |         sor.setLeafSize (0.3f, 0.3f, 0.3f);
68 |         sor.filter (*cloud);
69 |         frameMap = *cloud;
70 | 	}
71 | 	entireMap += frameMap;
72 | }
73 | 
74 | void Mapviewer::addCamera(Eigen::Affine3d& trans){
75 | 	//generata a set of points
76 | 	Eigen::Affine3d invTrans = trans.inverse();
77 | 	PointCloud<PointXYZRGB> curCamera;
78 | 	transformPointCloud(templateCamera, curCamera, invTrans);
79 | 	allCameras.push_back(curCamera);
80 | }
81 | 


--------------------------------------------------------------------------------
/include/random.h:
--------------------------------------------------------------------------------
 1 | // Ceres Solver - A fast non-linear least squares minimizer
 2 | // Copyright 2015 Google Inc. All rights reserved.
 3 | // http://ceres-solver.org/
 4 | //
 5 | // Redistribution and use in source and binary forms, with or without
 6 | // modification, are permitted provided that the following conditions are met:
 7 | //
 8 | // * Redistributions of source code must retain the above copyright notice,
 9 | //   this list of conditions and the following disclaimer.
10 | // * Redistributions in binary form must reproduce the above copyright notice,
11 | //   this list of conditions and the following disclaimer in the documentation
12 | //   and/or other materials provided with the distribution.
13 | // * Neither the name of Google Inc. nor the names of its contributors may be
14 | //   used to endorse or promote products derived from this software without
15 | //   specific prior written permission.
16 | //
17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 | // POSSIBILITY OF SUCH DAMAGE.
28 | //
29 | // Author: sameeragarwal@google.com (Sameer Agarwal)
30 | #ifndef CERES_EXAMPLES_RANDOM_H_
31 | #define CERES_EXAMPLES_RANDOM_H_
32 | #include <math.h>
33 | #include <stdlib.h>
34 | namespace ceres {
35 | namespace examples {
36 | // Return a random number sampled from a uniform distribution in the range
37 | // [0,1].
38 | inline double RandDouble() {
39 |   double r = static_cast<double>(rand());
40 |   return r / RAND_MAX;
41 | }
42 | // Marsaglia Polar method for generation standard normal (pseudo)
43 | // random numbers http://en.wikipedia.org/wiki/Marsaglia_polar_method
44 | inline double RandNormal() {
45 |   double x1, x2, w;
46 |   do {
47 |     x1 = 2.0 * RandDouble() - 1.0;
48 |     x2 = 2.0 * RandDouble() - 1.0;
49 |     w = x1 * x1 + x2 * x2;
50 |   } while ( w >= 1.0 || w == 0.0 );
51 |   w = sqrt((-2.0 * log(w)) / w);
52 |   return x1 * w;
53 | }
54 | }  // namespace examples
55 | }  // namespace ceres
56 | #endif  // CERES_EXAMPLES_RANDOM_H_


--------------------------------------------------------------------------------
/cmake_modules/FindCholmod.cmake:
--------------------------------------------------------------------------------
 1 | # Cholmod lib usually requires linking to a blas and lapack library.
 2 | # It is up to the user of this module to find a BLAS and link to it.
 3 | 
 4 | if (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
 5 |   set(CHOLMOD_FIND_QUIETLY TRUE)
 6 | endif (CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
 7 | 
 8 | find_path(CHOLMOD_INCLUDE_DIR
 9 |   NAMES
10 |   cholmod.h
11 |   PATHS
12 |   $ENV{CHOLMODDIR}
13 |   ${INCLUDE_INSTALL_DIR}
14 |   PATH_SUFFIXES
15 |   suitesparse
16 |   ufsparse
17 | )
18 | 
19 | find_library(CHOLMOD_LIBRARY cholmod PATHS $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
20 | set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY})
21 | 
22 | if(CHOLMOD_LIBRARIES)
23 | 
24 |   get_filename_component(CHOLMOD_LIBDIR ${CHOLMOD_LIBRARIES} PATH)
25 | 
26 |   find_library(AMD_LIBRARY amd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
27 |   if (AMD_LIBRARY)
28 |     set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${AMD_LIBRARY})
29 |   else ()
30 |     set(CHOLMOD_LIBRARIES FALSE)
31 |   endif ()
32 | 
33 | endif(CHOLMOD_LIBRARIES)
34 | 
35 | if(CHOLMOD_LIBRARIES)
36 | 
37 |   find_library(COLAMD_LIBRARY colamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
38 |   if (COLAMD_LIBRARY)
39 |     set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${COLAMD_LIBRARY})
40 |   else ()
41 |     set(CHOLMOD_LIBRARIES FALSE)
42 |   endif ()
43 | 
44 | endif(CHOLMOD_LIBRARIES)
45 | 
46 | if(CHOLMOD_LIBRARIES)
47 | 
48 |   find_library(CAMD_LIBRARY camd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
49 |   if (CAMD_LIBRARY)
50 |     set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CAMD_LIBRARY})
51 |   else ()
52 |     set(CHOLMOD_LIBRARIES FALSE)
53 |   endif ()
54 | 
55 | endif(CHOLMOD_LIBRARIES)
56 | 
57 | if(CHOLMOD_LIBRARIES)
58 | 
59 |   find_library(CCOLAMD_LIBRARY ccolamd PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
60 |   if (CCOLAMD_LIBRARY)
61 |     set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CCOLAMD_LIBRARY})
62 |   else ()
63 |     set(CHOLMOD_LIBRARIES FALSE)
64 |   endif ()
65 | 
66 | endif(CHOLMOD_LIBRARIES)
67 | 
68 | if(CHOLMOD_LIBRARIES)
69 | 
70 |   find_library(CHOLMOD_METIS_LIBRARY metis PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
71 |   if (CHOLMOD_METIS_LIBRARY)
72 |     set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_METIS_LIBRARY})
73 |   endif ()
74 | 
75 | endif(CHOLMOD_LIBRARIES)
76 | 
77 | if(CHOLMOD_LIBRARIES)
78 |   find_library(CHOLMOD_SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
79 |     PATHS ${CHOLMOD_LIBDIR} $ENV{CHOLMODDIR} ${LIB_INSTALL_DIR})
80 |   if (CHOLMOD_SUITESPARSECONFIG_LIBRARY)
81 |     set(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARIES} ${CHOLMOD_SUITESPARSECONFIG_LIBRARY})
82 |   endif ()
83 | endif(CHOLMOD_LIBRARIES)
84 | 
85 | include(FindPackageHandleStandardArgs)
86 | find_package_handle_standard_args(CHOLMOD DEFAULT_MSG
87 |                                   CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES)
88 | 
89 | mark_as_advanced(CHOLMOD_LIBRARIES)
90 | 


--------------------------------------------------------------------------------
/src/PoseOpt.cpp:
--------------------------------------------------------------------------------
 1 | #include "PoseOpt.h"
 2 | 
 3 | using namespace g2o;
 4 | 
 5 | PoseOpt::PoseOpt(){
 6 | 	SlamLinearSolver* linearSolver = new SlamLinearSolver();
 7 |     linearSolver->setBlockOrdering( false );
 8 |     SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
 9 |     g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );
10 |     //==============================================
11 |     optimizer.setAlgorithm( solver ); //the one will be used all the time
12 |     optimizer.setVerbose( true );
13 | 
14 |     //add the first vertex
15 |     g2o::VertexSE3* v = new g2o::VertexSE3();
16 |     v->setId(0);
17 |     v->setEstimate(Eigen::Isometry3d::Identity());
18 |     v->setFixed( true ); //fix the first vertex
19 | 	optimizer.addVertex(v);
20 | }
21 | 
22 | void PoseOpt::addNode(const vector<Frame*>& frames, int idx){
23 | 	if(0 == idx){return;}
24 | 
25 | 	VertexSE3 *v = new VertexSE3();
26 | 
27 | 	Eigen::Affine3d T = vectorToTransformation(frames[idx]->worldRvec, frames[idx]->worldTvec);
28 | 	Eigen::Affine3d invT = T.inverse();
29 | 
30 | 	Eigen::Isometry3d _invT;
31 | 	for(int r = 0; r < 4; r++){
32 | 		for(int c = 0; c < 4; c++){
33 | 			_invT(r,c) = invT(r,c);
34 | 		}
35 | 	}
36 | 	v->setId(frames[idx]->frameID);
37 | 	v->setEstimate(_invT);
38 | 	optimizer.addVertex(v);
39 | }
40 | 
41 | void PoseOpt::addEdge(const vector<Frame*>& frames, int fromIdx){
42 | 
43 | 	for(edgeConstrain::iterator eIt = frames[fromIdx]->relativePose.begin();
44 | 								eIt!= frames[fromIdx]->relativePose.end();
45 | 								eIt++) {
46 | 		Eigen::Affine3d T = vectorToTransformation((*eIt).second.first, (*eIt).second.second);
47 | 		Eigen::Isometry3d _T;
48 | 
49 | 		for(int r = 0; r < 4; r++){
50 | 			for(int c = 0; c < 4; c++){
51 | 				_T(r,c) = T(r,c);
52 | 			}
53 | 		}
54 | 
55 | 		EdgeSE3* edge = new EdgeSE3();
56 | 		edge->vertices()[0] = optimizer.vertex(frames[fromIdx]->frameID);
57 | 		edge->vertices()[1] = optimizer.vertex((*eIt).first);
58 | 
59 | 		//information matrix
60 | 	    Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
61 | 	    information(0,0) = information(1,1) = information(2,2) = 80;
62 | 	    information(3,3) = information(4,4) = information(5,5) = 110;
63 | 		edge->setInformation(information);
64 | 		edge->setMeasurement(_T.inverse());
65 | 		optimizer.addEdge(edge);
66 | 		cout << "added new edge from " << frames[fromIdx]->frameID << " to " << (*eIt).first << endl;
67 | 	} 
68 | 
69 | }
70 | 
71 | void PoseOpt::solve() {
72 | 	cout << "optimizing pose graph, vertice numbers: " << optimizer.vertices().size() << endl;
73 | 	optimizer.save("../pose_graph/before_full_optimize.g2o");    
74 | 	optimizer.computeActiveErrors();
75 |     cout << "Initial chi2 = " << optimizer.chi2() << endl;
76 | 	optimizer.initializeOptimization();
77 | 	optimizer.optimize(25);
78 | 	optimizer.save("../pose_graph/after_full_optimize.g2o");
79 | 	cout<<"Optimization done."<<endl;
80 | }
81 | 


--------------------------------------------------------------------------------
/include/Frame.h:
--------------------------------------------------------------------------------
 1 | #ifndef FRAME_H
 2 | #define FRAME_H
 3 | 
 4 | #include "MapPoint.h"
 5 | #include "Map.h"
 6 | #include "utils.h"
 7 | #include "ORBextractor.h"
 8 | 
 9 | #include <vector>
10 | #include <unordered_map>
11 | #include <stdio.h>
12 | #include <limits>
13 | 
14 | #include "opencv2/core/core.hpp"
15 | #include "opencv2/nonfree/nonfree.hpp"
16 | #include "opencv2/nonfree/features2d.hpp"
17 | #include <opencv2/features2d/features2d.hpp>
18 | 
19 | typedef std::unordered_map<unsigned int, std::pair<cv::Mat, cv::Mat> > edgeConstrain;
20 | 
21 | class Map;
22 | class MapPoint;
23 | class Frame{
24 | public:
25 | 	unsigned int frameID;
26 | 	STEREO_RECTIFY_PARAMS srp;
27 | 	cv::Mat K;
28 | 	double fx;
29 | 	double fy;
30 | 	double cx;
31 | 	double cy;
32 | 	double b;
33 | 
34 | 	cv::Mat imgL, imgR;
35 | 	cv::Mat despL, despR;
36 | 
37 | 	cv::Mat rvec, tvec;
38 | 	cv::Mat worldRvec, worldTvec;
39 | 
40 | 	std::vector<cv::KeyPoint> keypointL, keypointR;
41 | 	std::vector<cv::Point3f> scenePts;        //scene points in the current frame coordinate
42 | 	std::vector<cv::Point3f> scenePtsinWorld; //scene points in the world corrdinate
43 | 	
44 | 	std::vector<cv::DMatch> matchesBetweenFrame;
45 | 
46 | 	edgeConstrain relativePose; //record relative transformation
47 | 	std::vector<MapPoint*> mappoints; // 3d points in world coordinate
48 | 	std::vector<bool> originality;
49 | 	Map* map;
50 | 
51 | public:
52 | 	Frame(string leftImgFile, string rightImgFile,
53 | 		  STEREO_RECTIFY_PARAMS _srp, int _id, Map* _map);
54 | 
55 | 	void matchFrame(Frame* frame, bool useMappoints = false);
56 | 	void addEdgeConstrain(unsigned int id, cv::Mat relativeRvec, cv::Mat relativeTvec);
57 | 	void manageMapPoints(Frame* frame);
58 | 
59 | 	void setWrdTransVectorAndTransScenePts(cv::Mat _worldRvec, cv::Mat _worldTvec);
60 | 	void transformScenePtsToWorldCoordinate();
61 |     
62 |     void matchFeatureKNN(const cv::Mat& desp1, const cv::Mat& desp2, 
63 |                             const std::vector<cv::KeyPoint>& keypoint1, 
64 |                             const std::vector<cv::KeyPoint>& keypoint2,
65 |                             std::vector<cv::KeyPoint>& matchedKeypoint1,
66 |                             std::vector<cv::KeyPoint>& matchedKeypoint2,
67 |                             std::vector<cv::DMatch>& matches,
68 |                             double knn_match_ratio = 0.8,
69 |                             bool hamming = false);
70 | 
71 |     void compute3Dpoints(std::vector<cv::KeyPoint>& kl, 
72 | 					 	 std::vector<cv::KeyPoint>& kr,
73 | 					 	 std::vector<cv::KeyPoint>& trikl,
74 | 					 	 std::vector<cv::KeyPoint>& trikr,
75 | 					 	 std::vector<int>& inliers);
76 | 
77 | 	void PnP(std::vector<cv::Point3f> obj_pts, 
78 | 	                std::vector<cv::Point2f> img_pts,
79 | 	                cv::Mat& inliers);
80 |     void judgeBadPoints();
81 |     void judgeBadPointsKdTree();
82 | 	MapPoint* createNewMapPoint(unsigned int pointIdx);
83 | 	void pointToExistingMapPoint(Frame* frame, MapPoint* mp, unsigned int currIdx);
84 | 	Eigen::Affine3d getWorldTransformationMatrix();
85 | 
86 | 
87 |     void releaseMemory();
88 | };
89 | 	
90 | 
91 | #endif


--------------------------------------------------------------------------------
/cmake_modules/FindSuiteSparse.cmake:
--------------------------------------------------------------------------------
  1 | FIND_PATH(CHOLMOD_INCLUDE_DIR NAMES cholmod.h amd.h camd.h
  2 |     PATHS
  3 |     ${SUITE_SPARSE_ROOT}/include
  4 |     /usr/include/suitesparse
  5 |     /usr/include/ufsparse
  6 |     /opt/local/include/ufsparse
  7 |     /usr/local/include/ufsparse
  8 |     /sw/include/ufsparse
  9 |   )
 10 | 
 11 | FIND_LIBRARY(CHOLMOD_LIBRARY NAMES cholmod
 12 |      PATHS
 13 |      ${SUITE_SPARSE_ROOT}/lib
 14 |      /usr/lib
 15 |      /usr/local/lib
 16 |      /opt/local/lib
 17 |      /sw/lib
 18 |    )
 19 | 
 20 | FIND_LIBRARY(AMD_LIBRARY NAMES SHARED NAMES amd
 21 |   PATHS
 22 |   ${SUITE_SPARSE_ROOT}/lib
 23 |   /usr/lib
 24 |   /usr/local/lib
 25 |   /opt/local/lib
 26 |   /sw/lib
 27 |   )
 28 | 
 29 | FIND_LIBRARY(CAMD_LIBRARY NAMES camd
 30 |   PATHS
 31 |   ${SUITE_SPARSE_ROOT}/lib
 32 |   /usr/lib
 33 |   /usr/local/lib
 34 |   /opt/local/lib
 35 |   /sw/lib
 36 |   )
 37 | 
 38 | FIND_LIBRARY(SUITESPARSECONFIG_LIBRARY NAMES suitesparseconfig
 39 |   PATHS
 40 |   ${SUITE_SPARSE_ROOT}/lib
 41 |   /usr/lib
 42 |   /usr/local/lib
 43 |   /opt/local/lib
 44 |   /sw/lib
 45 |   )
 46 | 
 47 | 
 48 | # Different platforms seemingly require linking against different sets of libraries
 49 | IF(CYGWIN)
 50 |   FIND_PACKAGE(PkgConfig)
 51 |   FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd
 52 |     PATHS
 53 |     /usr/lib
 54 |     /usr/local/lib
 55 |     /opt/local/lib
 56 |     /sw/lib
 57 |     )
 58 |   PKG_CHECK_MODULES(LAPACK lapack)
 59 | 
 60 |   SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${LAPACK_LIBRARIES})
 61 | 
 62 | # MacPorts build of the SparseSuite requires linking against extra libraries
 63 | 
 64 | ELSEIF(APPLE)
 65 | 
 66 |   FIND_LIBRARY(COLAMD_LIBRARY NAMES colamd
 67 |     PATHS
 68 |     /usr/lib
 69 |     /usr/local/lib
 70 |     /opt/local/lib
 71 |     /sw/lib
 72 |     )
 73 | 
 74 |   FIND_LIBRARY(CCOLAMD_LIBRARY NAMES ccolamd
 75 |     PATHS
 76 |     /usr/lib
 77 |     /usr/local/lib
 78 |     /opt/local/lib
 79 |     /sw/lib
 80 |     )
 81 | 
 82 |   FIND_LIBRARY(METIS_LIBRARY NAMES metis
 83 |     PATHS
 84 |     /usr/lib
 85 |     /usr/local/lib
 86 |     /opt/local/lib
 87 |     /sw/lib
 88 |     )
 89 | 
 90 |   SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY} ${CAMD_LIBRARY} ${COLAMD_LIBRARY} ${CCOLAMD_LIBRARY} ${METIS_LIBRARY} "-framework Accelerate")
 91 | ELSE(APPLE)
 92 |   SET(CHOLMOD_LIBRARIES ${CHOLMOD_LIBRARY} ${AMD_LIBRARY})
 93 | ENDIF(CYGWIN)
 94 | 
 95 | IF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
 96 |   SET(CHOLMOD_FOUND TRUE)
 97 | ELSE(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
 98 |   SET(CHOLMOD_FOUND FALSE)
 99 | ENDIF(CHOLMOD_INCLUDE_DIR AND CHOLMOD_LIBRARIES)
100 | 
101 | # Look for csparse; note the difference in the directory specifications!
102 | FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
103 |   PATHS
104 |   /usr/include/suitesparse
105 |   /usr/include
106 |   /opt/local/include
107 |   /usr/local/include
108 |   /sw/include
109 |   /usr/include/ufsparse
110 |   /opt/local/include/ufsparse
111 |   /usr/local/include/ufsparse
112 |   /sw/include/ufsparse
113 |   )
114 | 
115 | FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
116 |   PATHS
117 |   /usr/lib
118 |   /usr/local/lib
119 |   /opt/local/lib
120 |   /sw/lib
121 |   )
122 | 
123 | IF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
124 |   SET(CSPARSE_FOUND TRUE)
125 | ELSE(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
126 |   SET(CSPARSE_FOUND FALSE)
127 | ENDIF(CSPARSE_INCLUDE_DIR AND CSPARSE_LIBRARY)
128 | 


--------------------------------------------------------------------------------
/cmake_modules/FindEigen3.cmake:
--------------------------------------------------------------------------------
 1 | # - Try to find Eigen3 lib
 2 | #
 3 | # This module supports requiring a minimum version, e.g. you can do
 4 | #   find_package(Eigen3 3.1.2)
 5 | # to require version 3.1.2 or newer of Eigen3.
 6 | #
 7 | # Once done this will define
 8 | #
 9 | #  EIGEN3_FOUND - system has eigen lib with correct version
10 | #  EIGEN3_INCLUDE_DIR - the eigen include directory
11 | #  EIGEN3_VERSION - eigen version
12 | 
13 | # Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
14 | # Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
15 | # Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license.
17 | 
18 | if(NOT Eigen3_FIND_VERSION)
19 |   if(NOT Eigen3_FIND_VERSION_MAJOR)
20 |     set(Eigen3_FIND_VERSION_MAJOR 2)
21 |   endif(NOT Eigen3_FIND_VERSION_MAJOR)
22 |   if(NOT Eigen3_FIND_VERSION_MINOR)
23 |     set(Eigen3_FIND_VERSION_MINOR 91)
24 |   endif(NOT Eigen3_FIND_VERSION_MINOR)
25 |   if(NOT Eigen3_FIND_VERSION_PATCH)
26 |     set(Eigen3_FIND_VERSION_PATCH 0)
27 |   endif(NOT Eigen3_FIND_VERSION_PATCH)
28 | 
29 |   set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
30 | endif(NOT Eigen3_FIND_VERSION)
31 | 
32 | macro(_eigen3_check_version)
33 |   file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
34 | 
35 |   string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
36 |   set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
37 |   string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
38 |   set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
39 |   string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
40 |   set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
41 | 
42 |   set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
43 |   if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
44 |     set(EIGEN3_VERSION_OK FALSE)
45 |   else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
46 |     set(EIGEN3_VERSION_OK TRUE)
47 |   endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
48 | 
49 |   if(NOT EIGEN3_VERSION_OK)
50 | 
51 |     message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
52 |                    "but at least version ${Eigen3_FIND_VERSION} is required")
53 |   endif(NOT EIGEN3_VERSION_OK)
54 | endmacro(_eigen3_check_version)
55 | 
56 | if (EIGEN3_INCLUDE_DIR)
57 | 
58 |   # in cache already
59 |   _eigen3_check_version()
60 |   set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
61 | 
62 | else (EIGEN3_INCLUDE_DIR)
63 | 
64 |   # specific additional paths for some OS
65 |   if (WIN32)
66 |     set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include")
67 |   endif(WIN32)
68 | 
69 |   find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
70 |       PATHS
71 |       ${CMAKE_INSTALL_PREFIX}/include
72 |       ${EIGEN_ADDITIONAL_SEARCH_PATHS}
73 |       ${KDE4_INCLUDE_DIR}
74 |       PATH_SUFFIXES eigen3 eigen
75 |     )
76 | 
77 |   if(EIGEN3_INCLUDE_DIR)
78 |     _eigen3_check_version()
79 |   endif(EIGEN3_INCLUDE_DIR)
80 | 
81 |   include(FindPackageHandleStandardArgs)
82 |   find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
83 | 
84 |   mark_as_advanced(EIGEN3_INCLUDE_DIR)
85 | 
86 | endif(EIGEN3_INCLUDE_DIR)
87 | 
88 | 


--------------------------------------------------------------------------------
/include/ORBextractor.h:
--------------------------------------------------------------------------------
  1 | /**
  2 | * This file is part of ORB-SLAM2.
  3 | *
  4 | * Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
  5 | * For more information see <https://github.com/raulmur/ORB_SLAM2>
  6 | *
  7 | * ORB-SLAM2 is free software: you can redistribute it and/or modify
  8 | * it under the terms of the GNU General Public License as published by
  9 | * the Free Software Foundation, either version 3 of the License, or
 10 | * (at your option) any later version.
 11 | *
 12 | * ORB-SLAM2 is distributed in the hope that it will be useful,
 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of
 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 15 | * GNU General Public License for more details.
 16 | *
 17 | * You should have received a copy of the GNU General Public License
 18 | * along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
 19 | */
 20 | 
 21 | #ifndef ORBEXTRACTOR_H
 22 | #define ORBEXTRACTOR_H
 23 | 
 24 | #include <vector>
 25 | #include <list>
 26 | #include <opencv/cv.h>
 27 | 
 28 | 
 29 | namespace ORB_SLAM2
 30 | {
 31 | 
 32 | class ExtractorNode
 33 | {
 34 | public:
 35 |     ExtractorNode():bNoMore(false){}
 36 | 
 37 |     void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4);
 38 | 
 39 |     std::vector<cv::KeyPoint> vKeys;
 40 |     cv::Point2i UL, UR, BL, BR;
 41 |     std::list<ExtractorNode>::iterator lit;
 42 |     bool bNoMore;
 43 | };
 44 | 
 45 | class ORBextractor
 46 | {
 47 | public:
 48 |     
 49 |     enum {HARRIS_SCORE=0, FAST_SCORE=1 };
 50 | 
 51 |     ORBextractor(int nfeatures, float scaleFactor, int nlevels,
 52 |                  int iniThFAST, int minThFAST);
 53 | 
 54 |     ~ORBextractor(){}
 55 | 
 56 |     // Compute the ORB features and descriptors on an image.
 57 |     // ORB are dispersed on the image using an octree.
 58 |     // Mask is ignored in the current implementation.
 59 |     void operator()( cv::InputArray image, cv::InputArray mask,
 60 |       std::vector<cv::KeyPoint>& keypoints,
 61 |       cv::OutputArray descriptors);
 62 | 
 63 |     int inline GetLevels(){
 64 |         return nlevels;}
 65 | 
 66 |     float inline GetScaleFactor(){
 67 |         return scaleFactor;}
 68 | 
 69 |     std::vector<float> inline GetScaleFactors(){
 70 |         return mvScaleFactor;
 71 |     }
 72 | 
 73 |     std::vector<float> inline GetInverseScaleFactors(){
 74 |         return mvInvScaleFactor;
 75 |     }
 76 | 
 77 |     std::vector<float> inline GetScaleSigmaSquares(){
 78 |         return mvLevelSigma2;
 79 |     }
 80 | 
 81 |     std::vector<float> inline GetInverseScaleSigmaSquares(){
 82 |         return mvInvLevelSigma2;
 83 |     }
 84 | 
 85 |     std::vector<cv::Mat> mvImagePyramid;
 86 | 
 87 | protected:
 88 | 
 89 |     void ComputePyramid(cv::Mat image);
 90 |     void ComputeKeyPointsOctTree(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);    
 91 |     std::vector<cv::KeyPoint> DistributeOctTree(const std::vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
 92 |                                            const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level);
 93 | 
 94 |     void ComputeKeyPointsOld(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
 95 |     std::vector<cv::Point> pattern;
 96 | 
 97 |     int nfeatures;
 98 |     double scaleFactor;
 99 |     int nlevels;
100 |     int iniThFAST;
101 |     int minThFAST;
102 | 
103 |     std::vector<int> mnFeaturesPerLevel;
104 | 
105 |     std::vector<int> umax;
106 | 
107 |     std::vector<float> mvScaleFactor;
108 |     std::vector<float> mvInvScaleFactor;    
109 |     std::vector<float> mvLevelSigma2;
110 |     std::vector<float> mvInvLevelSigma2;
111 | };
112 | 
113 | } //namespace ORB_SLAM
114 | 
115 | #endif
116 | 
117 | 


--------------------------------------------------------------------------------
/cmake_modules/FindG2O.cmake:
--------------------------------------------------------------------------------
  1 | # Find the header files
  2 | 
  3 | FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
  4 |   ${G2O_ROOT}/include
  5 |   $ENV{G2O_ROOT}/include
  6 |   $ENV{G2O_ROOT}
  7 |   /usr/local/include
  8 |   /usr/include
  9 |   /opt/local/include
 10 |   /sw/local/include
 11 |   /sw/include
 12 |   NO_DEFAULT_PATH
 13 |   )
 14 | 
 15 | # Macro to unify finding both the debug and release versions of the
 16 | # libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
 17 | # macro.
 18 | 
 19 | MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
 20 | 
 21 |   FIND_LIBRARY("${MYLIBRARY}_DEBUG"
 22 |     NAMES "g2o_${MYLIBRARYNAME}_d"
 23 |     PATHS
 24 |     ${G2O_ROOT}/lib/Debug
 25 |     ${G2O_ROOT}/lib
 26 |     $ENV{G2O_ROOT}/lib/Debug
 27 |     $ENV{G2O_ROOT}/lib
 28 |     NO_DEFAULT_PATH
 29 |     )
 30 | 
 31 |   FIND_LIBRARY("${MYLIBRARY}_DEBUG"
 32 |     NAMES "g2o_${MYLIBRARYNAME}_d"
 33 |     PATHS
 34 |     ~/Library/Frameworks
 35 |     /Library/Frameworks
 36 |     /usr/local/lib
 37 |     /usr/local/lib64
 38 |     /usr/lib
 39 |     /usr/lib64
 40 |     /opt/local/lib
 41 |     /sw/local/lib
 42 |     /sw/lib
 43 |     )
 44 |   
 45 |   FIND_LIBRARY(${MYLIBRARY}
 46 |     NAMES "g2o_${MYLIBRARYNAME}"
 47 |     PATHS
 48 |     ${G2O_ROOT}/lib/Release
 49 |     ${G2O_ROOT}/lib
 50 |     $ENV{G2O_ROOT}/lib/Release
 51 |     $ENV{G2O_ROOT}/lib
 52 |     NO_DEFAULT_PATH
 53 |     )
 54 | 
 55 |   FIND_LIBRARY(${MYLIBRARY}
 56 |     NAMES "g2o_${MYLIBRARYNAME}"
 57 |     PATHS
 58 |     ~/Library/Frameworks
 59 |     /Library/Frameworks
 60 |     /usr/local/lib
 61 |     /usr/local/lib64
 62 |     /usr/lib
 63 |     /usr/lib64
 64 |     /opt/local/lib
 65 |     /sw/local/lib
 66 |     /sw/lib
 67 |     )
 68 |   
 69 |   IF(NOT ${MYLIBRARY}_DEBUG)
 70 |     IF(MYLIBRARY)
 71 |       SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
 72 |     ENDIF(MYLIBRARY)
 73 |   ENDIF( NOT ${MYLIBRARY}_DEBUG)
 74 |   
 75 | ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
 76 | 
 77 | # Find the core elements
 78 | FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
 79 | FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
 80 | 
 81 | # Find the CLI library
 82 | FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
 83 | 
 84 | # Find the pluggable solvers
 85 | FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
 86 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
 87 | FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
 88 | FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
 89 | FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
 90 | FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
 91 | FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
 92 | FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
 93 | 
 94 | # Find the predefined types
 95 | FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
 96 | FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
 97 | FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
 98 | FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
 99 | FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
100 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
101 | FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
102 | 
103 | # G2O solvers declared found if we found at least one solver
104 | SET(G2O_SOLVERS_FOUND "NO")
105 | 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)
106 |   SET(G2O_SOLVERS_FOUND "YES")
107 | 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)
108 | 
109 | # G2O itself declared found if we found the core libraries and at least one solver
110 | SET(G2O_FOUND "NO")
111 | IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
112 |   SET(G2O_FOUND "YES")
113 | ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
114 | 


--------------------------------------------------------------------------------
/include/utils.h:
--------------------------------------------------------------------------------
  1 | #ifndef UTILS_H
  2 | #define UTILS_H
  3 | 
  4 | #include <stdio.h>
  5 | #include <iostream>
  6 | #include <iomanip>
  7 | #include <time.h>
  8 | #include <string.h>
  9 | #include <dirent.h>
 10 | #include <fstream>
 11 | #include <algorithm>    // std::sort
 12 | #include <sys/types.h>
 13 | #include <sys/stat.h>
 14 | 
 15 | #include <opencv2/core/core.hpp>
 16 | #include <opencv2/highgui/highgui.hpp>
 17 | #include <opencv2/imgproc/imgproc.hpp>
 18 | #include <opencv2/calib3d/calib3d.hpp>
 19 | 
 20 | #include <pcl/point_types.h>
 21 | #include <pcl/point_cloud.h>
 22 | #include <pcl/io/pcd_io.h>
 23 | #include <pcl/common/transforms.h>
 24 | #include <pcl/filters/voxel_grid.h>
 25 | 
 26 | #include <Eigen/Core>
 27 | #include <Eigen/Geometry>
 28 | 
 29 | #include "draw.hpp"
 30 | 
 31 | using namespace std;
 32 | struct STEREO_RECTIFY_PARAMS{
 33 |     cv::Mat P1, P2;
 34 |     cv::Mat R1, R2;
 35 |     cv::Mat Q;
 36 |     cv::Mat map11, map12;
 37 |     cv::Mat map21, map22;
 38 |     cv::Size imageSize;
 39 | };
 40 | 
 41 | std::vector<string> getIMUFileName(string &strPathIMU);
 42 | 
 43 | std::vector<string> getPCDFileName(string &strPathPCD);
 44 | 
 45 | std::vector<string> getImgFileName(string &strPathImg);
 46 | 
 47 | std::vector<Eigen::Affine3d> loadGPSIMU(const std::vector<string>& strPathIMU);
 48 | 
 49 | void getRectificationParams(cv::Mat K1, cv::Mat K2, 
 50 |                             cv::Mat R, cv::Mat t,
 51 |                             cv::Mat distort1,
 52 |                             cv::Mat distort2,
 53 |                             STEREO_RECTIFY_PARAMS& srp);
 54 | 
 55 | std::vector<Eigen::Affine3d> getTransformFromGPSIMUinCamera(std::vector<std::string> IMUFileName);
 56 | 
 57 | void GetTransformationFromEulerAngleAndTranslation(Eigen::Affine3d& Transformation,
 58 |                                                    double rx, double ry, double rz, 
 59 |                                                    double tx, double ty, double tz);
 60 | 
 61 | 
 62 | void convertTransformMatrixToVector(std::vector<Eigen::Affine3d> TransformaMatrix,
 63 |                                		  std::vector<cv::Mat>& Rvec,
 64 |                                		  std::vector<cv::Mat>& Tvec);
 65 | 
 66 | void convertGPScoordinateToTranslation(double lati, double lon, double alt,
 67 |                                        double& tx, double& ty, double& tz);
 68 | 
 69 | void GetTransformationFromRollPitchYawAndTranslation(Eigen::Affine3d& Transformation,
 70 |                                                      double roll, double pitch, double yaw, 
 71 |                                                      double tx, double ty, double tz);
 72 | 
 73 | void getAccumulateMotion(const std::vector<cv::Mat>& Rvec,
 74 |                          const std::vector<cv::Mat>& Tvec,
 75 |                          int startIdx, int endIdx,
 76 |                          cv::Mat& accumRvec, cv::Mat& accumTvec);
 77 | 
 78 | void getRelativeMotion(const cv::Mat startPosR, const cv::Mat startPosT,
 79 | 						           const cv::Mat endPosR, const cv::Mat endPosT,
 80 | 					             cv::Mat& r2to1, cv::Mat& t2to1);
 81 | 
 82 | Eigen::Affine3d vectorToTransformation(cv::Mat rvec, cv::Mat tvec);
 83 | 
 84 | void transformationToVector(Eigen::Affine3d trans, cv::Mat& rvec, cv::Mat& tvec);
 85 | 
 86 | std::vector<cv::Point3f> transformPoints(Eigen::Affine3d trans, std::vector<cv::Point3f> obj_pts);
 87 | 
 88 | cv::Point3f transformPoint(Eigen::Affine3d trans, cv::Point3f pt);
 89 | 
 90 | pcl::PointCloud<pcl::PointXYZ> Point3ftoPointCloud(std::vector<cv::Point3f> pts);
 91 | 
 92 | std::vector<cv::Point3f> PointCloudtoPoint3f(pcl::PointCloud<pcl::PointXYZ> ptcloud);
 93 | 
 94 | std::vector<cv::Point3f> PointCloudtoPoint3f(pcl::PointCloud<pcl::PointXYZI> ptcloud);
 95 | 
 96 | std::vector<cv::Point3f> readLiDARandConvertToWorldCoor(string PCDPath, 
 97 |                                                cv::Mat accumRvec, 
 98 |                                                cv::Mat accumTvec);
 99 | double normOfTransform( cv::Mat rvec, cv::Mat tvec );
100 | cv::Point3f getCameraCenter(const cv::Mat Rvec,
101 |                             const cv::Mat Tvec);
102 | #endif
103 | 


--------------------------------------------------------------------------------
/src/draw.cpp:
--------------------------------------------------------------------------------
  1 | #include "draw.hpp"
  2 | 
  3 | using namespace cv;
  4 | using namespace std;
  5 | #define matched  1
  6 | 
  7 | void drawMatch(Mat img_1,
  8 |                vector<KeyPoint> keypoints1,
  9 |                vector<KeyPoint> keypoints2,
 10 |                vector<DMatch> matches,
 11 |                int radius,
 12 |                char* windowName){
 13 |     int ptsNum = (int)matches.size();
 14 |     float minDist = 10000;
 15 |     float dist;
 16 |     Point2f pt1, pt2;
 17 |     Mat copy;
 18 |     img_1.copyTo(copy);
 19 |     
 20 |     for(int n = 0; n < ptsNum; n++){
 21 |         dist = matches[n].distance;
 22 |         if(dist < minDist) minDist = dist;
 23 |         
 24 |     }
 25 |     
 26 |     for(int n = 0; n < ptsNum; n++){
 27 |         if(matches[n].distance <= 4*minDist){
 28 |             pt1 = keypoints1[matches[n].queryIdx].pt;
 29 |             pt2 = keypoints2[matches[n].trainIdx].pt;
 30 |             
 31 |             circle(copy, pt1, radius, Scalar(0,0,255));
 32 |             circle(copy, pt2, radius, Scalar(255,0,0));
 33 |             
 34 |             line(copy, pt1, pt2, Scalar(0,255,0));
 35 |         }
 36 |     }
 37 |     //-- Show detected matches
 38 |     imshow( windowName, copy );
 39 |     //    imshow("connected Features", copy);
 40 |     if(waitKey(1) == 27){
 41 |         exit;
 42 |     }
 43 | }
 44 | 
 45 | void drawMatch(Mat img_1,
 46 |                vector<KeyPoint> keypoints1,
 47 |                vector<KeyPoint> keypoints2,
 48 |                int radius,
 49 |                char* windowName){
 50 |     int ptsNum = (int)keypoints1.size();
 51 |     Point2f pt1, pt2;
 52 |     Mat copy;
 53 |     img_1.copyTo(copy);
 54 |     for(int n = 0; n < ptsNum; n++){
 55 |         pt1 = keypoints1[n].pt;
 56 |         pt2 = keypoints2[n].pt;
 57 |         
 58 |         circle(copy, pt1, radius, Scalar(0,0,255));
 59 |         circle(copy, pt2, radius, Scalar(255,0,0));
 60 |         
 61 |         line(copy, pt1, pt2, Scalar(0,255,0));
 62 |     }
 63 |     imshow(windowName, copy);
 64 |     if(waitKey(1) == 27){
 65 |         exit;
 66 |     }
 67 | }
 68 | 
 69 | void drawMatch(Mat img_1,
 70 |                vector<Point2f> p_keypoints1,
 71 |                vector<Point2f> p_keypoints2,
 72 |                int radius,
 73 |                char* windowName){
 74 |     int ptsNum = (int)p_keypoints1.size();
 75 |     Point2f pt1, pt2;
 76 |     Mat copy;
 77 |     img_1.copyTo(copy);
 78 |     for(int n = 0; n < ptsNum; n++){
 79 |         pt1 = p_keypoints1[n];
 80 |         pt2 = p_keypoints2[n];
 81 |         
 82 |         circle(copy, pt1, radius, Scalar(0,0,255));
 83 |         circle(copy, pt2, radius, Scalar(255,0,0));
 84 |         
 85 |         line(copy, pt1, pt2, Scalar(0,255,0));
 86 |     }
 87 |     imshow(windowName, copy);
 88 |     if(waitKey(1) == 27){
 89 |         exit;
 90 |     }
 91 | }
 92 | 
 93 | void drawFeature(Mat img, vector<KeyPoint> features, char* windowName){
 94 |     
 95 |     int ptsNum = (int)features.size();
 96 |     vector<Point2f> p_features;
 97 |     p_features.resize(ptsNum);
 98 |     
 99 |     Mat copy;
100 |     copy = img.clone();
101 |     KeyPoint::convert(features, p_features);
102 |     for( int i = 0; i < p_features.size(); i++ ){
103 |         circle( copy, p_features[i], 2, Scalar(0,250,0), 1, 8, 0 );
104 |     }
105 |     imshow(windowName, copy);
106 |     if(waitKey(1) == 27){
107 |         exit;
108 |     }
109 | }
110 | 
111 | void drawFeature(Mat img, vector<Point2f> features, char* windowName){
112 |     Mat copy;
113 |     copy = img.clone();
114 |     for( int i = 0; i < features.size(); i++ ){
115 |         circle( copy, features[i], 2, Scalar(0, 255,0), 0.5, 1, 0 );
116 |     }
117 |     imshow(windowName, copy);
118 |     if(waitKey(1) == 27){
119 |         exit;
120 |     }
121 | }
122 | 
123 | void drawFarandCloseFeatures(Mat img, vector<Point2f> closePts, 
124 |                              vector<Point2f> farPts, const char* windowName){
125 |     Mat copy;
126 |     copy = img.clone();
127 |     for( int i = 0; i < closePts.size(); i++){
128 |         circle( copy, closePts[i], 2, Scalar(0, 255,0), 0.5, 1, 0 );
129 |     }
130 |     for( int i = 0; i < farPts.size(); i++){
131 |         circle( copy, farPts[i], 4, Scalar(0, 0, 255), 0.5, 1, 0 );
132 |     }
133 |     imshow(windowName, copy);
134 |     if(waitKey(1) == 27){
135 |        exit;
136 |     }
137 | }
138 | 
139 | void drawFarandCloseFeatures(Mat img, vector<Point2f> pts, 
140 |                              vector<int> farIdx, char* windowName){
141 |     Mat copy;
142 |     copy = img.clone();
143 |     for( int i = 0; i < farIdx.size(); i++){
144 |         if(farIdx[i] == 0)
145 |             circle( copy, pts[i], 2, Scalar(0, 255,0), 0.5, 1, 0 );
146 |         else
147 |             circle( copy, pts[i], 2, Scalar(0, 0, 255), 0.5, 1, 0 );
148 |     }
149 |     imshow(windowName, copy);
150 |     if(waitKey(1) == 27){
151 |        exit;
152 |     }
153 | }
154 | 


--------------------------------------------------------------------------------
/include/bal_problem.h:
--------------------------------------------------------------------------------
 1 | // Ceres Solver - A fast non-linear least squares minimizer
 2 | // Copyright 2015 Google Inc. All rights reserved.
 3 | // http://ceres-solver.org/
 4 | //
 5 | // Redistribution and use in source and binary forms, with or without
 6 | // modification, are permitted provided that the following conditions are met:
 7 | //
 8 | // * Redistributions of source code must retain the above copyright notice,
 9 | //   this list of conditions and the following disclaimer.
10 | // * Redistributions in binary form must reproduce the above copyright notice,
11 | //   this list of conditions and the following disclaimer in the documentation
12 | //   and/or other materials provided with the distribution.
13 | // * Neither the name of Google Inc. nor the names of its contributors may be
14 | //   used to endorse or promote products derived from this software without
15 | //   specific prior written permission.
16 | //
17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 | // POSSIBILITY OF SUCH DAMAGE.
28 | //
29 | // Author: sameeragarwal@google.com (Sameer Agarwal)
30 | //
31 | // Class for loading and holding in memory bundle adjustment problems
32 | // from the BAL (Bundle Adjustment in the Large) dataset from the
33 | // University of Washington.
34 | //
35 | // For more details see http://grail.cs.washington.edu/projects/bal/
36 | #ifndef CERES_EXAMPLES_BAL_PROBLEM_H_
37 | #define CERES_EXAMPLES_BAL_PROBLEM_H_
38 | #include <string>
39 | namespace ceres {
40 | namespace examples {
41 | class BALProblem {
42 |  public:
43 |   explicit BALProblem(const std::string& filename, bool use_quaternions);
44 |   ~BALProblem();
45 |   void WriteToFile(const std::string& filename) const;
46 |   void WriteToPLYFile(const std::string& filename) const;
47 |   // Move the "center" of the reconstruction to the origin, where the
48 |   // center is determined by computing the marginal median of the
49 |   // points. The reconstruction is then scaled so that the median
50 |   // absolute deviation of the points measured from the origin is
51 |   // 100.0.
52 |   //
53 |   // The reprojection error of the problem remains the same.
54 |   void Normalize();
55 |   // Perturb the camera pose and the geometry with random normal
56 |   // numbers with corresponding standard deviations.
57 |   void Perturb(const double rotation_sigma,
58 |                const double translation_sigma,
59 |                const double point_sigma);
60 |   int camera_block_size()      const { return use_quaternions_ ? 10 : 9; }
61 |   int point_block_size()       const { return 3;                         }
62 |   int num_cameras()            const { return num_cameras_;              }
63 |   int num_points()             const { return num_points_;               }
64 |   int num_observations()       const { return num_observations_;         }
65 |   int num_parameters()         const { return num_parameters_;           }
66 |   const int* point_index()     const { return point_index_;              }
67 |   const int* camera_index()    const { return camera_index_;             }
68 |   const double* observations() const { return observations_;             }
69 |   const double* parameters()   const { return parameters_;               }
70 |   const double* cameras()      const { return parameters_;               }
71 |   double* mutable_cameras()          { return parameters_;               }
72 |   double* mutable_points() {
73 |     return parameters_  + camera_block_size() * num_cameras_;
74 |   }
75 |  private:
76 |   void CameraToAngleAxisAndCenter(const double* camera,
77 |                                   double* angle_axis,
78 |                                   double* center) const;
79 |   void AngleAxisAndCenterToCamera(const double* angle_axis,
80 |                                   const double* center,
81 |                                   double* camera) const;
82 |   int num_cameras_;
83 |   int num_points_;
84 |   int num_observations_;
85 |   int num_parameters_;
86 |   bool use_quaternions_;
87 |   int* point_index_;
88 |   int* camera_index_;
89 |   double* observations_;
90 |   // The parameter vector is laid out as follows
91 |   // [camera_1, ..., camera_n, point_1, ..., point_m]
92 |   double* parameters_;
93 | };
94 | }  // namespace examples
95 | }  // namespace ceres
96 | #endif  // CERES_EXAMPLES_BAL_PROBLEM_H_


--------------------------------------------------------------------------------
/src/utils.cpp:
--------------------------------------------------------------------------------
  1 | #include "utils.h"
  2 | #include <opencv2/core/eigen.hpp>
  3 | 
  4 | using namespace std;
  5 | using namespace cv;
  6 | using namespace pcl;
  7 | 
  8 | vector<string> getImgFileName(string &strPathImg){
  9 |     string cpstrPathImg = strPathImg;
 10 |     vector<string> vstrImage;
 11 | 
 12 |     DIR *dir;
 13 |     class dirent *ent;
 14 |     class stat st;
 15 |     dir = opendir(cpstrPathImg.c_str());
 16 |     while ((ent = readdir(dir)) != NULL) {
 17 |         const string file_name = ent->d_name;
 18 |         const string full_file_name = cpstrPathImg + "/" + file_name;
 19 | 
 20 |         if (file_name[0] == '.')
 21 |             continue;
 22 | 
 23 |         if (stat(full_file_name.c_str(), &st) == -1)
 24 |             continue;
 25 | 
 26 |         const bool is_directory = (st.st_mode & S_IFDIR) != 0;
 27 |         if (is_directory)
 28 |             continue;
 29 | 
 30 |         vstrImage.push_back(full_file_name);
 31 |     }
 32 |     closedir(dir);
 33 |     sort(vstrImage.begin(), vstrImage.end());
 34 |     return vstrImage;
 35 | }
 36 | 
 37 | void transformationToVector(Eigen::Affine3d transformMatrix, Mat& rvec, Mat& tvec)
 38 | {
 39 |     Mat R = Mat::zeros(3,3,CV_64F)  ;
 40 |     for(int r = 0; r < 3; r++){
 41 |         for(int c = 0; c < 3; c++){
 42 |             R.at<double>(r,c) = transformMatrix(r,c);
 43 |         }
 44 |     }
 45 |     Rodrigues(R, rvec);
 46 |     tvec = Mat::zeros(3,1,CV_64F);
 47 |     tvec.at<double>(0,0) = transformMatrix(0,3);
 48 |     tvec.at<double>(1,0) = transformMatrix(1,3);
 49 |     tvec.at<double>(2,0) = transformMatrix(2,3);
 50 | }
 51 | 
 52 | void convertTransformMatrixToVector(vector<Eigen::Affine3d> TransformaMatrix,
 53 |                                	    vector<Mat>& Rvec,
 54 |                                     vector<Mat>& Tvec){
 55 | 
 56 |     int numTrans = TransformaMatrix.size();
 57 |     Rvec.clear();
 58 |     Tvec.clear();
 59 | 
 60 |     for(int n = 0; n < numTrans; n++){
 61 |         Mat rvec;
 62 |         Mat tvec;
 63 |         transformationToVector(TransformaMatrix[n], rvec, tvec);
 64 |         Rvec.push_back(rvec);
 65 |         Tvec.push_back(tvec);
 66 |     }
 67 | }
 68 | 
 69 | void getAccumulateMotion(const vector<Mat>& Rvec,
 70 |                          const vector<Mat>& Tvec,
 71 |                          int startIdx, int endIdx,
 72 |                          Mat& accumRvec, Mat& accumTvec){
 73 |     accumRvec = Mat::zeros(3,1,CV_64F);
 74 |     accumTvec = Mat::zeros(3,1,CV_64F);
 75 |     Eigen::Affine3d accumMatrix = vectorToTransformation(accumRvec, accumTvec);
 76 |     for(int n = startIdx; n < endIdx; n++){
 77 |         Mat tempRvec, tempTvec;
 78 |         tempRvec = Rvec[n].clone();
 79 |         tempTvec = Tvec[n].clone();
 80 |         Eigen::Affine3d tempMatrix = vectorToTransformation(tempRvec, tempTvec);
 81 |         accumMatrix = tempMatrix * accumMatrix;
 82 |     }
 83 | 
 84 |     Mat R = Mat::zeros(3,3,CV_64F);
 85 |     for(int r = 0; r < 3; r++){
 86 |         for(int c = 0; c < 3; c++){
 87 |             R.at<double>(r,c) = accumMatrix(r,c);
 88 |         }
 89 |     }
 90 | 
 91 |     Rodrigues(R, accumRvec);
 92 |     accumTvec.at<double>(0,0) = accumMatrix(0,3);
 93 |     accumTvec.at<double>(1,0) = accumMatrix(1,3);
 94 |     accumTvec.at<double>(2,0) = accumMatrix(2,3);
 95 | }
 96 | 
 97 | void getRelativeMotion(const Mat startPosR, const Mat startPosT,
 98 |                        const Mat endPosR, const Mat endPosT,
 99 |                        Mat& r2to1, Mat& t2to1){
100 |     r2to1 = Mat::zeros(3,1,CV_64F);
101 |     t2to1 = Mat::zeros(3,1,CV_64F);
102 | 
103 |     Eigen::Affine3d startTrans, endTrans, trans1to2;
104 |     Mat startPosRMat, endPosRMat;
105 |     Rodrigues(startPosR, startPosRMat);
106 |     Rodrigues(endPosR, endPosRMat);
107 |     for(int r = 0; r < 3; r++){
108 |         for(int c = 0; c < 3; c++){
109 |             startTrans(r,c) = startPosRMat.at<double>(r,c);
110 |             endTrans(r,c) = endPosRMat.at<double>(r,c);
111 |         }
112 |     }
113 | 
114 |     startTrans(0,3) = startPosT.at<double>(0,0);
115 |     startTrans(1,3) = startPosT.at<double>(1,0);
116 |     startTrans(2,3) = startPosT.at<double>(2,0);
117 |     endTrans(0,3) = endPosT.at<double>(0,0);
118 |     endTrans(1,3) = endPosT.at<double>(1,0);
119 |     endTrans(2,3) = endPosT.at<double>(2,0);
120 | 
121 |     trans1to2 = endTrans * startTrans.inverse();
122 |     Mat R = Mat::zeros(3,3,CV_64F);
123 |     for(int r = 0; r < 3; r++){
124 |         for(int c = 0; c < 3; c++){
125 |             R.at<double>(r,c) = trans1to2(r,c);
126 |         }
127 |     }
128 |     Rodrigues(R, r2to1);
129 | 
130 |     t2to1.at<double>(0,0) = trans1to2(0,3);
131 |     t2to1.at<double>(1,0) = trans1to2(1,3);
132 |     t2to1.at<double>(2,0) = trans1to2(2,3);
133 | }
134 | 
135 | Eigen::Affine3d vectorToTransformation(Mat rvec, Mat tvec){
136 | 	Mat R;
137 | 	Eigen::Affine3d result;
138 | 	Rodrigues(rvec, R);
139 | 	for(int r = 0; r < 3; r++){
140 | 		for(int c = 0; c < 3; c++){
141 | 			result(r,c) = R.at<double>(r,c);
142 | 		}
143 | 	}
144 | 	result(0, 3) = tvec.at<double>(0, 0);
145 | 	result(1, 3) = tvec.at<double>(1, 0);
146 | 	result(2, 3) = tvec.at<double>(2, 0);
147 | 	return result;
148 | }
149 | 
150 | vector<Point3f> transformPoints(Eigen::Affine3d trans, vector<Point3f> obj_pts){
151 | 	vector<Point3f> result;
152 | 	PointCloud<PointXYZ> tempCloud = Point3ftoPointCloud(obj_pts);
153 | 	Eigen::Affine3d invTrans = trans.inverse();
154 | 	transformPointCloud(tempCloud, tempCloud, invTrans.matrix());
155 | 	result = PointCloudtoPoint3f(tempCloud);
156 | 	return result;
157 | }
158 | 
159 | 
160 | Point3f transformPoint(Eigen::Affine3d trans, Point3f pt){
161 |     Point3f result;
162 |     Eigen::Affine3d invTrans = trans.inverse();
163 |     result.x = (float)invTrans(0,0)*pt.x + (float)invTrans(0,1)*pt.y + (float)invTrans(0,2)*pt.z + (float)invTrans(0,3);
164 |     result.y = (float)invTrans(1,0)*pt.x + (float)invTrans(1,1)*pt.y + (float)invTrans(1,2)*pt.z + (float)invTrans(1,3);
165 |     result.z = (float)invTrans(2,0)*pt.x + (float)invTrans(2,1)*pt.y + (float)invTrans(2,2)*pt.z + (float)invTrans(2,3);
166 |     return result;
167 | }
168 | 
169 | vector<Point3f> PointCloudtoPoint3f(PointCloud<PointXYZ> ptcloud){
170 |     PointXYZ ptxyz;
171 |     Point3f pt;
172 |     vector<Point3f> result;
173 |     for(int i = 0; i < ptcloud.points.size(); i++){
174 |         ptxyz = ptcloud.points[i];
175 |         pt.x = ptxyz.x;
176 |         pt.y = ptxyz.y;
177 |         pt.z = ptxyz.z;
178 | 
179 |         result.push_back(pt);
180 |     }
181 |     return result;
182 | }
183 | 
184 | vector<Point3f> PointCloudtoPoint3f(PointCloud<PointXYZI> ptcloud){
185 |     PointXYZI ptxyzi;
186 |     Point3f pt;
187 |     vector<Point3f> result;
188 |     for(int i = 0; i < ptcloud.points.size(); i++){
189 |         ptxyzi = ptcloud.points[i];
190 |         pt.x = ptxyzi.x;
191 |         pt.y = ptxyzi.y;
192 |         pt.z = ptxyzi.z;
193 |         result.push_back(pt);
194 |     }
195 |     return result;
196 | }
197 | 
198 | PointCloud<PointXYZ> Point3ftoPointCloud(vector<Point3f> pts){
199 |     PointXYZ ptxyz;
200 |     Point3f pt;
201 |     PointCloud<PointXYZ> result;
202 |     for(int i = 0; i < pts.size(); i++){
203 |         pt = pts[i];
204 |         ptxyz.x = pt.x;
205 |         ptxyz.y = pt.y;
206 |         ptxyz.z = pt.z;
207 |         result.points.push_back(ptxyz);
208 |     }
209 |     return result;
210 | }
211 | 
212 | double normOfTransform( cv::Mat rvec, cv::Mat tvec ){
213 |     return fabs(MIN(norm(rvec), 2*M_PI-norm(rvec)))+ fabs(norm(tvec));
214 | }
215 | 
216 | Point3f getCameraCenter(const Mat Rvec,
217 |                         const Mat Tvec){
218 |     Point3f cameraCenter;
219 |     Mat worldCenter = Mat::zeros(4,1,CV_64F);
220 |     Mat homCameraCenter = Mat::zeros(4,1,CV_64F);
221 |     worldCenter.at<double>(3,0)=1;
222 |     Eigen::Affine3d accumMatrix = vectorToTransformation(Rvec, Tvec);
223 |     for (int i=0;i<4;i++){
224 | 	double sum=0;
225 | 	for (int j=0;j<4;j++){
226 | 	    sum+=accumMatrix(i,j)*worldCenter.at<double>(j);
227 | 	}
228 | 	homCameraCenter.at<double>(i,0)=sum;
229 |     }
230 |     cameraCenter.x = homCameraCenter.at<double>(0,0)/homCameraCenter.at<double>(3,0);
231 |     cameraCenter.y = homCameraCenter.at<double>(1,0)/homCameraCenter.at<double>(3,0);
232 |     cameraCenter.z = homCameraCenter.at<double>(2,0)/homCameraCenter.at<double>(3,0);
233 |     return cameraCenter;
234 | }
235 | 


--------------------------------------------------------------------------------
/cmake_modules/FindLAPACK.cmake:
--------------------------------------------------------------------------------
  1 | # Find LAPACK library
  2 | #
  3 | # This module finds an installed library that implements the LAPACK
  4 | # linear-algebra interface (see http://www.netlib.org/lapack/).
  5 | # The approach follows mostly that taken for the autoconf macro file, acx_lapack.m4
  6 | # (distributed at http://ac-archive.sourceforge.net/ac-archive/acx_lapack.html).
  7 | #
  8 | # This module sets the following variables:
  9 | #  LAPACK_FOUND - set to true if a library implementing the LAPACK interface
 10 | #    is found
 11 | #  LAPACK_INCLUDE_DIR - Directories containing the LAPACK header files
 12 | #  LAPACK_DEFINITIONS - Compilation options to use LAPACK
 13 | #  LAPACK_LINKER_FLAGS - Linker flags to use LAPACK (excluding -l
 14 | #    and -L).
 15 | #  LAPACK_LIBRARIES_DIR - Directories containing the LAPACK libraries.
 16 | #     May be null if LAPACK_LIBRARIES contains libraries name using full path.
 17 | #  LAPACK_LIBRARIES - List of libraries to link against LAPACK interface.
 18 | #     May be null if the compiler supports auto-link (e.g. VC++).
 19 | #  LAPACK_USE_FILE - The name of the cmake module to include to compile
 20 | #     applications or libraries using LAPACK.
 21 | #
 22 | # This module was modified by CGAL team:
 23 | # - find libraries for a C++ compiler, instead of Fortran
 24 | # - added LAPACK_INCLUDE_DIR, LAPACK_DEFINITIONS and LAPACK_LIBRARIES_DIR
 25 | # - removed LAPACK95_LIBRARIES
 26 | 
 27 | 
 28 | include(CheckFunctionExists)
 29 | 
 30 | # This macro checks for the existence of the combination of fortran libraries
 31 | # given by _list.  If the combination is found, this macro checks (using the
 32 | # check_function_exists macro) whether can link against that library
 33 | # combination using the name of a routine given by _name using the linker
 34 | # flags given by _flags.  If the combination of libraries is found and passes
 35 | # the link test, LIBRARIES is set to the list of complete library paths that
 36 | # have been found and DEFINITIONS to the required definitions.
 37 | # Otherwise, LIBRARIES is set to FALSE.
 38 | # N.B. _prefix is the prefix applied to the names of all cached variables that
 39 | # are generated internally and marked advanced by this macro.
 40 | macro(check_lapack_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _blas _path)
 41 |   #message("DEBUG: check_lapack_libraries(${_list} in ${_path} with ${_blas})")
 42 | 
 43 |   # Check for the existence of the libraries given by _list
 44 |   set(_libraries_found TRUE)
 45 |   set(_libraries_work FALSE)
 46 |   set(${DEFINITIONS} "")
 47 |   set(${LIBRARIES} "")
 48 |   set(_combined_name)
 49 |   foreach(_library ${_list})
 50 |     set(_combined_name ${_combined_name}_${_library})
 51 | 
 52 |     if(_libraries_found)
 53 |       # search first in ${_path}
 54 |       find_library(${_prefix}_${_library}_LIBRARY
 55 |                   NAMES ${_library}
 56 |                   PATHS ${_path} NO_DEFAULT_PATH
 57 |                   )
 58 |       # if not found, search in environment variables and system
 59 |       if ( WIN32 )
 60 |         find_library(${_prefix}_${_library}_LIBRARY
 61 |                     NAMES ${_library}
 62 |                     PATHS ENV LIB
 63 |                     )
 64 |       elseif ( APPLE )
 65 |         find_library(${_prefix}_${_library}_LIBRARY
 66 |                     NAMES ${_library}
 67 |                     PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH
 68 |                     )
 69 |       else ()
 70 |         find_library(${_prefix}_${_library}_LIBRARY
 71 |                     NAMES ${_library}
 72 |                     PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH
 73 |                     )
 74 |       endif()
 75 |       mark_as_advanced(${_prefix}_${_library}_LIBRARY)
 76 |       set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})
 77 |       set(_libraries_found ${${_prefix}_${_library}_LIBRARY})
 78 |     endif(_libraries_found)
 79 |   endforeach(_library ${_list})
 80 |   if(_libraries_found)
 81 |     set(_libraries_found ${${LIBRARIES}})
 82 |   endif()
 83 | 
 84 |   # Test this combination of libraries with the Fortran/f2c interface.
 85 |   # We test the Fortran interface first as it is well standardized.
 86 |   if(_libraries_found AND NOT _libraries_work)
 87 |     set(${DEFINITIONS}  "-D${_prefix}_USE_F2C")
 88 |     set(${LIBRARIES}    ${_libraries_found})
 89 |     # Some C++ linkers require the f2c library to link with Fortran libraries.
 90 |     # I do not know which ones, thus I just add the f2c library if it is available.
 91 |     find_package( F2C QUIET )
 92 |     if ( F2C_FOUND )
 93 |       set(${DEFINITIONS}  ${${DEFINITIONS}} ${F2C_DEFINITIONS})
 94 |       set(${LIBRARIES}    ${${LIBRARIES}} ${F2C_LIBRARIES})
 95 |     endif()
 96 |     set(CMAKE_REQUIRED_DEFINITIONS  ${${DEFINITIONS}})
 97 |     set(CMAKE_REQUIRED_LIBRARIES    ${_flags} ${${LIBRARIES}} ${_blas})
 98 |     #message("DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}")
 99 |     #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
100 |     # Check if function exists with f2c calling convention (ie a trailing underscore)
101 |     check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
102 |     set(CMAKE_REQUIRED_DEFINITIONS} "")
103 |     set(CMAKE_REQUIRED_LIBRARIES    "")
104 |     mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
105 |     set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})
106 |   endif(_libraries_found AND NOT _libraries_work)
107 | 
108 |   # If not found, test this combination of libraries with a C interface.
109 |   # A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.
110 |   if(_libraries_found AND NOT _libraries_work)
111 |     set(${DEFINITIONS} "")
112 |     set(${LIBRARIES}   ${_libraries_found})
113 |     set(CMAKE_REQUIRED_DEFINITIONS "")
114 |     set(CMAKE_REQUIRED_LIBRARIES   ${_flags} ${${LIBRARIES}} ${_blas})
115 |     #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
116 |     check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)
117 |     set(CMAKE_REQUIRED_LIBRARIES "")
118 |     mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)
119 |     set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})
120 |   endif(_libraries_found AND NOT _libraries_work)
121 | 
122 |   # on failure
123 |   if(NOT _libraries_work)
124 |     set(${DEFINITIONS} "")
125 |     set(${LIBRARIES}   FALSE)
126 |   endif()
127 |   #message("DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}")
128 |   #message("DEBUG: ${LIBRARIES} = ${${LIBRARIES}}")
129 | endmacro(check_lapack_libraries)
130 | 
131 | 
132 | #
133 | # main
134 | #
135 | 
136 | # LAPACK requires BLAS
137 | if(LAPACK_FIND_QUIETLY OR NOT LAPACK_FIND_REQUIRED)
138 |   find_package(BLAS)
139 | else()
140 |   find_package(BLAS REQUIRED)
141 | endif()
142 | 
143 | if (NOT BLAS_FOUND)
144 | 
145 |   message(STATUS "LAPACK requires BLAS.")
146 |   set(LAPACK_FOUND FALSE)
147 | 
148 | # Is it already configured?
149 | elseif (LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)
150 | 
151 |   set(LAPACK_FOUND TRUE)
152 | 
153 | else()
154 | 
155 |   # reset variables
156 |   set( LAPACK_INCLUDE_DIR "" )
157 |   set( LAPACK_DEFINITIONS "" )
158 |   set( LAPACK_LINKER_FLAGS "" ) # unused (yet)
159 |   set( LAPACK_LIBRARIES "" )
160 |   set( LAPACK_LIBRARIES_DIR "" )
161 | 
162 |     #
163 |     # If Unix, search for LAPACK function in possible libraries
164 |     #
165 | 
166 |     #intel mkl lapack?
167 |     if(NOT LAPACK_LIBRARIES)
168 |       check_lapack_libraries(
169 |       LAPACK_DEFINITIONS
170 |       LAPACK_LIBRARIES
171 |       LAPACK
172 |       cheev
173 |       ""
174 |       "mkl_lapack"
175 |       "${BLAS_LIBRARIES}"
176 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
177 |       )
178 |     endif()
179 | 
180 |     #acml lapack?
181 |     if(NOT LAPACK_LIBRARIES)
182 |       check_lapack_libraries(
183 |       LAPACK_DEFINITIONS
184 |       LAPACK_LIBRARIES
185 |       LAPACK
186 |       cheev
187 |       ""
188 |       "acml"
189 |       "${BLAS_LIBRARIES}"
190 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
191 |       )
192 |     endif()
193 | 
194 |     # Apple LAPACK library?
195 |     if(NOT LAPACK_LIBRARIES)
196 |       check_lapack_libraries(
197 |       LAPACK_DEFINITIONS
198 |       LAPACK_LIBRARIES
199 |       LAPACK
200 |       cheev
201 |       ""
202 |       "Accelerate"
203 |       "${BLAS_LIBRARIES}"
204 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
205 |       )
206 |     endif()
207 | 
208 |     if ( NOT LAPACK_LIBRARIES )
209 |       check_lapack_libraries(
210 |       LAPACK_DEFINITIONS
211 |       LAPACK_LIBRARIES
212 |       LAPACK
213 |       cheev
214 |       ""
215 |       "vecLib"
216 |       "${BLAS_LIBRARIES}"
217 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
218 |       )
219 |     endif ( NOT LAPACK_LIBRARIES )
220 | 
221 |     # Generic LAPACK library?
222 |     # This configuration *must* be the last try as this library is notably slow.
223 |     if ( NOT LAPACK_LIBRARIES )
224 |       check_lapack_libraries(
225 |       LAPACK_DEFINITIONS
226 |       LAPACK_LIBRARIES
227 |       LAPACK
228 |       cheev
229 |       ""
230 |       "lapack"
231 |       "${BLAS_LIBRARIES}"
232 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV LAPACK_LIB_DIR"
233 |       )
234 |     endif()
235 | 
236 |   if(LAPACK_LIBRARIES_DIR OR LAPACK_LIBRARIES)
237 |     set(LAPACK_FOUND TRUE)
238 |   else()
239 |     set(LAPACK_FOUND FALSE)
240 |   endif()
241 | 
242 |   if(NOT LAPACK_FIND_QUIETLY)
243 |     if(LAPACK_FOUND)
244 |       message(STATUS "A library with LAPACK API found.")
245 |     else(LAPACK_FOUND)
246 |       if(LAPACK_FIND_REQUIRED)
247 |         message(FATAL_ERROR "A required library with LAPACK API not found. Please specify library location.")
248 |       else()
249 |         message(STATUS "A library with LAPACK API not found. Please specify library location.")
250 |       endif()
251 |     endif(LAPACK_FOUND)
252 |   endif(NOT LAPACK_FIND_QUIETLY)
253 | 
254 |   # Add variables to cache
255 |   set( LAPACK_INCLUDE_DIR   "${LAPACK_INCLUDE_DIR}"
256 |                             CACHE PATH "Directories containing the LAPACK header files" FORCE )
257 |   set( LAPACK_DEFINITIONS   "${LAPACK_DEFINITIONS}"
258 |                             CACHE STRING "Compilation options to use LAPACK" FORCE )
259 |   set( LAPACK_LINKER_FLAGS  "${LAPACK_LINKER_FLAGS}"
260 |                             CACHE STRING "Linker flags to use LAPACK" FORCE )
261 |   set( LAPACK_LIBRARIES     "${LAPACK_LIBRARIES}"
262 |                             CACHE FILEPATH "LAPACK libraries name" FORCE )
263 |   set( LAPACK_LIBRARIES_DIR "${LAPACK_LIBRARIES_DIR}"
264 |                             CACHE PATH "Directories containing the LAPACK libraries" FORCE )
265 | 
266 |   #message("DEBUG: LAPACK_INCLUDE_DIR = ${LAPACK_INCLUDE_DIR}")
267 |   #message("DEBUG: LAPACK_DEFINITIONS = ${LAPACK_DEFINITIONS}")
268 |   #message("DEBUG: LAPACK_LINKER_FLAGS = ${LAPACK_LINKER_FLAGS}")
269 |   #message("DEBUG: LAPACK_LIBRARIES = ${LAPACK_LIBRARIES}")
270 |   #message("DEBUG: LAPACK_LIBRARIES_DIR = ${LAPACK_LIBRARIES_DIR}")
271 |   #message("DEBUG: LAPACK_FOUND = ${LAPACK_FOUND}")
272 | 
273 | endif(NOT BLAS_FOUND)
274 | 


--------------------------------------------------------------------------------
/src/bal_problem.cc:
--------------------------------------------------------------------------------
  1 | // Ceres Solver - A fast non-linear least squares minimizer
  2 | // Copyright 2015 Google Inc. All rights reserved.
  3 | // http://ceres-solver.org/
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // * Redistributions of source code must retain the above copyright notice,
  9 | //   this list of conditions and the following disclaimer.
 10 | // * Redistributions in binary form must reproduce the above copyright notice,
 11 | //   this list of conditions and the following disclaimer in the documentation
 12 | //   and/or other materials provided with the distribution.
 13 | // * Neither the name of Google Inc. nor the names of its contributors may be
 14 | //   used to endorse or promote products derived from this software without
 15 | //   specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // Author: sameeragarwal@google.com (Sameer Agarwal)
 30 | #include "bal_problem.h"
 31 | #include <cstdio>
 32 | #include <cstdlib>
 33 | #include <fstream>
 34 | #include <string>
 35 | #include <vector>
 36 | #include "Eigen/Core"
 37 | #include "ceres/rotation.h"
 38 | #include "glog/logging.h"
 39 | #include "random.h"
 40 | namespace ceres {
 41 | namespace examples {
 42 | namespace {
 43 | typedef Eigen::Map<Eigen::VectorXd> VectorRef;
 44 | typedef Eigen::Map<const Eigen::VectorXd> ConstVectorRef;
 45 | template<typename T>
 46 | void FscanfOrDie(FILE* fptr, const char* format, T* value) {
 47 |   int num_scanned = fscanf(fptr, format, value);
 48 |   if (num_scanned != 1) {
 49 |     LOG(FATAL) << "Invalid UW data file.";
 50 |   }
 51 | }
 52 | void PerturbPoint3(const double sigma, double* point) {
 53 |   for (int i = 0; i < 3; ++i) {
 54 |     point[i] += RandNormal() * sigma;
 55 |   }
 56 | }
 57 | double Median(std::vector<double>* data) {
 58 |   int n = data->size();
 59 |   std::vector<double>::iterator mid_point = data->begin() + n / 2;
 60 |   std::nth_element(data->begin(), mid_point, data->end());
 61 |   return *mid_point;
 62 | }
 63 | }  // namespace
 64 | BALProblem::BALProblem(const std::string& filename, bool use_quaternions) {
 65 |   FILE* fptr = fopen(filename.c_str(), "r");
 66 |   if (fptr == NULL) {
 67 |     LOG(FATAL) << "Error: unable to open file " << filename;
 68 |     return;
 69 |   };
 70 |   // This wil die horribly on invalid files. Them's the breaks.
 71 |   FscanfOrDie(fptr, "%d", &num_cameras_);
 72 |   FscanfOrDie(fptr, "%d", &num_points_);
 73 |   FscanfOrDie(fptr, "%d", &num_observations_);
 74 |   VLOG(1) << "Header: " << num_cameras_
 75 |           << " " << num_points_
 76 |           << " " << num_observations_;
 77 |   point_index_ = new int[num_observations_];
 78 |   camera_index_ = new int[num_observations_];
 79 |   observations_ = new double[2 * num_observations_];
 80 |   num_parameters_ = 9 * num_cameras_ + 3 * num_points_;
 81 |   parameters_ = new double[num_parameters_];
 82 |   for (int i = 0; i < num_observations_; ++i) {
 83 |     FscanfOrDie(fptr, "%d", camera_index_ + i);
 84 |     FscanfOrDie(fptr, "%d", point_index_ + i);
 85 |     for (int j = 0; j < 2; ++j) {
 86 |       FscanfOrDie(fptr, "%lf", observations_ + 2*i + j);
 87 |     }
 88 |   }
 89 |   for (int i = 0; i < num_parameters_; ++i) {
 90 |     FscanfOrDie(fptr, "%lf", parameters_ + i);
 91 |   }
 92 |   fclose(fptr);
 93 |   use_quaternions_ = use_quaternions;
 94 |   if (use_quaternions) {
 95 |     // Switch the angle-axis rotations to quaternions.
 96 |     num_parameters_ = 10 * num_cameras_ + 3 * num_points_;
 97 |     double* quaternion_parameters = new double[num_parameters_];
 98 |     double* original_cursor = parameters_;
 99 |     double* quaternion_cursor = quaternion_parameters;
100 |     for (int i = 0; i < num_cameras_; ++i) {
101 |       AngleAxisToQuaternion(original_cursor, quaternion_cursor);
102 |       quaternion_cursor += 4;
103 |       original_cursor += 3;
104 |       for (int j = 4; j < 10; ++j) {
105 |        *quaternion_cursor++ = *original_cursor++;
106 |       }
107 |     }
108 |     // Copy the rest of the points.
109 |     for (int i = 0; i < 3 * num_points_; ++i) {
110 |       *quaternion_cursor++ = *original_cursor++;
111 |     }
112 |     // Swap in the quaternion parameters.
113 |     delete []parameters_;
114 |     parameters_ = quaternion_parameters;
115 |   }
116 | }
117 | // This function writes the problem to a file in the same format that
118 | // is read by the constructor.
119 | void BALProblem::WriteToFile(const std::string& filename) const {
120 |   FILE* fptr = fopen(filename.c_str(), "w");
121 |   if (fptr == NULL) {
122 |     LOG(FATAL) << "Error: unable to open file " << filename;
123 |     return;
124 |   };
125 |   fprintf(fptr, "%d %d %d\n", num_cameras_, num_points_, num_observations_);
126 |   for (int i = 0; i < num_observations_; ++i) {
127 |     fprintf(fptr, "%d %d", camera_index_[i], point_index_[i]);
128 |     for (int j = 0; j < 2; ++j) {
129 |       fprintf(fptr, " %g", observations_[2 * i + j]);
130 |     }
131 |     fprintf(fptr, "\n");
132 |   }
133 |   for (int i = 0; i < num_cameras(); ++i) {
134 |     double angleaxis[9];
135 |     if (use_quaternions_) {
136 |       // Output in angle-axis format.
137 |       QuaternionToAngleAxis(parameters_ + 10 * i, angleaxis);
138 |       memcpy(angleaxis + 3, parameters_ + 10 * i + 4, 6 * sizeof(double));
139 |     } else {
140 |       memcpy(angleaxis, parameters_ + 9 * i, 9 * sizeof(double));
141 |     }
142 |     for (int j = 0; j < 9; ++j) {
143 |       fprintf(fptr, "%.16g\n", angleaxis[j]);
144 |     }
145 |   }
146 |   const double* points = parameters_ + camera_block_size() * num_cameras_;
147 |   for (int i = 0; i < num_points(); ++i) {
148 |     const double* point = points + i * point_block_size();
149 |     for (int j = 0; j < point_block_size(); ++j) {
150 |       fprintf(fptr, "%.16g\n", point[j]);
151 |     }
152 |   }
153 |   fclose(fptr);
154 | }
155 | // Write the problem to a PLY file for inspection in Meshlab or CloudCompare.
156 | void BALProblem::WriteToPLYFile(const std::string& filename) const {
157 |   std::ofstream of(filename.c_str());
158 |   of << "ply"
159 |      << '\n' << "format ascii 1.0"
160 |      << '\n' << "element vertex " << num_cameras_ + num_points_
161 |      << '\n' << "property float x"
162 |      << '\n' << "property float y"
163 |      << '\n' << "property float z"
164 |      << '\n' << "property uchar red"
165 |      << '\n' << "property uchar green"
166 |      << '\n' << "property uchar blue"
167 |      << '\n' << "end_header" << std::endl;
168 |   // Export extrinsic data (i.e. camera centers) as green points.
169 |   double angle_axis[3];
170 |   double center[3];
171 |   for (int i = 0; i < num_cameras(); ++i)  {
172 |     const double* camera = cameras() + camera_block_size() * i;
173 |     CameraToAngleAxisAndCenter(camera, angle_axis, center);
174 |     of << center[0] << ' ' << center[1] << ' ' << center[2]
175 |        << " 0 255 0" << '\n';
176 |   }
177 |   // Export the structure (i.e. 3D Points) as white points.
178 |   const double* points = parameters_ + camera_block_size() * num_cameras_;
179 |   for (int i = 0; i < num_points(); ++i) {
180 |     const double* point = points + i * point_block_size();
181 |     for (int j = 0; j < point_block_size(); ++j) {
182 |       of << point[j] << ' ';
183 |     }
184 |     of << "255 255 255\n";
185 |   }
186 |   of.close();
187 | }
188 | void BALProblem::CameraToAngleAxisAndCenter(const double* camera,
189 |                                             double* angle_axis,
190 |                                             double* center) const {
191 |   VectorRef angle_axis_ref(angle_axis, 3);
192 |   if (use_quaternions_) {
193 |     QuaternionToAngleAxis(camera, angle_axis);
194 |   } else {
195 |     angle_axis_ref = ConstVectorRef(camera, 3);
196 |   }
197 |   // c = -R't
198 |   Eigen::VectorXd inverse_rotation = -angle_axis_ref;
199 |   AngleAxisRotatePoint(inverse_rotation.data(),
200 |                        camera + camera_block_size() - 6,
201 |                        center);
202 |   VectorRef(center, 3) *= -1.0;
203 | }
204 | void BALProblem::AngleAxisAndCenterToCamera(const double* angle_axis,
205 |                                             const double* center,
206 |                                             double* camera) const {
207 |   ConstVectorRef angle_axis_ref(angle_axis, 3);
208 |   if (use_quaternions_) {
209 |     AngleAxisToQuaternion(angle_axis, camera);
210 |   } else {
211 |     VectorRef(camera, 3) = angle_axis_ref;
212 |   }
213 |   // t = -R * c
214 |   AngleAxisRotatePoint(angle_axis,
215 |                        center,
216 |                        camera + camera_block_size() - 6);
217 |   VectorRef(camera + camera_block_size() - 6, 3) *= -1.0;
218 | }
219 | void BALProblem::Normalize() {
220 |   // Compute the marginal median of the geometry.
221 |   std::vector<double> tmp(num_points_);
222 |   Eigen::Vector3d median;
223 |   double* points = mutable_points();
224 |   for (int i = 0; i < 3; ++i) {
225 |     for (int j = 0; j < num_points_; ++j) {
226 |       tmp[j] = points[3 * j + i];
227 |     }
228 |     median(i) = Median(&tmp);
229 |   }
230 |   for (int i = 0; i < num_points_; ++i) {
231 |     VectorRef point(points + 3 * i, 3);
232 |     tmp[i] = (point - median).lpNorm<1>();
233 |   }
234 |   const double median_absolute_deviation = Median(&tmp);
235 |   // Scale so that the median absolute deviation of the resulting
236 |   // reconstruction is 100.
237 |   const double scale = 100.0 / median_absolute_deviation;
238 |   VLOG(2) << "median: " << median.transpose();
239 |   VLOG(2) << "median absolute deviation: " << median_absolute_deviation;
240 |   VLOG(2) << "scale: " << scale;
241 |   // X = scale * (X - median)
242 |   for (int i = 0; i < num_points_; ++i) {
243 |     VectorRef point(points + 3 * i, 3);
244 |     point = scale * (point - median);
245 |   }
246 |   double* cameras = mutable_cameras();
247 |   double angle_axis[3];
248 |   double center[3];
249 |   for (int i = 0; i < num_cameras_; ++i) {
250 |     double* camera = cameras + camera_block_size() * i;
251 |     CameraToAngleAxisAndCenter(camera, angle_axis, center);
252 |     // center = scale * (center - median)
253 |     VectorRef(center, 3) = scale * (VectorRef(center, 3) - median);
254 |     AngleAxisAndCenterToCamera(angle_axis, center, camera);
255 |   }
256 | }
257 | void BALProblem::Perturb(const double rotation_sigma,
258 |                          const double translation_sigma,
259 |                          const double point_sigma) {
260 |   CHECK_GE(point_sigma, 0.0);
261 |   CHECK_GE(rotation_sigma, 0.0);
262 |   CHECK_GE(translation_sigma, 0.0);
263 |   double* points = mutable_points();
264 |   if (point_sigma > 0) {
265 |     for (int i = 0; i < num_points_; ++i) {
266 |       PerturbPoint3(point_sigma, points + 3 * i);
267 |     }
268 |   }
269 |   for (int i = 0; i < num_cameras_; ++i) {
270 |     double* camera = mutable_cameras() + camera_block_size() * i;
271 |     double angle_axis[3];
272 |     double center[3];
273 |     // Perturb in the rotation of the camera in the angle-axis
274 |     // representation.
275 |     CameraToAngleAxisAndCenter(camera, angle_axis, center);
276 |     if (rotation_sigma > 0.0) {
277 |       PerturbPoint3(rotation_sigma, angle_axis);
278 |     }
279 |     AngleAxisAndCenterToCamera(angle_axis, center, camera);
280 |     if (translation_sigma > 0.0) {
281 |       PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
282 |     }
283 |   }
284 | }
285 | BALProblem::~BALProblem() {
286 |   delete []point_index_;
287 |   delete []camera_index_;
288 |   delete []observations_;
289 |   delete []parameters_;
290 | }
291 | }  // namespace examples
292 | }  // namespace ceres


--------------------------------------------------------------------------------
/src/Frame.cpp:
--------------------------------------------------------------------------------
  1 | #include "Frame.h"
  2 | #include "utils.h"
  3 | 
  4 | using namespace cv;
  5 | using namespace std;
  6 | 
  7 | 
  8 | Frame::Frame(string leftImgFile, string rightImgFile, 
  9 |              STEREO_RECTIFY_PARAMS _srp, int _id, Map* _map):
 10 | 			 frameID(_id), map(_map), srp(_srp){
 11 |     imgL = imread(leftImgFile);
 12 |     imgR = imread(rightImgFile);
 13 |     if(!imgL.data || ! imgR.data){
 14 |         cout << "image does not exist..." << endl;
 15 |         exit;
 16 |     }
 17 | 
 18 |     fx = _srp.P1.at<double>(0,0);
 19 |     fy = _srp.P1.at<double>(1,1);
 20 |     cx = _srp.P1.at<double>(0,2);
 21 |     cy = _srp.P1.at<double>(1,2);
 22 |     b = -_srp.P2.at<double>(0,3)/fx;
 23 | 
 24 |     SurfFeatureDetector detector(10, 6, 3);
 25 |     SurfDescriptorExtractor descriptor;
 26 |     
 27 |     detector.detect(imgL, keypointL);
 28 |     detector.detect(imgR, keypointR);
 29 |     
 30 |     Mat tempDespL;
 31 |     descriptor.compute(imgL, keypointL, tempDespL);
 32 |     descriptor.compute(imgR, keypointR, despR);
 33 | 
 34 |     rvec = Mat::zeros(3, 1, CV_64F);
 35 |     tvec = Mat::zeros(3, 1, CV_64F);
 36 | 
 37 |     worldRvec = Mat::zeros(3, 1, CV_64F);
 38 |     worldTvec = Mat::zeros(3, 1, CV_64F);
 39 |     vector<KeyPoint> stereoKeypointLeft, stereoKeypointRight;
 40 |     vector<DMatch> stereoMatches;
 41 | 
 42 |     //compute stereo matches
 43 |     vector<int> sceneInliers;
 44 |     matchFeatureKNN(tempDespL, despR, keypointL, keypointR,
 45 |     				stereoKeypointLeft, stereoKeypointRight,
 46 |     				stereoMatches, 0.8);
 47 | 
 48 |     compute3Dpoints(stereoKeypointLeft, stereoKeypointRight,
 49 |     				keypointL, keypointR, sceneInliers);
 50 | 
 51 |     vector<int> keypointLinliers;
 52 |     for(int idx : sceneInliers){
 53 |         keypointLinliers.push_back(stereoMatches[idx].queryIdx);
 54 |     }
 55 | 
 56 |     // extract descriptors
 57 |     despL = Mat(keypointLinliers.size(), tempDespL.cols, tempDespL.type());
 58 | 
 59 |     for(int n = 0; n < keypointLinliers.size(); n++){
 60 |         tempDespL.row(keypointLinliers[n]).copyTo(despL.row(n));
 61 |     }
 62 |     mappoints = vector<MapPoint*>(keypointL.size(), static_cast<MapPoint*>(NULL));
 63 |     originality = vector<bool>(keypointL.size(), false);
 64 | }
 65 | 
 66 | bool idx_comparator(const DMatch& m1, const DMatch& m2){
 67 | 	return m1.queryIdx < m2.queryIdx;
 68 | }
 69 | 
 70 | void Frame::matchFrame(Frame* frame, bool useMappoints){
 71 |     rvec.release();
 72 |     tvec.release();
 73 | 
 74 |     matchesBetweenFrame.clear();
 75 | 	//first step, match using features
 76 |     vector<KeyPoint> matchedPrev, matchedCurr;
 77 |     vector<DMatch> matches;
 78 |     matchFeatureKNN(despL, frame->despL, 
 79 |                     keypointL, frame->keypointL,
 80 |                     matchedPrev, matchedCurr,
 81 |                     matches, 0.8);
 82 | 
 83 |     // obtain obj_pts, img_pts and matchedIdx
 84 |     vector<Point3f> obj_pts;
 85 |     vector<Point2f> img_pts;
 86 |     vector<DMatch> tempMatches;
 87 |     for(auto m : matches){
 88 |         if(mappoints[m.queryIdx]!=NULL && 
 89 |            !mappoints[m.queryIdx]->isBad &&
 90 |            useMappoints){
 91 |             MapPoint* curMp = mappoints[m.queryIdx];
 92 |             obj_pts.push_back(curMp->getPositionInCameraCoordinate(worldRvec, worldTvec));
 93 |         }
 94 |         else{
 95 |             obj_pts.push_back(scenePts[m.queryIdx]);
 96 |         }
 97 |         
 98 |         img_pts.push_back(frame->keypointL[m.trainIdx].pt);
 99 |     }
100 |     Mat inliers;
101 |     PnP(obj_pts, img_pts, inliers);
102 |     for(int n = 0; n < inliers.rows; n++){
103 |         matchesBetweenFrame.push_back(matches[inliers.at<int>(n,0)]);
104 |     }
105 | 
106 |     frame->rvec = rvec.clone();
107 |     frame->tvec = tvec.clone();
108 |     sort(matchesBetweenFrame.begin(), matchesBetweenFrame.end(), idx_comparator);
109 |     vector<Point2f> finalP1, finalP2;
110 |     for(int n = 0; n < matchesBetweenFrame.size(); n++){
111 |         finalP1.push_back(keypointL[matchesBetweenFrame[n].queryIdx].pt);
112 |         finalP2.push_back(frame->keypointL[matchesBetweenFrame[n].trainIdx].pt);
113 |     }
114 | 
115 |     drawMatch(imgL, finalP1, finalP2, 1, "pnp inliers");
116 | }
117 | 
118 | void Frame::setWrdTransVectorAndTransScenePts(cv::Mat _worldRvec, cv::Mat _worldTvec) {
119 |     worldRvec = _worldRvec.clone();
120 |     worldTvec = _worldTvec.clone();
121 |     transformScenePtsToWorldCoordinate();
122 | }
123 | 
124 | void Frame::transformScenePtsToWorldCoordinate(){
125 | 	Eigen::Affine3d worldTrans = vectorToTransformation(worldRvec, worldTvec);
126 | 	scenePtsinWorld = transformPoints(worldTrans, scenePts);
127 | }
128 | 
129 | void Frame::manageMapPoints(Frame* frame){    
130 |     int newMappointCount = 0;
131 |     int newObservationCount = 0;
132 |     for(int n = 0; n < matchesBetweenFrame.size(); n++){
133 |     	unsigned int qIdx = (unsigned int)matchesBetweenFrame[n].queryIdx;
134 |     	unsigned int tIdx = (unsigned int)matchesBetweenFrame[n].trainIdx;
135 |     	if(mappoints[qIdx] == NULL && frame->mappoints[tIdx] == NULL){
136 |     		//create new mappoint in current frame
137 |     		//create a pointer to this mappoint in the matched frame
138 |     		//also need to add an observation
139 |     		mappoints[qIdx] = createNewMapPoint(qIdx);
140 |             pointToExistingMapPoint(frame, mappoints[qIdx], tIdx);
141 | 
142 |     		mappoints[qIdx]->addObservation(frame, tIdx);
143 |     		mappoints[qIdx]->addObservation(this, qIdx);
144 | 
145 |     		map->addMapPoint(mappoints[qIdx]);
146 |     	}
147 |         else if(mappoints[qIdx] == NULL && frame->mappoints[tIdx] != NULL){
148 |             pointToExistingMapPoint(this, frame->mappoints[tIdx], qIdx);
149 |             frame->mappoints[tIdx]->addObservation(this, qIdx);
150 |         }
151 |         else if(mappoints[qIdx] != NULL && frame->mappoints[tIdx] == NULL){
152 |             pointToExistingMapPoint(frame, mappoints[qIdx], tIdx);
153 |             mappoints[qIdx]->addObservation(frame,tIdx);
154 |         }
155 | 
156 |     	else if(mappoints[qIdx] != NULL && frame->mappoints[tIdx] != NULL){
157 |     		//add observation to existing mappoint
158 | //    		mappoints[qIdx]->addObservation(frame, tIdx);
159 | //            pointToExistingMapPoint(frame, mappoints[qIdx], tIdx);
160 | //    		newObservationCount++;
161 |     	}
162 |     }
163 | }
164 | 
165 | void Frame::addEdgeConstrain(unsigned int id, Mat relativeRvec, Mat relativeTvec){
166 |     relativePose.insert(make_pair(id, make_pair(relativeRvec, relativeTvec)));
167 | }
168 | 
169 | 
170 | void Frame::judgeBadPoints(){
171 |     //count total number
172 |     int validPointNumber = 0;
173 |     for(auto mappoint : mappoints){
174 |         if(mappoint != NULL){validPointNumber++;}
175 |     }
176 |     //compute each point to other points average distance, if too large, discard
177 |     for(auto mappoint : mappoints){
178 |         if(mappoint != NULL) {
179 |             int closeCount = 0, farCount = 0;
180 |             for (auto compareMappoint : mappoints) {
181 |                 if (compareMappoint != NULL) {
182 |                     float distX = compareMappoint->pos.x - mappoint->pos.x;
183 |                     float distY = compareMappoint->pos.y - mappoint->pos.y;
184 |                     float distZ = compareMappoint->pos.z - mappoint->pos.z;
185 |                     if(sqrt(pow(distX, 2) + pow(distY, 2) + pow(distZ, 2)) < 100 * b){
186 |                         closeCount++;
187 |                     }
188 |                     else{
189 |                         farCount++;
190 |                     }
191 |                 }
192 |             }
193 |             if ((float)farCount/(float)closeCount > 0.1) { mappoint->isBad = true; }
194 |         }
195 |     }
196 | }
197 | 
198 | void Frame::judgeBadPointsKdTree() {
199 |     //build kd-tree
200 |     int validPointNumber = 0;
201 |     vector<Point3f> mappoints3f;
202 |     for(auto mappoint : mappoints) {
203 |         if(mappoint != NULL) {
204 |             mappoints3f.push_back(mappoint->pos);
205 |             validPointNumber++;
206 |         }
207 |     }
208 | 
209 |     KDTree pointsKDTree(mappoints3f, false);
210 |     int K =10, Emax = INT_MAX;
211 |     int idx[K];
212 |     float dist[K];
213 |     
214 |     // for(auto mappoint3f : mappoints3f) {
215 |     //     pointsKDTree.findNearest(mappoint3f, K, Emax, idx, 0, dist);
216 |     //     float aveDist = 0;
217 |     //     for(int n = 0; n < K; n++) {aveDist += dist[n];}
218 |     //     aveDist /= (float)K;
219 |     //     cout << "averag distance: " << aveDist << endl;
220 | 
221 |     // }
222 | }
223 | 
224 | 
225 | 
226 | MapPoint* Frame::createNewMapPoint(unsigned int pointIdx){
227 | 	return new MapPoint(scenePtsinWorld[pointIdx], frameID, pointIdx);
228 | }
229 | 
230 | void Frame::pointToExistingMapPoint(Frame* frame, MapPoint* mp, unsigned int currIdx){
231 | 	frame->mappoints[currIdx] = mp;
232 | }
233 | 
234 | void Frame::PnP(vector<Point3f> obj_pts, 
235 |                 vector<Point2f> img_pts,
236 |                 Mat& inliers){
237 | 
238 |      //solve PnP
239 |     Mat K = srp.P2.colRange(0,3).clone();
240 |     // Mat temprvec, temptvec;
241 |     if(obj_pts.size() == 0){
242 |         cout << "points for PnP is 0, cannot solve." << endl;
243 |         return;
244 |     }
245 |     solvePnPRansac(obj_pts, img_pts, K, Mat(),
246 |                    rvec, tvec, false, 2000,3.0, 300, inliers);
247 | }
248 | 
249 | void Frame::releaseMemory(){
250 |     imgL.release();
251 |     imgR.release();
252 |     despL.release();
253 |     despR.release();
254 | 
255 | }
256 | 
257 | void Frame::matchFeatureKNN(const Mat& desp1, const Mat& desp2, 
258 |                             const vector<KeyPoint>& keypoint1, 
259 |                             const vector<KeyPoint>& keypoint2,
260 |                             vector<KeyPoint>& matchedKeypoint1,
261 |                             vector<KeyPoint>& matchedKeypoint2,
262 |                             vector<DMatch>& matches,
263 |                             double knn_match_ratio,
264 |                             bool hamming){
265 | 
266 |     matchedKeypoint1.clear();
267 |     matchedKeypoint2.clear();
268 |     matches.clear();
269 | 
270 |     float imgThres = 0.15 * std::sqrt(std::pow(imgL.rows, 2)+std::pow(imgL.cols, 2));
271 | 
272 |     if(hamming){
273 |     	cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create("BruteForce-Hamming(2)");
274 |     	vector< vector<cv::DMatch> > matches_knn;
275 | 	    matcher->knnMatch( desp1, desp2, matches_knn, 2 );
276 | 	    vector< cv::DMatch > tMatches;
277 | 
278 | 	    for ( size_t i=0; i<matches_knn.size(); i++ )
279 | 	    {
280 | 	        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
281 | 	            tMatches.push_back( matches_knn[i][0] );
282 | 	    }
283 | 	    
284 | 	    if (tMatches.size() <= 20) //too few matches
285 | 	        return;
286 | 
287 | 	    vector<KeyPoint> tMatchedKeypoint1, tMatchedKeypoint2;
288 | 	    for ( auto m:tMatches )
289 | 	    {
290 | 	        Point2f pt1, pt2;
291 | 	        pt1 = keypoint1[m.queryIdx].pt;
292 | 	        pt2 = keypoint2[m.trainIdx].pt;
293 | 	        float ptdist = std::sqrt(std::pow(pt1.x-pt2.x, 2) + std::pow(pt1.y-pt2.y, 2));
294 | 	        if(ptdist < imgThres){
295 | 	            matchedKeypoint1.push_back(keypoint1[m.queryIdx]);
296 | 	            matchedKeypoint2.push_back(keypoint2[m.trainIdx]);
297 | 	            matches.push_back(m);
298 | 	        } 
299 | 	    }  
300 |     }
301 |     else{
302 |     	cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create("BruteForce");
303 | 	    vector< vector<cv::DMatch> > matches_knn;
304 | 	    matcher->knnMatch( desp1, desp2, matches_knn, 2 );
305 | 	    vector< cv::DMatch > tMatches;
306 | 
307 | 	    for ( size_t i=0; i<matches_knn.size(); i++ )
308 | 	    {
309 | 	        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
310 | 	            tMatches.push_back( matches_knn[i][0] );
311 | 	    }
312 | 	    
313 | 	    if (tMatches.size() <= 20) //too few matches
314 | 	        return;
315 | 
316 | 	    vector<KeyPoint> tMatchedKeypoint1, tMatchedKeypoint2;
317 | 	    for ( auto m:tMatches )
318 | 	    {
319 | 	        Point2f pt1, pt2;
320 | 	        pt1 = keypoint1[m.queryIdx].pt;
321 | 	        pt2 = keypoint2[m.trainIdx].pt;
322 | 	        float ptdist = std::sqrt(std::pow(pt1.x-pt2.x, 2) + std::pow(pt1.y-pt2.y, 2));
323 | 	        if(ptdist < imgThres){
324 | 	            matchedKeypoint1.push_back(keypoint1[m.queryIdx]);
325 | 	            matchedKeypoint2.push_back(keypoint2[m.trainIdx]);
326 | 	            matches.push_back(m);
327 | 	        } 
328 | 	    } 
329 |     } 
330 | }
331 | 
332 | void Frame::compute3Dpoints(vector<KeyPoint>& kl, 
333 | 					 	    vector<KeyPoint>& kr,
334 | 					 		vector<KeyPoint>& trikl,
335 | 					 		vector<KeyPoint>& trikr,
336 |                             vector<int>& inliers){
337 | 
338 | 	vector<KeyPoint> copy_kl, copy_kr;
339 | 	copy_kl = kl;
340 | 	copy_kr = kr;
341 | 
342 | 	scenePts.clear();
343 | 	trikl.clear();
344 | 	trikr.clear();
345 |     inliers.clear();
346 | 	double thres = 45*b;
347 | 	for(int n = 0; n < copy_kl.size(); n++){
348 | 		Point3f pd;
349 | 		double d = fabs(copy_kl[n].pt.x - copy_kr[n].pt.x);
350 | 		pd.x = (float)(b*(copy_kl[n].pt.x - cx)/d);
351 | 		pd.y = (float)(b*(copy_kl[n].pt.y - cy)/d);
352 | 		pd.z = (float)(b*fx/d);
353 | 		if(pd.z < thres &&
354 | 		   fabs(copy_kl[n].pt.y - copy_kr[n].pt.y) < 4){
355 | 			scenePts.push_back(pd);
356 | 			trikl.push_back(copy_kl[n]);
357 | 			trikr.push_back(copy_kr[n]);
358 |             inliers.push_back(n);
359 | 		}
360 | 	}
361 | }
362 | 
363 | Eigen::Affine3d Frame::getWorldTransformationMatrix(){
364 |     Mat R;
365 |     Eigen::Affine3d result;
366 |     Rodrigues(worldRvec, R);
367 |     for(int r = 0; r < 3; r++){
368 |         for(int c = 0; c < 3; c++){
369 |             result(r,c) = R.at<double>(r,c);
370 |         }
371 |     }
372 | 
373 |     result(0,3) = worldTvec.at<double>(0,0);
374 |     result(1,3) = worldTvec.at<double>(1,0);
375 |     result(2,3) = worldTvec.at<double>(2,0);
376 |     return result;
377 | }
378 | 


--------------------------------------------------------------------------------
/cmake_modules/FindBLAS.cmake:
--------------------------------------------------------------------------------
  1 | # Find BLAS library
  2 | #
  3 | # This module finds an installed library that implements the BLAS
  4 | # linear-algebra interface (see http://www.netlib.org/blas/).
  5 | # The list of libraries searched for is mainly taken
  6 | # from the autoconf macro file, acx_blas.m4 (distributed at
  7 | # http://ac-archive.sourceforge.net/ac-archive/acx_blas.html).
  8 | #
  9 | # This module sets the following variables:
 10 | #  BLAS_FOUND - set to true if a library implementing the BLAS interface
 11 | #    is found
 12 | #  BLAS_INCLUDE_DIR - Directories containing the BLAS header files
 13 | #  BLAS_DEFINITIONS - Compilation options to use BLAS
 14 | #  BLAS_LINKER_FLAGS - Linker flags to use BLAS (excluding -l
 15 | #    and -L).
 16 | #  BLAS_LIBRARIES_DIR - Directories containing the BLAS libraries.
 17 | #     May be null if BLAS_LIBRARIES contains libraries name using full path.
 18 | #  BLAS_LIBRARIES - List of libraries to link against BLAS interface.
 19 | #     May be null if the compiler supports auto-link (e.g. VC++).
 20 | #  BLAS_USE_FILE - The name of the cmake module to include to compile
 21 | #     applications or libraries using BLAS.
 22 | #
 23 | # This module was modified by CGAL team:
 24 | # - find libraries for a C++ compiler, instead of Fortran
 25 | # - added BLAS_INCLUDE_DIR, BLAS_DEFINITIONS and BLAS_LIBRARIES_DIR
 26 | # - removed BLAS95_LIBRARIES
 27 | 
 28 | include(CheckFunctionExists)
 29 | 
 30 | 
 31 | # This macro checks for the existence of the combination of fortran libraries
 32 | # given by _list.  If the combination is found, this macro checks (using the
 33 | # check_function_exists macro) whether can link against that library
 34 | # combination using the name of a routine given by _name using the linker
 35 | # flags given by _flags.  If the combination of libraries is found and passes
 36 | # the link test, LIBRARIES is set to the list of complete library paths that
 37 | # have been found and DEFINITIONS to the required definitions.
 38 | # Otherwise, LIBRARIES is set to FALSE.
 39 | # N.B. _prefix is the prefix applied to the names of all cached variables that
 40 | # are generated internally and marked advanced by this macro.
 41 | macro(check_fortran_libraries DEFINITIONS LIBRARIES _prefix _name _flags _list _path)
 42 |   #message("DEBUG: check_fortran_libraries(${_list} in ${_path})")
 43 | 
 44 |   # Check for the existence of the libraries given by _list
 45 |   set(_libraries_found TRUE)
 46 |   set(_libraries_work FALSE)
 47 |   set(${DEFINITIONS} "")
 48 |   set(${LIBRARIES} "")
 49 |   set(_combined_name)
 50 |   foreach(_library ${_list})
 51 |     set(_combined_name ${_combined_name}_${_library})
 52 | 
 53 |     if(_libraries_found)
 54 |       # search first in ${_path}
 55 |       find_library(${_prefix}_${_library}_LIBRARY
 56 |                   NAMES ${_library}
 57 |                   PATHS ${_path} NO_DEFAULT_PATH
 58 |                   )
 59 |       # if not found, search in environment variables and system
 60 |       if ( WIN32 )
 61 |         find_library(${_prefix}_${_library}_LIBRARY
 62 |                     NAMES ${_library}
 63 |                     PATHS ENV LIB
 64 |                     )
 65 |       elseif ( APPLE )
 66 |         find_library(${_prefix}_${_library}_LIBRARY
 67 |                     NAMES ${_library}
 68 |                     PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV DYLD_LIBRARY_PATH
 69 |                     )
 70 |       else ()
 71 |         find_library(${_prefix}_${_library}_LIBRARY
 72 |                     NAMES ${_library}
 73 |                     PATHS /usr/local/lib /usr/lib /usr/local/lib64 /usr/lib64 ENV LD_LIBRARY_PATH
 74 |                     )
 75 |       endif()
 76 |       mark_as_advanced(${_prefix}_${_library}_LIBRARY)
 77 |       set(${LIBRARIES} ${${LIBRARIES}} ${${_prefix}_${_library}_LIBRARY})
 78 |       set(_libraries_found ${${_prefix}_${_library}_LIBRARY})
 79 |     endif(_libraries_found)
 80 |   endforeach(_library ${_list})
 81 |   if(_libraries_found)
 82 |     set(_libraries_found ${${LIBRARIES}})
 83 |   endif()
 84 | 
 85 |   # Test this combination of libraries with the Fortran/f2c interface.
 86 |   # We test the Fortran interface first as it is well standardized.
 87 |   if(_libraries_found AND NOT _libraries_work)
 88 |     set(${DEFINITIONS}  "-D${_prefix}_USE_F2C")
 89 |     set(${LIBRARIES}    ${_libraries_found})
 90 |     # Some C++ linkers require the f2c library to link with Fortran libraries.
 91 |     # I do not know which ones, thus I just add the f2c library if it is available.
 92 |     find_package( F2C QUIET )
 93 |     if ( F2C_FOUND )
 94 |       set(${DEFINITIONS}  ${${DEFINITIONS}} ${F2C_DEFINITIONS})
 95 |       set(${LIBRARIES}    ${${LIBRARIES}} ${F2C_LIBRARIES})
 96 |     endif()
 97 |     set(CMAKE_REQUIRED_DEFINITIONS  ${${DEFINITIONS}})
 98 |     set(CMAKE_REQUIRED_LIBRARIES    ${_flags} ${${LIBRARIES}})
 99 |     #message("DEBUG: CMAKE_REQUIRED_DEFINITIONS = ${CMAKE_REQUIRED_DEFINITIONS}")
100 |     #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
101 |     # Check if function exists with f2c calling convention (ie a trailing underscore)
102 |     check_function_exists(${_name}_ ${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
103 |     set(CMAKE_REQUIRED_DEFINITIONS} "")
104 |     set(CMAKE_REQUIRED_LIBRARIES    "")
105 |     mark_as_advanced(${_prefix}_${_name}_${_combined_name}_f2c_WORKS)
106 |     set(_libraries_work ${${_prefix}_${_name}_${_combined_name}_f2c_WORKS})
107 |   endif(_libraries_found AND NOT _libraries_work)
108 | 
109 |   # If not found, test this combination of libraries with a C interface.
110 |   # A few implementations (ie ACML) provide a C interface. Unfortunately, there is no standard.
111 |   if(_libraries_found AND NOT _libraries_work)
112 |     set(${DEFINITIONS} "")
113 |     set(${LIBRARIES}   ${_libraries_found})
114 |     set(CMAKE_REQUIRED_DEFINITIONS "")
115 |     set(CMAKE_REQUIRED_LIBRARIES   ${_flags} ${${LIBRARIES}})
116 |     #message("DEBUG: CMAKE_REQUIRED_LIBRARIES = ${CMAKE_REQUIRED_LIBRARIES}")
117 |     check_function_exists(${_name} ${_prefix}_${_name}${_combined_name}_WORKS)
118 |     set(CMAKE_REQUIRED_LIBRARIES "")
119 |     mark_as_advanced(${_prefix}_${_name}${_combined_name}_WORKS)
120 |     set(_libraries_work ${${_prefix}_${_name}${_combined_name}_WORKS})
121 |   endif(_libraries_found AND NOT _libraries_work)
122 | 
123 |   # on failure
124 |   if(NOT _libraries_work)
125 |     set(${DEFINITIONS} "")
126 |     set(${LIBRARIES}   FALSE)
127 |   endif()
128 |   #message("DEBUG: ${DEFINITIONS} = ${${DEFINITIONS}}")
129 |   #message("DEBUG: ${LIBRARIES} = ${${LIBRARIES}}")
130 | endmacro(check_fortran_libraries)
131 | 
132 | 
133 | #
134 | # main
135 | #
136 | 
137 | # Is it already configured?
138 | if (BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
139 | 
140 |   set(BLAS_FOUND TRUE)
141 | 
142 | else()
143 | 
144 |   # reset variables
145 |   set( BLAS_INCLUDE_DIR "" )
146 |   set( BLAS_DEFINITIONS "" )
147 |   set( BLAS_LINKER_FLAGS "" )
148 |   set( BLAS_LIBRARIES "" )
149 |   set( BLAS_LIBRARIES_DIR "" )
150 | 
151 |     #
152 |     # If Unix, search for BLAS function in possible libraries
153 |     #
154 | 
155 |     # BLAS in ATLAS library? (http://math-atlas.sourceforge.net/)
156 |     if(NOT BLAS_LIBRARIES)
157 |       check_fortran_libraries(
158 |       BLAS_DEFINITIONS
159 |       BLAS_LIBRARIES
160 |       BLAS
161 |       sgemm
162 |       ""
163 |       "cblas;f77blas;atlas"
164 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
165 |       )
166 |     endif()
167 | 
168 |     # BLAS in PhiPACK libraries? (requires generic BLAS lib, too)
169 |     if(NOT BLAS_LIBRARIES)
170 |       check_fortran_libraries(
171 |       BLAS_DEFINITIONS
172 |       BLAS_LIBRARIES
173 |       BLAS
174 |       sgemm
175 |       ""
176 |       "sgemm;dgemm;blas"
177 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
178 |       )
179 |     endif()
180 | 
181 |     # BLAS in Alpha CXML library?
182 |     if(NOT BLAS_LIBRARIES)
183 |       check_fortran_libraries(
184 |       BLAS_DEFINITIONS
185 |       BLAS_LIBRARIES
186 |       BLAS
187 |       sgemm
188 |       ""
189 |       "cxml"
190 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
191 |       )
192 |     endif()
193 | 
194 |     # BLAS in Alpha DXML library? (now called CXML, see above)
195 |     if(NOT BLAS_LIBRARIES)
196 |       check_fortran_libraries(
197 |       BLAS_DEFINITIONS
198 |       BLAS_LIBRARIES
199 |       BLAS
200 |       sgemm
201 |       ""
202 |       "dxml"
203 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
204 |       )
205 |     endif()
206 | 
207 |     # BLAS in Sun Performance library?
208 |     if(NOT BLAS_LIBRARIES)
209 |       check_fortran_libraries(
210 |       BLAS_DEFINITIONS
211 |       BLAS_LIBRARIES
212 |       BLAS
213 |       sgemm
214 |       "-xlic_lib=sunperf"
215 |       "sunperf;sunmath"
216 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
217 |       )
218 |       if(BLAS_LIBRARIES)
219 |         # Extra linker flag
220 |         set(BLAS_LINKER_FLAGS "-xlic_lib=sunperf")
221 |       endif()
222 |     endif()
223 | 
224 |     # BLAS in SCSL library?  (SGI/Cray Scientific Library)
225 |     if(NOT BLAS_LIBRARIES)
226 |       check_fortran_libraries(
227 |       BLAS_DEFINITIONS
228 |       BLAS_LIBRARIES
229 |       BLAS
230 |       sgemm
231 |       ""
232 |       "scsl"
233 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
234 |       )
235 |     endif()
236 | 
237 |     # BLAS in SGIMATH library?
238 |     if(NOT BLAS_LIBRARIES)
239 |       check_fortran_libraries(
240 |       BLAS_DEFINITIONS
241 |       BLAS_LIBRARIES
242 |       BLAS
243 |       sgemm
244 |       ""
245 |       "complib.sgimath"
246 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
247 |       )
248 |     endif()
249 | 
250 |     # BLAS in IBM ESSL library? (requires generic BLAS lib, too)
251 |     if(NOT BLAS_LIBRARIES)
252 |       check_fortran_libraries(
253 |       BLAS_DEFINITIONS
254 |       BLAS_LIBRARIES
255 |       BLAS
256 |       sgemm
257 |       ""
258 |       "essl;blas"
259 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
260 |       )
261 |     endif()
262 | 
263 |     #BLAS in intel mkl 10 library? (em64t 64bit)
264 |     if(NOT BLAS_LIBRARIES)
265 |       check_fortran_libraries(
266 |       BLAS_DEFINITIONS
267 |       BLAS_LIBRARIES
268 |       BLAS
269 |       sgemm
270 |       ""
271 |       "mkl_intel_lp64;mkl_intel_thread;mkl_core;guide;pthread"
272 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
273 |       )
274 |     endif()
275 | 
276 |     ### windows version of intel mkl 10?
277 |     if(NOT BLAS_LIBRARIES)
278 |       check_fortran_libraries(
279 |       BLAS_DEFINITIONS
280 |       BLAS_LIBRARIES
281 |       BLAS
282 |       SGEMM
283 |       ""
284 |       "mkl_c_dll;mkl_intel_thread_dll;mkl_core_dll;libguide40"
285 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
286 |       )
287 |     endif()
288 | 
289 |     #older versions of intel mkl libs
290 | 
291 |     # BLAS in intel mkl library? (shared)
292 |     if(NOT BLAS_LIBRARIES)
293 |       check_fortran_libraries(
294 |       BLAS_DEFINITIONS
295 |       BLAS_LIBRARIES
296 |       BLAS
297 |       sgemm
298 |       ""
299 |       "mkl;guide;pthread"
300 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
301 |       )
302 |     endif()
303 | 
304 |     #BLAS in intel mkl library? (static, 32bit)
305 |     if(NOT BLAS_LIBRARIES)
306 |       check_fortran_libraries(
307 |       BLAS_DEFINITIONS
308 |       BLAS_LIBRARIES
309 |       BLAS
310 |       sgemm
311 |       ""
312 |       "mkl_ia32;guide;pthread"
313 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
314 |       )
315 |     endif()
316 | 
317 |     #BLAS in intel mkl library? (static, em64t 64bit)
318 |     if(NOT BLAS_LIBRARIES)
319 |       check_fortran_libraries(
320 |       BLAS_DEFINITIONS
321 |       BLAS_LIBRARIES
322 |       BLAS
323 |       sgemm
324 |       ""
325 |       "mkl_em64t;guide;pthread"
326 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
327 |       )
328 |     endif()
329 | 
330 |     #BLAS in acml library?
331 |     if(NOT BLAS_LIBRARIES)
332 |       check_fortran_libraries(
333 |       BLAS_DEFINITIONS
334 |       BLAS_LIBRARIES
335 |       BLAS
336 |       sgemm
337 |       ""
338 |       "acml"
339 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
340 |       )
341 |     endif()
342 | 
343 |     # Apple BLAS library?
344 |     if(NOT BLAS_LIBRARIES)
345 |       check_fortran_libraries(
346 |       BLAS_DEFINITIONS
347 |       BLAS_LIBRARIES
348 |       BLAS
349 |       sgemm
350 |       ""
351 |       "Accelerate"
352 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
353 |       )
354 |     endif()
355 | 
356 |     if ( NOT BLAS_LIBRARIES )
357 |       check_fortran_libraries(
358 |       BLAS_DEFINITIONS
359 |       BLAS_LIBRARIES
360 |       BLAS
361 |       sgemm
362 |       ""
363 |       "vecLib"
364 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
365 |       )
366 |     endif ( NOT BLAS_LIBRARIES )
367 | 
368 |     # Generic BLAS library?
369 |     # This configuration *must* be the last try as this library is notably slow.
370 |     if ( NOT BLAS_LIBRARIES )
371 |       check_fortran_libraries(
372 |       BLAS_DEFINITIONS
373 |       BLAS_LIBRARIES
374 |       BLAS
375 |       sgemm
376 |       ""
377 |       "blas"
378 |       "${CGAL_TAUCS_LIBRARIES_DIR} ENV BLAS_LIB_DIR"
379 |       )
380 |     endif()
381 | 
382 |   if(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
383 |     set(BLAS_FOUND TRUE)
384 |   else()
385 |     set(BLAS_FOUND FALSE)
386 |   endif()
387 | 
388 |   if(NOT BLAS_FIND_QUIETLY)
389 |     if(BLAS_FOUND)
390 |       message(STATUS "A library with BLAS API found.")
391 |     else(BLAS_FOUND)
392 |       if(BLAS_FIND_REQUIRED)
393 |         message(FATAL_ERROR "A required library with BLAS API not found. Please specify library location.")
394 |       else()
395 |         message(STATUS "A library with BLAS API not found. Please specify library location.")
396 |       endif()
397 |     endif(BLAS_FOUND)
398 |   endif(NOT BLAS_FIND_QUIETLY)
399 | 
400 |   # Add variables to cache
401 |   set( BLAS_INCLUDE_DIR   "${BLAS_INCLUDE_DIR}"
402 |                           CACHE PATH "Directories containing the BLAS header files" FORCE )
403 |   set( BLAS_DEFINITIONS   "${BLAS_DEFINITIONS}"
404 |                           CACHE STRING "Compilation options to use BLAS" FORCE )
405 |   set( BLAS_LINKER_FLAGS  "${BLAS_LINKER_FLAGS}"
406 |                           CACHE STRING "Linker flags to use BLAS" FORCE )
407 |   set( BLAS_LIBRARIES     "${BLAS_LIBRARIES}"
408 |                           CACHE FILEPATH "BLAS libraries name" FORCE )
409 |   set( BLAS_LIBRARIES_DIR "${BLAS_LIBRARIES_DIR}"
410 |                           CACHE PATH "Directories containing the BLAS libraries" FORCE )
411 | 
412 |   #message("DEBUG: BLAS_INCLUDE_DIR = ${BLAS_INCLUDE_DIR}")
413 |   #message("DEBUG: BLAS_DEFINITIONS = ${BLAS_DEFINITIONS}")
414 |   #message("DEBUG: BLAS_LINKER_FLAGS = ${BLAS_LINKER_FLAGS}")
415 |   #message("DEBUG: BLAS_LIBRARIES = ${BLAS_LIBRARIES}")
416 |   #message("DEBUG: BLAS_LIBRARIES_DIR = ${BLAS_LIBRARIES_DIR}")
417 |   #message("DEBUG: BLAS_FOUND = ${BLAS_FOUND}")
418 | 
419 | endif(BLAS_LIBRARIES_DIR OR BLAS_LIBRARIES)
420 | 


--------------------------------------------------------------------------------
/src/system.cpp:
--------------------------------------------------------------------------------
  1 | #include "system.h"
  2 | #include <typeinfo>
  3 | 
  4 | using namespace std;
  5 | using namespace pcl;
  6 | using namespace cv;
  7 | 
  8 | void SLAMsystem(string commonPath, string yamlPath){
  9 | 
 10 | 	STEREO_RECTIFY_PARAMS srp; // used to store
 11 | 	//================obtain file name================================
 12 | 	string leftImgFilePath, rightImgFilePath;
 13 | 	leftImgFilePath = commonPath + "/image_0";
 14 | 	rightImgFilePath = commonPath + "/image_1";
 15 | 
 16 | 	vector<string> leftImgName = getImgFileName(leftImgFilePath);
 17 | 	vector<string> rightImgName = getImgFileName(rightImgFilePath);
 18 | 
 19 | 	//================read settings===================================
 20 | 	cv::FileStorage fsSettings(yamlPath, cv::FileStorage::READ);
 21 | 	if(!fsSettings.isOpened()){
 22 | 		cerr << "ERROR: Wrong path to settings" << endl;
 23 | 		return;
 24 | 	}
 25 | 	string savePathBefore = fsSettings["trajectory file before"];
 26 | 	string savePath = fsSettings["trajectory file after"];
 27 | 	fsSettings["LEFT.P"] >> srp.P1;
 28 | 	fsSettings["RIGHT.P"] >> srp.P2;
 29 | 
 30 | 	srp.imageSize.height = fsSettings["height"];
 31 | 	srp.imageSize.width = fsSettings["width"];
 32 | 	//================threshold======================================
 33 | 	int localTimes = fsSettings["local optimization times"];
 34 | 	int dataLength = fsSettings["data length"];
 35 | 	int startIdx = fsSettings["start index"];
 36 | 
 37 | 	//================set up data length ============================
 38 | 	if(dataLength == 0){
 39 | 		dataLength = (int)leftImgName.size();
 40 | 	}
 41 | 	else{
 42 | 		vector<string>::const_iterator first = leftImgName.begin()+ startIdx;
 43 | 		vector<string>::const_iterator last = leftImgName.begin()+ startIdx + dataLength;
 44 | 		vector<string> tempLeft(first, last);
 45 | 		leftImgName = tempLeft;
 46 | 
 47 | 		first = rightImgName.begin()+ startIdx;
 48 | 		last = rightImgName.begin() + startIdx+ dataLength;
 49 | 		vector<string> tempRight(first, last);
 50 | 		rightImgName = tempRight;
 51 | 	}
 52 | 	if(srp.P1.empty() || srp.P2.empty()){
 53 | 		cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
 54 | 		return;
 55 | 	}
 56 | 
 57 | 	//=============== initialize system ============================================
 58 | 	vector<cv::Mat> relativeRvec_cam, relativeTvec_cam;
 59 | 	Map map;
 60 | 	Mapviewer mapviewer;
 61 | 	vector<Frame* > allFrame;
 62 | 	vector<cv::Point3f> cameraCenters;
 63 | 
 64 | 	int deleteFrame = -5;
 65 | 	int minFrameIdDist=10;
 66 | 	float maxFramePoseDist=120;
 67 | 	float matchThreshold=15.0;
 68 | 	int K = 10;
 69 | 	int lastGlobalBAFrame=0;
 70 | 
 71 | 	Problem globalBAProblem;
 72 | 	Optimizer ba(false, globalBAProblem);
 73 | 
 74 | 	pcl::visualization::CloudViewer viewer("Cloud Viewer");
 75 | 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
 76 | 
 77 | 	cv::Mat accumRvec = cv::Mat::zeros(3,1,CV_64F);
 78 | 	cv::Mat accumTvec = cv::Mat::zeros(3,1,CV_64F);
 79 | 
 80 | 	Frame* prevFrame = new Frame(leftImgName[0], rightImgName[0], srp, 0, &map);
 81 | 	prevFrame->scenePtsinWorld = prevFrame->scenePts;
 82 | 
 83 | 	allFrame.push_back(prevFrame);
 84 | 
 85 | 	Frame* refFrame = allFrame.back();
 86 | 
 87 | 	for(int n = 1; n < dataLength; n++){
 88 | 		cout << endl<<endl<<"current frame number: " << n << endl;
 89 | 		Frame* currFrame = new Frame(leftImgName[n], rightImgName[n], srp, n, &map);
 90 | 		prevFrame->matchFrame(currFrame);
 91 | 		
 92 | 		//=================== accumlate motion ====================
 93 | 		relativeRvec_cam.push_back(currFrame->rvec);
 94 | 		relativeTvec_cam.push_back(currFrame->tvec);
 95 | 		getAccumulateMotion(relativeRvec_cam, relativeTvec_cam,
 96 | 				0, n, accumRvec, accumTvec);
 97 | 
 98 | 		currFrame->setWrdTransVectorAndTransScenePts(accumRvec, accumTvec);
 99 | 		cout << accumTvec.at<double>(0,0) << "  " << accumTvec.at<double>(1,0) << "  " << accumTvec.at<double>(2,0) << endl;
100 | 		prevFrame->manageMapPoints(currFrame);
101 | 		allFrame.push_back(currFrame);
102 | 
103 | 		//=================== pose tree ===============================
104 | 		cout<<"\nBuiling KD Tree\n";
105 | 		if (!lastGlobalBAFrame || n-lastGlobalBAFrame>10) 
106 | 		{
107 | 			cv::Point3f currCameraCenter=getCameraCenter(accumRvec, accumTvec);
108 | 			cameraCenters.push_back(currCameraCenter);
109 | 			pcl::PointCloud<pcl::PointXYZ> cameraCloud = Point3ftoPointCloud(cameraCenters);
110 | 			pcl::PointCloud<pcl::PointXYZ>::Ptr cameraCloudPtr(new pcl::PointCloud<pcl::PointXYZ>(cameraCloud));
111 | 			pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
112 | 
113 | 			kdtree.setInputCloud(cameraCloudPtr);
114 | 			pcl::PointXYZ searchPoint;
115 | 			searchPoint.x=currCameraCenter.x;
116 | 			searchPoint.y=currCameraCenter.y;
117 | 			searchPoint.z=currCameraCenter.z;
118 | 
119 | 			vector<int> pointIdxNKNSearch(K);
120 | 			vector<float> pointNKNSquaredDistance(K);
121 | 
122 | 			kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
123 | 			vector<int> nearFrames;
124 | 			for (int i=0; i<pointIdxNKNSearch.size();i++){
125 | 				if (pointNKNSquaredDistance[i]>0 &&
126 | 						pointNKNSquaredDistance[i]<=maxFramePoseDist &&
127 | 						(currFrame->frameID - pointIdxNKNSearch[i]-1)>minFrameIdDist){
128 | 					nearFrames.push_back(pointIdxNKNSearch[i]+1);
129 | 				}
130 | 			}
131 | 
132 | 			cout<<"\nSquared distances of frame "<<n<<" from frames (Frame:Dist)\n";
133 | 			for (int i=0;i<pointIdxNKNSearch.size();i++)
134 | 				cout<<pointIdxNKNSearch[i]+1<<":"<<pointNKNSquaredDistance[i]<<"||";
135 | 			cout<<endl;
136 | 
137 | 			if (nearFrames.size())
138 | 			{
139 | 				cout<<"\nChecking for loops for frame"<<n<<"!!!\nNumber of Near Frames: "<<nearFrames.size()<<endl; 
140 | 				int maxFrame=-1;
141 | 				float maxRatio=-1;
142 | 				for(int i=0;i<nearFrames.size();i++) 
143 | 				{
144 | 					cout<<"Near Frame: "<<nearFrames[i]<<endl;
145 | 					vector<cv::KeyPoint> matchedPrev, matchedCurr;
146 | 					vector<cv::DMatch> matches;
147 | 					cout<<"Matching Feature\n";
148 | 					currFrame->matchFeatureKNN(currFrame->despL, allFrame[nearFrames[i]-1]->despL,
149 | 							currFrame->keypointL, allFrame[nearFrames[i]]->keypointL,
150 | 							matchedPrev, matchedCurr, matches, 0.8);
151 | 					cout<<"Features matched";
152 | 					float matchRatio=(matches.size()+1.0)*100.0/(currFrame->keypointL.size()+1);
153 | 					cout<<" Match Ratio: "<<matchRatio<<endl;
154 | 					if (matchRatio>=matchThreshold)
155 | 					{
156 | 						cout<<"\nMatch Found!!!\n";
157 | 						if (matchRatio>maxRatio)
158 | 						{
159 | 							maxFrame=nearFrames[i];
160 | 							maxRatio=matchRatio;
161 | 						}
162 | 					}
163 | 				}
164 | 				if (maxFrame!=-1)
165 | 				{
166 | 					cout<<"Best frame: "<<maxFrame<<" with ration: "<<maxRatio<<endl;
167 | 					currFrame->matchFrame(allFrame[maxFrame]);
168 | 					currFrame->manageMapPoints(allFrame[maxFrame]);
169 | 					currFrame->addEdgeConstrain(allFrame[maxFrame]->frameID, 
170 | 							currFrame->rvec,
171 | 							currFrame->tvec);
172 | 					cout<<"creating pose graph..." << endl;
173 | 					PoseOpt poseOpt;
174 | 					for(int j = 0; j < allFrame.size(); j++){
175 | 						poseOpt.addNode(allFrame, j);   
176 | 					}
177 | 					for(int j = 0; j < allFrame.size(); j++){
178 | 						poseOpt.addEdge(allFrame, j);
179 | 					}
180 | 					poseOpt.solve();
181 | 					for(int j = 1; j < allFrame.size(); j++){
182 | 
183 | 						getRelativeMotion(allFrame[j-1]->worldRvec, allFrame[j-1]->worldTvec,
184 | 								allFrame[j]->worldRvec,   allFrame[j]->worldTvec,
185 | 								relativeRvec_cam[j-1],
186 | 								relativeTvec_cam[j-1]);
187 | 					}
188 | 					cv::Mat tempAccumRvec, tempAccumTvec;
189 | 					for(int j = 0; j < allFrame.size(); j++) {
190 | 
191 | 						getAccumulateMotion(relativeRvec_cam, relativeTvec_cam,
192 | 								0, j, tempAccumRvec, tempAccumTvec);
193 | 						allFrame[j]->setWrdTransVectorAndTransScenePts(tempAccumRvec,tempAccumTvec);
194 | 					}
195 | 					cout<<"Pose-graph Optimization Done!!!\n";
196 | 					lastGlobalBAFrame=n;
197 | 					continue;
198 | 				}
199 | 			}
200 | 		}
201 | 
202 | 		//=================== perform more matching ====================
203 | 		double prevMove = 0;
204 | 		if(n > 0){
205 | 			for(int t = localTimes-1; t > 0; t--){
206 | 				refFrame->matchFrame(allFrame[MAX(0, n-t)], true);
207 | 				refFrame->manageMapPoints(allFrame[MAX(0, n-t)]);
208 | 			}
209 | 			ba.localBundleAdjustment(allFrame, MAX(0, (int)allFrame.size()-localTimes), localTimes);
210 | 
211 | 			//============== update accumulative transformation ================
212 | 			for(int n = MAX(0, (int)allFrame.size()-localTimes)+1; n < allFrame.size(); n++) {
213 | 
214 | 				getRelativeMotion(allFrame[n-1]->worldRvec, allFrame[n-1]->worldTvec,
215 | 						allFrame[n]->worldRvec,   allFrame[n]->worldTvec,
216 | 						relativeRvec_cam[n-1],
217 | 						relativeTvec_cam[n-1]);
218 | 				allFrame[n-1]->addEdgeConstrain(n,
219 | 						relativeRvec_cam[n-1],
220 | 						relativeTvec_cam[n-1]);
221 | 			}
222 | 		}
223 | 
224 | 		//=============draw map=============================================
225 | 		cv::Mat tempAccumRvec, tempAccumTvec;
226 | 		for(int n = MAX(0, (int)allFrame.size()-localTimes); n < allFrame.size(); n++) {
227 | 
228 | 			getAccumulateMotion(relativeRvec_cam, relativeTvec_cam,
229 | 					0, n, tempAccumRvec, tempAccumTvec);
230 | 			allFrame[n]->setWrdTransVectorAndTransScenePts(tempAccumRvec,tempAccumTvec);
231 | 		}
232 | 		Eigen::Affine3d curTrans =  Eigen::Affine3d::Identity();
233 | 		Eigen::Affine3d curCam = vectorToTransformation(tempAccumRvec, tempAccumTvec);
234 | 		vector<cv::Point3f> allPoints = map.getAllMapPoints();
235 | 
236 | 		mapviewer.addCamera(curCam);
237 | 		mapviewer.jointToMap(mapviewer.pointToPointCloud(allPoints), curTrans);
238 | 		*cloud = mapviewer.entireMap;
239 | 
240 | 		viewer.showCloud(cloud);
241 | 		if(cv::waitKey(5) == 27){};
242 | 		
243 | 		//============ prepare next loop ================================
244 | 		refFrame = allFrame[MAX(0, (int)allFrame.size()-localTimes-1 )];
245 | 		prevFrame = allFrame.back();
246 | 
247 | 		deleteFrame++;
248 | 		if(deleteFrame > 15 && deleteFrame < 1570) {
249 | 		//	allFrame[deleteFrame]->releaseMemory();
250 | 		}
251 | 	}
252 | 	//============== end of main loop =========================================================================
253 | 	
254 | 	ofstream outfile;
255 | 	outfile.open(savePathBefore);
256 | 	//evaluate before global BA
257 | 	for(int n = 0; n < dataLength; n++){
258 | 		getAccumulateMotion(relativeRvec_cam, relativeTvec_cam,
259 | 				0, n,
260 | 				accumRvec, accumTvec);
261 | 
262 | 		double diff1 = allFrame[n]->worldRvec.at<double>(0,0) - accumRvec.at<double>(0,0);
263 | 		cout << setprecision(15) <<diff1/accumRvec.at<double>(0,0) * 100.0 <<"\%" <<"  ";
264 | 
265 | 		double diff2 = allFrame[n]->worldRvec.at<double>(1,0) - accumRvec.at<double>(1,0);
266 | 		cout << setprecision(15)<< diff2/accumRvec.at<double>(1,0) * 100.0 <<"\%" <<"  ";
267 | 
268 | 		double diff3 = allFrame[n]->worldRvec.at<double>(2,0) - accumRvec.at<double>(2,0);
269 | 		cout << setprecision(15)<< diff3/accumRvec.at<double>(2,0) * 100.0 <<"\%" <<"  ";
270 | 
271 | 		double diffx = allFrame[n]->worldTvec.at<double>(0,0) - accumTvec.at<double>(0,0);
272 | 		cout << setprecision(15)<< diffx/accumTvec.at<double>(0,0) * 100.0 <<"\%" <<"  ";
273 | 
274 | 		double diffy = allFrame[n]->worldTvec.at<double>(1,0) - accumTvec.at<double>(1,0);
275 | 		cout << setprecision(15)<< diffy/accumTvec.at<double>(1,0) * 100.0 <<"\%" <<"  ";
276 | 
277 | 		double diffz = allFrame[n]->worldTvec.at<double>(2,0) - accumTvec.at<double>(2,0);
278 | 		cout << setprecision(15)<< diffz/accumTvec.at<double>(2,0) * 100.0 <<"\%" <<endl;
279 | 
280 | 		Eigen::Affine3d curPose = vectorToTransformation(accumRvec, accumTvec);
281 | 		Eigen::Affine3d invPose = curPose.inverse();
282 | 
283 | 		for(int r = 0; r < 3; r++){
284 | 			for(int c = 0; c < 4; c++){
285 | 				outfile << invPose(r,c) << " ";
286 | 			}
287 | 		}
288 | 		outfile << endl;
289 | 	}
290 | 	outfile.close();
291 | 	outfile.open(savePath);
292 | 
293 | 	//======== test: manually add loop ======================
294 | 	cout<<"creating pose graph..." << endl;
295 | 	PoseOpt poseOpt;
296 | 	for(int n = 0; n < allFrame.size(); n++){
297 | 		poseOpt.addNode(allFrame, n);   
298 | 	}
299 | 	for(int n = 0; n < allFrame.size(); n++){
300 | 		poseOpt.addEdge(allFrame, n);
301 | 	}
302 | 	poseOpt.solve();
303 | 	cv::Mat tempAccumRvec, tempAccumTvec;
304 | 	for(int j = 1; j < allFrame.size(); j++){
305 | 
306 | 		getRelativeMotion(allFrame[j-1]->worldRvec, allFrame[j-1]->worldTvec,
307 | 				allFrame[j]->worldRvec,   allFrame[j]->worldTvec,
308 | 				relativeRvec_cam[j-1],
309 | 				relativeTvec_cam[j-1]);
310 | 	}
311 | 	for(int j = 0; j < allFrame.size(); j++) {
312 | 
313 | 		getAccumulateMotion(relativeRvec_cam, relativeTvec_cam,
314 | 				0, j, tempAccumRvec, tempAccumTvec);
315 | 		allFrame[j]->setWrdTransVectorAndTransScenePts(tempAccumRvec,tempAccumTvec);
316 | 	}
317 | 
318 | 	//======== extract poses after g2o =================================
319 | 	for(int n = 0; n < poseOpt.optimizer.vertices().size(); n++){
320 | 		g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(poseOpt.optimizer.vertex(n));
321 | 		Eigen::Isometry3d pose = v->estimate();
322 | 		for(int r = 0; r < 3; r++){
323 | 			for(int c = 0; c < 4; c++){
324 | 				outfile << pose(r,c) << "  ";
325 | 			}
326 | 		}
327 | 		outfile << endl;
328 | 	}
329 | 	outfile.close();
330 | 	//======= update pose after pose optimization ======================
331 | 	outfile.open("../09recordPose.txt");
332 | 	for(int n = 0; n < poseOpt.optimizer.vertices().size(); n++){
333 | 		g2o::VertexSE3* v = dynamic_cast<g2o::VertexSE3*>(poseOpt.optimizer.vertex(n));
334 | 		Eigen::Isometry3d pose = v->estimate();
335 | 		Eigen::Affine3d affinePose;
336 | 		for(int r = 0; r < 4; r++){
337 | 			for(int c = 0; c < 4; c++){
338 | 				affinePose(r,c) = pose(r,c);
339 | 			}
340 | 		}
341 | 		affinePose = affinePose.inverse();
342 | 		transformationToVector(affinePose, allFrame[n]->worldRvec, allFrame[n]->worldTvec);
343 | 		outfile << allFrame[n]->worldRvec.at<double>(0,0)<<" "<<allFrame[n]->worldRvec.at<double>(1,0)<<" "<<allFrame[n]->worldRvec.at<double>(2,0)<<" ";
344 | 		outfile << allFrame[n]->worldTvec.at<double>(0,0)<<" "<<allFrame[n]->worldTvec.at<double>(1,0)<<" "<<allFrame[n]->worldTvec.at<double>(2,0)<<endl; 
345 | 	}
346 | 	outfile.close();
347 | 	//======== test ends ===============================================
348 | 	
349 | 	//=========== draw points after BA ================================
350 | 	long unsigned int totalNumPoints = map.allMapPointNumber();
351 | 	// convert to Point3f
352 | 	Eigen::Affine3d curTrans =  Eigen::Affine3d::Identity();
353 | 
354 | 	vector<cv::Point3f> afterBAPoints3f;
355 | 	for(set<MapPoint*>::iterator pIt = map.allMapPoints.begin();
356 | 			pIt!= map.allMapPoints.end();
357 | 			pIt++){
358 | 		if((*pIt)->isBad == false){
359 | 			cv::Point3f pt;
360 | 			pt.x = (*pIt)->pos.x;
361 | 			pt.y = (*pIt)->pos.y;
362 | 			pt.z = (*pIt)->pos.z;
363 | 			afterBAPoints3f.push_back(pt);
364 | 		}
365 | 	}
366 | 
367 | 	mapviewer.addMorePoints(mapviewer.pointToPointCloud(afterBAPoints3f,0,255,0), curTrans);
368 | 	*cloud = mapviewer.entireMap;
369 | 	viewer.showCloud(cloud);
370 | 	if(cv::waitKey(5) == 27){};
371 | 	cout<<"Finished!!!"<<endl;
372 | 	cv::waitKey(0);
373 | }
374 | 


--------------------------------------------------------------------------------
/include/snavely_reprojection_error.h:
--------------------------------------------------------------------------------
  1 | // Ceres Solver - A fast non-linear least squares minimizer
  2 | // Copyright 2015 Google Inc. All rights reserved.
  3 | // http://ceres-solver.org/
  4 | //
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //
  8 | // * Redistributions of source code must retain the above copyright notice,
  9 | //   this list of conditions and the following disclaimer.
 10 | // * Redistributions in binary form must reproduce the above copyright notice,
 11 | //   this list of conditions and the following disclaimer in the documentation
 12 | //   and/or other materials provided with the distribution.
 13 | // * Neither the name of Google Inc. nor the names of its contributors may be
 14 | //   used to endorse or promote products derived from this software without
 15 | //   specific prior written permission.
 16 | //
 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | // POSSIBILITY OF SUCH DAMAGE.
 28 | //
 29 | // Author: sameeragarwal@google.com (Sameer Agarwal)
 30 | //
 31 | // Templated struct implementing the camera model and residual
 32 | // computation for bundle adjustment used by Noah Snavely's Bundler
 33 | // SfM system. This is also the camera model/residual for the bundle
 34 | // adjustment problems in the BAL dataset. It is templated so that we
 35 | // can use Ceres's automatic differentiation to compute analytic
 36 | // jacobians.
 37 | //
 38 | // For details see: http://phototour.cs.washington.edu/bundler/
 39 | // and http://grail.cs.washington.edu/projects/bal/
 40 | #ifndef CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_
 41 | #define CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_
 42 | #include "ceres/rotation.h"
 43 | #include <stdio.h>
 44 | using namespace std;
 45 | namespace ceres {
 46 | namespace examples {
 47 | // Templated pinhole camera model for used with Ceres.  The camera is
 48 | // parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
 49 | // focal length and 2 for radial distortion. The principal point is not modeled
 50 | // (i.e. it is assumed be located at the image center).
 51 | struct SnavelyReprojectionError {
 52 |   SnavelyReprojectionError(double observed_x, double observed_y, double f, double cx, double cy)
 53 |       : observed_x(observed_x), observed_y(observed_y), f(f), cx(cx), cy(cy) {}
 54 | 
 55 |   template <typename T>
 56 |   bool operator()(const T* const camera,
 57 |                   const T* const point,
 58 |                   T* residuals) const {
 59 |     // camera[0,1,2] are the angle-axis rotation.
 60 |     T p[3];
 61 |     AngleAxisRotatePoint(camera, point, p);
 62 |     // camera[3,4,5] are the translation.
 63 |     p[0] += camera[3];
 64 |     p[1] += camera[4];
 65 |     p[2] += camera[5];
 66 |     // Compute the center of distortion. The sign change comes from
 67 |     // the camera model that Noah Snavely's Bundler assumes, whereby
 68 |     // the camera coordinate system has a negative z axis.
 69 |     const T xp = T(f) * p[0] / p[2] + T(cx);
 70 |     const T yp = T(f) * p[1] / p[2] + T(cy);
 71 | 
 72 |     const T predicted_x = xp;
 73 |     const T predicted_y = yp;
 74 | 
 75 | 
 76 |     // The error is the difference between the predicted and observed position.
 77 |     residuals[0] = predicted_x - observed_x;
 78 |     residuals[1] = predicted_y - observed_y;
 79 | 
 80 |     // std::cout << "predicted: " << focal * distortion * xp-observed_x<< "  "<<focal * distortion * yp-observed_y << std::endl;
 81 |     return true;
 82 |   }
 83 |   // Factory to hide the construction of the CostFunction object from
 84 |   // the client code.
 85 |   static ceres::CostFunction* Create(const double observed_x,
 86 |                                      const double observed_y,
 87 |                                      const double f,
 88 |                                      const double cx,
 89 |                                      const double cy) {
 90 |     return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 6, 3>(new SnavelyReprojectionError(observed_x, observed_y, f, cx, cy)));
 91 |   }
 92 |   double observed_x;
 93 |   double observed_y;
 94 |   double f, cx, cy;
 95 | };
 96 | 
 97 | struct PoseSmoothnessError
 98 | {
 99 |     Eigen::Affine3d fromSrcToRefInv;
100 |     Eigen::Affine3d refPoseInv;
101 |     double rScale;
102 |     double tScale;
103 | 
104 |     PoseSmoothnessError(const Eigen::Affine3d _fromSrcToRefInv, const Eigen::Affine3d _refPoseInv,
105 |                         const double _rScale, const double _tScale):
106 |     fromSrcToRefInv(_fromSrcToRefInv.matrix()), refPoseInv(_refPoseInv.matrix()), rScale(_rScale), tScale(_tScale)
107 |     {}
108 | 
109 |     // Factory to hide the construction of the CostFunction object from the client code.
110 |     static ceres::CostFunction* Create(const Eigen::Affine3d _fromSrcToRefInv, 
111 |                                        const Eigen::Affine3d _refPoseInv,
112 |                                        const double _rScale, 
113 |                                        const double _tScale) {
114 |         return (new ceres::AutoDiffCostFunction<PoseSmoothnessError, 6, 6>(new PoseSmoothnessError(_fromSrcToRefInv, _refPoseInv, _rScale, _tScale)));
115 |     }
116 | 
117 |     template <typename T>
118 |     bool operator()(const T* const camera, 
119 |                           T* residuals) const
120 |     {
121 | 
122 |         Eigen::Matrix<T, 3, 3> RSrcPose;
123 |         ceres::AngleAxisToRotationMatrix(camera, RSrcPose.data());
124 | 
125 |         Eigen::Matrix<T, 4, 4> SrcPoseT;
126 |         Eigen::Matrix<T, 4, 4> curFromSrcToRefT;
127 |         Eigen::Matrix<T, 4, 4> posDifT;
128 | 
129 |         SrcPoseT.block(0, 0, 3, 3) = RSrcPose;
130 |         SrcPoseT(0, 3) = camera[3];
131 |         SrcPoseT(1, 3) = camera[4];
132 |         SrcPoseT(2, 3) = camera[5];
133 |         SrcPoseT(3, 0) = T(0);
134 |         SrcPoseT(3, 1) = T(0);
135 |         SrcPoseT(3, 2) = T(0);
136 |         SrcPoseT(3, 3) = T(1.0);
137 |         
138 |         Eigen::Matrix<T, 4, 4> fromSrcToRefInvT;
139 |         Eigen::Matrix<T, 4, 4> refPoseInvT;
140 |         for (int i = 0; i < 4; ++i)
141 |         {
142 |             for (int j = 0; j < 4; ++j)
143 |             {
144 |                 fromSrcToRefInvT(i,j) = T(fromSrcToRefInv(i,j));
145 |                 refPoseInvT(i,j) = T(refPoseInv(i,j));
146 |             }
147 |         }
148 | 
149 |         curFromSrcToRefT = refPoseInvT * SrcPoseT;
150 |         posDifT = fromSrcToRefInvT * curFromSrcToRefT;
151 | 
152 |         Eigen::Matrix<T, 3, 3> posDifRT;
153 |         for (int i = 0; i < 3; ++i)
154 |         {
155 |             for (int j = 0; j < 3; ++j)
156 |             {
157 |                 posDifRT(i,j) = posDifT(i,j);
158 |             }
159 |         }
160 | 
161 |         T posDifInParams[6];
162 |         ceres::RotationMatrixToAngleAxis(posDifRT.data(), posDifInParams);
163 |         posDifInParams[3] = posDifT(0, 3);
164 |         posDifInParams[4] = posDifT(1, 3);
165 |         posDifInParams[5] = posDifT(2, 3);
166 |         // cout << "posDifInParams:\n";
167 | 
168 |         residuals[0] = T(rScale)*posDifInParams[0];
169 |         residuals[1] = T(rScale)*posDifInParams[1];
170 |         residuals[2] = T(rScale)*posDifInParams[2];
171 |         residuals[3] = T(tScale)*posDifInParams[3];
172 |         residuals[4] = T(tScale)*posDifInParams[4];
173 |         residuals[5] = T(tScale)*posDifInParams[5];
174 | 
175 |         return true;
176 |     }
177 | };
178 | 
179 | struct BinaryPoseSmoothnessError
180 | {
181 |     Eigen::Affine3d fromSrcToRef;
182 |     double rScale;
183 |     double tScale;
184 | 
185 |     BinaryPoseSmoothnessError(const Eigen::Affine3d _fromSrcToRef,
186 |                         const double _rScale, const double _tScale):
187 |     fromSrcToRef(_fromSrcToRef.matrix()), rScale(_rScale), tScale(_tScale)
188 |     {}
189 | 
190 |     // Factory to hide the construction of the CostFunction object from the client code.
191 |     static ceres::CostFunction* Create(const Eigen::Affine3d _fromSrcToRef,
192 |                                         const double _rScale, const double _tScale)
193 |     {
194 |         return (new ceres::NumericDiffCostFunction<BinaryPoseSmoothnessError, ceres::CENTRAL, 6, 6, 6>(new BinaryPoseSmoothnessError(_fromSrcToRef, _rScale, _tScale)));
195 |     }
196 | 
197 |     template <typename T>
198 |     bool operator()(const T * const srcPoseParam, const T * const refPoseParam, T* residuals) const
199 |     {
200 | 
201 |         Eigen::Matrix<T, 3, 3> srcPoseR;
202 |         Eigen::Matrix<T, 3, 3> refPoseR;
203 |         ceres::AngleAxisToRotationMatrix(srcPoseParam, srcPoseR.data());
204 |         ceres::AngleAxisToRotationMatrix(refPoseParam, refPoseR.data());
205 | 
206 |         Eigen::Matrix<T, 4, 4> srcPoseT;
207 |         Eigen::Matrix<T, 4, 4> refPoseT;
208 |         Eigen::Matrix<T, 4, 4> curFromRefToSrcT;
209 |         Eigen::Matrix<T, 4, 4> posDifT;
210 | 
211 |         srcPoseT.block(0, 0, 3, 3) = srcPoseR;
212 |         srcPoseT(0, 3) = srcPoseParam[3];
213 |         srcPoseT(1, 3) = srcPoseParam[4];
214 |         srcPoseT(2, 3) = srcPoseParam[5];
215 |         srcPoseT(3, 0) = T(0);
216 |         srcPoseT(3, 1) = T(0);
217 |         srcPoseT(3, 2) = T(0);
218 |         srcPoseT(3, 3) = T(1.0);
219 | 
220 |         refPoseT.block(0, 0, 3, 3) = refPoseR;
221 |         refPoseT(0, 3) = refPoseParam[3];
222 |         refPoseT(1, 3) = refPoseParam[4];
223 |         refPoseT(2, 3) = refPoseParam[5];
224 |         refPoseT(3, 0) = T(0);
225 |         refPoseT(3, 1) = T(0);
226 |         refPoseT(3, 2) = T(0);
227 |         refPoseT(3, 3) = T(1.0);
228 |         Eigen::Matrix<T, 4, 4> fromSrcToRefT;
229 |         for (int i = 0; i < 4; ++i)
230 |         {
231 |             for (int j = 0; j < 4; ++j)
232 |             {
233 |                 fromSrcToRefT(i,j) = T(fromSrcToRef(i,j));
234 |             }
235 |         }
236 | 
237 |         curFromRefToSrcT = srcPoseT.inverse() * refPoseT;
238 |         posDifT = fromSrcToRefT * curFromRefToSrcT;
239 | 
240 |         Eigen::Matrix<T, 3, 3> posDifRT;
241 |         for (int i = 0; i < 3; ++i)
242 |         {
243 |             for (int j = 0; j < 3; ++j)
244 |             {
245 |                 posDifRT(i,j) = posDifT(i,j);
246 |             }
247 |         }
248 | 
249 |         T posDifInParams[6];
250 |         ceres::RotationMatrixToAngleAxis(posDifRT.data(), posDifInParams);
251 |         posDifInParams[3] = posDifT(0, 3);
252 |         posDifInParams[4] = posDifT(1, 3);
253 |         posDifInParams[5] = posDifT(2, 3);
254 | //        // cout << "posDifInParams:\n";
255 | 
256 |         residuals[0] = T(rScale)*posDifInParams[0];
257 |         residuals[1] = T(rScale)*posDifInParams[1];
258 |         residuals[2] = T(rScale)*posDifInParams[2];
259 |         residuals[3] = T(tScale)*posDifInParams[3];
260 |         residuals[4] = T(tScale)*posDifInParams[4];
261 |         residuals[5] = T(tScale)*posDifInParams[5];
262 | 
263 |         return true;
264 |     }
265 | };
266 | 
267 | 
268 | // Projection Error Only
269 | struct SnavelyReprojectionOnlyError {
270 |   SnavelyReprojectionOnlyError(double observed_x, double observed_y, 
271 |                                double f, double cx, double cy, double* camera)
272 |       : observed_x(observed_x), observed_y(observed_y), f(f), cx(cx), cy(cy) {
273 |         for (int i = 0; i < 6; ++i)
274 |         {
275 |           cam[i] = camera[i];
276 |         }
277 |       }
278 | 
279 |   template <typename T>
280 |   bool operator()(const T* const point,
281 |                   T* residuals) const {
282 |     // camera[0,1,2] are the angle-axis rotation.
283 |     T camera[6];
284 |     for (int i = 0; i < 6; ++i)
285 |     {
286 |       camera[i] = T(cam[i]);
287 |     }
288 |     T p[3];
289 |     AngleAxisRotatePoint(camera, point, p);
290 |     // camera[3,4,5] are the translation.
291 |     p[0] += camera[3];
292 |     p[1] += camera[4];
293 |     p[2] += camera[5];
294 | 
295 |     const T xp = T(f) * p[0] / p[2] + T(cx);
296 |     const T yp = T(f) * p[1] / p[2] + T(cy);
297 | 
298 |     const T predicted_x = xp;
299 |     const T predicted_y = yp;
300 | 
301 |     // The error is the difference between the predicted and observed position.
302 |     residuals[0] = predicted_x - observed_x;
303 |     residuals[1] = predicted_y - observed_y;
304 |     
305 |     return true;
306 |   }
307 |   // Factory to hide the construction of the CostFunction object from
308 |   // the client code.
309 |   static ceres::CostFunction* Create(const double observed_x,
310 |                                      const double observed_y,
311 |                                      const double f,
312 |                                      const double cx,
313 |                                      const double cy,
314 |                                      double* camera) {
315 |     // cout << residuals[0] << endl;
316 |     return (new ceres::AutoDiffCostFunction<SnavelyReprojectionOnlyError, 2, 3>(new SnavelyReprojectionOnlyError(observed_x, observed_y, f, cx, cy, camera)));
317 |   }
318 |   double observed_x;
319 |   double observed_y;
320 |   double f, cx, cy;
321 |   double cam[6];
322 | };
323 | 
324 | // Templated pinhole camera model for used with Ceres.  The camera is
325 | // parameterized using 10 parameters. 4 for rotation, 3 for
326 | // translation, 1 for focal length and 2 for radial distortion. The
327 | // principal point is not modeled (i.e. it is assumed be located at
328 | // the image center).
329 | struct SnavelyReprojectionErrorWithQuaternions {
330 |   // (u, v): the position of the observation with respect to the image
331 |   // center point.
332 |   SnavelyReprojectionErrorWithQuaternions(double observed_x, double observed_y)
333 |       : observed_x(observed_x), observed_y(observed_y) {}
334 |   template <typename T>
335 |   bool operator()(const T* const camera,
336 |                   const T* const point,
337 |                   T* residuals) const {
338 |     // camera[0,1,2,3] is are the rotation of the camera as a quaternion.
339 |     //
340 |     // We use QuaternionRotatePoint as it does not assume that the
341 |     // quaternion is normalized, since one of the ways to run the
342 |     // bundle adjuster is to let Ceres optimize all 4 quaternion
343 |     // parameters without a local parameterization.
344 |     T p[3];
345 |     QuaternionRotatePoint(camera, point, p);
346 |     p[0] += camera[4];
347 |     p[1] += camera[5];
348 |     p[2] += camera[6];
349 |     // Compute the center of distortion. The sign change comes from
350 |     // the camera model that Noah Snavely's Bundler assumes, whereby
351 |     // the camera coordinate system has a negative z axis.
352 |     const T xp = - p[0] / p[2];
353 |     const T yp = - p[1] / p[2];
354 |     // Apply second and fourth order radial distortion.
355 |     const T& l1 = camera[8];
356 |     const T& l2 = camera[9];
357 |     const T r2 = xp*xp + yp*yp;
358 |     const T distortion = 1.0 + r2  * (l1 + l2  * r2);
359 |     // Compute final projected point position.
360 |     const T& focal = camera[7];
361 |     const T predicted_x = focal * distortion * xp;
362 |     const T predicted_y = focal * distortion * yp;
363 |     // The error is the difference between the predicted and observed position.
364 |     residuals[0] = predicted_x - observed_x;
365 |     residuals[1] = predicted_y - observed_y;
366 |     return true;
367 |   }
368 |   // Factory to hide the construction of the CostFunction object from
369 |   // the client code.
370 |   static ceres::CostFunction* Create(const double observed_x,
371 |                                      const double observed_y) {
372 |     return (new ceres::AutoDiffCostFunction<
373 |             SnavelyReprojectionErrorWithQuaternions, 2, 10, 3>(
374 |                 new SnavelyReprojectionErrorWithQuaternions(observed_x,
375 |                                                             observed_y)));
376 |   }
377 |   double observed_x;
378 |   double observed_y;
379 | };
380 | }  // namespace examples
381 | }  // namespace ceres
382 | #endif  // CERES_EXAMPLES_SNAVELY_REPROJECTION_ERROR_H_


--------------------------------------------------------------------------------
/cmake_modules/CeresConfig.cmake:
--------------------------------------------------------------------------------
  1 | # Ceres Solver - A fast non-linear least squares minimizer
  2 | # Copyright 2015 Google Inc. All rights reserved.
  3 | # http://ceres-solver.org/
  4 | #
  5 | # Redistribution and use in source and binary forms, with or without
  6 | # modification, are permitted provided that the following conditions are met:
  7 | #
  8 | # * Redistributions of source code must retain the above copyright notice,
  9 | #   this list of conditions and the following disclaimer.
 10 | # * Redistributions in binary form must reproduce the above copyright notice,
 11 | #   this list of conditions and the following disclaimer in the documentation
 12 | #   and/or other materials provided with the distribution.
 13 | # * Neither the name of Google Inc. nor the names of its contributors may be
 14 | #   used to endorse or promote products derived from this software without
 15 | #   specific prior written permission.
 16 | #
 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | # POSSIBILITY OF SUCH DAMAGE.
 28 | #
 29 | # Authors: pablo.speciale@gmail.com (Pablo Speciale)
 30 | #          alexs.mac@gmail.com (Alex Stewart)
 31 | #
 32 | 
 33 | # Config file for Ceres Solver - Find Ceres & dependencies.
 34 | #
 35 | # This file is used by CMake when find_package(Ceres) is invoked and either
 36 | # the directory containing this file either is present in CMAKE_MODULE_PATH
 37 | # (if Ceres was installed), or exists in the local CMake package registry if
 38 | # the Ceres build directory was exported.
 39 | #
 40 | # This module defines the following variables:
 41 | #
 42 | # Ceres_FOUND / CERES_FOUND: True if Ceres has been successfully
 43 | #                            found. Both variables are set as although
 44 | #                            FindPackage() only references Ceres_FOUND
 45 | #                            in Config mode, given the conventions for
 46 | #                            <package>_FOUND when FindPackage() is
 47 | #                            called in Module mode, users could
 48 | #                            reasonably expect to use CERES_FOUND
 49 | #                            instead.
 50 | #
 51 | # CERES_VERSION: Version of Ceres found.
 52 | #
 53 | # CERES_INCLUDE_DIRS: Include directories for Ceres and the
 54 | #                     dependencies which appear in the Ceres public
 55 | #                     API and are thus required to use Ceres.
 56 | #
 57 | # CERES_LIBRARIES: Libraries for Ceres and all
 58 | #                  dependencies against which Ceres was
 59 | #                  compiled. This will not include any optional
 60 | #                  dependencies that were disabled when Ceres was
 61 | #                  compiled.
 62 | #
 63 | # The following variables are also defined for legacy compatibility
 64 | # only.  Any new code should not use them as they do not conform to
 65 | # the standard CMake FindPackage naming conventions.
 66 | #
 67 | # CERES_INCLUDES = ${CERES_INCLUDE_DIRS}.
 68 | 
 69 | # Called if we failed to find Ceres or any of its required dependencies,
 70 | # unsets all public (designed to be used externally) variables and reports
 71 | # error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
 72 | macro(CERES_REPORT_NOT_FOUND REASON_MSG)
 73 |   # FindPackage() only references Ceres_FOUND, and requires it to be
 74 |   # explicitly set FALSE to denote not found (not merely undefined).
 75 |   set(Ceres_FOUND FALSE)
 76 |   set(CERES_FOUND FALSE)
 77 |   unset(CERES_INCLUDE_DIRS)
 78 |   unset(CERES_LIBRARIES)
 79 | 
 80 |   # Reset the CMake module path to its state when this script was called.
 81 |   set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
 82 | 
 83 |   # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
 84 |   # FindPackage() use the camelcase library name, not uppercase.
 85 |   if (Ceres_FIND_QUIETLY)
 86 |     message(STATUS "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
 87 |   else (Ceres_FIND_REQUIRED)
 88 |     message(FATAL_ERROR "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
 89 |   else()
 90 |     # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
 91 |     # that prevents generation, but continues configuration.
 92 |     message(SEND_ERROR "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
 93 |   endif ()
 94 |   return()
 95 | endmacro(CERES_REPORT_NOT_FOUND)
 96 | 
 97 | # ceres_pretty_print_cmake_list( OUTPUT_VAR [item1 [item2 ... ]] )
 98 | #
 99 | # Sets ${OUTPUT_VAR} in the caller's scope to a human-readable string
100 | # representation of the list passed as the remaining arguments formed
101 | # as: "[item1, item2, ..., itemN]".
102 | function(ceres_pretty_print_cmake_list OUTPUT_VAR)
103 |   string(REPLACE ";" ", " PRETTY_LIST_STRING "[${ARGN}]")
104 |   set(${OUTPUT_VAR} "${PRETTY_LIST_STRING}" PARENT_SCOPE)
105 | endfunction()
106 | 
107 | # The list of (optional) components this version of Ceres was compiled with.
108 | set(CERES_COMPILED_COMPONENTS "@CERES_COMPILED_COMPONENTS@")
109 | 
110 | # If Ceres was not installed, then by definition it was exported
111 | # from a build directory.
112 | set(CERES_WAS_INSTALLED @SETUP_CERES_CONFIG_FOR_INSTALLATION@)
113 | 
114 | # Record the state of the CMake module path when this script was
115 | # called so that we can ensure that we leave it in the same state on
116 | # exit as it was on entry, but modify it locally.
117 | set(CALLERS_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
118 | 
119 | # Get the (current, i.e. installed) directory containing this file.
120 | get_filename_component(CERES_CURRENT_CONFIG_DIR
121 |   "${CMAKE_CURRENT_LIST_FILE}" PATH)
122 | 
123 | if (CERES_WAS_INSTALLED)
124 |   # Reset CMake module path to the installation directory of this
125 |   # script, thus we will use the FindPackage() scripts shipped with
126 |   # Ceres to find Ceres' dependencies, even if the user has equivalently
127 |   # named FindPackage() scripts in their project.
128 |   set(CMAKE_MODULE_PATH ${CERES_CURRENT_CONFIG_DIR})
129 | 
130 |   # Build the absolute root install directory as a relative path
131 |   # (determined when Ceres was configured & built) from the current
132 |   # install directory for this this file.  This allows for the install
133 |   # tree to be relocated, after Ceres was built, outside of CMake.
134 |   get_filename_component(CURRENT_ROOT_INSTALL_DIR
135 |     ${CERES_CURRENT_CONFIG_DIR}/@INSTALL_ROOT_REL_CONFIG_INSTALL_DIR@
136 |     ABSOLUTE)
137 |   if (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
138 |     ceres_report_not_found(
139 |       "Ceres install root: ${CURRENT_ROOT_INSTALL_DIR}, "
140 |       "determined from relative path from CeresConfig.cmake install location: "
141 |       "${CERES_CURRENT_CONFIG_DIR}, does not exist. Either the install "
142 |       "directory was deleted, or the install tree was only partially relocated "
143 |       "outside of CMake after Ceres was built.")
144 |   endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
145 | 
146 |   # Set the include directories for Ceres (itself).
147 |   set(CERES_INCLUDE_DIR "${CURRENT_ROOT_INSTALL_DIR}/include")
148 |   if (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
149 |     ceres_report_not_found(
150 |       "Ceres install root: ${CURRENT_ROOT_INSTALL_DIR}, "
151 |       "determined from relative path from CeresConfig.cmake install location: "
152 |       "${CERES_CURRENT_CONFIG_DIR}, does not contain Ceres headers. "
153 |       "Either the install directory was deleted, or the install tree was only "
154 |       "partially relocated outside of CMake after Ceres was built.")
155 |   endif (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
156 |   list(APPEND CERES_INCLUDE_DIRS ${CERES_INCLUDE_DIR})
157 | 
158 | else(CERES_WAS_INSTALLED)
159 |   # Ceres was exported from the build tree.
160 |   set(CERES_EXPORTED_BUILD_DIR ${CERES_CURRENT_CONFIG_DIR})
161 |   get_filename_component(CERES_EXPORTED_SOURCE_DIR
162 |     ${CERES_EXPORTED_BUILD_DIR}/@INSTALL_ROOT_REL_CONFIG_INSTALL_DIR@
163 |     ABSOLUTE)
164 |   if (NOT EXISTS ${CERES_EXPORTED_SOURCE_DIR})
165 |     ceres_report_not_found(
166 |       "Ceres exported source directory: ${CERES_EXPORTED_SOURCE_DIR}, "
167 |       "determined from relative path from CeresConfig.cmake exported build "
168 |       "directory: ${CERES_EXPORTED_BUILD_DIR} does not exist.")
169 |   endif()
170 | 
171 |   # Reset CMake module path to the cmake directory in the Ceres source
172 |   # tree which was exported, thus we will use the FindPackage() scripts shipped
173 |   # with Ceres to find Ceres' dependencies, even if the user has equivalently
174 |   # named FindPackage() scripts in their project.
175 |   set(CMAKE_MODULE_PATH ${CERES_EXPORTED_SOURCE_DIR}/cmake)
176 | 
177 |   # Set the include directories for Ceres (itself).
178 |   set(CERES_INCLUDE_DIR "${CERES_EXPORTED_SOURCE_DIR}/include")
179 |   if (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
180 |     ceres_report_not_found(
181 |       "Ceres exported source directory: ${CERES_EXPORTED_SOURCE_DIR}, "
182 |       "determined from relative path from CeresConfig.cmake exported build "
183 |       "directory: ${CERES_EXPORTED_BUILD_DIR}, does not contain Ceres headers.")
184 |   endif (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
185 |   list(APPEND CERES_INCLUDE_DIRS ${CERES_INCLUDE_DIR})
186 | 
187 |   # Append the path to the configured config.h in the exported build directory
188 |   # to the Ceres include directories.
189 |   set(CERES_CONFIG_FILE
190 |     ${CERES_EXPORTED_BUILD_DIR}/config/ceres/internal/config.h)
191 |   if (NOT EXISTS ${CERES_CONFIG_FILE})
192 |     ceres_report_not_found(
193 |       "Ceres exported build directory: ${CERES_EXPORTED_BUILD_DIR}, "
194 |       "does not contain required configured Ceres config.h, it is not here: "
195 |       "${CERES_CONFIG_FILE}.")
196 |   endif (NOT EXISTS ${CERES_CONFIG_FILE})
197 |   list(APPEND CERES_INCLUDE_DIRS ${CERES_EXPORTED_BUILD_DIR}/config)
198 | endif(CERES_WAS_INSTALLED)
199 | 
200 | # Set the version.
201 | set(CERES_VERSION @CERES_VERSION@ )
202 | 
203 | # Eigen.
204 | # Flag set during configuration and build of Ceres.
205 | set(CERES_EIGEN_VERSION @EIGEN_VERSION@)
206 | # Append the locations of Eigen when Ceres was built to the search path hints.
207 | list(APPEND EIGEN_INCLUDE_DIR_HINTS @EIGEN_INCLUDE_DIR@)
208 | # Search quietly to control the timing of the error message if not found. The
209 | # search should be for an exact match, but for usability reasons do a soft
210 | # match and reject with an explanation below.
211 | find_package(Eigen ${CERES_EIGEN_VERSION} QUIET)
212 | if (EIGEN_FOUND)
213 |   if (NOT EIGEN_VERSION VERSION_EQUAL CERES_EIGEN_VERSION)
214 |     # CMake's VERSION check in FIND_PACKAGE() will accept any version >= the
215 |     # specified version. However, only version = is supported. Improve
216 |     # usability by explaining why we don't accept non-exact version matching.
217 |     ceres_report_not_found("Found Eigen dependency, but the version of Eigen "
218 |       "found (${EIGEN_VERSION}) does not exactly match the version of Eigen "
219 |       "Ceres was compiled with (${CERES_EIGEN_VERSION}). This can cause subtle "
220 |       "bugs by triggering violations of the One Definition Rule. See the "
221 |       "Wikipedia article http://en.wikipedia.org/wiki/One_Definition_Rule "
222 |       "for more details")
223 |   endif ()
224 |   message(STATUS "Found required Ceres dependency: "
225 |     "Eigen version ${CERES_EIGEN_VERSION} in ${EIGEN_INCLUDE_DIRS}")
226 | else (EIGEN_FOUND)
227 |   ceres_report_not_found("Missing required Ceres "
228 |     "dependency: Eigen version ${CERES_EIGEN_VERSION}, please set "
229 |     "EIGEN_INCLUDE_DIR.")
230 | endif (EIGEN_FOUND)
231 | list(APPEND CERES_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
232 | 
233 | # Glog.
234 | # Flag set during configuration and build of Ceres.
235 | set(CERES_USES_MINIGLOG @MINIGLOG@)
236 | if (CERES_USES_MINIGLOG)
237 |   set(MINIGLOG_INCLUDE_DIR ${CERES_INCLUDE_DIR}/ceres/internal/miniglog)
238 |   if (NOT EXISTS ${MINIGLOG_INCLUDE_DIR})
239 |     ceres_report_not_found(
240 |       "Ceres include directory: "
241 |       "${CERES_INCLUDE_DIR} does not include miniglog, but Ceres was "
242 |       "compiled with MINIGLOG enabled (in place of Glog).")
243 |   endif (NOT EXISTS ${MINIGLOG_INCLUDE_DIR})
244 |   list(APPEND CERES_INCLUDE_DIRS ${MINIGLOG_INCLUDE_DIR})
245 |   # Output message at standard log level (not the lower STATUS) so that
246 |   # the message is output in GUI during configuration to warn user.
247 |   message("-- Found Ceres compiled with miniglog substitute "
248 |     "for glog, beware this will likely cause problems if glog is later linked.")
249 | else (CERES_USES_MINIGLOG)
250 |   # Append the locations of glog when Ceres was built to the search path hints.
251 |   list(APPEND GLOG_INCLUDE_DIR_HINTS @GLOG_INCLUDE_DIR@)
252 |   get_filename_component(CERES_BUILD_GLOG_LIBRARY_DIR @GLOG_LIBRARY@ PATH)
253 |   list(APPEND GLOG_LIBRARY_DIR_HINTS ${CERES_BUILD_GLOG_LIBRARY_DIR})
254 | 
255 |   # Search quietly s/t we control the timing of the error message if not found.
256 |   find_package(Glog QUIET)
257 |   if (GLOG_FOUND)
258 |     message(STATUS "Found required Ceres dependency: "
259 |       "Glog in ${GLOG_INCLUDE_DIRS}")
260 |   else (GLOG_FOUND)
261 |     ceres_report_not_found("Missing required Ceres "
262 |       "dependency: Glog, please set GLOG_INCLUDE_DIR.")
263 |   endif (GLOG_FOUND)
264 |   list(APPEND CERES_INCLUDE_DIRS ${GLOG_INCLUDE_DIRS})
265 | endif (CERES_USES_MINIGLOG)
266 | 
267 | # Import exported Ceres targets, if they have not already been imported.
268 | if (NOT TARGET ceres AND NOT Ceres_BINARY_DIR)
269 |   include(${CERES_CURRENT_CONFIG_DIR}/CeresTargets.cmake)
270 | endif (NOT TARGET ceres AND NOT Ceres_BINARY_DIR)
271 | # Set the expected XX_LIBRARIES variable for FindPackage().
272 | set(CERES_LIBRARIES ceres)
273 | 
274 | # Make user aware of any compile flags that will be added to their targets
275 | # which use Ceres (i.e. flags exported in the Ceres target).  Only CMake
276 | # versions >= 2.8.12 support target_compile_options().
277 | if (TARGET ${CERES_LIBRARIES} AND
278 |     NOT CMAKE_VERSION VERSION_LESS "2.8.12")
279 |   get_target_property(CERES_INTERFACE_COMPILE_OPTIONS
280 |     ${CERES_LIBRARIES} INTERFACE_COMPILE_OPTIONS)
281 | 
282 |   if (CERES_WAS_INSTALLED)
283 |     set(CERES_LOCATION "${CURRENT_ROOT_INSTALL_DIR}")
284 |   else()
285 |     set(CERES_LOCATION "${CERES_EXPORTED_BUILD_DIR}")
286 |   endif()
287 | 
288 |   # Check for -std=c++11 flags.
289 |   if (CERES_INTERFACE_COMPILE_OPTIONS MATCHES ".*std=c\\+\\+11.*")
290 |     message(STATUS "Ceres version ${CERES_VERSION} detected here: "
291 |       "${CERES_LOCATION} was built with C++11. Ceres target will add "
292 |       "C++11 flags to compile options for targets using it.")
293 |   endif()
294 | endif()
295 | 
296 | # Set legacy include directories variable for backwards compatibility.
297 | set(CERES_INCLUDES ${CERES_INCLUDE_DIRS})
298 | 
299 | # Reset CMake module path to its state when this script was called.
300 | set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
301 | 
302 | # Build the detected Ceres version string to correctly capture whether it
303 | # was installed, or exported.
304 | ceres_pretty_print_cmake_list(CERES_COMPILED_COMPONENTS_STRING
305 |   ${CERES_COMPILED_COMPONENTS})
306 | if (CERES_WAS_INSTALLED)
307 |   set(CERES_DETECTED_VERSION_STRING "Ceres version: ${CERES_VERSION} "
308 |     "installed in: ${CURRENT_ROOT_INSTALL_DIR} with components: "
309 |     "${CERES_COMPILED_COMPONENTS_STRING}")
310 | else (CERES_WAS_INSTALLED)
311 |   set(CERES_DETECTED_VERSION_STRING "Ceres version: ${CERES_VERSION} "
312 |     "exported from build directory: ${CERES_EXPORTED_BUILD_DIR} with "
313 |     "components: ${CERES_COMPILED_COMPONENTS_STRING}")
314 | endif()
315 | 
316 | # If the user called this script through find_package() whilst specifying
317 | # particular Ceres components that should be found via:
318 | # find_package(Ceres COMPONENTS XXX YYY), check the requested components against
319 | # those with which Ceres was compiled.  In this case, we should only report
320 | # Ceres as found if all the requested components have been found.
321 | if (Ceres_FIND_COMPONENTS)
322 |   foreach (REQUESTED_COMPONENT ${Ceres_FIND_COMPONENTS})
323 |     list(FIND CERES_COMPILED_COMPONENTS ${REQUESTED_COMPONENT} HAVE_REQUESTED_COMPONENT)
324 |     # list(FIND ..) returns -1 if the element was not in the list, but CMake
325 |     # interprets if (VAR) to be true if VAR is any non-zero number, even
326 |     # negative ones, hence we have to explicitly check for >= 0.
327 |     if (HAVE_REQUESTED_COMPONENT EQUAL -1)
328 |       # Check for the presence of all requested components before reporting
329 |       # not found, such that we report all of the missing components rather
330 |       # than just the first.
331 |       list(APPEND MISSING_CERES_COMPONENTS ${REQUESTED_COMPONENT})
332 |     endif()
333 |   endforeach()
334 |   if (MISSING_CERES_COMPONENTS)
335 |     ceres_pretty_print_cmake_list(REQUESTED_CERES_COMPONENTS_STRING
336 |       ${Ceres_FIND_COMPONENTS})
337 |     ceres_pretty_print_cmake_list(MISSING_CERES_COMPONENTS_STRING
338 |       ${MISSING_CERES_COMPONENTS})
339 |     ceres_report_not_found("Missing requested Ceres components: "
340 |       "${MISSING_CERES_COMPONENTS_STRING} (components requested: "
341 |       "${REQUESTED_CERES_COMPONENTS_STRING}). Detected "
342 |       "${CERES_DETECTED_VERSION_STRING}.")
343 |   endif()
344 | endif()
345 | 
346 | # As we use CERES_REPORT_NOT_FOUND() to abort, if we reach this point we have
347 | # found Ceres and all required dependencies.
348 | message(STATUS "Found " ${CERES_DETECTED_VERSION_STRING})
349 | 
350 | # Set CERES_FOUND to be equivalent to Ceres_FOUND, which is set to
351 | # TRUE by FindPackage() if this file is found and run, and after which
352 | # Ceres_FOUND is not (explicitly, i.e. undefined does not count) set
353 | # to FALSE.
354 | set(CERES_FOUND TRUE)


--------------------------------------------------------------------------------
/cmake_modules/CeresConfig.cmake.in:
--------------------------------------------------------------------------------
  1 | # Ceres Solver - A fast non-linear least squares minimizer
  2 | # Copyright 2015 Google Inc. All rights reserved.
  3 | # http://ceres-solver.org/
  4 | #
  5 | # Redistribution and use in source and binary forms, with or without
  6 | # modification, are permitted provided that the following conditions are met:
  7 | #
  8 | # * Redistributions of source code must retain the above copyright notice,
  9 | #   this list of conditions and the following disclaimer.
 10 | # * Redistributions in binary form must reproduce the above copyright notice,
 11 | #   this list of conditions and the following disclaimer in the documentation
 12 | #   and/or other materials provided with the distribution.
 13 | # * Neither the name of Google Inc. nor the names of its contributors may be
 14 | #   used to endorse or promote products derived from this software without
 15 | #   specific prior written permission.
 16 | #
 17 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 18 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 19 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 20 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 21 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 22 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 23 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 24 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 25 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 26 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 27 | # POSSIBILITY OF SUCH DAMAGE.
 28 | #
 29 | # Authors: pablo.speciale@gmail.com (Pablo Speciale)
 30 | #          alexs.mac@gmail.com (Alex Stewart)
 31 | #
 32 | 
 33 | # Config file for Ceres Solver - Find Ceres & dependencies.
 34 | #
 35 | # This file is used by CMake when find_package(Ceres) is invoked and either
 36 | # the directory containing this file either is present in CMAKE_MODULE_PATH
 37 | # (if Ceres was installed), or exists in the local CMake package registry if
 38 | # the Ceres build directory was exported.
 39 | #
 40 | # This module defines the following variables:
 41 | #
 42 | # Ceres_FOUND / CERES_FOUND: True if Ceres has been successfully
 43 | #                            found. Both variables are set as although
 44 | #                            FindPackage() only references Ceres_FOUND
 45 | #                            in Config mode, given the conventions for
 46 | #                            <package>_FOUND when FindPackage() is
 47 | #                            called in Module mode, users could
 48 | #                            reasonably expect to use CERES_FOUND
 49 | #                            instead.
 50 | #
 51 | # CERES_VERSION: Version of Ceres found.
 52 | #
 53 | # CERES_INCLUDE_DIRS: Include directories for Ceres and the
 54 | #                     dependencies which appear in the Ceres public
 55 | #                     API and are thus required to use Ceres.
 56 | #
 57 | # CERES_LIBRARIES: Libraries for Ceres and all
 58 | #                  dependencies against which Ceres was
 59 | #                  compiled. This will not include any optional
 60 | #                  dependencies that were disabled when Ceres was
 61 | #                  compiled.
 62 | #
 63 | # The following variables are also defined for legacy compatibility
 64 | # only.  Any new code should not use them as they do not conform to
 65 | # the standard CMake FindPackage naming conventions.
 66 | #
 67 | # CERES_INCLUDES = ${CERES_INCLUDE_DIRS}.
 68 | 
 69 | # Called if we failed to find Ceres or any of its required dependencies,
 70 | # unsets all public (designed to be used externally) variables and reports
 71 | # error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
 72 | macro(CERES_REPORT_NOT_FOUND REASON_MSG)
 73 |   # FindPackage() only references Ceres_FOUND, and requires it to be
 74 |   # explicitly set FALSE to denote not found (not merely undefined).
 75 |   set(Ceres_FOUND FALSE)
 76 |   set(CERES_FOUND FALSE)
 77 |   unset(CERES_INCLUDE_DIRS)
 78 |   unset(CERES_LIBRARIES)
 79 | 
 80 |   # Reset the CMake module path to its state when this script was called.
 81 |   set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
 82 | 
 83 |   # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by
 84 |   # FindPackage() use the camelcase library name, not uppercase.
 85 |   if (Ceres_FIND_QUIETLY)
 86 |     message(STATUS "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
 87 |   else (Ceres_FIND_REQUIRED)
 88 |     message(FATAL_ERROR "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
 89 |   else()
 90 |     # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error
 91 |     # that prevents generation, but continues configuration.
 92 |     message(SEND_ERROR "Failed to find Ceres - " ${REASON_MSG} ${ARGN})
 93 |   endif ()
 94 |   return()
 95 | endmacro(CERES_REPORT_NOT_FOUND)
 96 | 
 97 | # ceres_pretty_print_cmake_list( OUTPUT_VAR [item1 [item2 ... ]] )
 98 | #
 99 | # Sets ${OUTPUT_VAR} in the caller's scope to a human-readable string
100 | # representation of the list passed as the remaining arguments formed
101 | # as: "[item1, item2, ..., itemN]".
102 | function(ceres_pretty_print_cmake_list OUTPUT_VAR)
103 |   string(REPLACE ";" ", " PRETTY_LIST_STRING "[${ARGN}]")
104 |   set(${OUTPUT_VAR} "${PRETTY_LIST_STRING}" PARENT_SCOPE)
105 | endfunction()
106 | 
107 | # The list of (optional) components this version of Ceres was compiled with.
108 | set(CERES_COMPILED_COMPONENTS "@CERES_COMPILED_COMPONENTS@")
109 | 
110 | # If Ceres was not installed, then by definition it was exported
111 | # from a build directory.
112 | set(CERES_WAS_INSTALLED @SETUP_CERES_CONFIG_FOR_INSTALLATION@)
113 | 
114 | # Record the state of the CMake module path when this script was
115 | # called so that we can ensure that we leave it in the same state on
116 | # exit as it was on entry, but modify it locally.
117 | set(CALLERS_CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH})
118 | 
119 | # Get the (current, i.e. installed) directory containing this file.
120 | get_filename_component(CERES_CURRENT_CONFIG_DIR
121 |   "${CMAKE_CURRENT_LIST_FILE}" PATH)
122 | 
123 | if (CERES_WAS_INSTALLED)
124 |   # Reset CMake module path to the installation directory of this
125 |   # script, thus we will use the FindPackage() scripts shipped with
126 |   # Ceres to find Ceres' dependencies, even if the user has equivalently
127 |   # named FindPackage() scripts in their project.
128 |   set(CMAKE_MODULE_PATH ${CERES_CURRENT_CONFIG_DIR})
129 | 
130 |   # Build the absolute root install directory as a relative path
131 |   # (determined when Ceres was configured & built) from the current
132 |   # install directory for this this file.  This allows for the install
133 |   # tree to be relocated, after Ceres was built, outside of CMake.
134 |   get_filename_component(CURRENT_ROOT_INSTALL_DIR
135 |     ${CERES_CURRENT_CONFIG_DIR}/@INSTALL_ROOT_REL_CONFIG_INSTALL_DIR@
136 |     ABSOLUTE)
137 |   if (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
138 |     ceres_report_not_found(
139 |       "Ceres install root: ${CURRENT_ROOT_INSTALL_DIR}, "
140 |       "determined from relative path from CeresConfig.cmake install location: "
141 |       "${CERES_CURRENT_CONFIG_DIR}, does not exist. Either the install "
142 |       "directory was deleted, or the install tree was only partially relocated "
143 |       "outside of CMake after Ceres was built.")
144 |   endif (NOT EXISTS ${CURRENT_ROOT_INSTALL_DIR})
145 | 
146 |   # Set the include directories for Ceres (itself).
147 |   set(CERES_INCLUDE_DIR "${CURRENT_ROOT_INSTALL_DIR}/include")
148 |   if (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
149 |     ceres_report_not_found(
150 |       "Ceres install root: ${CURRENT_ROOT_INSTALL_DIR}, "
151 |       "determined from relative path from CeresConfig.cmake install location: "
152 |       "${CERES_CURRENT_CONFIG_DIR}, does not contain Ceres headers. "
153 |       "Either the install directory was deleted, or the install tree was only "
154 |       "partially relocated outside of CMake after Ceres was built.")
155 |   endif (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
156 |   list(APPEND CERES_INCLUDE_DIRS ${CERES_INCLUDE_DIR})
157 | 
158 | else(CERES_WAS_INSTALLED)
159 |   # Ceres was exported from the build tree.
160 |   set(CERES_EXPORTED_BUILD_DIR ${CERES_CURRENT_CONFIG_DIR})
161 |   get_filename_component(CERES_EXPORTED_SOURCE_DIR
162 |     ${CERES_EXPORTED_BUILD_DIR}/@INSTALL_ROOT_REL_CONFIG_INSTALL_DIR@
163 |     ABSOLUTE)
164 |   if (NOT EXISTS ${CERES_EXPORTED_SOURCE_DIR})
165 |     ceres_report_not_found(
166 |       "Ceres exported source directory: ${CERES_EXPORTED_SOURCE_DIR}, "
167 |       "determined from relative path from CeresConfig.cmake exported build "
168 |       "directory: ${CERES_EXPORTED_BUILD_DIR} does not exist.")
169 |   endif()
170 | 
171 |   # Reset CMake module path to the cmake directory in the Ceres source
172 |   # tree which was exported, thus we will use the FindPackage() scripts shipped
173 |   # with Ceres to find Ceres' dependencies, even if the user has equivalently
174 |   # named FindPackage() scripts in their project.
175 |   set(CMAKE_MODULE_PATH ${CERES_EXPORTED_SOURCE_DIR}/cmake)
176 | 
177 |   # Set the include directories for Ceres (itself).
178 |   set(CERES_INCLUDE_DIR "${CERES_EXPORTED_SOURCE_DIR}/include")
179 |   if (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
180 |     ceres_report_not_found(
181 |       "Ceres exported source directory: ${CERES_EXPORTED_SOURCE_DIR}, "
182 |       "determined from relative path from CeresConfig.cmake exported build "
183 |       "directory: ${CERES_EXPORTED_BUILD_DIR}, does not contain Ceres headers.")
184 |   endif (NOT EXISTS ${CERES_INCLUDE_DIR}/ceres/ceres.h)
185 |   list(APPEND CERES_INCLUDE_DIRS ${CERES_INCLUDE_DIR})
186 | 
187 |   # Append the path to the configured config.h in the exported build directory
188 |   # to the Ceres include directories.
189 |   set(CERES_CONFIG_FILE
190 |     ${CERES_EXPORTED_BUILD_DIR}/config/ceres/internal/config.h)
191 |   if (NOT EXISTS ${CERES_CONFIG_FILE})
192 |     ceres_report_not_found(
193 |       "Ceres exported build directory: ${CERES_EXPORTED_BUILD_DIR}, "
194 |       "does not contain required configured Ceres config.h, it is not here: "
195 |       "${CERES_CONFIG_FILE}.")
196 |   endif (NOT EXISTS ${CERES_CONFIG_FILE})
197 |   list(APPEND CERES_INCLUDE_DIRS ${CERES_EXPORTED_BUILD_DIR}/config)
198 | endif(CERES_WAS_INSTALLED)
199 | 
200 | # Set the version.
201 | set(CERES_VERSION @CERES_VERSION@ )
202 | 
203 | # Eigen.
204 | # Flag set during configuration and build of Ceres.
205 | set(CERES_EIGEN_VERSION @EIGEN_VERSION@)
206 | # Append the locations of Eigen when Ceres was built to the search path hints.
207 | list(APPEND EIGEN_INCLUDE_DIR_HINTS @EIGEN_INCLUDE_DIR@)
208 | # Search quietly to control the timing of the error message if not found. The
209 | # search should be for an exact match, but for usability reasons do a soft
210 | # match and reject with an explanation below.
211 | find_package(Eigen ${CERES_EIGEN_VERSION} QUIET)
212 | if (EIGEN_FOUND)
213 |   if (NOT EIGEN_VERSION VERSION_EQUAL CERES_EIGEN_VERSION)
214 |     # CMake's VERSION check in FIND_PACKAGE() will accept any version >= the
215 |     # specified version. However, only version = is supported. Improve
216 |     # usability by explaining why we don't accept non-exact version matching.
217 |     ceres_report_not_found("Found Eigen dependency, but the version of Eigen "
218 |       "found (${EIGEN_VERSION}) does not exactly match the version of Eigen "
219 |       "Ceres was compiled with (${CERES_EIGEN_VERSION}). This can cause subtle "
220 |       "bugs by triggering violations of the One Definition Rule. See the "
221 |       "Wikipedia article http://en.wikipedia.org/wiki/One_Definition_Rule "
222 |       "for more details")
223 |   endif ()
224 |   message(STATUS "Found required Ceres dependency: "
225 |     "Eigen version ${CERES_EIGEN_VERSION} in ${EIGEN_INCLUDE_DIRS}")
226 | else (EIGEN_FOUND)
227 |   ceres_report_not_found("Missing required Ceres "
228 |     "dependency: Eigen version ${CERES_EIGEN_VERSION}, please set "
229 |     "EIGEN_INCLUDE_DIR.")
230 | endif (EIGEN_FOUND)
231 | list(APPEND CERES_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
232 | 
233 | # Glog.
234 | # Flag set during configuration and build of Ceres.
235 | set(CERES_USES_MINIGLOG @MINIGLOG@)
236 | if (CERES_USES_MINIGLOG)
237 |   set(MINIGLOG_INCLUDE_DIR ${CERES_INCLUDE_DIR}/ceres/internal/miniglog)
238 |   if (NOT EXISTS ${MINIGLOG_INCLUDE_DIR})
239 |     ceres_report_not_found(
240 |       "Ceres include directory: "
241 |       "${CERES_INCLUDE_DIR} does not include miniglog, but Ceres was "
242 |       "compiled with MINIGLOG enabled (in place of Glog).")
243 |   endif (NOT EXISTS ${MINIGLOG_INCLUDE_DIR})
244 |   list(APPEND CERES_INCLUDE_DIRS ${MINIGLOG_INCLUDE_DIR})
245 |   # Output message at standard log level (not the lower STATUS) so that
246 |   # the message is output in GUI during configuration to warn user.
247 |   message("-- Found Ceres compiled with miniglog substitute "
248 |     "for glog, beware this will likely cause problems if glog is later linked.")
249 | else (CERES_USES_MINIGLOG)
250 |   # Append the locations of glog when Ceres was built to the search path hints.
251 |   list(APPEND GLOG_INCLUDE_DIR_HINTS @GLOG_INCLUDE_DIR@)
252 |   get_filename_component(CERES_BUILD_GLOG_LIBRARY_DIR @GLOG_LIBRARY@ PATH)
253 |   list(APPEND GLOG_LIBRARY_DIR_HINTS ${CERES_BUILD_GLOG_LIBRARY_DIR})
254 | 
255 |   # Search quietly s/t we control the timing of the error message if not found.
256 |   find_package(Glog QUIET)
257 |   if (GLOG_FOUND)
258 |     message(STATUS "Found required Ceres dependency: "
259 |       "Glog in ${GLOG_INCLUDE_DIRS}")
260 |   else (GLOG_FOUND)
261 |     ceres_report_not_found("Missing required Ceres "
262 |       "dependency: Glog, please set GLOG_INCLUDE_DIR.")
263 |   endif (GLOG_FOUND)
264 |   list(APPEND CERES_INCLUDE_DIRS ${GLOG_INCLUDE_DIRS})
265 | endif (CERES_USES_MINIGLOG)
266 | 
267 | # Import exported Ceres targets, if they have not already been imported.
268 | if (NOT TARGET ceres AND NOT Ceres_BINARY_DIR)
269 |   include(${CERES_CURRENT_CONFIG_DIR}/CeresTargets.cmake)
270 | endif (NOT TARGET ceres AND NOT Ceres_BINARY_DIR)
271 | # Set the expected XX_LIBRARIES variable for FindPackage().
272 | set(CERES_LIBRARIES ceres)
273 | 
274 | # Make user aware of any compile flags that will be added to their targets
275 | # which use Ceres (i.e. flags exported in the Ceres target).  Only CMake
276 | # versions >= 2.8.12 support target_compile_options().
277 | if (TARGET ${CERES_LIBRARIES} AND
278 |     NOT CMAKE_VERSION VERSION_LESS "2.8.12")
279 |   get_target_property(CERES_INTERFACE_COMPILE_OPTIONS
280 |     ${CERES_LIBRARIES} INTERFACE_COMPILE_OPTIONS)
281 | 
282 |   if (CERES_WAS_INSTALLED)
283 |     set(CERES_LOCATION "${CURRENT_ROOT_INSTALL_DIR}")
284 |   else()
285 |     set(CERES_LOCATION "${CERES_EXPORTED_BUILD_DIR}")
286 |   endif()
287 | 
288 |   # Check for -std=c++11 flags.
289 |   if (CERES_INTERFACE_COMPILE_OPTIONS MATCHES ".*std=c\\+\\+11.*")
290 |     message(STATUS "Ceres version ${CERES_VERSION} detected here: "
291 |       "${CERES_LOCATION} was built with C++11. Ceres target will add "
292 |       "C++11 flags to compile options for targets using it.")
293 |   endif()
294 | endif()
295 | 
296 | # Set legacy include directories variable for backwards compatibility.
297 | set(CERES_INCLUDES ${CERES_INCLUDE_DIRS})
298 | 
299 | # Reset CMake module path to its state when this script was called.
300 | set(CMAKE_MODULE_PATH ${CALLERS_CMAKE_MODULE_PATH})
301 | 
302 | # Build the detected Ceres version string to correctly capture whether it
303 | # was installed, or exported.
304 | ceres_pretty_print_cmake_list(CERES_COMPILED_COMPONENTS_STRING
305 |   ${CERES_COMPILED_COMPONENTS})
306 | if (CERES_WAS_INSTALLED)
307 |   set(CERES_DETECTED_VERSION_STRING "Ceres version: ${CERES_VERSION} "
308 |     "installed in: ${CURRENT_ROOT_INSTALL_DIR} with components: "
309 |     "${CERES_COMPILED_COMPONENTS_STRING}")
310 | else (CERES_WAS_INSTALLED)
311 |   set(CERES_DETECTED_VERSION_STRING "Ceres version: ${CERES_VERSION} "
312 |     "exported from build directory: ${CERES_EXPORTED_BUILD_DIR} with "
313 |     "components: ${CERES_COMPILED_COMPONENTS_STRING}")
314 | endif()
315 | 
316 | # If the user called this script through find_package() whilst specifying
317 | # particular Ceres components that should be found via:
318 | # find_package(Ceres COMPONENTS XXX YYY), check the requested components against
319 | # those with which Ceres was compiled.  In this case, we should only report
320 | # Ceres as found if all the requested components have been found.
321 | if (Ceres_FIND_COMPONENTS)
322 |   foreach (REQUESTED_COMPONENT ${Ceres_FIND_COMPONENTS})
323 |     list(FIND CERES_COMPILED_COMPONENTS ${REQUESTED_COMPONENT} HAVE_REQUESTED_COMPONENT)
324 |     # list(FIND ..) returns -1 if the element was not in the list, but CMake
325 |     # interprets if (VAR) to be true if VAR is any non-zero number, even
326 |     # negative ones, hence we have to explicitly check for >= 0.
327 |     if (HAVE_REQUESTED_COMPONENT EQUAL -1)
328 |       # Check for the presence of all requested components before reporting
329 |       # not found, such that we report all of the missing components rather
330 |       # than just the first.
331 |       list(APPEND MISSING_CERES_COMPONENTS ${REQUESTED_COMPONENT})
332 |     endif()
333 |   endforeach()
334 |   if (MISSING_CERES_COMPONENTS)
335 |     ceres_pretty_print_cmake_list(REQUESTED_CERES_COMPONENTS_STRING
336 |       ${Ceres_FIND_COMPONENTS})
337 |     ceres_pretty_print_cmake_list(MISSING_CERES_COMPONENTS_STRING
338 |       ${MISSING_CERES_COMPONENTS})
339 |     ceres_report_not_found("Missing requested Ceres components: "
340 |       "${MISSING_CERES_COMPONENTS_STRING} (components requested: "
341 |       "${REQUESTED_CERES_COMPONENTS_STRING}). Detected "
342 |       "${CERES_DETECTED_VERSION_STRING}.")
343 |   endif()
344 | endif()
345 | 
346 | # As we use CERES_REPORT_NOT_FOUND() to abort, if we reach this point we have
347 | # found Ceres and all required dependencies.
348 | message(STATUS "Found " ${CERES_DETECTED_VERSION_STRING})
349 | 
350 | # Set CERES_FOUND to be equivalent to Ceres_FOUND, which is set to
351 | # TRUE by FindPackage() if this file is found and run, and after which
352 | # Ceres_FOUND is not (explicitly, i.e. undefined does not count) set
353 | # to FALSE.
354 | set(CERES_FOUND TRUE)


--------------------------------------------------------------------------------
/src/optimizer.cpp:
--------------------------------------------------------------------------------
  1 | #include "optimizer.hpp"
  2 | 
  3 | Optimizer::Optimizer(bool printOut, Problem& prob):globalBAProblem(prob){
  4 |     // BAProblem = problem;
  5 |     options.linear_solver_type = SPARSE_NORMAL_CHOLESKY;
  6 |     options.minimizer_progress_to_stdout = printOut;
  7 | //    options.max_num_iterations = 10;
  8 | }
  9 | 
 10 | void Optimizer::poseOptimization(vector<Frame* >& frames) {
 11 | 
 12 |     Problem poseOptProblem;
 13 |     double* cameraParameter_ = new double[cameraBlkSize * frames.size()];
 14 |     //construct camera parameters
 15 |     for(int n = 0; n < frames.size(); n++) {
 16 |         cameraParameter_[cameraBlkSize*n + 0] = frames[n]->worldRvec.at<double>(0,0);
 17 |         cameraParameter_[cameraBlkSize*n + 1] = frames[n]->worldRvec.at<double>(1,0);
 18 |         cameraParameter_[cameraBlkSize*n + 2] = frames[n]->worldRvec.at<double>(2,0);
 19 | 
 20 |         cameraParameter_[cameraBlkSize*n + 3] = frames[n]->worldTvec.at<double>(0,0);
 21 |         cameraParameter_[cameraBlkSize*n + 4] = frames[n]->worldTvec.at<double>(1,0);
 22 |         cameraParameter_[cameraBlkSize*n + 5] = frames[n]->worldTvec.at<double>(2,0);
 23 |     }   
 24 |     double* cameras = cameraParameter_;
 25 |     double* camera = cameras;
 26 | 
 27 |     //add constrain
 28 |     for(int n = 0; n < frames.size(); n++) {
 29 |         for(auto obs : frames[n]->relativePose){
 30 |             camera = cameras + n*cameraBlkSize;
 31 |             double* refCamera = cameras + obs.first*cameraBlkSize;
 32 |             //add an edge for the current camera
 33 |             Eigen::Affine3d fromSrcToRef, refPose;
 34 |             fromSrcToRef = vectorToTransformation(obs.second.first, 
 35 |                                                   obs.second.second);
 36 | 
 37 |             Eigen::Affine3d fromSrcToRefInv;
 38 |             fromSrcToRefInv = fromSrcToRef.inverse();
 39 |             CostFunction* costFunc = BinaryPoseSmoothnessError::Create(fromSrcToRefInv, 
 40 |                                                                  		1.,1.);
 41 |             poseOptProblem.AddResidualBlock(costFunc, NULL, camera, refCamera);
 42 |         }   
 43 |     }
 44 | 
 45 |     Solve(options, &poseOptProblem, &summary);
 46 | 
 47 |     //update all poses
 48 |     for(int n = 0; n < frames.size(); n++){
 49 |         frames[n]->worldRvec.at<double>(0,0) = cameraParameter_[cameraBlkSize*n + 0];
 50 |         frames[n]->worldRvec.at<double>(1,0) = cameraParameter_[cameraBlkSize*n + 1];
 51 |         frames[n]->worldRvec.at<double>(2,0) = cameraParameter_[cameraBlkSize*n + 2];
 52 |         frames[n]->worldTvec.at<double>(0,0) = cameraParameter_[cameraBlkSize*n + 3];
 53 |         frames[n]->worldTvec.at<double>(1,0) = cameraParameter_[cameraBlkSize*n + 4];
 54 |         frames[n]->worldTvec.at<double>(2,0) = cameraParameter_[cameraBlkSize*n + 5];
 55 |     }
 56 | }
 57 | 
 58 | void Optimizer::globalBundleAdjustment(Map* slammap, vector<Frame*> frames){
 59 |     // set camera poses
 60 |     double* cameraParameter_ = new double[cameraBlkSize*frames.size()];
 61 |     cout << "frame size: " << frames.size() << endl;
 62 |     for(int n = 0; n < frames.size(); n++){
 63 |         cameraParameter_[cameraBlkSize*n + 0] = frames[n]->worldRvec.at<double>(0,0);
 64 |         cameraParameter_[cameraBlkSize*n + 1] = frames[n]->worldRvec.at<double>(1,0);
 65 |         cameraParameter_[cameraBlkSize*n + 2] = frames[n]->worldRvec.at<double>(2,0);
 66 |         cameraParameter_[cameraBlkSize*n + 3] = frames[n]->worldTvec.at<double>(0,0);
 67 |         cameraParameter_[cameraBlkSize*n + 4] = frames[n]->worldTvec.at<double>(1,0);
 68 |         cameraParameter_[cameraBlkSize*n + 5] = frames[n]->worldTvec.at<double>(2,0);
 69 |     }
 70 | 
 71 |     long unsigned int numPointsParameters = pointBlkSize*slammap->allMapPointNumber();
 72 |     double* pointParameters_;
 73 |     pointParameters_ = new double[numPointsParameters];
 74 |     //convert container of mappoints to vector
 75 |     vector<MapPoint*> vallMapPoints;
 76 |     //add all map points into parameter
 77 |     cout << "adding mappoints into problem...";
 78 |     int n = 0;
 79 |     for(set<MapPoint*>::iterator it = slammap->allMapPoints.begin(); 
 80 |                                  it!= slammap->allMapPoints.end(); 
 81 |                                  it++){
 82 |         vallMapPoints.push_back(*it);
 83 | 
 84 |         pointParameters_[pointBlkSize*n + 0] = (double)(*it)->pos.x;
 85 |         pointParameters_[pointBlkSize*n + 1] = (double)(*it)->pos.y;
 86 |         pointParameters_[pointBlkSize*n + 2] = (double)(*it)->pos.z;
 87 | 
 88 |         n++;
 89 |     }
 90 |     cout <<" done!" << endl;
 91 |     double* points = pointParameters_;
 92 |     double* cameras = cameraParameter_;
 93 |     double* point = points;
 94 |     double* camera = cameras;
 95 | 
 96 |     //add mappoints
 97 |     cout << "adding observations into problem...";
 98 |     long unsigned int count = 0;//used to count mappoints
 99 |     for(vector<MapPoint*>::iterator it = vallMapPoints.begin();
100 |                                     it!= vallMapPoints.end();
101 |                                     it++){
102 |         //iterate all observations of a mappoint
103 |         for(map<Frame*, unsigned int>::iterator pIt = (*it)->observations.begin();
104 |                                                 pIt!= (*it)->observations.end();
105 |                                                 pIt++){
106 |             cv::Point2f observedPt;
107 |             observedPt.x = pIt->first->keypointL[pIt->second].pt.x;
108 |             observedPt.y = pIt->first->keypointL[pIt->second].pt.y;
109 |             camera = cameras + cameraBlkSize*pIt->first->frameID;
110 |             point = points + pointBlkSize*count;
111 |             //if first camera, fix
112 |             if(pIt->first->frameID == 0){
113 |                 CostFunction* costFunc = SnavelyReprojectionOnlyError::Create((double)observedPt.x, 
114 |                                                                               (double)observedPt.y,
115 |                                                                                pIt->first->fx,
116 |                                                                                pIt->first->cx,
117 |                                                                                pIt->first->cy,
118 |                                                                                camera);
119 |                 globalBAProblem.AddResidualBlock(costFunc, NULL, point);
120 |             }
121 |             //else do not fix
122 |             else{
123 |                 CostFunction* costFunc = SnavelyReprojectionError::Create((double)observedPt.x, 
124 |                                                                           (double)observedPt.y,
125 |                                                                           pIt->first->fx,
126 |                                                                           pIt->first->cx,
127 |                                                                           pIt->first->cy);
128 |                 globalBAProblem.AddResidualBlock(costFunc, NULL, camera, point);
129 |             }
130 |         }
131 |         count++;
132 |     }
133 |     cout << "  done!" << endl;
134 |     cout << "solving problem..." <<endl;
135 |     Solve(options, &globalBAProblem, &summary);
136 | 
137 |     //update mappoints;
138 |     n = 0;
139 |     for(set<MapPoint*>::iterator it = slammap->allMapPoints.begin(); 
140 |                                  it!= slammap->allMapPoints.end(); 
141 |                                  it++){
142 |         (*it)->pos.x = (float)pointParameters_[pointBlkSize*n + 0];
143 |         (*it)->pos.y = (float)pointParameters_[pointBlkSize*n + 1];
144 |         (*it)->pos.z = (float)pointParameters_[pointBlkSize*n + 2];
145 | 
146 |         n++;
147 |     }
148 |     for(int n = 0; n < frames.size(); n++){
149 |         frames[n]->worldRvec.at<double>(0,0) = cameraParameter_[cameraBlkSize*n + 0];
150 |         frames[n]->worldRvec.at<double>(1,0) = cameraParameter_[cameraBlkSize*n + 1];
151 |         frames[n]->worldRvec.at<double>(2,0) = cameraParameter_[cameraBlkSize*n + 2];
152 |         frames[n]->worldTvec.at<double>(0,0) = cameraParameter_[cameraBlkSize*n + 3];
153 |         frames[n]->worldTvec.at<double>(1,0) = cameraParameter_[cameraBlkSize*n + 4];
154 |         frames[n]->worldTvec.at<double>(2,0) = cameraParameter_[cameraBlkSize*n + 5];
155 |     }
156 | }
157 | 
158 | 
159 | void Optimizer::reprojectionOnlyAdjustment(Map* slammap, vector<Frame*> frames){
160 |   // set camera poses
161 |     double* cameraParameter_ = new double[cameraBlkSize*frames.size()];
162 |     cout << "frame size: " << frames.size() << endl;
163 |     for(int n = 0; n < frames.size(); n++){
164 |         cameraParameter_[cameraBlkSize*n + 0] = frames[n]->worldRvec.at<double>(0,0);
165 |         cameraParameter_[cameraBlkSize*n + 1] = frames[n]->worldRvec.at<double>(1,0);
166 |         cameraParameter_[cameraBlkSize*n + 2] = frames[n]->worldRvec.at<double>(2,0);
167 |         cameraParameter_[cameraBlkSize*n + 3] = frames[n]->worldTvec.at<double>(0,0);
168 |         cameraParameter_[cameraBlkSize*n + 4] = frames[n]->worldTvec.at<double>(1,0);
169 |         cameraParameter_[cameraBlkSize*n + 5] = frames[n]->worldTvec.at<double>(2,0);
170 |     }
171 | 
172 |     long unsigned int numPointsParameters = pointBlkSize*slammap->allMapPointNumber();
173 |     double* pointParameters_;
174 |     pointParameters_ = new double[numPointsParameters];
175 |     //convert container of mappoints to vector
176 |     vector<MapPoint*> vallMapPoints;
177 |     //add all map points into parameter
178 |     cout << "adding mappoints into problem...";
179 |     int n = 0;
180 |     for(set<MapPoint*>::iterator it = slammap->allMapPoints.begin(); 
181 |                                  it!= slammap->allMapPoints.end(); 
182 |                                  it++){
183 |         vallMapPoints.push_back(*it);
184 | 
185 |         pointParameters_[pointBlkSize*n + 0] = (double)(*it)->pos.x;
186 |         pointParameters_[pointBlkSize*n + 1] = (double)(*it)->pos.y;
187 |         pointParameters_[pointBlkSize*n + 2] = (double)(*it)->pos.z;
188 |         n++;
189 |     }
190 | 
191 |     cout <<" done!" << endl;
192 |     double* points = pointParameters_;
193 |     double* cameras = cameraParameter_;
194 |     double* point = points;
195 |     double* camera = cameras;
196 | 
197 |     //add mappoints
198 |     cout << "adding observations into problem...";
199 |     long unsigned int count = 0;//used to count mappoints
200 |     for(vector<MapPoint*>::iterator it = vallMapPoints.begin();
201 |                                     it!= vallMapPoints.end();
202 |                                     it++){
203 |         //iterate all observations of a mappoint
204 |         for(map<Frame*, unsigned int>::iterator pIt = (*it)->observations.begin();
205 |                                                 pIt!= (*it)->observations.end();
206 |                                                 pIt++){
207 |             cv::Point2f observedPt;
208 |             observedPt.x = pIt->first->keypointL[pIt->second].pt.x;
209 |             observedPt.y = pIt->first->keypointL[pIt->second].pt.y;
210 |             camera = cameras + cameraBlkSize*pIt->first->frameID;
211 |             point = points + pointBlkSize*count;
212 |             //fix all cameras
213 |             CostFunction* costFunc = SnavelyReprojectionOnlyError::Create((double)observedPt.x, 
214 |                                                                           (double)observedPt.y,
215 |                                                                            pIt->first->fx,
216 |                                                                            pIt->first->cx,
217 |                                                                            pIt->first->cy,
218 |                                                                            camera);
219 |             globalBAProblem.AddResidualBlock(costFunc, NULL, point);
220 |         
221 |         }
222 |         count++;
223 |     }
224 |     cout << "  done!" << endl;
225 |     cout << "solving problem..." <<endl;
226 |     Solve(options, &globalBAProblem, &summary);
227 | 
228 |     //update mappoints;
229 |     n = 0;
230 |     for(set<MapPoint*>::iterator it = slammap->allMapPoints.begin(); 
231 |                                  it!= slammap->allMapPoints.end(); 
232 |                                  it++){
233 |         (*it)->pos.x = (float)pointParameters_[pointBlkSize*n + 0];
234 |         (*it)->pos.y = (float)pointParameters_[pointBlkSize*n + 1];
235 |         (*it)->pos.z = (float)pointParameters_[pointBlkSize*n + 2];
236 |         n++;
237 |     }
238 | }
239 | 
240 | void Optimizer::solveProblem(Problem& pb){
241 | 	cout << "solving problem..." <<endl;
242 | 	Solve(options, &pb, &summary);
243 | }
244 | 
245 | void Optimizer::localBundleAdjustment(vector<Frame* > frames, int startIdx, int length){
246 | 	unsigned int minFrameID = frames[startIdx]->frameID;
247 | 	unsigned int maxFrameID = frames[MIN(startIdx+length-1, frames.size()-1)]->frameID;
248 | 	//get a deep copy of frames
249 | 	vector<Frame> localFrames;
250 | 	for(int n = minFrameID; n < maxFrameID+1; n++){
251 | 		localFrames.push_back(*frames[n]);
252 | 	}
253 | 	//construct camera parameters
254 | 	double* cameraParameter_ = new double[6*(maxFrameID-minFrameID+1)];
255 | 
256 | 	for(int n = minFrameID; n < maxFrameID+1; n++){//only store poses need to be adjusted
257 | 		cameraParameter_[cameraBlkSize*(n-minFrameID) + 0] = localFrames[n-minFrameID].worldRvec.at<double>(0,0);
258 | 		cameraParameter_[cameraBlkSize*(n-minFrameID) + 1] = localFrames[n-minFrameID].worldRvec.at<double>(1,0);
259 | 		cameraParameter_[cameraBlkSize*(n-minFrameID) + 2] = localFrames[n-minFrameID].worldRvec.at<double>(2,0);
260 | 
261 | 		cameraParameter_[cameraBlkSize*(n-minFrameID) + 3] = localFrames[n-minFrameID].worldTvec.at<double>(0,0);
262 | 		cameraParameter_[cameraBlkSize*(n-minFrameID) + 4] = localFrames[n-minFrameID].worldTvec.at<double>(1,0);
263 | 		cameraParameter_[cameraBlkSize*(n-minFrameID) + 5] = localFrames[n-minFrameID].worldTvec.at<double>(2,0);
264 | 	}
265 | 
266 | 	// build problem
267 | 	Problem localProblem;
268 | 	vector<MapPoint> localMapPoints; //not using pointer
269 | 	unordered_set<MapPoint* > localMapPointsSet;
270 | 	vector<cv::Point3f> localScenePoints;
271 | 	vector<int> localScenePointsIdx2;
272 | 	vector<int> localScenePointsIdx3;
273 | 
274 | 	vector<cv::Point2f> localKeypoints;
275 | 	vector<cv::Point2f> localKeypoints2;
276 | 	vector<cv::Point2f> localKeypoints3;
277 | 
278 | 	double* cameras = cameraParameter_;
279 | 	double* camera = cameras;
280 | 	int offset = 0;
281 | 
282 | 	//============= test 2: add all points into parameters ========================
283 | 	for(int f = 0; f < localFrames.size(); f++){
284 | 		for(int p = 0; p < localFrames[f].mappoints.size(); p++){
285 | 			if(localFrames[f].mappoints[p] != NULL && !localFrames[f].mappoints[p]->isBad){
286 | 				localMapPointsSet.insert(localFrames[f].mappoints[p]);
287 | 			}
288 | 		}
289 | 	}
290 | 	//create point parameters
291 | 	long unsigned int numPointsParameters = pointBlkSize*localMapPointsSet.size();
292 | 	double* pointParameters_;
293 | 	pointParameters_ = new double[numPointsParameters];
294 | 
295 | 	//add all mappoints into problem
296 | 	int ptCount = 0;
297 | 
298 | 	//============= test 2: add all points into parameters =========================
299 | 	for(unordered_set<MapPoint* >::iterator pIt = localMapPointsSet.begin();
300 | 			pIt != localMapPointsSet.end();
301 | 			pIt++){
302 | 		pointParameters_[pointBlkSize*ptCount + 0] = (double)(*pIt)->pos.x;
303 | 		pointParameters_[pointBlkSize*ptCount + 1] = (double)(*pIt)->pos.y;
304 | 		pointParameters_[pointBlkSize*ptCount + 2] = (double)(*pIt)->pos.z;
305 | 		ptCount++;
306 | 	}
307 | 	//============= test 2 ends ===================================================
308 | 
309 | 	//add observations
310 | 	double* points = pointParameters_;
311 | 	double* point = points;
312 | 	vector<cv::Point2f> observedPts;
313 | 	int pointCount = 0;
314 | 	for(unordered_set<MapPoint*>::iterator mappointIt = localMapPointsSet.begin();
315 | 			mappointIt != localMapPointsSet.end();
316 | 			mappointIt++){
317 | 		for(map<Frame*, unsigned int>::iterator pIt = (*mappointIt)->observations.begin();
318 | 				pIt!= (*mappointIt)->observations.end();
319 | 				pIt++){
320 | 			cv::Point2f observedPt;
321 | 
322 | 			observedPt.x = pIt->first->keypointL[pIt->second].pt.x;
323 | 			observedPt.y = pIt->first->keypointL[pIt->second].pt.y;
324 | 
325 | 			point = points + pointBlkSize*pointCount;
326 | 
327 | 			//only fix the first frame in the entire sequence
328 | 			if(pIt->first->frameID == 0 && startIdx< 1){
329 | 				// if(pIt->first->frameID == 0){
330 | 				camera = cameras;
331 | 				CostFunction* costFunc = SnavelyReprojectionOnlyError::Create((double)observedPt.x,
332 | 						(double)observedPt.y,
333 | 						pIt->first->fx,
334 | 						pIt->first->cx,
335 | 						pIt->first->cy,
336 | 						camera);
337 | 				localProblem.AddResidualBlock(costFunc, NULL, point);
338 | 			}
339 | 			//else do not fix
340 | 			else if(pIt->first->frameID >= minFrameID &&
341 | 					pIt->first->frameID <= maxFrameID){
342 | 				camera = cameras + cameraBlkSize*(pIt->first->frameID - minFrameID);
343 | 				// ==== this part is to extract keypoint in the corresponding frame
344 | 				// ==== only for evaluation
345 | 				if(pIt->first->frameID == minFrameID + 1){
346 | 					localScenePointsIdx2.push_back(pointCount);
347 | 					localKeypoints2.push_back(observedPt);
348 | 				}
349 | 				if(pIt->first->frameID == minFrameID + 2){
350 | 					localScenePointsIdx3.push_back(pointCount);
351 | 					localKeypoints3.push_back(observedPt);
352 | 				}
353 | 				//========= test end ======================================
354 | 
355 | 				CostFunction* costFunc = SnavelyReprojectionError::Create((double)observedPt.x,
356 | 						(double)observedPt.y,
357 | 						pIt->first->fx,
358 | 						pIt->first->cx,
359 | 						pIt->first->cy);
360 | 				localProblem.AddResidualBlock(costFunc, NULL, camera, point);
361 | 			}
362 | 			}
363 | 			pointCount++;
364 | 		}
365 | 
366 | 		Solve(options, &localProblem, &summary);
367 | 		if(options.minimizer_progress_to_stdout){
368 | 			cout << summary.FullReport() << endl;
369 | 		}
370 | 		//update points and pose
371 | 		for(int n = minFrameID; n < maxFrameID+1; n++){
372 | 			frames[n]->worldRvec.at<double>(0,0) = cameraParameter_[cameraBlkSize*(n-minFrameID) + 0];
373 | 			frames[n]->worldRvec.at<double>(1,0) = cameraParameter_[cameraBlkSize*(n-minFrameID) + 1];
374 | 			frames[n]->worldRvec.at<double>(2,0) = cameraParameter_[cameraBlkSize*(n-minFrameID) + 2];
375 | 
376 | 			frames[n]->worldTvec.at<double>(0,0) = cameraParameter_[cameraBlkSize*(n-minFrameID) + 3];
377 | 			frames[n]->worldTvec.at<double>(1,0) = cameraParameter_[cameraBlkSize*(n-minFrameID) + 4];
378 | 			frames[n]->worldTvec.at<double>(2,0) = cameraParameter_[cameraBlkSize*(n-minFrameID) + 5];
379 | 		}
380 | 
381 | 		pointCount = 0;
382 | 		for(unordered_set<MapPoint* >::iterator pIt = localMapPointsSet.begin();
383 | 				pIt != localMapPointsSet.end();
384 | 				pIt++){
385 | 			(*pIt)->pos.x = (float)pointParameters_[pointBlkSize*pointCount + 0];
386 | 			(*pIt)->pos.y = (float)pointParameters_[pointBlkSize*pointCount + 1];
387 | 			(*pIt)->pos.z = (float)pointParameters_[pointBlkSize*pointCount + 2];
388 | 			pointCount++;
389 | 		}
390 | 		delete [] cameraParameter_;
391 | 	}
392 | 
393 | 


--------------------------------------------------------------------------------