├── .gitattributes
├── inc
    └── eight
    │   ├── normalize.h
    │   ├── essential.h
    │   ├── select.h
    │   ├── distance.h
    │   ├── structure.h
    │   ├── project.h
    │   ├── fundamental.h
    │   └── triangulate.h
├── src
    ├── normalize.cpp
    ├── distance.cpp
    ├── project.cpp
    ├── essential.cpp
    ├── structure.cpp
    └── fundamental.cpp
├── .gitignore
├── tests
    ├── utils.h
    ├── test_triangulation.cpp
    ├── test_structure.cpp
    ├── test_pose.cpp
    └── test_pose_outliers.cpp
├── CMakeLists.txt
└── examples
    ├── calibrate.cpp
    └── defocus.cpp


/.gitattributes:
--------------------------------------------------------------------------------
 1 | # Auto detect text files and perform LF normalization
 2 | * text=auto
 3 | 
 4 | # Custom for Visual Studio
 5 | *.cs     diff=csharp
 6 | 
 7 | # Standard to msysgit
 8 | *.doc	 diff=astextplain
 9 | *.DOC	 diff=astextplain
10 | *.docx diff=astextplain
11 | *.DOCX diff=astextplain
12 | *.dot  diff=astextplain
13 | *.DOT  diff=astextplain
14 | *.pdf  diff=astextplain
15 | *.PDF	 diff=astextplain
16 | *.rtf	 diff=astextplain
17 | *.RTF	 diff=astextplain
18 | 


--------------------------------------------------------------------------------
/inc/eight/normalize.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of 8point.
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #ifndef EIGHT_NORMALIZE_H
12 | #define EIGHT_NORMALIZE_H
13 | 
14 | #include <Eigen/Core>
15 | #include <Eigen/Geometry>
16 | 
17 | namespace eight {
18 |     
19 |     /**
20 |         Find normalizing transform of image points to support the conditioning of fundamental matrices.
21 |     */
22 |     Eigen::Affine2d findIsotropicNormalizingTransform(Eigen::Ref<const Eigen::MatrixXd> a);
23 |     
24 | }
25 | 
26 | #endif


--------------------------------------------------------------------------------
/src/normalize.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |  This file is part of 8point.
 3 |  
 4 |  Copyright(C) 2015/2016 Christoph Heindl
 5 |  All rights reserved.
 6 |  
 7 |  This software may be modified and distributed under the terms
 8 |  of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #include <eight/normalize.h>
12 | 
13 | namespace eight {
14 |     
15 |     Eigen::Affine2d findIsotropicNormalizingTransform(Eigen::Ref<const Eigen::MatrixXd> a) {
16 |         Eigen::Vector2d mean = a.rowwise().mean();
17 |         Eigen::Vector2d stddev = (a.colwise() - mean).array().square().rowwise().mean().sqrt();
18 |         
19 |         Eigen::Affine2d t;
20 |         t = Eigen::Scaling(1.0 / stddev.norm()) *  Eigen::Translation2d(-mean);
21 |         return t;
22 |     }
23 |     
24 | }


--------------------------------------------------------------------------------
/src/distance.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |  This file is part of 8point.
 3 |  
 4 |  Copyright(C) 2015/2016 Christoph Heindl
 5 |  All rights reserved.
 6 |  
 7 |  This software may be modified and distributed under the terms
 8 |  of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #include <eight/distance.h>
12 | 
13 | namespace eight {
14 |     
15 |     double SampsonDistanceSquared::operator()(const Eigen::Matrix3d &f, Eigen::Ref<const Eigen::Vector2d> a, Eigen::Ref<const Eigen::Vector2d> b) const {
16 |         Eigen::Vector3d fa = f.transpose() * a.homogeneous();
17 |         Eigen::Vector3d fb = f * b.homogeneous();
18 |         
19 |         double bfa = a.homogeneous().transpose() * fb;
20 |         
21 |         return (bfa * bfa) / (fa.topRows(2).squaredNorm() + fb.topRows(2).squaredNorm());
22 |     }
23 |     
24 |     
25 | }


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
 1 | # Windows image file caches
 2 | Thumbs.db
 3 | ehthumbs.db
 4 | 
 5 | # Folder config file
 6 | Desktop.ini
 7 | 
 8 | # Recycle Bin used on file shares
 9 | $RECYCLE.BIN/
10 | 
11 | # Windows Installer files
12 | *.cab
13 | *.msi
14 | *.msm
15 | *.msp
16 | 
17 | # Windows shortcuts
18 | *.lnk
19 | 
20 | # =========================
21 | # Operating System Files
22 | # =========================
23 | 
24 | # OSX
25 | # =========================
26 | 
27 | .DS_Store
28 | .AppleDouble
29 | .LSOverride
30 | 
31 | # Thumbnails
32 | ._*
33 | 
34 | # Files that might appear in the root of a volume
35 | .DocumentRevisions-V100
36 | .fseventsd
37 | .Spotlight-V100
38 | .TemporaryItems
39 | .Trashes
40 | .VolumeIcon.icns
41 | 
42 | # Directories potentially created on remote AFP share
43 | .AppleDB
44 | .AppleDesktop
45 | Network Trash Folder
46 | Temporary Items
47 | .apdisk
48 | 


--------------------------------------------------------------------------------
/tests/utils.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |  This file is part of 8point.
 3 |  
 4 |  Copyright(C) 2015/2016 Christoph Heindl
 5 |  All rights reserved.
 6 |  
 7 |  This software may be modified and distributed under the terms
 8 |  of the BSD license.See the LICENSE file for details.
 9 |  */
10 | 
11 | #include <Eigen/Core>
12 | #include <Eigen/Geometry>
13 | 
14 | namespace eight {
15 |     namespace utils {
16 |         
17 |         inline Eigen::Matrix<double, 3, Eigen::Dynamic> samplePointsInBox(const Eigen::Vector3d &minCorner, const Eigen::Vector3d &maxCorner, Eigen::DenseIndex count) {
18 |             Eigen::AlignedBox3d box(minCorner, maxCorner);
19 |             Eigen::Matrix<double, 3, Eigen::Dynamic> points(3, count);
20 |             for (int i = 0; i < count; ++i) {
21 |                 points.col(i) = box.sample();
22 |             }
23 |             return points;
24 |         }
25 |     }
26 | }


--------------------------------------------------------------------------------
/inc/eight/essential.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of 8point.
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #ifndef EIGHT_ESSENTIAL_H
12 | #define EIGHT_ESSENTIAL_H
13 | 
14 | #include <Eigen/Core>
15 | #include <Eigen/Geometry>
16 | 
17 | namespace eight {
18 |     
19 |     /**
20 |         Essential matrix from camera instrinsics and fundamental matrix.
21 |     */
22 |     Eigen::Matrix3d essentialMatrix(const Eigen::Matrix3d &k, const Eigen::Matrix3d &f);
23 |     
24 |     
25 |     /**
26 |         Recover pose from essential matrix up to scale.
27 |     */
28 |     Eigen::Matrix<double, 3, 4> pose(const Eigen::Matrix3d &e, const Eigen::Matrix3d &k, Eigen::Ref<const Eigen::MatrixXd> a, Eigen::Ref<const Eigen::MatrixXd> b);
29 |     
30 | }
31 | 
32 | #endif


--------------------------------------------------------------------------------
/inc/eight/select.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of 8point.
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #ifndef EIGHT_SELECT_H
12 | #define EIGHT_SELECT_H
13 | 
14 | #include <Eigen/Core>
15 | #include <iterator>
16 | 
17 | namespace eight {
18 |     
19 |     template<class IndexIterator>
20 |     Eigen::MatrixXd
21 |     selectColumnsByIndex(const Eigen::Ref<const Eigen::MatrixXd > &m, IndexIterator begin, IndexIterator end) {
22 |         
23 |         Eigen::DenseIndex count = (Eigen::DenseIndex)std::distance(begin, end);
24 | 
25 |         Eigen::MatrixXd r(m.rows(), count);
26 |         
27 |         Eigen::DenseIndex i = 0;
28 |         while (begin != end) {
29 |             r.col(i++) = m.col(*begin++);
30 |         }
31 | 
32 |         return r;
33 |     }
34 |     
35 | }
36 | 
37 | #endif


--------------------------------------------------------------------------------
/inc/eight/distance.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of 8point.
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #ifndef EIGHT_ERROR_H
12 | #define EIGHT_ERROR_H
13 | 
14 | #include <Eigen/Core>
15 | #include <Eigen/Geometry>
16 | 
17 | namespace eight {
18 |     
19 |     /** 
20 |         First order approximation of geometric error.
21 |     */
22 |     class SampsonDistanceSquared {
23 |     public:
24 |         double operator()(const Eigen::Matrix3d &f, Eigen::Ref<const Eigen::Vector2d> a,  Eigen::Ref<const Eigen::Vector2d> b) const;
25 |     };
26 |     
27 |     /**
28 |         Compute distances for each pair of correspondences.
29 |     */
30 |     template<class Functor>
31 |     Eigen::VectorXd distances(const Eigen::Matrix3d &f, Eigen::Ref<const Eigen::MatrixXd> a, Eigen::Ref<const Eigen::MatrixXd> b, Functor err) {
32 |         Eigen::VectorXd errs(a.cols());
33 |         for (Eigen::DenseIndex i = 0; i < a.cols(); ++i) {
34 |             errs(i) = err(f, a.col(i), b.col(i));
35 |         }
36 |         return errs;
37 |     }
38 |     
39 | }
40 | 
41 | #endif


--------------------------------------------------------------------------------
/inc/eight/structure.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of 8point.
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #ifndef EIGHT_STRUCTURE_H
12 | #define EIGHT_STRUCTURE_H
13 | 
14 | #include <Eigen/Core>
15 | 
16 | namespace eight {
17 |     
18 |     /** 
19 |         Reconstruct 3d points from two views.
20 |      
21 |         Simultanously solves for all pixel correspondences using linear least squares.
22 |      
23 |         \param k Camera intrinsic matrix
24 |         \param pose1 Translation and orientation of the second camera
25 |         \param u0 pixel coordinates in the first image
26 |         \param u1 pixel coordinates in the second image
27 |      
28 |         Ma, Yi, et al. 
29 |         An invitation to 3-d vision: from images to geometric models.
30 |         Vol. 26. Springer Science & Business Media, 2012. section 5.2.2
31 |      
32 |      */
33 |     Eigen::Matrix<double, 3, Eigen::Dynamic> structureFromTwoViews(const Eigen::Matrix<double, 3, 3> &k,
34 |                                                                    const Eigen::Matrix<double, 3, 4> &pose1,
35 |                                                                    Eigen::Ref<const Eigen::MatrixXd> u0,
36 |                                                                    Eigen::Ref<const Eigen::MatrixXd> u1);
37 |     
38 | }
39 | 
40 | #endif


--------------------------------------------------------------------------------
/src/project.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |  This file is part of 8point.
 3 |  
 4 |  Copyright(C) 2015/2016 Christoph Heindl
 5 |  All rights reserved.
 6 |  
 7 |  This software may be modified and distributed under the terms
 8 |  of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #include <eight/project.h>
12 | #include <Eigen/Geometry>
13 | 
14 | namespace eight {
15 | 
16 |     Eigen::Matrix<double, 3, 4> cameraPose(const Eigen::Matrix<double, 3, 3> &r, const Eigen::Vector3d &t)
17 |     {
18 |         Eigen::AffineCompact3d iso;
19 |         iso.linear() = r;
20 |         iso.translation() = t;
21 | 
22 |         return iso.matrix();
23 |     }
24 |     
25 |     Eigen::Matrix<double, 3, 4> cameraMatrix(const Eigen::Matrix<double, 3, 3> &k, const Eigen::Matrix<double, 3, 3> &r, const Eigen::Vector3d &t)
26 |     {
27 |         return cameraMatrix(k, cameraPose(r, t));
28 |     }
29 | 
30 |     Eigen::Matrix<double, 3, 4> cameraMatrix(const Eigen::Matrix<double, 3, 3> &k, const Eigen::AffineCompact3d &pose)
31 |     {
32 |         return k * pose.inverse(Eigen::Isometry).matrix();
33 |     }
34 | 
35 |     Eigen::Matrix<double, 3, 4> cameraMatrix(const Eigen::Matrix<double, 3, 3> &k,
36 |                                              const Eigen::Matrix<double, 3, 4> &pose)
37 |     {        
38 |         Eigen::AffineCompact3d iso(pose);        
39 |         return k * iso.inverse(Eigen::Isometry).matrix();
40 |     }
41 |     
42 |     Eigen::Matrix<double, 3, Eigen::Dynamic> perspectiveProject(Eigen::Ref<const Eigen::MatrixXd> points, const Eigen::Matrix<double, 3, 4>  &p) {
43 |         return p * points.colwise().homogeneous();
44 |     }
45 |     
46 |     
47 | }


--------------------------------------------------------------------------------
/inc/eight/project.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of 8point.
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #ifndef EIGHT_PROJECT_H
12 | #define EIGHT_PROJECT_H
13 | 
14 | #include <Eigen/Core>
15 | #include <Eigen/Geometry>
16 | 
17 | namespace eight {
18 | 
19 |     /**
20 |     Assemble perspective projection matrix.
21 |     */
22 |     Eigen::Matrix<double, 3, 4> cameraPose(const Eigen::Matrix<double, 3, 3> &r, const Eigen::Vector3d &t);
23 |     
24 |     /**
25 |         Assemble perspective projection matrix.
26 |     */
27 |     Eigen::Matrix<double, 3, 4> cameraMatrix(const Eigen::Matrix<double, 3, 3> &k,
28 |                                              const Eigen::Matrix<double, 3, 3> &r,
29 |                                              const Eigen::Vector3d &t);
30 | 
31 |     /**
32 |     Assemble perspective projection matrix.
33 |     */
34 |     Eigen::Matrix<double, 3, 4> cameraMatrix(const Eigen::Matrix<double, 3, 3> &k,
35 |                                              const Eigen::AffineCompact3d &pose);
36 | 
37 |     /**
38 |     Assemble perspective projection matrix.
39 |     */
40 |     Eigen::Matrix<double, 3, 4> cameraMatrix(const Eigen::Matrix<double, 3, 3> &k,
41 |                                              const Eigen::Matrix<double, 3, 4> &pose);
42 |     
43 |     /**
44 |         Perspective projection of three-dimensional points.
45 |     */
46 |     Eigen::Matrix<double, 3, Eigen::Dynamic> perspectiveProject(Eigen::Ref<const Eigen::MatrixXd> points, const Eigen::Matrix<double, 3, 4>  &p);
47 |     
48 | }
49 | 
50 | #endif


--------------------------------------------------------------------------------
/inc/eight/fundamental.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of 8point.
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #ifndef EIGHT_FUNDAMENTAL_H
12 | #define EIGHT_FUNDAMENTAL_H
13 | 
14 | #include <Eigen/Core>
15 | #include <vector>
16 | 
17 | namespace eight {
18 |     
19 |     /**
20 |         Estimate fundamental matrix from pairs of corresponding image points
21 |     */
22 |     Eigen::Matrix3d fundamentalMatrixUnnormalized(Eigen::Ref<const Eigen::MatrixXd> a, Eigen::Ref<const Eigen::MatrixXd> b);
23 |     
24 |     /**
25 |         Estimate fundamental matrix from pairs of corresponding image points.
26 | 
27 |         All points are considered inliers.
28 |      */
29 |     Eigen::Matrix3d fundamentalMatrix(Eigen::Ref<const Eigen::MatrixXd> a, Eigen::Ref<const Eigen::MatrixXd> b);
30 | 
31 |     /**
32 |         Estimate fundamental matrix from pairs of corresponding image points when outliers are present.
33 | 
34 |         \param a Image coordinates in first image.
35 |         \param b Image coordinates in second image.
36 |         \param inliers Inlier column indices of best solution.
37 |         \param d Tolerated distance from the model for inliers (Sampson Error).
38 |         \param e Assumed outlier percent in data set.
39 |         \param p Probability that at least one valid set of inliers is chosen.
40 |     */
41 |     Eigen::Matrix3d fundamentalMatrixRobust(
42 |         Eigen::Ref<const Eigen::MatrixXd> a, 
43 |         Eigen::Ref<const Eigen::MatrixXd> b, 
44 |         std::vector<Eigen::DenseIndex> &inliers, 
45 |         double d = 3.0, double e = 0.2, double p = 0.99);
46 |     
47 | }
48 | 
49 | #endif


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8)
 2 | 
 3 | project(8point)
 4 | 
 5 | if (WIN32)
 6 | 	add_definitions("-D_SCL_SECURE_NO_WARNINGS -D_CRT_SECURE_NO_WARNINGS")
 7 | else()
 8 |     set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 9 |     set(CMAKE_XCODE_ATTRIBUTE_CLANG_CXX_LIBRARY "libc++")
10 | endif()
11 | 
12 | set(8POINT_LINK_TARGETS)
13 | 
14 | set(8POINT_EIGEN_DIR "../eigen" CACHE PATH "Where is the include directory of Eigen located")
15 | if (WIN32)
16 |   set(OpenCV_STATIC OFF)
17 |   set(OpenCV_SHARED ON)
18 | endif()
19 | find_package(OpenCV REQUIRED)
20 | 
21 | find_package(Ceres REQUIRED) 
22 | 
23 | include_directories(${OpenCV_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS})
24 | list(APPEND 8POINT_LINK_TARGETS ${OpenCV_LIBRARIES} ${CERES_LIBRARIES})
25 | 
26 | include_directories(${CMAKE_CURRENT_BINARY_DIR} ${8POINT_EIGEN_DIR} "inc")
27 | 
28 | add_library(8point
29 |     inc/eight/fundamental.h
30 |     inc/eight/essential.h
31 |     inc/eight/normalize.h
32 |     inc/eight/distance.h
33 |     inc/eight/triangulate.h
34 |     inc/eight/project.h
35 |     inc/eight/select.h
36 |     inc/eight/structure.h
37 |     src/fundamental.cpp
38 |     src/essential.cpp
39 |     src/normalize.cpp
40 |     src/distance.cpp
41 |     src/project.cpp
42 |     src/structure.cpp
43 | )
44 | 
45 | add_executable(8point_tests
46 |     tests/catch.hpp
47 |     tests/utils.h
48 |     tests/test_pose.cpp
49 |     tests/test_pose_outliers.cpp
50 |     tests/test_triangulation.cpp
51 |     tests/test_structure.cpp
52 | )
53 | target_link_libraries(8point_tests 8point ${8POINT_LINK_TARGETS})
54 | 
55 | 
56 | add_executable(8point_defocus
57 |     examples/defocus.cpp    
58 | )
59 | target_link_libraries(8point_defocus 8point ${8POINT_LINK_TARGETS})
60 | 
61 | add_executable(8point_calibrate
62 |     examples/calibrate.cpp    
63 | )
64 | target_link_libraries(8point_calibrate 8point ${8POINT_LINK_TARGETS})


--------------------------------------------------------------------------------
/tests/test_triangulation.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of Deformable Shape Tracking (DEST).
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #include "catch.hpp"
12 | 
13 | #include <eight/project.h>
14 | #include <eight/triangulate.h>
15 | #include "utils.h"
16 | 
17 | TEST_CASE("test_triangulation")
18 | {
19 |     const double foc = 530.0;
20 |     const int width = 640;
21 |     const int height = 480;
22 |     const int nPoints = 60;
23 | 
24 |     Eigen::Matrix3d k;
25 |     k << 
26 |         foc, 0.0, 0.5 *(width - 1),
27 |         0.0, foc, 0.5 *(height - 1),
28 |         0.0, 0.0, 1.0;
29 | 
30 |     // Generate random 3D points
31 |     Eigen::Matrix<double, 3, Eigen::Dynamic> points = eight::utils::samplePointsInBox(Eigen::Vector3d(-500, -500, 300), Eigen::Vector3d(500, 500, 1500), nPoints);
32 | 
33 |     // Assume the first camera at origin and the second freely transformed.
34 |     Eigen::AffineCompact3d t0;
35 |     t0.setIdentity();
36 | 
37 |     Eigen::AffineCompact3d t1;
38 |     t1 = Eigen::Translation3d(15.0, 0.0, 3.5) * Eigen::AngleAxisd(0.25*M_PI, Eigen::Vector3d(0.5, -0.3, 0.2).normalized());
39 | 
40 |     // Generate projected image points
41 |     Eigen::Matrix<double, 3, 4> cam0 = eight::cameraMatrix(k, t0);
42 |     Eigen::Matrix<double, 3, 4> cam1 = eight::cameraMatrix(k, t1);
43 | 
44 |     Eigen::Matrix<double, 2, Eigen::Dynamic> image0 = eight::perspectiveProject(points, cam0).colwise().hnormalized();
45 |     Eigen::Matrix<double, 2, Eigen::Dynamic> image1 = eight::perspectiveProject(points, cam1).colwise().hnormalized();
46 | 
47 |     Eigen::Matrix<double, 3, Eigen::Dynamic > pointsTriangulated = eight::triangulateMany(cam0, cam1, image0, image1);
48 | 
49 |     REQUIRE(pointsTriangulated.isApprox(points));
50 | 
51 | }


--------------------------------------------------------------------------------
/tests/test_structure.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of Deformable Shape Tracking (DEST).
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #include "catch.hpp"
12 | 
13 | #include <eight/project.h>
14 | #include <eight/structure.h>
15 | #include "utils.h"
16 | #include <iostream>
17 | 
18 | TEST_CASE("test_structure")
19 | {
20 |     const double foc = 530.0;
21 |     const int width = 640;
22 |     const int height = 480;
23 |     const int nPoints = 20;
24 | 
25 |     Eigen::Matrix3d k;
26 |     k << 
27 |         foc, 0.0, 0.5 *(width - 1),
28 |         0.0, foc, 0.5 *(height - 1),
29 |         0.0, 0.0, 1.0;
30 | 
31 |     // Generate random 3D points
32 |     Eigen::Matrix<double, 3, Eigen::Dynamic> points = eight::utils::samplePointsInBox(Eigen::Vector3d(-500, -500, 300), Eigen::Vector3d(500, 500, 1500), nPoints);
33 | 
34 |     // Assume the first camera at origin and the second freely transformed.
35 |     Eigen::AffineCompact3d t0;
36 |     t0.setIdentity();
37 | 
38 |     Eigen::AffineCompact3d t1;
39 |     t1 = Eigen::Translation3d(15.0, 0.0, 3.5) * Eigen::AngleAxisd(0.25*M_PI, Eigen::Vector3d(0.5, -0.3, 0.2).normalized());
40 | 
41 |     // Generate projected image points
42 |     Eigen::Matrix<double, 3, 4> cam0 = eight::cameraMatrix(k, t0);
43 |     Eigen::Matrix<double, 3, 4> cam1 = eight::cameraMatrix(k, t1);
44 | 
45 |     Eigen::Matrix<double, 2, Eigen::Dynamic> image0 = eight::perspectiveProject(points, cam0).colwise().hnormalized();
46 |     Eigen::Matrix<double, 2, Eigen::Dynamic> image1 = eight::perspectiveProject(points, cam1).colwise().hnormalized();
47 | 
48 |     Eigen::Matrix<double, 3, Eigen::Dynamic > pointsTriangulated = eight::structureFromTwoViews(k, t1.matrix(), image0, image1);
49 |     
50 |     // Test up to scale
51 |     double scale = points.col(0).z() / pointsTriangulated.col(0).z();
52 |     pointsTriangulated.array() *=  scale;
53 |     
54 | 
55 |     REQUIRE(pointsTriangulated.isApprox(points, 1e-3));
56 | 
57 | }


--------------------------------------------------------------------------------
/tests/test_pose.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of Deformable Shape Tracking (DEST).
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #define CATCH_CONFIG_MAIN
12 | #include "catch.hpp"
13 | 
14 | #include <eight/fundamental.h>
15 | #include <eight/normalize.h>
16 | #include <eight/distance.h>
17 | #include <eight/essential.h>
18 | #include <eight/project.h>
19 | #include "utils.h"
20 | 
21 | TEST_CASE("test_pose")
22 | {
23 |     const double foc = 530.0;
24 |     const int width = 640;
25 |     const int height = 480;
26 |     const int nPoints = 8;
27 | 
28 |     Eigen::Matrix3d k;
29 |     k << 
30 |         foc, 0.0, 0.5 *(width - 1),
31 |         0.0, foc, 0.5 *(height - 1),
32 |         0.0, 0.0, 1.0;
33 | 
34 |     // Generate random 3D points
35 |     Eigen::Matrix<double, 3, Eigen::Dynamic> points = eight::utils::samplePointsInBox(Eigen::Vector3d(-500, -500, 300), Eigen::Vector3d(500, 500, 1500), nPoints);
36 | 
37 |     // Assume the first camera at origin and the second freely transformed.
38 |     Eigen::AffineCompact3d t0;
39 |     t0.setIdentity();
40 | 
41 |     Eigen::AffineCompact3d t1;
42 |     t1 = Eigen::Translation3d(15.0, 0.0, 3.5) * Eigen::AngleAxisd(0.25*M_PI, Eigen::Vector3d(0.5, -0.3, 0.2).normalized());
43 | 
44 |     // Generate projected image points
45 |     Eigen::Matrix<double, 3, 4> cam0 = eight::cameraMatrix(k, t0);
46 |     Eigen::Matrix<double, 3, 4> cam1 = eight::cameraMatrix(k, t1);
47 | 
48 |     Eigen::Matrix<double, 2, Eigen::Dynamic> image0 = eight::perspectiveProject(points, cam0).colwise().hnormalized();
49 |     Eigen::Matrix<double, 2, Eigen::Dynamic> image1 = eight::perspectiveProject(points, cam1).colwise().hnormalized();
50 | 
51 |     Eigen::Matrix3d F = eight::fundamentalMatrix(image0, image1);   
52 |     std::cout << F << std::endl;
53 |     Eigen::Matrix3d E = eight::essentialMatrix(k, F);
54 |     Eigen::Matrix<double, 3, 4> pose = eight::pose(E, k, image0, image1);
55 | 
56 |     std::cout << "Should be: " << std::endl <<  t1.matrix() << std::endl;
57 |     std::cout << "Pose: " << std::endl << pose << std::endl;
58 | 
59 |     // Note: Translation can only be compared up to scale.
60 |     Eigen::Matrix<double, 3, 4> tm = t1.matrix();
61 |     tm.block<3, 1>(0, 3).normalize();
62 | 
63 |     REQUIRE(pose.isApprox(tm, 1e-3));
64 | }


--------------------------------------------------------------------------------
/inc/eight/triangulate.h:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of 8point.
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #ifndef EIGHT_TRIANGULATE_H
12 | #define EIGHT_TRIANGULATE_H
13 | 
14 | #include <Eigen/Core>
15 | 
16 | namespace eight {
17 |     
18 |     /** Triangulate rays using linear least squares.
19 |      
20 |         Based on
21 |         Hartley, Richard I., and Peter Sturm. "Triangulation."
22 |         Computer vision and image understanding 68.2 (1997): 146-157.
23 |      */
24 |     inline Eigen::Vector3d triangulate(const Eigen::Matrix<double, 3, 4> &cam0,
25 |                                        const Eigen::Matrix<double, 3, 4> &cam1,
26 |                                        const Eigen::Vector2d &u0,
27 |                                        const Eigen::Vector2d &u1)
28 |     {
29 |         // Build 4x3 A and 4x1 b in a single 4x4
30 |         Eigen::Matrix<double, 4, 4> X;
31 |         X.row(0) = u0(0) * cam0.row(2) - cam0.row(0);
32 |         X.row(1) = u0(1) * cam0.row(2) - cam0.row(1);
33 |         X.row(2) = u1(0) * cam1.row(2) - cam1.row(0);
34 |         X.row(3) = u1(1) * cam1.row(2) - cam1.row(1);
35 |         
36 |         return X.block<4,3>(0,0).jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(-X.col(3));
37 |     }
38 |     
39 |     /** Triangulate rays using linear least squares.
40 |      
41 |         Based on
42 |         Hartley, Richard I., and Peter Sturm. "Triangulation."
43 |         Computer vision and image understanding 68.2 (1997): 146-157.
44 |      */
45 |     inline Eigen::Matrix<double, 3, Eigen::Dynamic> triangulateMany(const Eigen::Matrix<double, 3, 4> &cam0,
46 |                                                                     const Eigen::Matrix<double, 3, 4> &cam1,
47 |                                                                     Eigen::Ref<const Eigen::MatrixXd> u0,
48 |                                                                     Eigen::Ref<const Eigen::MatrixXd> u1)
49 |     {
50 |         Eigen::Matrix<double, 3, Eigen::Dynamic> vecs(3, u0.cols());
51 |         
52 |         Eigen::Matrix<double, 4, 4> X;
53 |         for (Eigen::DenseIndex i  = 0; i < u0.cols(); ++i) {
54 |             // Build 4x3 A and 4x1 b in a single 4x4
55 |             X.row(0) = u0(0, i) * cam0.row(2) - cam0.row(0);
56 |             X.row(1) = u0(1, i) * cam0.row(2) - cam0.row(1);
57 |             X.row(2) = u1(0, i) * cam1.row(2) - cam1.row(0);
58 |             X.row(3) = u1(1, i) * cam1.row(2) - cam1.row(1);
59 |             vecs.col(i) = X.block<4,3>(0,0).jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(-X.col(3));
60 |         }
61 |         
62 |         return vecs;
63 |     }
64 |     
65 | }
66 | 
67 | #endif


--------------------------------------------------------------------------------
/tests/test_pose_outliers.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of Deformable Shape Tracking (DEST).
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | 
12 | #include "catch.hpp"
13 | 
14 | #include <eight/fundamental.h>
15 | #include <eight/normalize.h>
16 | #include <eight/distance.h>
17 | #include <eight/essential.h>
18 | #include <eight/project.h>
19 | #include <eight/select.h>
20 | #include <random>
21 | #include <iostream>
22 | #include "utils.h"
23 | 
24 | TEST_CASE("test_pose_outliers")
25 | {
26 |     const double foc = 530.0;
27 |     const int width = 640;
28 |     const int height = 480;
29 |     const int nPoints = 60;
30 | 
31 |     Eigen::Matrix3d k;
32 |     k << 
33 |         foc, 0.0, 0.5 *(width - 1),
34 |         0.0, foc, 0.5 *(height - 1),
35 |         0.0, 0.0, 1.0;
36 | 
37 |     // Generate random 3D points
38 |     Eigen::Matrix<double, 3, Eigen::Dynamic> points = eight::utils::samplePointsInBox(Eigen::Vector3d(-500, -500, 300), Eigen::Vector3d(500, 500, 1500), nPoints);
39 | 
40 |     // Assume the first camera at origin and the second freely transformed.
41 |     Eigen::AffineCompact3d t0;
42 |     t0.setIdentity();
43 | 
44 |     Eigen::AffineCompact3d t1;
45 |     t1 = Eigen::Translation3d(15.0, 0.0, 3.5) * Eigen::AngleAxisd(0.25*M_PI, Eigen::Vector3d(0.5, -0.3, 0.2).normalized());
46 | 
47 |     // Generate projected image points
48 |     Eigen::Matrix<double, 3, 4> cam0 = eight::cameraMatrix(k, t0);
49 |     Eigen::Matrix<double, 3, 4> cam1 = eight::cameraMatrix(k, t1);
50 | 
51 |     Eigen::Matrix<double, 2, Eigen::Dynamic> image0 = eight::perspectiveProject(points, cam0).colwise().hnormalized();
52 |     Eigen::Matrix<double, 2, Eigen::Dynamic> image1 = eight::perspectiveProject(points, cam1).colwise().hnormalized();
53 | 
54 |     // Disturb around 10% of outliers
55 |     std::random_device rd;
56 |     std::mt19937 gen(rd());
57 |     std::uniform_int_distribution<Eigen::DenseIndex> dis(0, image0.cols() - 1);
58 |     for (int i = 0; i < nPoints / 10; i++) {
59 |         Eigen::DenseIndex idx = dis(gen);
60 |         image0.col(idx) += Eigen::Vector2d::Random() * 20.0;
61 |         image1.col(idx) += Eigen::Vector2d::Random() * 20.0;
62 |     }
63 | 
64 |     std::vector<Eigen::DenseIndex> inliers;
65 |     Eigen::Matrix3d F = eight::fundamentalMatrixRobust(image0, image1, inliers, 0.1);
66 |     Eigen::Matrix3d E = eight::essentialMatrix(k, F);
67 | 
68 |     image0 = eight::selectColumnsByIndex(image0, inliers.begin(), inliers.end());
69 |     image1 = eight::selectColumnsByIndex(image1, inliers.begin(), inliers.end());
70 | 
71 |     Eigen::Matrix<double, 3, 4> pose = eight::pose(E, k, image0, image1);
72 | 
73 |     std::cout << "Should be: " << std::endl <<  t1.matrix() << std::endl;
74 |     std::cout << "Pose: " << std::endl << pose << std::endl;
75 | 
76 |     // Note: Translation can only be compared up to scale.
77 |     Eigen::Matrix<double, 3, 4> tm = t1.matrix();
78 |     tm.block<3, 1>(0, 3).normalize();
79 | 
80 |     REQUIRE(pose.isApprox(tm, 1e-3));
81 | }


--------------------------------------------------------------------------------
/src/essential.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |  This file is part of 8point.
 3 |  
 4 |  Copyright(C) 2015/2016 Christoph Heindl
 5 |  All rights reserved.
 6 |  
 7 |  This software may be modified and distributed under the terms
 8 |  of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #include <eight/essential.h>
12 | #include <eight/triangulate.h>
13 | #include <eight/project.h>
14 | #include <Eigen/SVD>
15 | #include <iostream>
16 | 
17 | namespace eight {
18 |     
19 |     Eigen::Matrix3d essentialMatrix(const Eigen::Matrix3d &k, const Eigen::Matrix3d &f) {
20 |         return k.transpose() * f * k;
21 |     }
22 |     
23 |     Eigen::Matrix<double, 3, 4> pose(const Eigen::Matrix3d &e, const Eigen::Matrix3d &k, Eigen::Ref<const Eigen::MatrixXd> a, Eigen::Ref<const Eigen::MatrixXd> b) {
24 |         
25 |         Eigen::JacobiSVD<Eigen::Matrix3d> svd(e, Eigen::ComputeFullU | Eigen::ComputeFullV);
26 |         
27 |         // Assuming the first camera at identity, there are four possible solutions that need to be tested for
28 |         // the second camera.
29 |         
30 |         Eigen::Matrix3d u = svd.matrixU();
31 |         Eigen::Matrix3d v = svd.matrixV();
32 |         
33 |         if (u.determinant() < 0.0)
34 |             u *= -1.0;
35 |         if (v.determinant() < 0.0)
36 |             v *= -1.0;
37 |         
38 |         
39 |         Eigen::Matrix3d w;
40 |         w <<
41 |             0.0, -1.0, 0.0,
42 |             1.0, 0.0, 0.0,
43 |             0.0, 0.0, 1.0;
44 |         
45 |         
46 |         Eigen::Matrix3d r0 = u * w * v.transpose();
47 |         Eigen::Matrix3d r1 = u * w.transpose() * v.transpose();
48 |         Eigen::Vector3d t = u.col(2);
49 |        
50 |         // Test possible solutions. According to Hartley testing one point for being infront of both cameras should be
51 |         // enough.
52 |         
53 |         Eigen::Matrix<double, 3, 4> camFirst = cameraMatrix(k, Eigen::Matrix<double, 3, 4>::Identity());
54 |         Eigen::Matrix<double, 3, 4> camPosesSecond[4] = {
55 |             cameraPose(r0, t),
56 |             cameraPose(r0, -t),
57 |             cameraPose(r1, t),
58 |             cameraPose(r1, -t)
59 |         };
60 |         
61 |         // Unhandled: triangulate does not work for points at infinity. How to handle these?
62 | 
63 |         int bestId = 0;
64 |         int bestCount = 0;
65 |         for (int i = 0 ; i < 4; ++i) {
66 | 
67 |             Eigen::Matrix<double, 3, 4> camSecond = cameraMatrix(k, camPosesSecond[i]);
68 | 
69 |             Eigen::Matrix<double, 3, Eigen::Dynamic> p = triangulateMany(camFirst, camSecond, a, b);
70 |             Eigen::Matrix<double, 3, Eigen::Dynamic> pSecond = camSecond * p.colwise().homogeneous();
71 |         
72 |             int count = 0;
73 |             for (Eigen::DenseIndex j = 0; j < p.cols(); ++j) {
74 |                 if (p(2, j) >= 0.0 && pSecond(2, j) >= 0.0) {
75 |                     ++count;                    
76 |                 }
77 |             }
78 |             
79 |             if (count > bestCount) {
80 |                 bestCount = count;
81 |                 bestId = i;
82 |             }
83 |         }
84 |         
85 |         return camPosesSecond[bestId];
86 |     }
87 |     
88 | }


--------------------------------------------------------------------------------
/examples/calibrate.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |     This file is part of Deformable Shape Tracking (DEST).
 3 | 
 4 |     Copyright(C) 2015/2016 Christoph Heindl
 5 |     All rights reserved.
 6 | 
 7 |     This software may be modified and distributed under the terms
 8 |     of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #include <iostream>
12 | #include <opencv2/opencv.hpp>
13 | 
14 | void calcBoardCornerPositions(cv::Size boardSize, float squareSize, std::vector<cv::Point3f>& corners)
15 | {
16 |     corners.clear();
17 |     for (int i = 0; i < boardSize.height; ++i)
18 |         for (int j = 0; j < boardSize.width; ++j)
19 |             corners.push_back(cv::Point3f(j*squareSize, i*squareSize, 0));
20 | }
21 | 
22 | int main(int argc, char **argv) {
23 | 
24 |     if (argc != 2) {
25 |         std::cerr << argv[0] << " videofile" << std::endl;
26 |         return -1;
27 |     }
28 | 
29 |     cv::VideoCapture vc;
30 |     if (!vc.open(argv[1])) {
31 |         std::cerr << "Failed to open video" << std::endl;
32 |         return -1;
33 |     }
34 | 
35 | 
36 |     std::vector<cv::Mat> imgs;
37 | 
38 |     cv::Mat img, imgShow;
39 |     while (vc.grab()) {
40 |         vc.retrieve(img);
41 | 
42 |         cv::resize(img, imgShow, cv::Size(), 0.5, 0.5);
43 |         cv::imshow("Image", imgShow);
44 |         int key = cv::waitKey();
45 | 
46 |         if (key == 's') {
47 |             imgs.push_back(img.clone());
48 |         }
49 |     }
50 | 
51 | 
52 |     std::vector<std::vector<cv::Point2f> > imagePoints;   
53 | 
54 |     const int chessBoardFlags = cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE;
55 |     const cv::Size boardSize(10, 7);
56 |     const float squareSize = 33.85f;
57 | 
58 |     for (size_t i = 0; i < imgs.size(); ++i) {
59 |         std::vector<cv::Point2f> points;
60 |         bool found = cv::findChessboardCorners(imgs[i], boardSize, points, chessBoardFlags);
61 | 
62 |         if (found) {
63 |             cv::Mat viewGray;
64 |             cv::cvtColor(imgs[i], viewGray, cv::COLOR_BGR2GRAY);
65 |             cornerSubPix(viewGray, points, cv::Size(11, 11), cv::Size(-1, -1), cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
66 |             imagePoints.push_back(points);
67 |         }
68 |         cv::drawChessboardCorners(imgs[i], boardSize, cv::Mat(points), found);
69 |         cv::imshow("chessboard", imgs[i]);
70 |         cv::waitKey(100);
71 |     }
72 | 
73 |     // Perform calibration
74 |     std::vector<std::vector<cv::Point3f> > objectPoints(1);
75 |     calcBoardCornerPositions(boardSize, squareSize, objectPoints[0]);
76 |     objectPoints.resize(imagePoints.size(), objectPoints[0]);
77 | 
78 |     std::vector<cv::Mat> rvecs;
79 |     std::vector<cv::Mat> tvecs;
80 |     
81 |     cv::Mat cameraMatrix = cv::Mat::eye(3, 3, CV_64FC1);
82 |     cv::Mat distCoeffs = cv::Mat::zeros(8, 1, CV_64FC1);
83 |     double rms = cv::calibrateCamera(objectPoints, imagePoints, imgs[0].size(), cameraMatrix, distCoeffs, rvecs, tvecs, CV_CALIB_FIX_K3 | CV_CALIB_FIX_K2 | CV_CALIB_FIX_ASPECT_RATIO);
84 | 
85 |     std::cout << "RMS " << rms << std::endl;
86 |     std::cout << cameraMatrix << std::endl;
87 |     std::cout << distCoeffs << std::endl;
88 |     
89 | }
90 | 


--------------------------------------------------------------------------------
/src/structure.cpp:
--------------------------------------------------------------------------------
 1 | /**
 2 |  This file is part of 8point.
 3 |  
 4 |  Copyright(C) 2015/2016 Christoph Heindl
 5 |  All rights reserved.
 6 |  
 7 |  This software may be modified and distributed under the terms
 8 |  of the BSD license.See the LICENSE file for details.
 9 | */
10 | 
11 | #include <eight/structure.h>
12 | #include <Eigen/Dense>
13 | #include <Eigen/Geometry>
14 | 
15 | namespace eight {
16 |     
17 |     inline Eigen::Matrix<double, 3, 3> hat(Eigen::Ref<const Eigen::Vector3d> u) {
18 |         
19 |         Eigen::Matrix<double, 3, 3> h(3,3);
20 |         h <<
21 |             0.0, -u.z(), u.y(),
22 |             u.z(), 0.0, -u.x(),
23 |             -u.y(), u.x(), 0.0;
24 |         
25 |         return h;
26 |     }
27 | 
28 |     Eigen::Matrix<double, 3, Eigen::Dynamic> structureFromTwoViews(const Eigen::Matrix<double, 3, 3> &k,
29 |                                                                    const Eigen::Matrix<double, 3, 4> &pose1,
30 |                                                                    Eigen::Ref<const Eigen::MatrixXd> u0,
31 |                                                                    Eigen::Ref<const Eigen::MatrixXd> u1)
32 |     {
33 |         eigen_assert(u0.cols() == u1.cols());
34 |         eigen_assert(u0.cols() > 0);
35 |         
36 |         
37 |         Eigen::Matrix<double, 3, Eigen::Dynamic> points(3, u0.cols());
38 |         
39 |         // Note the system of equations is modified to only have depths of u0 and the scale factor as unknowns.
40 |         
41 |         Eigen::MatrixXd m = Eigen::MatrixXd::Zero(3 * u0.cols(), u0.cols() + 1);
42 |         
43 |         auto u0h = u0.colwise().homogeneous();
44 |         auto u1h = u1.colwise().homogeneous();
45 |         
46 |         Eigen::Matrix<double, 3, 3> kinv = k.inverse();
47 |         
48 |         // Note, we need the inverse pose because of the way the linear systme is build.
49 |         // Also note that the following equations do not work on pixel coordinates but normalized image coordinates.
50 |         //
51 |         //      d1 * u1 = d0 * R * u0 + yT.
52 |         // Multiply by hat(u1) from the left, erases the first term (cross product with itself)
53 |         //      hat(u1) * d1 * u1 = d0 * hat(u1) * R * u0 + y * hat(u1) * T
54 |         //      0 = d0 * hat(u1) * R * u0 + y * hat(u1) * T
55 |         // Here y and di are the unknown depths for the first set of normalized image coordinates.
56 |         
57 |         Eigen::Matrix<double, 3, 4> poseinv = Eigen::AffineCompact3d(pose1).inverse(Eigen::Isometry).matrix();
58 |         Eigen::Matrix<double, 3, 3> r = poseinv.block<3,3>(0, 0);
59 |         Eigen::Vector3d t = poseinv.block<3,1>(0, 3);
60 |         
61 |         for (Eigen::DenseIndex i  = 0; i < u0.cols(); ++i) {
62 |             Eigen::Matrix<double, 3, 3> u1hat = hat(kinv * u1h.col(i));
63 |             
64 |             // Rotational part
65 |             m.block<3,1>(i*3, i) = u1hat * r * kinv * u0h.col(i);
66 |             
67 |             // Translational part
68 |             m.block<3,1>(i*3, m.cols()-1) = u1hat * t;
69 |         }
70 |         
71 |         Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> e;
72 |         e.compute(m.transpose() * m);
73 |         Eigen::VectorXd x = e.eigenvectors().col(0);
74 |         
75 |         for (Eigen::DenseIndex i  = 0; i < u0.cols(); ++i) {
76 |             points.col(i) = x(i) * kinv * u0h.col(i);
77 |         }
78 |         
79 |         
80 |         return points;
81 |     }
82 |     
83 | }


--------------------------------------------------------------------------------
/src/fundamental.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |  This file is part of 8point.
  3 |  
  4 |  Copyright(C) 2015/2016 Christoph Heindl
  5 |  All rights reserved.
  6 |  
  7 |  This software may be modified and distributed under the terms
  8 |  of the BSD license.See the LICENSE file for details.
  9 | */
 10 | 
 11 | #include <eight/fundamental.h>
 12 | #include <eight/normalize.h>
 13 | #include <eight/distance.h>
 14 | #include <eight/select.h>
 15 | 
 16 | #include <Eigen/Eigenvalues>
 17 | #include <Eigen/SVD>
 18 | #include <vector>
 19 | #include <random>
 20 | 
 21 | namespace eight {
 22 |     
 23 |     Eigen::Matrix3d fundamentalMatrixUnnormalized(Eigen::Ref<const Eigen::MatrixXd> a, Eigen::Ref<const Eigen::MatrixXd> b) {
 24 |         
 25 |         eigen_assert(a.cols() == b.cols());
 26 |         eigen_assert(a.rows() == b.rows());
 27 |         eigen_assert(a.cols() >= 8);
 28 |         
 29 |         // Setup system of equations Ax = 0. There will be one row in A for each correspondence.
 30 |         Eigen::Matrix<double, Eigen::Dynamic, 9> A(a.cols(), 9);
 31 |         
 32 |         for (Eigen::DenseIndex i = 0; i < a.cols(); ++i) {
 33 |             const auto &ca = a.col(i);
 34 |             const auto &cb = b.col(i);
 35 |             
 36 |             auto r = A.row(i);
 37 |             
 38 |             r(0) = cb.x() * ca.x();     // F11
 39 |             r(1) = cb.x() * ca.y();     // F21
 40 |             r(2) = cb.x();              // F31
 41 |             r(3) = cb.y() * ca.x();     // F12
 42 |             r(4) = cb.y() * ca.y();     // F22
 43 |             r(5) = cb.y();              // F32
 44 |             r(6) = ca.x();              // F13
 45 |             r(7) = ca.y();              // F23
 46 |             r(8) = 1.0;                 // F33
 47 |         }
 48 |         
 49 |         // Seek for a least squares solution such that |Ax| = 1. Given by the unit eigenvector of A'A associated with the smallest eigenvalue.
 50 |         Eigen::SelfAdjointEigenSolver< Eigen::Matrix<double, Eigen::Dynamic, 9> > e;
 51 |         e.compute((A.transpose() * A));
 52 |         eigen_assert(e.info() == Eigen::Success);
 53 |         
 54 |         Eigen::Matrix<double, 1, 9> f = e.eigenvectors().col(0); // Sorted ascending by eigenvalue.
 55 |         
 56 |         Eigen::Matrix3d F;
 57 |         F <<
 58 |         f(0), f(3), f(6),
 59 |         f(1), f(4), f(7),
 60 |         f(2), f(5), f(8);
 61 |         
 62 |         // Enforce singularity constraint such that rank(F) = 2. Which is the closest singular matrix to F under Frobenius norm.
 63 |         Eigen::JacobiSVD<Eigen::Matrix3d> svd(F, Eigen::ComputeFullU | Eigen::ComputeFullV);
 64 |         Eigen::DiagonalMatrix<double, 3> dPrime(svd.singularValues()(0), svd.singularValues()(1), 0.0);
 65 |         Eigen::Matrix3d FPrime = svd.matrixU() * dPrime * svd.matrixV().transpose();
 66 |         
 67 |         return FPrime;   
 68 | 
 69 |     }
 70 |     
 71 |     Eigen::Matrix3d fundamentalMatrix(Eigen::Ref<const Eigen::MatrixXd> a, Eigen::Ref<const Eigen::MatrixXd> b)
 72 |     {
 73 |         Eigen::Transform<double, 2, Eigen::Affine> t0 = findIsotropicNormalizingTransform(a);
 74 |         Eigen::Transform<double, 2, Eigen::Affine> t1 = findIsotropicNormalizingTransform(b);
 75 |         
 76 |         Eigen::Matrix<double, 2, Eigen::Dynamic> na = (t0.matrix() * a.colwise().homogeneous()).colwise().hnormalized();
 77 |         Eigen::Matrix<double, 2, Eigen::Dynamic> nb = (t1.matrix() * b.colwise().homogeneous()).colwise().hnormalized();
 78 |         
 79 |         Eigen::Matrix3d Fn = eight::fundamentalMatrixUnnormalized(na, nb);
 80 |         Eigen::Matrix3d F = (t0.matrix().transpose() * Fn * t1.matrix());
 81 |         return F;
 82 |     }
 83 | 
 84 |     template <class ForwardIterator, class T>
 85 |     void fillIncremental(ForwardIterator first, ForwardIterator last, T val)
 86 |     {
 87 |         while (first != last) {
 88 |             *first = val;
 89 |             ++first;
 90 |             ++val;
 91 |         }
 92 |     }
 93 | 
 94 |     std::vector<Eigen::DenseIndex> samplePointIndices(Eigen::DenseIndex setSize, Eigen::DenseIndex sampleSize) {
 95 |         std::vector<Eigen::DenseIndex> ids(setSize);
 96 |         fillIncremental(ids.begin(), ids.end(), 0);
 97 |         
 98 |         std::random_device rd;
 99 |         std::mt19937 gen(rd());
100 |         
101 |         // Fisher-Yates sampling
102 |         std::vector<Eigen::DenseIndex> result(sampleSize);
103 |         for (Eigen::DenseIndex i = 0; i < sampleSize; i++) {
104 |             std::uniform_int_distribution<Eigen::DenseIndex> dis(i, setSize - 1);
105 |             std::swap(ids[i], ids[dis(gen)]);
106 |             result[i] = ids[i];
107 |         }
108 |         
109 |         return result;
110 |     }
111 | 
112 |     Eigen::Matrix3d fundamentalMatrixRobust(Eigen::Ref<const Eigen::MatrixXd> a, Eigen::Ref<const Eigen::MatrixXd> b, std::vector<Eigen::DenseIndex> &inliers, double d, double e, double p)
113 |     {
114 |         // First perform a ransac to find a a fundamental matrix. Then re-estimate fundamental matrix from all inliers.
115 | 
116 |         int niter = static_cast<int>(std::ceil(std::log(1.0 - p) / std::log(1.0 - std::pow(1.0 - e, 8))));
117 | 
118 | 
119 |         const double d2 = d * d;
120 | 
121 |         Eigen::Matrix3d bestF = Eigen::Matrix3d::Constant(std::numeric_limits<double>::quiet_NaN());
122 |         inliers.clear();
123 | 
124 |         for (int iter = 0; iter < niter; ++iter) {
125 |             // Randomly select 8 different points
126 |             std::vector<Eigen::DenseIndex> modelIds = samplePointIndices(a.cols(), 8);
127 |             
128 |             // Build model
129 |             Eigen::Matrix<double, 2, Eigen::Dynamic> sa = selectColumnsByIndex(a, modelIds.begin(), modelIds.end());
130 |             Eigen::Matrix<double, 2, Eigen::Dynamic> sb = selectColumnsByIndex(b, modelIds.begin(), modelIds.end());
131 |             Eigen::Matrix3d F = fundamentalMatrix(sa, sb);
132 | 
133 |             // Evaluate inliers
134 |             Eigen::VectorXd dists = distances(F, a, b, SampsonDistanceSquared());            
135 |             std::vector<Eigen::DenseIndex> in;
136 |             for (Eigen::DenseIndex i = 0; i < dists.size(); ++i) {
137 |                 if (dists(i) <= d2) {
138 |                     in.push_back(i);
139 |                 }
140 |             }
141 | 
142 |             if (in.size() > inliers.size()) {
143 |                 std::swap(in, inliers);
144 |                 std::swap(F, bestF);
145 |             }
146 |         }
147 | 
148 |         if (bestF.allFinite()) {
149 |             // Re-estimate using all inliers
150 |             Eigen::Matrix<double, 2, Eigen::Dynamic> sa = selectColumnsByIndex(a, inliers.begin(), inliers.end());
151 |             Eigen::Matrix<double, 2, Eigen::Dynamic> sb = selectColumnsByIndex(b, inliers.begin(), inliers.end());
152 |             bestF = fundamentalMatrix(sa, sb);
153 |         }
154 | 
155 |         return bestF;
156 |     }
157 | }


--------------------------------------------------------------------------------
/examples/defocus.cpp:
--------------------------------------------------------------------------------
  1 | /**
  2 |     This file is part of Deformable Shape Tracking (DEST).
  3 | 
  4 |     Copyright(C) 2015/2016 Christoph Heindl
  5 |     All rights reserved.
  6 | 
  7 |     This software may be modified and distributed under the terms
  8 |     of the BSD license.See the LICENSE file for details.
  9 | */
 10 | 
 11 | #include <eight/fundamental.h>
 12 | #include <eight/normalize.h>
 13 | #include <eight/distance.h>
 14 | #include <eight/essential.h>
 15 | #include <eight/project.h>
 16 | #include <eight/select.h>
 17 | #include <eight/triangulate.h>
 18 | #include <random>
 19 | #include <iostream>
 20 | 
 21 | #include <opencv2/opencv.hpp>
 22 | #include <fstream>
 23 | #include <random>
 24 | 
 25 | #include <Eigen/Sparse>
 26 | 
 27 | #ifdef _WIN32
 28 | #define GOOGLE_GLOG_DLL_DECL
 29 | #pragma warning(push)
 30 | #pragma warning(disable : 4251 4355)
 31 | #include <ceres/ceres.h>
 32 | #include "ceres/rotation.h"
 33 | #include <glog/logging.h>
 34 | #pragma warning(pop)
 35 | #else
 36 | #include <ceres/ceres.h>
 37 | #include "ceres/rotation.h"
 38 | #include <glog/logging.h>
 39 | #endif
 40 | 
 41 | #include "../tests/utils.h"
 42 | 
 43 | 
 44 | 
 45 | class KLT {
 46 | public:
 47 | 
 48 |     KLT(const cv::Mat &img, const std::vector<cv::Point2f> &initial)
 49 |         :_next(initial)
 50 |     {
 51 |         cv::cvtColor(img, _nextGray, CV_BGR2GRAY);
 52 |         _nextStatus.resize(initial.size(), 255);
 53 |     }
 54 | 
 55 |     void update(cv::Mat &img) {        
 56 |         
 57 |         std::swap(_prev, _next);
 58 |         std::swap(_prevGray, _nextGray);
 59 |         std::swap(_prevStatus, _nextStatus);
 60 | 
 61 |         cv::cvtColor(img, _nextGray, CV_BGR2GRAY);
 62 |         cv::TermCriteria term(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, 50, 0.001);
 63 |         cv::calcOpticalFlowPyrLK(_prevGray, _nextGray, _prev, _next, _nextStatus, _err, cv::Size(21, 21), 5, term);
 64 | 
 65 |         for (size_t i = 0; i < _nextStatus.size(); ++i) {
 66 |             _nextStatus[i] &= _prevStatus[i];
 67 |             _nextStatus[i] &= (_err[i] < 5);
 68 |         }
 69 |     }
 70 | 
 71 |     std::vector<cv::Point2f> &location() {
 72 |         return _next;
 73 |     }
 74 | 
 75 |     std::vector<uchar> &status() {
 76 |         return _nextStatus;
 77 |     }
 78 | 
 79 | 
 80 | private:
 81 |     std::vector<cv::Point2f> _prev, _next;
 82 |     cv::Mat _prevGray, _nextGray;
 83 |     std::vector<uchar> _prevStatus, _nextStatus;
 84 |     std::vector<float> _err;
 85 | };
 86 | 
 87 | Eigen::MatrixXd toEight(const std::vector<cv::Point2f> &x) {
 88 |     Eigen::MatrixXd m(2, x.size());
 89 | 
 90 |     for (size_t i = 0; i < x.size(); ++i) {
 91 |         m(0, i) = x[i].x;
 92 |         m(1, i) = x[i].y;
 93 |     }
 94 | 
 95 |     return m;
 96 | }
 97 | 
 98 | Eigen::MatrixXd imagePointsToRetinaPoints(const std::vector<cv::Point2f> &p, const Eigen::Matrix3d &kinv)
 99 | {
100 |     Eigen::MatrixXd r(3, p.size());
101 | 
102 |     for (size_t i = 0; i < p.size(); ++i) {
103 |         Eigen::Vector3d x(p[i].x, p[i].y, 1.0);
104 |         r.col(i) = kinv * x;
105 |     }
106 | 
107 |     return r;
108 | }
109 | 
110 | void findFeaturesInReference(cv::Mat &gray, std::vector<cv::Point2f> &corners) {
111 |     double qualityLevel = 0.02;
112 |     double minDistance = 10;
113 |     int blockSize = 5;
114 |     bool useHarrisDetector = false;
115 |     double k = 0.04;
116 |     int maxCorners = 2000;
117 | 
118 |     cv::goodFeaturesToTrack(gray,
119 |         corners,
120 |         maxCorners,
121 |         qualityLevel,
122 |         minDistance,
123 |         cv::Mat(),
124 |         blockSize,
125 |         useHarrisDetector,
126 |         k);
127 |     
128 |     cv::TermCriteria termcrit(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,20,0.03);
129 |     cv::cornerSubPix(gray, corners, cv::Size(10,10), cv::Size(-1,-1), termcrit);
130 | }
131 | 
132 | void trackFeatures(cv::Mat ref, const std::vector<cv::Point2f> &refLocs, cv::Mat target, std::vector<cv::Point2f> &targetLocs, std::vector<uchar> &status)
133 | {
134 |     KLT klt(ref, refLocs);
135 |     klt.update(target);
136 |     targetLocs = klt.location();
137 |     status = klt.status();
138 | }
139 | 
140 | struct GeneralReprojectionError {
141 |     GeneralReprojectionError(const double *no, const double *np)
142 |         : _no(no), _np(np)
143 |     {}
144 | 
145 |     template <typename T>
146 |     bool operator()(
147 |         const T* const camera,
148 |         const T* const depth,
149 |         T* residuals) const 
150 |     {
151 |         T p[3] = {T(_np[0]) / depth[0], T(_np[1]) / depth[0], T(1.0) / depth[0] };
152 | 
153 |         T rpt[3] = {
154 |             p[0] - camera[2] * p[1] + camera[1] * p[2] + camera[3],
155 |             p[0] * camera[2] + p[1] - camera[0] * p[2] + camera[4],
156 |             -p[0] * camera[1] + p[1] * camera[0] + p[2] + camera[5]
157 |         };
158 |         
159 |         residuals[0] = T(_no[0]) - rpt[0] / rpt[2] ;
160 |         residuals[1] = T(_no[1]) - rpt[1] / rpt[2] ;
161 | 
162 |         return true;
163 |     }
164 | 
165 |     static ceres::CostFunction* Create(const double *no, const double *np)
166 |     {
167 |         return (new ceres::AutoDiffCostFunction<GeneralReprojectionError, 2, 6, 1>(
168 |             new GeneralReprojectionError(no, np)));
169 |     }
170 | 
171 |     const double *_no;
172 |     const double *_np;
173 | };
174 | 
175 | void normalizePerspective(Eigen::Matrix<double, 3, Eigen::Dynamic> &x) {
176 |     for (Eigen::DenseIndex i = 0; i < x.cols(); ++i) {
177 |         x.col(i) /= x.col(i).z();
178 |     }
179 | }
180 | 
181 | void writePly(const char *path, const Eigen::MatrixXd &points, const Eigen::MatrixXd &colors, const std::vector<uchar> &status) {
182 |     std::ofstream ofs(path);
183 |     
184 |     size_t valid = std::count(status.begin(), status.end(), 1);
185 |     
186 |     ofs
187 |         << "ply" << std::endl
188 |         << "format ascii 1.0" << std::endl
189 |         << "element vertex " << valid << std::endl
190 |         << "property float x" << std::endl
191 |         << "property float y" << std::endl
192 |         << "property float z" << std::endl
193 |         << "property uchar red" << std::endl
194 |         << "property uchar green" << std::endl
195 |         << "property uchar blue" << std::endl
196 |         << "end_header" << std::endl;
197 |     
198 |     
199 |     for (Eigen::DenseIndex i = 0; i < points.cols(); ++i) {
200 |         if (!status[i])
201 |             continue;
202 |         
203 |         Eigen::Vector3d x = points.col(i);
204 |         Eigen::Vector3d c = colors.col(i);
205 |         
206 |         ofs << x(0) << " " << x(1) << " " << x(2) << " " << (int)c(0) << " " << (int)c(1) << " " << (int)c(2) << std::endl;
207 |     }
208 |     
209 |     ofs.close();
210 | }
211 | 
212 | void dense(cv::Mat &depths, cv::Mat &colors) {
213 |     // Variational with energy: (dout - dsparse)^2 + lambda * |nabla(dout)|^2
214 |     // First term is only defined for sparse pixel positions.
215 |     // leads to linear system of equations: Ax = b with one row per pixel: 
216 |     //  dout(x,y) + lambda * laplacian(dout(x,y)) = dsparse(x,y)
217 |     //  dout(x,y) + lambda * (-4*dout(x,y) + dout(x,y-1) + dout(x+1,y) + dout(x,y+1) + dout(x-1,y)) = dsparse(x,y)
218 | 
219 | 
220 |     typedef Eigen::Triplet<double> T;
221 |     std::vector<T> triplets;
222 | 
223 |     std::cout << __LINE__ << std::endl;
224 | 
225 |     int rows = depths.rows;
226 |     int cols = depths.cols;
227 | 
228 |     Eigen::MatrixXd rhs(rows * cols, 1);
229 |     rhs.setZero();
230 | 
231 |     std::cout << __LINE__ << std::endl;
232 | 
233 |     const double lambda = 0.1;
234 | 
235 |     int idx = 0;
236 |     for (int y = 0; y < depths.rows; ++y) {
237 |         for (int x = 0; x < depths.cols; ++x, ++idx) {
238 | 
239 |             double d = depths.at<double>(y, x);
240 |             
241 |             double c = 0.0;
242 | 
243 |             if (d > 0.0) {
244 |                 rhs(idx, 0) = d;
245 |                 c += 1.0;
246 |             }
247 |             
248 | 
249 |             if (y > 0) {
250 |                 // North neighbor 
251 |                 c -= lambda;
252 |                 triplets.push_back(T(idx, (y - 1)*cols + x, lambda));
253 |             }
254 | 
255 |             if (x > 0) {
256 |                 // West neighbor 
257 |                 c -= lambda;
258 |                 triplets.push_back(T(idx, (y)*cols + (x-1), lambda));
259 |             }
260 | 
261 |             if (y < (rows - 1)) {
262 |                 // South neighbor 
263 |                 c -= lambda;
264 |                 triplets.push_back(T(idx, (y+1)*cols + x, lambda));
265 |             }
266 | 
267 |             if (x < (cols - 1)) {
268 |                 // East neighbor 
269 |                 c -= lambda;
270 |                 triplets.push_back(T(idx, (y)*cols + (x+1), lambda));
271 |             }
272 | 
273 |             // Center
274 |             triplets.push_back(T(idx, idx, c));
275 |         }
276 |     }
277 | 
278 |     std::cout << __LINE__ << std::endl;
279 | 
280 |     Eigen::SparseMatrix<double> A(rows*cols, rows*cols);
281 |     A.setFromTriplets(triplets.begin(), triplets.end());
282 | 
283 |     std::cout << __LINE__ << std::endl;
284 | 
285 |     Eigen::SparseLU< Eigen::SparseMatrix<double> > solver;
286 |     solver.analyzePattern(A);
287 |     solver.factorize(A);
288 | 
289 |     Eigen::MatrixXd result(rows*cols, 1);
290 |     result = solver.solve(rhs);
291 | 
292 |     idx = 0;
293 |     for (int y = 0; y < depths.rows; ++y) {
294 |         for (int x = 0; x < depths.cols; ++x, ++idx) {
295 |             depths.at<double>(y, x) = result(idx, 0);
296 |         }
297 |     }
298 | 
299 | }
300 | 
301 | int main(int argc, char **argv) {
302 | 
303 |     google::InitGoogleLogging(argv[0]);
304 | 
305 |     if (argc != 2) {
306 |         std::cerr << argv[0] << " videofile" << std::endl;
307 |         return -1;
308 |     }
309 | 
310 |     cv::VideoCapture vc;
311 |     if (!vc.open(argv[1])) {
312 |         std::cerr << "Failed to open video" << std::endl;
313 |         return -1;
314 |     }
315 | 
316 |     // Taken from http://yf.io/p/tiny/
317 |     // Use 8point_defocus.exe "stream\stone6_still_%04d.png"
318 |     
319 |     Eigen::Matrix3d k;
320 |     k <<
321 |         1781.0, 0.0, 960.0,
322 |         0.0, 1781.0, 540.0,
323 |         0.0, 0.0, 1.0;
324 | 
325 |     Eigen::Matrix3d invk = k.inverse();
326 | 
327 | 
328 |     // Detect trackable features in reference frame
329 |     cv::Mat ref, refGray;
330 |     vc >> ref;
331 |     cv::cvtColor(ref, refGray, CV_BGR2GRAY);
332 | 
333 |     std::vector< Eigen::MatrixXd > retinapoints;
334 |     std::vector<cv::Point2f> corners;
335 |     
336 |     findFeaturesInReference(refGray, corners);
337 |     std::vector<uchar> status(corners.size(), 1);
338 |     
339 |     retinapoints.push_back(imagePointsToRetinaPoints(corners, invk));
340 | 
341 | 
342 |     cv::Mat f;
343 |     while (vc.grab()) {
344 |         vc.retrieve(f);
345 | 
346 |         std::vector<cv::Point2f> loc;
347 |         std::vector<uchar> s;
348 | 
349 |         trackFeatures(ref, corners, f, loc, s);
350 |         
351 |         retinapoints.push_back(imagePointsToRetinaPoints(loc, invk));
352 |         for (size_t i = 0; i < s.size(); ++i) {
353 |             status[i] &= s[i];
354 | 
355 |             if (status[i]) {
356 |                 cv::circle(f, loc[i], 2, cv::Scalar(0, 255, 0));
357 |             }
358 |         }
359 |         
360 |         cv::Mat tmp;
361 |         cv::resize(f, tmp, cv::Size(), 0.5, 0.5);
362 |         cv::imshow("track", tmp);
363 |         cv::waitKey(10);
364 |     }
365 |     
366 |     retinapoints[0].row(2).setRandom(); // Initial inverse depth
367 |     retinapoints[0].row(2).array() += 1.0;
368 | 
369 |     // Setup camera parameters
370 |     Eigen::MatrixXd camparams(6, retinapoints.size());
371 |     camparams.setZero();
372 | 
373 |     // Setup nnls system
374 |     ceres::Problem problem;
375 | 
376 |     // For each camera
377 |     for (size_t f = 1; f < retinapoints.size(); ++f) {
378 |         // For each point observed
379 |         for (Eigen::DenseIndex p = 0; p < retinapoints[f].cols(); ++p) {
380 |             if (!status[p])
381 |                 continue;
382 | 
383 |             ceres::CostFunction* cost_function = GeneralReprojectionError::Create(
384 |                 retinapoints[f].col(p).data(),
385 |                 retinapoints[0].col(p).data()
386 |             );
387 |             
388 | 
389 |             problem.AddResidualBlock(
390 |                 cost_function,
391 |                 NULL /* squared loss */,
392 |                 camparams.col(f).data(),
393 |                 retinapoints[0].col(p).data() + 2);
394 |         }
395 |     }
396 | 
397 | 
398 |     ceres::Solver::Options options;
399 |     options.linear_solver_type = ceres::DENSE_SCHUR;
400 |     options.max_num_iterations = 400;
401 |     options.minimizer_progress_to_stdout = true;
402 | 
403 |     ceres::Solver::Summary summary;
404 |     ceres::Solve(options, &problem, &summary);
405 |     std::cout << summary.FullReport() << "\n";
406 |     
407 |     cv::Mat depths(ref.size(), CV_64FC1);
408 |     depths.setTo(0);
409 | 
410 |     Eigen::MatrixXd colors(3, retinapoints[0].cols());
411 |     Eigen::MatrixXd points(3, retinapoints[0].cols());
412 |     Eigen::DenseIndex i = 0;
413 | 
414 |     for (Eigen::DenseIndex i = 0; i < retinapoints[0].cols(); ++i) {           
415 |         if (!status[i])
416 |             continue;
417 | 
418 |         Eigen::Vector3d q = retinapoints[0].col(i);
419 |         q.x() /= q.z();
420 |         q.y() /= q.z();
421 |         q.z() = 1.0 / q.z();
422 | 
423 |         points.col(i) = q;
424 | 
425 |         cv::Vec3b c = ref.at<cv::Vec3b>(corners[i]);
426 |         colors(0, i) = c(2);
427 |         colors(1, i) = c(1);
428 |         colors(2, i) = c(0);
429 | 
430 |             
431 |         depths.at<double>(corners[i]) = q.z();
432 |     }
433 |     writePly("points.ply", points, colors, status);
434 | 
435 |     dense(depths, ref);    
436 | 
437 |     double minv, maxv;
438 |     cv::minMaxLoc(depths, &minv, &maxv);
439 | 
440 |     cv::Mat tmp;
441 |     depths.convertTo(tmp, CV_8U, 255.0 / (maxv - minv), -minv * 255.0 / (maxv - minv));
442 |     cv::resize(tmp, tmp, cv::Size(), 0.5, 0.5);
443 |     cv::imshow("dense", tmp);
444 |     cv::waitKey();
445 | 
446 | 
447 |     //cv::FileStorage file("sparse.yml", cv::FileStorage::WRITE);
448 |     //file << ref;
449 |     //file << depths;
450 | 
451 | }
452 | 


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