├── README.md
├── include
    └── code_utils
    │   ├── cv_utils
    │       ├── pnp
    │       │   ├── utils.h
    │       │   ├── linearpnp.h
    │       │   ├── pnp.h
    │       │   └── nonlinearpnp.h
    │       ├── dlt
    │       │   └── dlt.h
    │       ├── cv_type.hpp
    │       ├── randomcolor.h
    │       └── scalartodata.h
    │   ├── sys_utils
    │       ├── float_equal.hpp
    │       ├── printcolor.hpp
    │       ├── timeinseconds.hpp
    │       ├── eigen_file_io.hpp
    │       ├── tic_toc.h
    │       └── cvmat_file_io.hpp
    │   ├── cv_utils.h
    │   ├── math_utils
    │       ├── acos_fast.h
    │       ├── Polynomial.h
    │       └── math_utils.h
    │   ├── eigen_utils.h
    │   ├── ros_utils.h
    │   └── backward.hpp
├── CMakeLists.txt
├── package.xml
└── src
    ├── mat_io_test.cpp
    ├── cv_utils.cc
    ├── sumpixel_test.cpp
    ├── cv_utils
        ├── pnp
        │   ├── pnp.cpp
        │   ├── nonlinearpnp.cpp
        │   └── linearpnp.cpp
        └── dlt
        │   └── dlt.cpp
    ├── poly_test.cpp
    └── math_utils
        └── Polynomial.cpp


/README.md:
--------------------------------------------------------------------------------
1 | # code_utils
2 | my code utils
3 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils/pnp/utils.h:
--------------------------------------------------------------------------------
 1 | #ifndef UTILS_H
 2 | #define UTILS_H
 3 | 
 4 | namespace math_utils
 5 | {
 6 | 
 7 | template< typename T >
 8 | inline T
 9 | max_in_three( T a, T b, T c )
10 | {
11 |     return a >= b ? ( a >= c ? a : c ) : ( b >= c ? b : c );
12 | }
13 | 
14 | template< typename T >
15 | inline T
16 | max_index_in_three( T a, T b, T c )
17 | {
18 |     return a >= b ? ( a >= c ? T( 0 ) : T( 2 ) ) : ( b >= c ? T( 1 ) : T( 2 ) );
19 | }
20 | }
21 | #endif // UTILS_H
22 | 


--------------------------------------------------------------------------------
/include/code_utils/sys_utils/float_equal.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef FLOAT_EQUAL_HPP
 2 | #define FLOAT_EQUAL_HPP
 3 | 
 4 | #include <cmath>
 5 | #include <iostream>
 6 | 
 7 | namespace sys_utils
 8 | {
 9 | 
10 | namespace equal
11 | {
12 | 
13 | inline bool
14 | float_equal( const float a, const float b )
15 | {
16 |     if ( std::abs( a - b ) < 1e-6 )
17 |         return true;
18 |     else
19 |         return false;
20 | }
21 | 
22 | inline bool
23 | double_equal( const double a, const double b )
24 | {
25 |     if ( std::abs( a - b ) < 1e-6 )
26 |         return true;
27 |     else
28 |         return false;
29 | }
30 | }
31 | }
32 | #endif // FLOAT_EQUAL_HPP
33 | 


--------------------------------------------------------------------------------
/include/code_utils/sys_utils/printcolor.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef PRINTCOLOR_HPP
 2 | #define PRINTCOLOR_HPP
 3 | 
 4 | #include <iostream>
 5 | 
 6 | namespace sys_utils
 7 | {
 8 | 
 9 | namespace print_color
10 | {
11 | 
12 | inline void
13 | PrintWarning( std::string str )
14 | {
15 |     std::cout << "\033[33;40;1m" << str << "\033[0m" << std::endl;
16 | }
17 | 
18 | inline void
19 | PrintError( std::string str )
20 | {
21 |     std::cout << "\033[31;47;1m" << str << "\033[0m" << std::endl;
22 | }
23 | 
24 | inline void
25 | PrintInfo( std::string str )
26 | {
27 |     std::cout << "\033[32;40;1m" << str << "\033[0m" << std::endl;
28 | }
29 | }
30 | }
31 | #endif // PRINTCOLOR_HPP
32 | 


--------------------------------------------------------------------------------
/include/code_utils/sys_utils/timeinseconds.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef TIMEINSECONDS_HPP
 2 | #define TIMEINSECONDS_HPP
 3 | 
 4 | #include <iostream>
 5 | #include <time.h>
 6 | 
 7 | namespace sys_utils
 8 | {
 9 | 
10 | inline unsigned long long
11 | timeInMicroseconds( void )
12 | {
13 |     struct timespec tp;
14 | 
15 |     clock_gettime( CLOCK_REALTIME, &tp );
16 | 
17 |     return ( tp.tv_sec * 1000000 + tp.tv_nsec / 1000 );
18 | }
19 | 
20 | inline double
21 | timeInSeconds( void )
22 | {
23 |     struct timespec tp;
24 | 
25 |     clock_gettime( CLOCK_REALTIME, &tp );
26 | 
27 |     return ( static_cast< double >( tp.tv_sec ) + static_cast< double >( tp.tv_nsec ) / 1000000000.0 );
28 | }
29 | }
30 | #endif // TIMEINSECONDS_HPP
31 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils.h:
--------------------------------------------------------------------------------
 1 | #ifndef CV_UTLIS_H
 2 | #define CV_UTLIS_H
 3 | 
 4 | #include <opencv2/opencv.hpp>
 5 | 
 6 | namespace cv_utils
 7 | {
 8 | namespace fisheye
 9 | {
10 | class PreProcess
11 | {
12 |     public:
13 |     PreProcess( );
14 |     PreProcess( const cv::Size _raw_image_size,
15 |                 const cv::Size _roi_size,
16 |                 const cv::Point _center,
17 |                 const float _resize_scale );
18 |     void resetPreProcess( cv::Size _roi_size, cv::Point _center, float _resize_scale );
19 | 
20 |     cv::Mat do_preprocess( cv::Mat image_input );
21 |     cv::Point2f preprocessPoint( const cv::Point2f& pt_in );
22 | 
23 |     float resize_scale;
24 |     int roi_row_start;
25 |     int roi_col_start;
26 |     int roi_row_end;
27 |     int roi_col_end;
28 | 
29 |     bool is_preprocess;
30 | };
31 | }
32 | }
33 | #endif // CV_UTLIS_H
34 | 


--------------------------------------------------------------------------------
/include/code_utils/math_utils/acos_fast.h:
--------------------------------------------------------------------------------
 1 | #ifndef ACOS_FAST_H
 2 | #define ACOS_FAST_H
 3 | 
 4 | template< typename T >
 5 | T
 6 | acosFaster_acc( const T x ) const
 7 | {
 8 |     T a = 1.43 + 0.59 * x;
 9 |     a   = ( a + ( 2 + 2 * x ) / a ) / 2;
10 |     T b = 1.65 - 1.41 * x;
11 |     b   = ( b + ( 2 - 2 * x ) / b ) / 2;
12 |     T c = 0.88 - 0.77 * x;
13 |     c   = ( c + ( 2 - a ) / c ) / 2;
14 |     return ( 8 * ( c + ( 2 - a ) / c ) - ( b + ( 2 - 2 * x ) / b ) ) / 6;
15 | }
16 | 
17 | template< typename T >
18 | T
19 | acosFaster_sqrt( const T x ) const
20 | {
21 |     T a = sqrt( T( 2 ) + T( 2 ) * x );
22 |     T b = sqrt( T( 2 ) - T( 2 ) * x );
23 |     T c = sqrt( T( 2 ) - a );
24 |     return T( 8 ) / T( 3 ) * c - b / T( 3 );
25 | }
26 | 
27 | template< typename T >
28 | T
29 | acosFaster_linear( const T x ) const
30 | {
31 |     return ( T( 3.14159 ) - T( 1.57079 ) * x );
32 | }
33 | 
34 | #endif // ACOS_FAST_H
35 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils/pnp/linearpnp.h:
--------------------------------------------------------------------------------
 1 | #ifndef Homography_H
 2 | #define Homography_H
 3 | 
 4 | #include "utils.h"
 5 | #include <eigen3/Eigen/Core>
 6 | #include <eigen3/Eigen/Eigen>
 7 | #include <eigen3/Eigen/SVD>
 8 | #include <iostream>
 9 | #include <opencv2/opencv.hpp>
10 | 
11 | namespace cv
12 | {
13 | class Homography
14 | {
15 |     public:
16 |     Homography( const std::vector< Eigen::Vector3d >& pts_2, const std::vector< Eigen::Vector3d >& pts_3 );
17 |     //    ~Homography( ) {}
18 | 
19 |     public:
20 |     Eigen::Matrix3d getR( ) const { return R; }
21 |     Eigen::Vector3d getT( ) const { return T; }
22 | 
23 |     private:
24 |     void readPointsPlanar( const std::vector< Eigen::Vector3d >& pts_2,
25 |                            const std::vector< Eigen::Vector3d >& pts_3 );
26 |     void solvePnP( );
27 | 
28 |     private:
29 |     Eigen::Matrix3d R;
30 |     Eigen::Vector3d T;
31 | 
32 |     Eigen::MatrixXd M;
33 |     Eigen::MatrixXd mat_tmp;
34 | 
35 |     public:
36 |     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 | };
38 | }
39 | 
40 | #endif // Homography_H
41 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils/pnp/pnp.h:
--------------------------------------------------------------------------------
 1 | #ifndef INITPNP_H
 2 | #define INITPNP_H
 3 | 
 4 | #include "../dlt/dlt.h"
 5 | #include "linearpnp.h"
 6 | #include "nonlinearpnp.h"
 7 | 
 8 | namespace cv
 9 | {
10 | class Pnp
11 | {
12 |     public:
13 |     Pnp( const std::vector< Eigen::Vector3d >& image_point, //
14 |          const std::vector< Eigen::Vector3d >& scene_point );
15 | 
16 |     Pnp( const std::vector< Eigen::Vector3d >& image_point,
17 |          const std::vector< Eigen::Vector3d >& scene_point,
18 |          Eigen::Quaterniond& q_dst,
19 |          Eigen::Vector3d& T_dst );
20 | 
21 |     Pnp( const std::vector< Eigen::Vector3d >& image_point,
22 |          const std::vector< Eigen::Vector3d >& scene_point,
23 |          const Eigen::Quaterniond& q_init,
24 |          const Eigen::Vector3d& T_init,
25 |          Eigen::Quaterniond& q_dst,
26 |          Eigen::Vector3d& T_dst );
27 | 
28 |     bool getRT( Eigen::Quaterniond& q_dst, Eigen::Vector3d& T_dst );
29 | 
30 |     private:
31 |     bool solved;
32 |     NonlinearPnP* npnp;
33 | };
34 | }
35 | #endif // INITPNP_H
36 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils/dlt/dlt.h:
--------------------------------------------------------------------------------
 1 | #ifndef DLT_H
 2 | #define DLT_H
 3 | 
 4 | #include <eigen3/Eigen/Core>
 5 | #include <eigen3/Eigen/Eigen>
 6 | #include <eigen3/Eigen/SVD>
 7 | #include <iostream>
 8 | 
 9 | namespace cv
10 | {
11 | 
12 | class DLT
13 | {
14 |     public:
15 |     DLT( const std::vector< Eigen::Vector3d >& pts_2, const std::vector< Eigen::Vector3d >& pts_3 );
16 | 
17 |     public:
18 |     Eigen::Matrix3d getR( ) const { return R; }
19 |     Eigen::Vector3d getT( ) const { return T; }
20 |     bool solved( ) const { return m_solve_ok; }
21 | 
22 |     private:
23 |     void readPointsPlanar( const std::vector< Eigen::Vector3d >& pts_2,
24 |                            const std::vector< Eigen::Vector3d >& pts_3 );
25 |     void solveDLT( );
26 | 
27 |     private:
28 |     Eigen::Matrix3d R;
29 |     Eigen::Matrix3d RR;
30 |     Eigen::Vector3d T;
31 | 
32 |     Eigen::MatrixXd M;
33 |     Eigen::MatrixXd mat_tmp;
34 |     int m_num_points;
35 |     bool m_solve_ok;
36 | 
37 |     public:
38 |     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 | };
40 | }
41 | #endif // DLT_H
42 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils/cv_type.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef CV_TYPE_HPP
 2 | #define CV_TYPE_HPP
 3 | 
 4 | #include <opencv2/core.hpp>
 5 | 
 6 | namespace cv
 7 | {
 8 | 
 9 | #define CV_8UC12 CV_MAKETYPE( CV_8U, 12 )
10 | 
11 | #define CV_8SC12 CV_MAKETYPE( CV_8S, 12 )
12 | #define CV_8SC16 CV_MAKETYPE( CV_8S, 16 )
13 | #define CV_8SC24 CV_MAKETYPE( CV_8S, 24 )
14 | #define CV_8SC32 CV_MAKETYPE( CV_8S, 32 )
15 | 
16 | #define CV_16SC12 CV_MAKETYPE( CV_8S, 12 )
17 | 
18 | #define CV_32SC12 CV_MAKETYPE( CV_32S, 12 )
19 | #define CV_32SC24 CV_MAKETYPE( CV_32S, 24 )
20 | 
21 | #define CV_32FC5 CV_MAKETYPE( CV_32F, 5 )
22 | 
23 | typedef Vec< uchar, 12 > Vec12b;
24 | 
25 | typedef Vec< short, 12 > Vec12s;
26 | typedef Vec< short, 16 > Vec16s;
27 | typedef Vec< short, 24 > Vec24s;
28 | typedef Vec< short, 32 > Vec32s;
29 | 
30 | typedef Vec< char, 3 > Vec3c;
31 | typedef Vec< char, 4 > Vec4c;
32 | 
33 | typedef Vec< char, 12 > Vec12c;
34 | typedef Vec< char, 16 > Vec16c;
35 | typedef Vec< char, 24 > Vec24c;
36 | typedef Vec< char, 32 > Vec32c;
37 | 
38 | typedef Vec< int, 12 > Vec12i;
39 | typedef Vec< int, 16 > Vec16i;
40 | typedef Vec< int, 24 > Vec24i;
41 | typedef Vec< int, 32 > Vec32i;
42 | 
43 | typedef Vec< float, 5 > Vec5f;
44 | }
45 | 
46 | #endif // CV_TYPE_HPP
47 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils/randomcolor.h:
--------------------------------------------------------------------------------
 1 | #ifndef RANDOMCOLOR_H
 2 | #define RANDOMCOLOR_H
 3 | 
 4 | #include <opencv2/core/core.hpp>
 5 | 
 6 | namespace cv
 7 | {
 8 | 
 9 | class RandomColor3
10 | {
11 |     public:
12 |     RandomColor3( ) { randColor( ); }
13 | 
14 |     void randColor( )
15 |     {
16 |         color0 = rand( ) % 256;
17 |         color1 = rand( ) % 256;
18 |         color2 = rand( ) % 256;
19 |     }
20 |     cv::Scalar getColor( ) { return cv::Scalar( color0, color1, color2 ); }
21 |     cv::Scalar getrandColor( )
22 |     {
23 |         randColor( );
24 |         return cv::Scalar( color0, color1, color2 );
25 |     }
26 |     cv::Vec3b getColorVec( )
27 |     {
28 |         return cv::Vec3b( ( uchar )color0, ( uchar )color1, ( uchar )color2 );
29 |     }
30 | 
31 |     private:
32 |     int color0;
33 |     int color1;
34 |     int color2;
35 | };
36 | 
37 | class RandomColor1
38 | {
39 |     public:
40 |     RandomColor1( ) { randColor( ); }
41 | 
42 |     void randColor( ) { color0 = rand( ) % 256; }
43 |     cv::Scalar getColor( ) { return cv::Scalar( color0 ); }
44 |     cv::Scalar getrandColor( )
45 |     {
46 |         randColor( );
47 |         return cv::Scalar( color0 );
48 |     }
49 | 
50 |     private:
51 |     int color0;
52 | };
53 | }
54 | 
55 | #endif // RANDOMCOLOR_H
56 | 


--------------------------------------------------------------------------------
/include/code_utils/sys_utils/eigen_file_io.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef EIGEN_FILE_IO_HPP
 2 | #define EIGEN_FILE_IO_HPP
 3 | 
 4 | #include <eigen3/Eigen/Eigen>
 5 | #include <fstream>
 6 | #include <iomanip>
 7 | #include <iostream>
 8 | 
 9 | namespace sys_utils
10 | {
11 | 
12 | namespace io
13 | {
14 | 
15 | template< class Matrix >
16 | void
17 | writeMatrixToBinary( const std::string filename, const Matrix& matrix )
18 | {
19 |     std::ofstream out( filename, std::ios::out | std::ios::binary | std::ios::trunc );
20 |     typename Matrix::Index rows = matrix.rows( ), cols = matrix.cols( );
21 |     out.write( ( char* )( &rows ), sizeof( typename Matrix::Index ) );
22 |     out.write( ( char* )( &cols ), sizeof( typename Matrix::Index ) );
23 |     out.write( ( char* )matrix.data( ), rows * cols * sizeof( typename Matrix::Scalar ) );
24 |     out.close( );
25 | }
26 | 
27 | template< class Matrix >
28 | void
29 | parseMatrixFromBinary( const std::string filename, Matrix& matrix )
30 | {
31 |     std::ifstream in( filename, std::ios::in | std::ios::binary );
32 |     while ( in.peek( ) != EOF )
33 |     {
34 |         typename Matrix::Index rows = 0, cols = 0;
35 |         in.read( ( char* )( &rows ), sizeof( typename Matrix::Index ) );
36 |         in.read( ( char* )( &cols ), sizeof( typename Matrix::Index ) );
37 |         matrix.resize( rows, cols );
38 |         in.read( ( char* )matrix.data( ), rows * cols * sizeof( typename Matrix::Scalar ) );
39 |     }
40 |     in.close( );
41 | }
42 | }
43 | }
44 | 
45 | #endif // EIGEN_FILE_IO_HPP
46 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(code_utils)
 3 | 
 4 | set(CMAKE_BUILD_TYPE "Release")
 5 | set(CMAKE_CXX_FLAGS "-std=c++11")
 6 | set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g -fPIC -fopenmp")
 7 | 
 8 | find_package(catkin REQUIRED
 9 |     roscpp
10 |     std_msgs
11 |     )
12 | 
13 | #set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3")
14 | find_package(Eigen3 REQUIRED)
15 | find_package(OpenCV REQUIRED)
16 | find_package(Ceres REQUIRED)
17 | 
18 | catkin_package(
19 |     INCLUDE_DIRS include
20 |     LIBRARIES polynomial cv_utils pnp
21 |     CATKIN_DEPENDS roscpp std_msgs
22 | #    DEPENDS system_lib
23 |     )
24 | 
25 | include_directories(
26 |     ${catkin_INCLUDE_DIRS}
27 |     ${EIGEN3_INCLUDE_DIR}
28 |      )
29 | include_directories("include")
30 | 
31 | add_library(polynomial STATIC
32 |     src/math_utils/Polynomial.cpp)
33 | target_link_libraries(polynomial ${Boost_LIBRARIES} )
34 | 
35 | add_library(pnp
36 |     src/cv_utils/dlt/dlt.cpp
37 |     src/cv_utils/pnp/pnp.cpp
38 |     src/cv_utils/pnp/linearpnp.cpp
39 |     src/cv_utils/pnp/nonlinearpnp.cpp)
40 | target_link_libraries(pnp ${Boost_LIBRARIES}  ${OpenCV_LIBS}  ${CERES_LIBRARIES})
41 | 
42 | add_library(cv_utils STATIC
43 |     src/cv_utils.cc
44 |     )
45 | target_link_libraries(cv_utils ${Boost_LIBRARIES}  ${OpenCV_LIBS} )
46 | 
47 | add_executable(matIO_test   src/mat_io_test.cpp )
48 | target_link_libraries(matIO_test dw ${OpenCV_LIBS})
49 | 
50 | add_executable(sumpixel_test   src/sumpixel_test.cpp )
51 | target_link_libraries(sumpixel_test dw ${OpenCV_LIBS})
52 | 


--------------------------------------------------------------------------------
/include/code_utils/sys_utils/tic_toc.h:
--------------------------------------------------------------------------------
 1 | #ifndef TicToc_H
 2 | #define TicToc_H
 3 | 
 4 | #include <chrono>
 5 | #include <cstdlib>
 6 | #include <ctime>
 7 | 
 8 | namespace sys_utils
 9 | {
10 | 
11 | namespace tic
12 | {
13 | 
14 | class TicToc
15 | {
16 |     public:
17 |     TicToc( ) { tic( ); }
18 | 
19 |     void tic( ) { start = std::chrono::system_clock::now( ); }
20 | 
21 |     double toc( )
22 |     {
23 |         end = std::chrono::system_clock::now( );
24 | 
25 |         std::chrono::duration< double > elapsed_seconds = end - start;
26 |         return elapsed_seconds.count( ) * 1000;
27 |     }
28 | 
29 |     private:
30 |     std::chrono::time_point< std::chrono::system_clock > start, end;
31 | };
32 | 
33 | class TicTocPart
34 | {
35 |     public:
36 |     TicTocPart( ) { tic( ); }
37 | 
38 |     void tic( )
39 |     {
40 |         start = std::chrono::system_clock::now( );
41 |         tmp   = start;
42 |     }
43 | 
44 |     double toc( )
45 |     {
46 |         std::chrono::time_point< std::chrono::system_clock > now = std::chrono::system_clock::now( );
47 |         std::chrono::duration< double > elapsed_seconds          = now - tmp;
48 |         tmp                                                      = now;
49 | 
50 |         return elapsed_seconds.count( ) * 1000;
51 |     }
52 | 
53 |     double tocEnd( )
54 |     {
55 |         end = std::chrono::system_clock::now( );
56 | 
57 |         std::chrono::duration< double > elapsed_seconds = end - start;
58 |         return elapsed_seconds.count( ) * 1000;
59 |     }
60 | 
61 |     private:
62 |     std::chrono::time_point< std::chrono::system_clock > start, end, tmp;
63 | };
64 | }
65 | }
66 | #endif // TicToc_H
67 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils/scalartodata.h:
--------------------------------------------------------------------------------
 1 | #ifndef SCALARTODATA_H
 2 | #define SCALARTODATA_H
 3 | 
 4 | #include <opencv2/core.hpp>
 5 | 
 6 | namespace cv_utils
 7 | {
 8 | template< typename T >
 9 | static inline void
10 | scalarToRawData_( const cv::Scalar& s, T* const buf, const int cn, const int unroll_to )
11 | {
12 |     int i = 0;
13 |     for ( ; i < cn; i++ )
14 |         buf[i] = cv::saturate_cast< T >( s.val[i] );
15 |     for ( ; i < unroll_to; i++ )
16 |         buf[i] = buf[i - cn];
17 | }
18 | 
19 | inline void
20 | scalarToData( const cv::Scalar& s, void* _buf, int type, int unroll_to )
21 | {
22 |     const int depth = CV_MAT_DEPTH( type ), cn = CV_MAT_CN( type );
23 |     CV_Assert( cn <= 4 );
24 |     switch ( depth )
25 |     {
26 |         case CV_8U:
27 |             scalarToRawData_< uchar >( s, ( uchar* )_buf, cn, unroll_to );
28 |             break;
29 |         case CV_8S:
30 |             scalarToRawData_< schar >( s, ( schar* )_buf, cn, unroll_to );
31 |             break;
32 |         case CV_16U:
33 |             scalarToRawData_< ushort >( s, ( ushort* )_buf, cn, unroll_to );
34 |             break;
35 |         case CV_16S:
36 |             scalarToRawData_< short >( s, ( short* )_buf, cn, unroll_to );
37 |             break;
38 |         case CV_32S:
39 |             scalarToRawData_< int >( s, ( int* )_buf, cn, unroll_to );
40 |             break;
41 |         case CV_32F:
42 |             scalarToRawData_< float >( s, ( float* )_buf, cn, unroll_to );
43 |             break;
44 |         case CV_64F:
45 |             scalarToRawData_< double >( s, ( double* )_buf, cn, unroll_to );
46 |             break;
47 |         default:
48 |             CV_Error( CV_StsUnsupportedFormat, "" );
49 |     }
50 | }
51 | }
52 | 
53 | #endif // SCALARTODATA_H
54 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>code_utils</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The code_utils package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="gao@todo.todo">gao</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/code_utils</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29 | 
30 | 
31 |   <!-- The *_depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use build_depend for packages you need at compile time: -->
35 |   <!--   <build_depend>message_generation</build_depend> -->
36 |   <!-- Use buildtool_depend for build tool packages: -->
37 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
38 |   <!-- Use run_depend for packages you need at runtime: -->
39 |   <!--   <run_depend>message_runtime</run_depend> -->
40 |   <!-- Use test_depend for packages you need only for testing: -->
41 |   <!--   <test_depend>gtest</test_depend> -->
42 |   <buildtool_depend>catkin</buildtool_depend>
43 |   <build_depend>roscpp</build_depend>
44 |   <build_depend>std_msgs</build_depend>
45 |   <run_depend>roscpp</run_depend>
46 |   <run_depend>std_msgs</run_depend>
47 | 
48 | 
49 |   <!-- The export tag contains other, unspecified, tags -->
50 |   <export>
51 |     <!-- Other tools can request additional information be placed here -->
52 | 
53 |   </export>
54 | </package>
55 | 


--------------------------------------------------------------------------------
/include/code_utils/sys_utils/cvmat_file_io.hpp:
--------------------------------------------------------------------------------
 1 | #ifndef CVMAT_FILE_IO_HPP
 2 | #define CVMAT_FILE_IO_HPP
 3 | 
 4 | #include <fstream>
 5 | #include <iomanip>
 6 | #include <iostream>
 7 | #include <opencv2/opencv.hpp>
 8 | 
 9 | namespace sys_utils
10 | {
11 | 
12 | namespace io
13 | {
14 | 
15 | //                 | C1  C2   C3	C4
16 | // CV_8U  | uchar  | 0   8    16	24 |  -0 %8
17 | // CV_8S  | char   | 1   9    17	25 |  -1 %8
18 | // CV_16U |        | 2   10   18	26 |  -2 %8
19 | // CV_16S | short  | 3   11   19	27 |  -3 %8
20 | // CV_32S | int    | 4   12   20	28 |  -4 %8
21 | // CV_32F | float  | 5   13   21	29 |  -5 %8
22 | // CV_64F | double | 6   14   22	30 |  -6 %8
23 | 
24 | inline void
25 | writeMatrixToBinary( const std::string filename, const cv::Mat& matrix )
26 | {
27 |     std::ofstream out( filename, std::ios::out | std::ios::binary | std::ios::trunc );
28 |     int rows = matrix.rows, cols = matrix.cols, step = matrix.step, type = matrix.type( );
29 | 
30 |     out.write( ( char* )( &rows ), sizeof( int ) );
31 |     out.write( ( char* )( &cols ), sizeof( int ) );
32 |     out.write( ( char* )( &step ), sizeof( int ) );
33 |     out.write( ( char* )( &type ), sizeof( int ) );
34 | 
35 |     // std::cout << " rows " << rows << "\n";
36 |     // std::cout << " cols " << cols << "\n";
37 |     // std::cout << " step " << step << "\n";
38 |     // std::cout << " type " << type << "\n";
39 | 
40 |     out.write( ( char* )matrix.data, rows * step * sizeof( uchar ) );
41 | 
42 |     out.close( );
43 | }
44 | 
45 | inline void
46 | parseMatrixFromBinary( const std::string filename, cv::Mat& matrix )
47 | {
48 |     std::ifstream in( filename, std::ios::in | std::ios::binary );
49 |     while ( in.peek( ) != EOF )
50 |     {
51 |         int rows = 0, cols = 0, step = 0, type = 0;
52 | 
53 |         in.read( ( char* )( &rows ), sizeof( int ) );
54 |         in.read( ( char* )( &cols ), sizeof( int ) );
55 |         in.read( ( char* )( &step ), sizeof( int ) );
56 |         in.read( ( char* )( &type ), sizeof( int ) );
57 | 
58 |         // std::cout << " rows " << rows << "\n";
59 |         // std::cout << " cols " << cols << "\n";
60 |         // std::cout << " step " << step << "\n";
61 |         // std::cout << " type " << type << "\n";
62 | 
63 |         matrix = cv::Mat( rows, cols, type );
64 | 
65 |         in.read( ( char* )matrix.data, rows * step * sizeof( uchar ) );
66 |     }
67 | 
68 |     in.close( );
69 | }
70 | }
71 | }
72 | 
73 | #endif // CVMAT_FILE_IO_HPP
74 | 


--------------------------------------------------------------------------------
/include/code_utils/eigen_utils.h:
--------------------------------------------------------------------------------
  1 | #ifndef EIGEN_UTLIS_H
  2 | #define EIGEN_UTLIS_H
  3 | 
  4 | #include <eigen3/Eigen/Eigen>
  5 | 
  6 | #include <iostream>
  7 | 
  8 | using namespace std;
  9 | 
 10 | namespace eigen_utils
 11 | {
 12 | 
 13 | typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
 14 | typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
 15 |   Matrix;
 16 | 
 17 | template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic>
 18 | struct EigenTypes
 19 | {
 20 | 
 21 |     typedef Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> Matrix;
 22 | 
 23 |     typedef Eigen::Matrix<double, num_rows, 1> Vector;
 24 | };
 25 | 
 26 | /**
 27 |  *
 28 |  */
 29 | template <class vector_t>
 30 | inline vector_t
 31 | SwapSequence(vector_t vec_in)
 32 | {
 33 | //    int size = vec_in.size();
 34 | //    vector_t vec_out(size);
 35 | 
 36 | //    for (int i = 0; i < size; i++)
 37 | //    {
 38 | //        vec_out(size - 1 - i) = vec_in(i);
 39 | //    }
 40 | 
 41 | //    return vec_out;
 42 | 
 43 |   return vec_in.reverse();
 44 | }
 45 | 
 46 | inline Vector
 47 | pushback(Vector vec_in, const double value)
 48 | {
 49 |     Vector vec_out(vec_in.size() + 1);
 50 | 
 51 |     vec_out.segment(0, vec_in.size()) = vec_in;
 52 |     vec_out(vec_in.size()) = value;
 53 | 
 54 |     return vec_out;
 55 | }
 56 | 
 57 | template <class T>
 58 | inline void
 59 | copyMat3ToArry(const Eigen::Matrix<T, 3, 3> mat, T* data)
 60 | {
 61 |     data[0] = T(mat(0, 0));
 62 |     data[1] = T(mat(1, 0));
 63 |     data[2] = T(mat(2, 0));
 64 |     data[3] = T(mat(0, 1));
 65 |     data[4] = T(mat(1, 1));
 66 |     data[5] = T(mat(2, 1));
 67 |     data[6] = T(mat(0, 2));
 68 |     data[7] = T(mat(1, 2));
 69 |     data[8] = T(mat(2, 2));
 70 | }
 71 | template <class T>
 72 | inline void
 73 | copyArryToMat3(T* data, Eigen::Matrix<T, 3, 3>& mat)
 74 | {
 75 |     mat(0, 0) = data[0];
 76 |     mat(1, 0) = data[1];
 77 |     mat(2, 0) = data[2];
 78 |     mat(0, 1) = data[3];
 79 |     mat(1, 1) = data[4];
 80 |     mat(2, 1) = data[5];
 81 |     mat(0, 2) = data[6];
 82 |     mat(1, 2) = data[7];
 83 |     mat(2, 2) = data[8];
 84 | }
 85 | template <class T>
 86 | inline void
 87 | copyVector3ToArry(const Eigen::Matrix<T, 3, 1> vec, T* data)
 88 | {
 89 |     data[0] = T(vec(0));
 90 |     data[1] = T(vec(1));
 91 |     data[2] = T(vec(2));
 92 | }
 93 | template <class T>
 94 | inline void
 95 | copyArryToVector3(T* data, Eigen::Matrix<T, 3, 1>& vec)
 96 | {
 97 |     vec(0) = data[0];
 98 |     vec(1) = data[1];
 99 |     vec(2) = data[2];
100 | }
101 | 
102 | };
103 | 
104 | #endif // EIGEN_UTLIS_H
105 | 


--------------------------------------------------------------------------------
/src/mat_io_test.cpp:
--------------------------------------------------------------------------------
 1 | #define BACKWARD_HAS_DW 1
 2 | #include "code_utils/backward.hpp"
 3 | namespace backward
 4 | {
 5 | backward::SignalHandling sh;
 6 | }
 7 | 
 8 | #include <code_utils/cv_utils/cv_type.hpp>
 9 | #include <code_utils/sys_utils/cvmat_file_io.hpp>
10 | #include <iostream>
11 | #include <opencv2/opencv.hpp>
12 | #include <time.h>
13 | 
14 | using namespace std;
15 | using namespace cv;
16 | 
17 | double t;
18 | 
19 | void
20 | showImg( std::string name, cv::Mat img )
21 | {
22 |     cv::namedWindow( name, WINDOW_NORMAL );
23 |     cv::imshow( name, img );
24 | }
25 | 
26 | int
27 | main( )
28 | {
29 |     Mat img;
30 | 
31 |     //    Mat img1 = imread( "/home/gao/ws/devel/lib/camera_model/image_down/IMG_35.png",
32 |     //    CV_LOAD_IMAGE_GRAYSCALE );
33 |     Mat img1 = imread( "/home/gao/IMG_1.png", CV_LOAD_IMAGE_UNCHANGED );
34 | 
35 |     cv::resize( img1, img, cv::Size( 640, 512 ) );
36 | 
37 |     sys_utils::io::writeMatrixToBinary( "/home/gao/img_data", img1 );
38 | 
39 |     cv::Mat img11;
40 |     sys_utils::io::parseMatrixFromBinary( "/home/gao/img_data", img11 );
41 |     showImg( "img_data", img11 );
42 | 
43 |     cv::Mat img2( 2, 2, CV_8UC3 );
44 |     for ( int i = 0; i < 3; ++i )
45 |     {
46 |         img2.at< cv::Vec12b >( 0, 0 )[i] = uchar( 1 );
47 |         img2.at< cv::Vec12b >( 0, 1 )[i] = uchar( 2 );
48 |         img2.at< cv::Vec12b >( 1, 0 )[i] = uchar( 3 );
49 |         img2.at< cv::Vec12b >( 1, 1 )[i] = uchar( 4 );
50 |     }
51 |     std::cout << img2 << "\n\n";
52 |     sys_utils::io::writeMatrixToBinary( "/home/gao/img_data2", img2 );
53 | 
54 |     cv::Mat img3( 2, 2, CV_32FC3 );
55 |     for ( int i = 0; i < 3; ++i )
56 |     {
57 |         img3.at< cv::Vec3f >( 0, 0 )[i] = float( 1.1 );
58 |         img3.at< cv::Vec3f >( 0, 1 )[i] = float( 2.2 );
59 |         img3.at< cv::Vec3f >( 1, 0 )[i] = float( 3.3 );
60 |         img3.at< cv::Vec3f >( 1, 1 )[i] = float( 4.4 );
61 |     }
62 |     std::cout << img3 << "\n\n";
63 |     sys_utils::io::writeMatrixToBinary( "/home/gao/img_data3", img3 );
64 | 
65 |     cv::Mat img4( 2, 2, CV_64FC3 );
66 |     for ( int i = 0; i < 3; ++i )
67 |     {
68 |         img4.at< cv::Vec3d >( 0, 0 )[i] = float( 1.11 );
69 |         img4.at< cv::Vec3d >( 0, 1 )[i] = float( 2.22 );
70 |         img4.at< cv::Vec3d >( 1, 0 )[i] = float( 3.33 );
71 |         img4.at< cv::Vec3d >( 1, 1 )[i] = float( 4.44 );
72 |     }
73 |     std::cout << img4 << "\n\n";
74 |     sys_utils::io::writeMatrixToBinary( "/home/gao/img_data4", img4 );
75 | 
76 |     std::cout << "===================================="
77 |               << "\n\n";
78 | 
79 |     cv::Mat img22;
80 |     sys_utils::io::parseMatrixFromBinary( "/home/gao/img_data2", img22 );
81 |     std::cout << img22 << "\n\n";
82 | 
83 |     cv::Mat img33;
84 |     sys_utils::io::parseMatrixFromBinary( "/home/gao/img_data3", img33 );
85 |     std::cout << img33 << "\n\n";
86 | 
87 |     cv::Mat img44;
88 |     sys_utils::io::parseMatrixFromBinary( "/home/gao/img_data4", img44 );
89 |     std::cout << img44 << "\n\n";
90 | 
91 |     waitKey( 0 );
92 | 
93 |     return 0;
94 | }
95 | 


--------------------------------------------------------------------------------
/src/cv_utils.cc:
--------------------------------------------------------------------------------
 1 | #include <code_utils/cv_utils.h>
 2 | 
 3 | cv_utils::fisheye::PreProcess::PreProcess( const cv::Size _raw_image_size,
 4 |                                            const cv::Size _roi_size,
 5 |                                            const cv::Point _center,
 6 |                                            const float _resize_scale )
 7 | : is_preprocess( false )
 8 | {
 9 |     /* clang-format off */
10 |     if (    _raw_image_size.width >= _roi_size.width
11 |          && _raw_image_size.height >= _roi_size.height
12 |          && _center.x <= _raw_image_size.width
13 |          && _center.y <= _raw_image_size.height
14 |          && _roi_size.width != 0
15 |          && _roi_size.height != 0 )
16 |       is_preprocess = true;
17 |     else
18 |       is_preprocess = false;
19 |     /* clang-format on */
20 | 
21 |     if ( is_preprocess )
22 |         resetPreProcess( _roi_size, _center, _resize_scale );
23 |     else
24 |         std::cout << "[#ERROR] Parameters error." << std::endl;
25 | }
26 | 
27 | void
28 | cv_utils::fisheye::PreProcess::resetPreProcess( cv::Size _roi_size, cv::Point _center, float _resize_scale )
29 | {
30 |     if ( _resize_scale < 0 )
31 |         resize_scale = 1.0;
32 |     else
33 |         resize_scale = _resize_scale;
34 | 
35 |     roi_row_start = _center.y - _roi_size.height / 2;
36 |     roi_row_end   = roi_row_start + _roi_size.height;
37 |     roi_col_start = _center.x - _roi_size.width / 2;
38 |     roi_col_end   = roi_col_start + _roi_size.width;
39 | 
40 |     std::cout << "[#INFO] ROI row: start " << roi_row_start << " ,end " << roi_row_end << std::endl;
41 |     std::cout << "[#INFO] ROI col: start " << roi_col_start << " ,end " << roi_col_end << std::endl;
42 | }
43 | 
44 | cv::Mat
45 | cv_utils::fisheye::PreProcess::do_preprocess( cv::Mat image_input )
46 | {
47 |     if ( is_preprocess )
48 |     {
49 |         cv::Mat image_input_roi = image_input( cv::Range( roi_row_start, roi_row_end ),
50 |                                                cv::Range( roi_col_start, roi_col_end ) );
51 | 
52 |         cv::Mat image_input_resized;
53 |         cv::resize( image_input_roi,
54 |                     image_input_resized,
55 |                     cv::Size( image_input_roi.cols * resize_scale, image_input_roi.rows * resize_scale ) );
56 |         return image_input_resized;
57 |     }
58 |     else
59 |     {
60 |         return image_input;
61 |     }
62 | }
63 | 
64 | cv::Point2f
65 | cv_utils::fisheye::PreProcess::preprocessPoint( const cv::Point2f& pt_in )
66 | {
67 |     if ( is_preprocess )
68 |     {
69 |         cv::Point2f pt( -1, -1 );
70 | 
71 |         if ( !pt_in.inside( cv::Rect( roi_col_start, //
72 |                                       roi_row_start,
73 |                                       roi_col_end - roi_col_start,
74 |                                       roi_row_end - roi_row_start ) ) )
75 |         {
76 |             return pt;
77 |         }
78 |         else
79 |         {
80 |             pt = cv::Point2f( ( pt_in.x - roi_col_start ) * resize_scale,
81 |                               ( pt_in.y - roi_row_start ) * resize_scale );
82 |             return pt;
83 |         }
84 |     }
85 |     else
86 |     {
87 |         return pt_in;
88 |     }
89 | }
90 | 


--------------------------------------------------------------------------------
/include/code_utils/math_utils/Polynomial.h:
--------------------------------------------------------------------------------
  1 | #ifndef POLYNOMIAL_H
  2 | #define POLYNOMIAL_H
  3 | 
  4 | #include <math.h>
  5 | #include <vector>
  6 | 
  7 | #include <iostream>
  8 | 
  9 | #include <code_utils/eigen_utils.h>
 10 | #include <eigen3/Eigen/Eigen>
 11 | 
 12 | namespace math_utils
 13 | {
 14 | 
 15 | class Polynomial
 16 | {
 17 | 
 18 |   public:
 19 |     Polynomial();
 20 |     Polynomial(const int _order);
 21 |     Polynomial(const eigen_utils::Vector _coeff);
 22 | 
 23 |     eigen_utils::Vector getRealRoot(double _y);
 24 |     eigen_utils::Vector getRealRoot(double _y, double x_min, double x_max);
 25 |     double getOneRealRoot(double _y, double x_min, double x_max);
 26 | 
 27 |     eigen_utils::Vector getValue(const eigen_utils::Vector& in);
 28 |     double getValue(const double in);
 29 | 
 30 |     void setPolyOrder(int _order);
 31 |     int getPolyOrder() const;
 32 | 
 33 |     void setPolyCoeff(const eigen_utils::Vector& value);
 34 |     void setPolyCoeff(int order_index, double value);
 35 | 
 36 |     eigen_utils::Vector getPolyCoeff() const;
 37 |     double getPolyCoeff(int order_index) const;
 38 | 
 39 |     Polynomial& operator=(const Polynomial& other);
 40 |     friend std::ostream& operator<<(std::ostream& out,
 41 |                                     const Polynomial& poly);
 42 |     std::string toString(void) const;
 43 |   private:
 44 |     double Evaluate(double _x);
 45 | 
 46 |     // _y = f(x)
 47 |     bool FindRoots(const double y, const eigen_utils::Vector& polynomial_in,
 48 |                    eigen_utils::Vector& real, eigen_utils::Vector& imaginary);
 49 | 
 50 |     void FindLinearPolynomialRoots(const eigen_utils::Vector& Polynomial,
 51 |                                    eigen_utils::Vector& real,
 52 |                                    eigen_utils::Vector& imag);
 53 | 
 54 |     void FindQuadraticPolynomialRoots(const eigen_utils::Vector& Polynomial,
 55 |                                       eigen_utils::Vector& real,
 56 |                                       eigen_utils::Vector& imag);
 57 | 
 58 |     void BuildCompanionMatrix(const eigen_utils::Vector& Polynomial,
 59 |                               eigen_utils::Matrix* companion_matrix_ptr);
 60 | 
 61 |     // Balancing function as described by B. N. Parlett and C. Reinsch,
 62 |     // "Balancing a Matrix for Calculation of Eigenvalues and Eigenvectors".
 63 |     // In: Numerische Mathematik, Volume 13, Number 4 (1969), 293-304,
 64 |     // Springer Berlin / Heidelberg. DOI: 10.1007/BF02165404
 65 |     void BalanceCompanionMatrix(eigen_utils::Matrix* companion_matrix_ptr);
 66 | 
 67 |   private:
 68 |     int m_order;
 69 |     // c0, c1, c2, ..., cn
 70 |     // c0 + c1X + c2X^2 + ... +cnX^n
 71 |     eigen_utils::Vector m_coeff;
 72 | };
 73 | 
 74 | typedef struct
 75 | {
 76 |     double x;
 77 |     double y;
 78 | } Sample;
 79 | 
 80 | class PolynomialFit : public Polynomial
 81 | {
 82 |   public:
 83 |     PolynomialFit(int _order);
 84 |     PolynomialFit(int _order, eigen_utils::Vector _x, eigen_utils::Vector _y);
 85 | 
 86 |     void loadSamples(const eigen_utils::Vector& x,
 87 |                      const eigen_utils::Vector& y);
 88 |     void loadSample(const Sample sample);
 89 |     void clearSamples();
 90 | 
 91 |     eigen_utils::Vector getCoeff();
 92 |     Polynomial& getPolynomial();
 93 | 
 94 |   private:
 95 |     eigen_utils::Vector Fit();
 96 | 
 97 |   private:
 98 |     int data_size;
 99 |     vector<Sample> samples;
100 |     Polynomial* poly;
101 |     eigen_utils::Vector coeff;
102 | };
103 | 
104 | }
105 | #endif // POLYNOMIAL_H
106 | 


--------------------------------------------------------------------------------
/src/sumpixel_test.cpp:
--------------------------------------------------------------------------------
  1 | #define BACKWARD_HAS_DW 1
  2 | #include "backward.hpp"
  3 | namespace backward
  4 | {
  5 | backward::SignalHandling sh;
  6 | }
  7 | 
  8 | #include <code_utils/sys_utils/tic_toc.h>
  9 | #include <iostream>
 10 | #include <opencv2/opencv.hpp>
 11 | #include <time.h>
 12 | 
 13 | using namespace std;
 14 | using namespace cv;
 15 | 
 16 | double t;
 17 | 
 18 | void
 19 | showImg( std::string name, cv::Mat img )
 20 | {
 21 |     cv::namedWindow( name, WINDOW_NORMAL );
 22 |     cv::imshow( name, img );
 23 | }
 24 | 
 25 | void
 26 | sumPixelRow( const cv::Mat& img, cv::Mat& integral )
 27 | {
 28 |     if ( img.type( ) == CV_8UC1 )
 29 |         integral = cv::Mat( img.rows, img.cols, CV_32SC1 );
 30 | 
 31 |     int nr = img.rows, nc = img.cols;
 32 |     int index_col, index_row;
 33 | 
 34 |     const uchar* pImg = img.ptr< uchar >( 0 );
 35 |     int* pIntegral    = integral.ptr< int >( 0 );
 36 | 
 37 |     for ( index_row = 0; index_row < nr; ++index_row )
 38 |     {
 39 |         pImg      = img.ptr< uchar >( index_row );
 40 |         pIntegral = integral.ptr< int >( index_row );
 41 | 
 42 |         pIntegral[0] = pImg[0];
 43 | 
 44 |         for ( index_col = 1; index_col < nc; ++index_col )
 45 |         {
 46 |             pIntegral[index_col] = pIntegral[index_col - 1] + pImg[index_col];
 47 |         }
 48 |     }
 49 | }
 50 | 
 51 | void
 52 | sumPixelRow2( const cv::Mat& img, cv::Mat& integral )
 53 | {
 54 |     if ( img.type( ) == CV_8UC1 )
 55 |         integral = cv::Mat( img.rows, img.cols, CV_32SC1 );
 56 | 
 57 |     int nr = img.rows, nc = img.cols;
 58 |     int index_col, index_row;
 59 | 
 60 |     const uchar* pImg = img.ptr< uchar >( 0 );
 61 |     int* pIntegral    = integral.ptr< int >( 0 );
 62 | 
 63 |     pIntegral[0] = pImg[0];
 64 |     for ( index_col = 1; index_col < nc; ++index_col )
 65 |     {
 66 |         pIntegral[index_col] = pIntegral[index_col - 1] + pImg[index_col];
 67 |     }
 68 | 
 69 |     for ( index_row = 1; index_row < nr; ++index_row )
 70 |     {
 71 |         pImg      = img.ptr< uchar >( index_row );
 72 |         pIntegral = integral.ptr< int >( index_row );
 73 |         for ( index_col = 0; index_col < nc; ++index_col )
 74 |         {
 75 |             pIntegral[index_col] = pIntegral[index_col - 1] + pImg[index_col];
 76 |         }
 77 |     }
 78 | }
 79 | 
 80 | void
 81 | test1( )
 82 | {
 83 |     Mat img;
 84 |     Mat img1 = imread( "/home/gao/IMG_1.png", CV_LOAD_IMAGE_GRAYSCALE );
 85 | 
 86 |     sys_utils::tic::TicTocPart time;
 87 | 
 88 |     for ( int i = 0; i < 100; i++ )
 89 |         sumPixelRow( img1, img );
 90 | 
 91 |     std::cout << "sumPixelRow cost " << time.toc( ) << " ms\n";
 92 | 
 93 |     cv::Mat img2;
 94 |     normalize( img, img2, 0, 255, CV_MINMAX );
 95 |     Mat imageIntegralNorm;
 96 |     convertScaleAbs( img2, imageIntegralNorm );
 97 | 
 98 |     showImg( "src1", img1 );
 99 |     showImg( "dst1", img );
100 |     showImg( "dst11", imageIntegralNorm );
101 | }
102 | 
103 | void
104 | test2( )
105 | {
106 |     Mat img;
107 |     Mat img1 = imread( "/home/gao/IMG_1.png", CV_LOAD_IMAGE_GRAYSCALE );
108 | 
109 |     sys_utils::tic::TicTocPart time;
110 | 
111 |     for ( int i = 0; i < 100; i++ )
112 |         integral( img1, img, img1.type( ) );
113 | 
114 |     std::cout << "sumPixelRow cost " << time.toc( ) << " ms\n";
115 | 
116 |     cv::Mat img2;
117 |     normalize( img, img2, 0, 255, CV_MINMAX );
118 |     Mat imageIntegralNorm;
119 |     convertScaleAbs( img2, imageIntegralNorm );
120 | 
121 |     showImg( "src2", img1 );
122 |     showImg( "dst2", img );
123 |     showImg( "dst22", imageIntegralNorm );
124 | }
125 | 
126 | int
127 | main( )
128 | {
129 |     test1( );
130 |     test2( );
131 | 
132 |     waitKey( 0 );
133 | 
134 |     return 0;
135 | }
136 | 


--------------------------------------------------------------------------------
/include/code_utils/cv_utils/pnp/nonlinearpnp.h:
--------------------------------------------------------------------------------
  1 | #ifndef NONLINEARPNP_H
  2 | #define NONLINEARPNP_H
  3 | 
  4 | #include <ceres/ceres.h>
  5 | #include <ceres/rotation.h>
  6 | #include <cmath>
  7 | #include <eigen3/Eigen/Eigen>
  8 | #include <iostream>
  9 | #include <opencv2/opencv.hpp>
 10 | 
 11 | namespace cv
 12 | {
 13 | 
 14 | class Utility
 15 | {
 16 |     public:
 17 |     template< typename Derived >
 18 |     static Eigen::Matrix< typename Derived::Scalar, 3, 3 >
 19 |     skewSymmetric( const Eigen::MatrixBase< Derived >& q )
 20 |     {
 21 |         Eigen::Matrix< typename Derived::Scalar, 3, 3 > ans;
 22 |         ans << typename Derived::Scalar( 0 ), -q( 2 ), q( 1 ), q( 2 ),
 23 |         typename Derived::Scalar( 0 ), -q( 0 ), -q( 1 ), q( 0 ), typename Derived::Scalar( 0 );
 24 |         return ans;
 25 |     }
 26 | };
 27 | 
 28 | class NonlinearPnP
 29 | {
 30 |     class ReprojectionError
 31 |     {
 32 |         public:
 33 |         ReprojectionError( const Eigen::Vector3d& image_point, const Eigen::Vector3d& scene_point );
 34 | 
 35 |         template< typename T >
 36 |         bool operator( )( const T* const ex_paramt, T* residuals ) const
 37 |         {
 38 | 
 39 |             Eigen::Matrix< T, 3, 1 > _t_c;
 40 |             _t_c[0] = ex_paramt[0];
 41 |             _t_c[1] = ex_paramt[1];
 42 |             _t_c[2] = ex_paramt[2];
 43 | 
 44 |             Eigen::Quaternion< T > _q_cw( ex_paramt[6], ex_paramt[3], ex_paramt[4], ex_paramt[5] );
 45 |             _q_cw.normalize( );
 46 | 
 47 |             Eigen::Matrix< T, 3, 1 > p_w;
 48 |             p_w[0] = T( scene_point_.x( ) );
 49 |             p_w[1] = T( scene_point_.y( ) );
 50 |             p_w[2] = T( scene_point_.z( ) );
 51 | 
 52 |             Eigen::Matrix< T, 3, 1 > p_c;
 53 |             p_c = _q_cw * p_w + _t_c;
 54 |             p_c.normalize( );
 55 | 
 56 |             T r_x = p_c( 0 ) - T( image_point_( 0 ) );
 57 |             T r_y = p_c( 1 ) - T( image_point_( 1 ) );
 58 |             T r_z = p_c( 2 ) - T( image_point_( 2 ) );
 59 | 
 60 |             // clang-format off
 61 |              residuals[0] = T( tangent_base( 0, 0 ) ) * r_x
 62 |                           + T( tangent_base( 0, 1 ) ) * r_y
 63 |                           + T( tangent_base( 0, 2 ) ) * r_z;
 64 |              residuals[1] = T( tangent_base( 1, 0 ) ) * r_x
 65 |                           + T( tangent_base( 1, 1 ) ) * r_y
 66 |                           + T( tangent_base( 1, 2 ) ) * r_z;
 67 |             // clang-format on
 68 | 
 69 |             return true;
 70 |         }
 71 | 
 72 |         Eigen::Vector3d image_point_; // 2D to 3D point on image
 73 |         Eigen::Vector3d scene_point_; // 3D corresponding point
 74 |         Eigen::Matrix< double, 2, 3 > tangent_base;
 75 | 
 76 |         public:
 77 |         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 78 |     };
 79 | 
 80 |     public:
 81 |     NonlinearPnP( const Eigen::Matrix3d& _R_initial,
 82 |                   const Eigen::Vector3d& _T_initial,
 83 |                   const std::vector< Eigen::Vector3d >& image_point,
 84 |                   const std::vector< Eigen::Vector3d >& scene_point );
 85 |     ~NonlinearPnP( ) {}
 86 | 
 87 |     bool getRT( Eigen::Vector3d& T_out, Eigen::Quaterniond& q_out );
 88 | 
 89 |     bool solved;
 90 |     Eigen::Quaterniond q;
 91 |     Eigen::Vector3d T;
 92 |     double data[9];
 93 |     int point_num;
 94 | };
 95 | 
 96 | class ProjectionFactor : public ceres::SizedCostFunction< 2, 7 >
 97 | {
 98 |     public:
 99 |     ProjectionFactor( const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j );
100 |     virtual bool Evaluate( const double* const* ex_paramt, double* residuals, double** jacobians ) const;
101 |     void check( double** parameters ) {}
102 | 
103 |     Eigen::Vector3d image_point_, scene_point_;
104 |     Eigen::Matrix< double, 2, 3 > tangent_base;
105 |     static Eigen::Matrix2d sqrt_info;
106 |     static double sum_t;
107 | };
108 | }
109 | #endif // NONLINEARPNP_H
110 | 


--------------------------------------------------------------------------------
/include/code_utils/math_utils/math_utils.h:
--------------------------------------------------------------------------------
  1 | #ifndef _MATH_UTILS_H
  2 | #define _MATH_UTILS_H
  3 | 
  4 | #include <math.h>
  5 | #include <eigen3/Eigen/Eigen>
  6 | #include <code_utils/eigen_utils.h>
  7 | 
  8 | #define DEG2RAD (M_PI/180.0)
  9 | #define RAD2DEG (180.0/M_PI)
 10 | 
 11 | #define DEG2RADF (float)(M_PI/180.0)
 12 | #define RAD2DEGF (float)(180.0/M_PI)
 13 | 
 14 | namespace math_utils
 15 | {
 16 | 
 17 | /// \brief Skew
 18 | /// \param vec :input 3-dof vector
 19 | /// \return   :output skew symmetric matrix
 20 | inline Eigen::Matrix3d
 21 | vectorToSkew(const Eigen::Vector3d vec)
 22 | {
 23 |     Eigen::Matrix3d mat;
 24 | 
 25 |     mat <<  0.0, -vec(2), vec(1),
 26 |             vec(2), 0.0, -vec(0),
 27 |            -vec(1), vec(0), 0.0 ;
 28 | 
 29 |     return mat;
 30 | }
 31 | 
 32 | template <typename T>
 33 | inline Eigen::Matrix<T,3,3>
 34 | vectorToSkew(const Eigen::Matrix<T,3,1> vec)
 35 | {
 36 |     Eigen::Matrix<T,3,3>   mat;
 37 | 
 38 |     mat(0, 0) = T(0);
 39 |     mat(0, 1) = -vec(2);
 40 |     mat(0, 2) = vec(1);
 41 |     mat(1, 0) =  vec(2);
 42 |     mat(1, 1) = T(0);
 43 |     mat(1, 2) = -vec(0);
 44 |     mat(2, 0) = -vec(1);
 45 |     mat(2, 1) =  vec(0);
 46 |     mat(2, 2) = T(0);
 47 | 
 48 |     return mat;
 49 | }
 50 | 
 51 | /// \brief Skew
 52 | /// \param vec :input 3-dof vector
 53 | /// \return   :output skew symmetric matrix
 54 | inline Eigen::Vector3d
 55 | skewToVector(const Eigen::Matrix3d mat)
 56 | {
 57 |     return Eigen::Vector3d( mat(2,1),
 58 |                             mat(0,2),
 59 |                             mat(1,0) );
 60 | }
 61 | 
 62 | /// \brief eigenQtoCeresQ
 63 | /// \param q_eigen
 64 | /// \return
 65 | template <typename T>
 66 | inline T*
 67 | eigenQtoCeresQ(const T* const q_eigen)
 68 | {
 69 |     // Eigen convention (x, y, z, w)
 70 |     // Ceres convention (w, x, y, z)
 71 |     T q_ceres[4] = { q_eigen[3],
 72 |                      q_eigen[0],
 73 |                      q_eigen[1],
 74 |                      q_eigen[2] };
 75 | 
 76 |     return q_ceres;
 77 | }
 78 | 
 79 | /// \brief ceresQtoEigenQ
 80 | /// \param q_ceres
 81 | /// \return
 82 | template <typename T>
 83 | inline T*
 84 | ceresQtoEigenQ(const T* const q_ceres)
 85 | {
 86 |     // Ceres convention (w, x, y, z)
 87 |     // Eigen convention (x, y, z, w)
 88 |     T q_eigen[4] = { q_ceres[1],
 89 |                      q_ceres[2],
 90 |                      q_ceres[3],
 91 |                      q_ceres[0] };
 92 | 
 93 |     return q_eigen;
 94 | }
 95 | 
 96 | /// \brief orientaionOfVectors
 97 | /// \param v
 98 | /// \param theta
 99 | /// \return
100 | inline Eigen::Quaterniond
101 | orientaionOfVectors( const Eigen::Vector3d v,
102 |                      const double theta)
103 | {
104 |     return Eigen::Quaterniond( cos(0.5 * theta),
105 |                                sin(0.5 * theta) * v(0),
106 |                                sin(0.5 * theta) * v(1),
107 |                                sin(0.5 * theta) * v(2) );
108 | }
109 | 
110 | /// \brief orientaionOfVectors :orientation betreen vector v2 to vetor v1
111 | /// \param v1 :begin vector
112 | /// \param v2 :end vector
113 | /// \return   :q_12
114 | inline Eigen::Quaterniond
115 | orientaionOfVectors( const Eigen::Vector3d v1,
116 |                      const Eigen::Vector3d v2)
117 | {
118 |     double theta = acos(v1.dot(v2));
119 | 
120 |     Eigen::Vector3d Vk = v1.cross(v2);
121 | 
122 |     return Eigen::Quaterniond( cos(0.5 * theta),
123 |                                sin(0.5 * theta) * Vk(0),
124 |                                sin(0.5 * theta) * Vk(1),
125 |                                sin(0.5 * theta) * Vk(2) );
126 | }
127 | 
128 | inline Eigen::Matrix3d
129 | eularToDCM(double yaw, double pitch, double roll)
130 | {
131 |     //    double y = ypr(0) / 180.0 * M_PI;
132 |     //    double p = ypr(1) / 180.0 * M_PI;
133 |     //    double r = ypr(2) / 180.0 * M_PI;
134 |     Eigen::Matrix3d Rz;
135 |     Rz << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1;
136 | 
137 |     Eigen::Matrix3d Ry;
138 |     Ry << cos(pitch), 0., sin(pitch), 0., 1., 0., -sin(pitch), 0., cos(pitch);
139 | 
140 |     Eigen::Matrix3d Rx;
141 |     Rx << 1., 0., 0., 0., cos(roll), -sin(roll), 0., sin(roll), cos(roll);
142 | 
143 |     return Rz * Ry * Rx;
144 | }
145 | 
146 | }
147 | #endif // _MATH_UTILS_H
148 | 


--------------------------------------------------------------------------------
/src/cv_utils/pnp/pnp.cpp:
--------------------------------------------------------------------------------
  1 | #include <code_utils/cv_utils/pnp/pnp.h>
  2 | 
  3 | cv::Pnp::Pnp( const std::vector< Eigen::Vector3d >& image_point,
  4 |               const std::vector< Eigen::Vector3d >& scene_point )
  5 | : solved( false )
  6 | {
  7 |     //    std::cout << "image_point " << image_point.size( ) << std::endl;
  8 |     //    std::cout << "scene_point " << scene_point.size( ) << std::endl;
  9 | 
 10 |     bool is_planar = false;
 11 |     if ( scene_point.size( ) < 6 )
 12 |     {
 13 |         is_planar = true;
 14 |     }
 15 |     else
 16 |     {
 17 |         int size = scene_point.size( );
 18 |         Eigen::MatrixXd scene_points( size, 3 );
 19 |         for ( int index = 0; index < size; ++index )
 20 |         {
 21 |             scene_points( index, 0 ) = scene_point[index]( 0 );
 22 |             scene_points( index, 1 ) = scene_point[index]( 1 );
 23 |             scene_points( index, 2 ) = scene_point[index]( 2 );
 24 |         }
 25 |         Eigen::MatrixXd scene_points2 = scene_points * scene_points.transpose( );
 26 | 
 27 |         Eigen::VectorXd svd_si = Eigen::JacobiSVD< Eigen::MatrixXd >( scene_points2, //
 28 |                                                                       Eigen::EigenvaluesOnly )
 29 |                                  .singularValues( );
 30 | 
 31 |         //        Eigen::Vector3d svd_si = svd.singularValues( );
 32 |         //        std::cout << svd_si << std::endl;
 33 |         //        std::cout << "is_planar ? " << svd_si( 2 ) / svd_si( 1 ) << std::endl;
 34 | 
 35 |         if ( ( svd_si( 2 ) / svd_si( 1 ) ) < 0.001 )
 36 |             is_planar = true;
 37 |         else
 38 |             is_planar = false;
 39 |     }
 40 |     //    std::cout << "is_planar " << is_planar << std::endl;
 41 | 
 42 |     Eigen::Matrix3d R_cw;
 43 |     Eigen::Vector3d t_cw;
 44 | 
 45 |     if ( is_planar )
 46 |     {
 47 |         cv::Homography llpnp( image_point, scene_point );
 48 |         R_cw = llpnp.getR( );
 49 |         t_cw = llpnp.getT( );
 50 |     }
 51 |     else
 52 |     {
 53 |         cv::DLT llpnp( image_point, scene_point );
 54 |         R_cw = llpnp.getR( );
 55 |         t_cw = llpnp.getT( );
 56 |     }
 57 | 
 58 |     //    std::cout << "R_cw " << std::endl << R_cw << std::endl;
 59 |     //    std::cout << "t_cw " << std::endl << t_cw.transpose( ) << std::endl;
 60 | 
 61 |     npnp = new cv::NonlinearPnP( R_cw, t_cw, image_point, scene_point );
 62 | }
 63 | 
 64 | cv::Pnp::Pnp( const std::vector< Eigen::Vector3d >& image_point,
 65 |               const std::vector< Eigen::Vector3d >& scene_point,
 66 |               Eigen::Quaterniond& q_dst,
 67 |               Eigen::Vector3d& T_dst )
 68 | : solved( false )
 69 | {
 70 |     bool is_planar = false;
 71 |     if ( scene_point.size( ) < 6 )
 72 |     {
 73 |         is_planar = true;
 74 |     }
 75 |     else
 76 |     {
 77 |         int size = scene_point.size( );
 78 |         Eigen::MatrixXd scene_points( size, 3 );
 79 |         for ( int index = 0; index < size; ++index )
 80 |         {
 81 |             scene_points( index, 0 ) = scene_point[index]( 0 );
 82 |             scene_points( index, 1 ) = scene_point[index]( 1 );
 83 |             scene_points( index, 2 ) = scene_point[index]( 2 );
 84 |         }
 85 |         Eigen::MatrixXd scene_points2 = scene_points * scene_points.transpose( );
 86 | 
 87 |         Eigen::JacobiSVD< Eigen::MatrixXd > svd( scene_points2, Eigen::EigenvaluesOnly );
 88 | 
 89 |         //        std::cout << svd.singularValues( ) << std::endl;
 90 | 
 91 |         if ( svd.singularValues( )( 2 ) / svd.singularValues( )( 1 ) < 1e-3 )
 92 |             is_planar = true;
 93 |         else
 94 |             is_planar = false;
 95 |     }
 96 | 
 97 |     Eigen::Matrix3d R_cw;
 98 |     Eigen::Vector3d t_cw;
 99 |     if ( is_planar )
100 |     {
101 |         cv::Homography llpnp( image_point, scene_point );
102 |         R_cw = llpnp.getR( );
103 |         t_cw = llpnp.getT( );
104 |     }
105 |     else
106 |     {
107 |         cv::DLT llpnp( image_point, scene_point );
108 |         R_cw = llpnp.getR( );
109 |         t_cw = llpnp.getT( );
110 |     }
111 | 
112 |     //    q_dst = Eigen::Quaterniond( R_cw );
113 |     //    T_dst = t_cw;
114 | 
115 |     //    std::cout << "P_2 " << std::endl << t_cw.transpose( ) << std::endl;
116 | 
117 |     npnp = new cv::NonlinearPnP( R_cw, t_cw, image_point, scene_point );
118 |     getRT( q_dst, T_dst );
119 | }
120 | 
121 | cv::Pnp::Pnp( const std::vector< Eigen::Vector3d >& image_point,
122 |               const std::vector< Eigen::Vector3d >& scene_point,
123 |               const Eigen::Quaterniond& q_init,
124 |               const Eigen::Vector3d& T_init,
125 |               Eigen::Quaterniond& q_dst,
126 |               Eigen::Vector3d& T_dst )
127 | : solved( false )
128 | {
129 |     npnp = new cv::NonlinearPnP( q_init.toRotationMatrix( ), T_init, image_point, scene_point );
130 |     getRT( q_dst, T_dst );
131 | }
132 | 
133 | bool
134 | cv::Pnp::getRT( Eigen::Quaterniond& q_dst, Eigen::Vector3d& T_dst )
135 | {
136 |     return npnp->getRT( T_dst, q_dst );
137 | }
138 | 


--------------------------------------------------------------------------------
/src/cv_utils/pnp/nonlinearpnp.cpp:
--------------------------------------------------------------------------------
  1 | #include <code_utils/cv_utils/pnp/nonlinearpnp.h>
  2 | 
  3 | cv::NonlinearPnP::NonlinearPnP( const Eigen::Matrix3d& _R_initial,
  4 |                                 const Eigen::Vector3d& _T_initial,
  5 |                                 const std::vector< Eigen::Vector3d >& image_point,
  6 |                                 const std::vector< Eigen::Vector3d >& scene_point )
  7 | : T( _T_initial )
  8 | {
  9 | 
 10 |     cv::ProjectionFactor::sqrt_info = Eigen::Matrix2d::Identity( );
 11 | 
 12 |     if ( image_point.size( ) != scene_point.size( ) )
 13 |         std::cerr << "Error of point size" << std::endl;
 14 |     q = _R_initial;
 15 | 
 16 |     // initialize the params to something close to the gt
 17 |     double ext[] = { T[0], T[1], T[2], q.x( ), q.y( ), q.z( ), q.w( ) };
 18 | 
 19 |     point_num = image_point.size( );
 20 | 
 21 |     ceres::Problem problem;
 22 | 
 23 |     for ( int i = 0; i < point_num; ++i )
 24 |     {
 25 | 
 26 |         // ProjectionFactor* f = new ProjectionFactor( image_point[i], scene_point[i] );
 27 | 
 28 |         ceres::CostFunction* f = new ceres::AutoDiffCostFunction< ReprojectionError, 2, 7 >(
 29 |         new ReprojectionError( image_point[i], scene_point[i] ) );
 30 | 
 31 |         problem.AddResidualBlock( f, NULL /* squared loss */, ext );
 32 |     }
 33 | 
 34 |     ceres::Solver::Options options;
 35 |     options.minimizer_progress_to_stdout = true;
 36 |     options.logging_type                 = ceres::SILENT;
 37 |     options.trust_region_strategy_type   = ceres::DOGLEG;
 38 |     //    options.max_num_iterations         = 5;
 39 | 
 40 |     ceres::Solver::Summary summary;
 41 |     ceres::Solve( options, &problem, &summary );
 42 |     //    std::cout << summary.FullReport( ) << "\n";
 43 | 
 44 |     q.w( ) = ext[6];
 45 |     q.x( ) = ext[3];
 46 |     q.y( ) = ext[4];
 47 |     q.z( ) = ext[5];
 48 |     T << ext[0], ext[1], ext[2];
 49 | 
 50 |     if ( summary.final_cost > 1 )
 51 |         solved = false;
 52 |     else
 53 |         solved = true;
 54 | }
 55 | 
 56 | bool
 57 | cv::NonlinearPnP::getRT( Eigen::Vector3d& T_out, Eigen::Quaterniond& q_out )
 58 | {
 59 |     q_out = q.normalized( );
 60 |     T_out = T;
 61 | 
 62 |     if ( solved )
 63 |         return true;
 64 |     else
 65 |         return false;
 66 | }
 67 | 
 68 | cv::NonlinearPnP::ReprojectionError::ReprojectionError( const Eigen::Vector3d& image_point,
 69 |                                                         const Eigen::Vector3d& scene_point )
 70 | : image_point_( image_point )
 71 | , scene_point_( scene_point )
 72 | {
 73 |     Eigen::Vector3d b1, b2;
 74 |     Eigen::Vector3d tmp( 0, 0, 1 );
 75 |     Eigen::Vector3d a = scene_point_.normalized( );
 76 |     if ( a == tmp )
 77 |         tmp << 1, 0, 0;
 78 | 
 79 |     b1 = ( tmp - a * ( a.transpose( ) * tmp ) ).normalized( );
 80 |     b2 = a.cross( b1 );
 81 | 
 82 |     tangent_base.block< 1, 3 >( 0, 0 ) = b1.transpose( );
 83 |     tangent_base.block< 1, 3 >( 1, 0 ) = b2.transpose( );
 84 | }
 85 | 
 86 | cv::ProjectionFactor::ProjectionFactor( const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j )
 87 | : image_point_( _pts_i )
 88 | , scene_point_( _pts_j )
 89 | {
 90 |     Eigen::Vector3d b1, b2;
 91 |     Eigen::Vector3d a = scene_point_.normalized( );
 92 |     Eigen::Vector3d tmp( 0, 0, 1 );
 93 |     if ( a == tmp )
 94 |         tmp << 1, 0, 0;
 95 |     b1 = ( tmp - a * ( a.transpose( ) * tmp ) ).normalized( );
 96 |     b2 = a.cross( b1 );
 97 |     tangent_base.block< 1, 3 >( 0, 0 ) = b1.transpose( );
 98 |     tangent_base.block< 1, 3 >( 1, 0 ) = b2.transpose( );
 99 | }
100 | 
101 | Eigen::Matrix2d cv::ProjectionFactor::sqrt_info;
102 | 
103 | bool
104 | cv::ProjectionFactor::Evaluate( const double* const* ex_paramt, double* residuals, double** jacobians ) const
105 | {
106 | 
107 |     Eigen::Vector3d _t_c( ex_paramt[0][0], ex_paramt[0][1], ex_paramt[0][2] );
108 |     Eigen::Quaterniond _q_cw( ex_paramt[0][6], ex_paramt[0][3], ex_paramt[0][4], ex_paramt[0][5] );
109 |     //    _q_cw.normalize( );
110 | 
111 |     Eigen::Matrix< double, 3, 1 > p_c;
112 |     p_c = _q_cw * scene_point_ + _t_c;
113 | 
114 |     Eigen::Map< Eigen::Vector2d > residual( residuals );
115 | 
116 |     residual = tangent_base * ( p_c.normalized( ) - image_point_ );
117 |     //    residual = sqrt_info * residual;
118 | 
119 |     if ( jacobians )
120 |     {
121 |         Eigen::Matrix3d R_cw = _q_cw.toRotationMatrix( );
122 |         Eigen::Matrix< double, 2, 3 > reduce( 2, 3 );
123 | 
124 |         double norm = p_c.norm( );
125 |         Eigen::Matrix3d norm_jaco;
126 |         double x1, x2, x3;
127 |         x1 = p_c( 0 );
128 |         x2 = p_c( 1 );
129 |         x3 = p_c( 2 );
130 | 
131 |         // clang-format off
132 |         norm_jaco <<
133 |             1.0 / norm - x1 * x1 / pow( norm, 3 ), -x1 * x2 / pow( norm, 3 ), -x1 * x3 / pow( norm, 3 ),
134 |             -x1 * x2 / pow( norm, 3 ), 1.0 / norm - x2 * x2 / pow( norm, 3 ), -x2 * x3 / pow( norm, 3 ),
135 |             -x1 * x3 / pow( norm, 3 ), -x2 * x3 / pow( norm, 3 ), 1.0 / norm - x3 * x3 / pow( norm, 3 );
136 |         // clang-format on
137 | 
138 |         if ( jacobians[0] )
139 |         {
140 |             Eigen::Map< Eigen::Matrix< double, 2, 7, Eigen::RowMajor > > jacobian_pose_i( jacobians[0] );
141 |             // clang-format on
142 |             Eigen::Matrix< double, 3, 6 > jaco_i;
143 |             jaco_i.leftCols< 3 >( )  = Eigen::Matrix3d::Identity( );
144 |             jaco_i.rightCols< 3 >( ) = -R_cw * Utility::skewSymmetric( scene_point_ );
145 |             // clang-format on
146 | 
147 |             jacobian_pose_i.leftCols< 6 >( ) = tangent_base * jaco_i;
148 |             jacobian_pose_i.rightCols< 1 >( ).setZero( );
149 |         }
150 |     }
151 |     return true;
152 | }
153 | 


--------------------------------------------------------------------------------
/src/cv_utils/pnp/linearpnp.cpp:
--------------------------------------------------------------------------------
  1 | #include <code_utils/cv_utils/pnp/linearpnp.h>
  2 | 
  3 | cv::Homography::Homography( const std::vector< Eigen::Vector3d >& pts_2,
  4 |                             const std::vector< Eigen::Vector3d >& pts_3 )
  5 | {
  6 |     mat_tmp.resize( 2, 9 );
  7 | 
  8 |     readPointsPlanar( pts_2, pts_3 );
  9 | 
 10 |     solvePnP( );
 11 | }
 12 | 
 13 | void
 14 | cv::Homography::solvePnP( )
 15 | {
 16 |     //    std::cout << "Here is the matrix m:" << std::endl << M << std::endl;
 17 |     Eigen::JacobiSVD< Eigen::MatrixXd > svd( M, Eigen::ComputeThinV );
 18 |     //    std::cout << "Its singular values are:" << std::endl << svd.singularValues( ) <<
 19 |     //    std::endl;
 20 |     //    std::cout << "svd.matrixV( )" << std::endl << svd.matrixV( ) << std::endl;
 21 | 
 22 |     // the last column of matrixV
 23 |     Eigen::Matrix3d hat_H;
 24 | 
 25 |     hat_H.block< 1, 3 >( 0, 0 ) = svd.matrixV( ).block< 3, 1 >( 0, 8 ).transpose( );
 26 |     hat_H.block< 1, 3 >( 1, 0 ) = svd.matrixV( ).block< 3, 1 >( 3, 8 ).transpose( );
 27 |     hat_H.block< 1, 3 >( 2, 0 ) = svd.matrixV( ).block< 3, 1 >( 6, 8 ).transpose( );
 28 | 
 29 |     std::cout << hat_H << std::endl;
 30 | 
 31 |     Eigen::Matrix3d RRT = hat_H;
 32 | 
 33 |     T( 0 ) = RRT.coeff( 0, 2 );
 34 |     T( 1 ) = RRT.coeff( 1, 2 );
 35 |     T( 2 ) = RRT.coeff( 2, 2 );
 36 | 
 37 |     Eigen::Vector3d h1 = RRT.block< 3, 1 >( 0, 0 );
 38 |     Eigen::Vector3d h2 = RRT.block< 3, 1 >( 0, 1 );
 39 |     Eigen::Vector3d h3 = h1.cross( h2 );
 40 | 
 41 |     if ( RRT.determinant( ) < 0 )
 42 |     {
 43 |         h1 = -h1;
 44 |         h2 = -h2;
 45 |         T  = -T;
 46 |     }
 47 | 
 48 |     Eigen::Matrix3d hat_H_2;
 49 |     hat_H_2 << h1, h2, h3;
 50 | 
 51 |     Eigen::JacobiSVD< Eigen::MatrixXd > svd2( hat_H_2, Eigen::ComputeThinU | Eigen::ComputeThinV );
 52 | 
 53 |     R = svd2.matrixU( ) * svd2.matrixV( ).transpose( );
 54 |     //    std::cout << "R " << std::endl << R << std::endl;
 55 |     T = T / ( h1.norm( ) );
 56 | }
 57 | 
 58 | void
 59 | cv::Homography::readPointsPlanar( const std::vector< Eigen::Vector3d >& pts_2,
 60 |                                   const std::vector< Eigen::Vector3d >& pts_3 )
 61 | {
 62 |     int index_max = pts_3.size( );
 63 |     //    std::cout << " size " << index_max << std::endl;
 64 |     M.resize( 2 * index_max, 9 );
 65 |     //    M.setZero( );
 66 | 
 67 |     int max_i;
 68 | 
 69 |     for ( int index = 0; index < index_max; index++ )
 70 |     {
 71 |         // std::cout << pts_3.at( index ).x( ) << " " << pts_3.at( index ).y( ) << " "
 72 |         //          << pts_3.at( index ).z( ) << std::endl;
 73 |         // std::cout << pts_2.at( index ).x( ) << " " << pts_2.at( index ).y( ) << " "
 74 |         //          << pts_2.at( index ).z( ) << std::endl;
 75 | 
 76 |         max_i = math_utils::max_index_in_three( pts_2[index].x( ), //
 77 |                                                 pts_2[index].y( ),
 78 |                                                 pts_2[index].z( ) );
 79 | 
 80 |         switch ( max_i )
 81 |         {
 82 |             case 0:
 83 |                 //                std::cout << " x " << std::endl;
 84 |                 mat_tmp << -pts_3[index].x( ) * pts_2[index].y( ) / pts_2[index].x( ),
 85 |                 -pts_3[index].y( ) * pts_2[index].y( ) / pts_2[index].x( ),
 86 |                 -pts_2[index].y( ) / pts_2[index].x( ), pts_3[index].x( ), pts_3[index].y( ),
 87 |                 1, 0, 0, 0, -pts_3[index].x( ) * pts_2[index].z( ) / pts_2[index].x( ),
 88 |                 -pts_3[index].y( ) * pts_2[index].z( ) / pts_2[index].x( ),
 89 |                 -pts_2[index].z( ) / pts_2[index].x( ), 0, 0, 0, pts_3[index].x( ),
 90 |                 pts_3[index].y( ), 1;
 91 |                 break;
 92 |             case 1:
 93 |                 //                std::cout << " y " << std::endl;
 94 |                 mat_tmp << pts_3[index].x( ), pts_3[index].y( ), 1,
 95 |                 -pts_3[index].x( ) * pts_2[index].x( ) / pts_2[index].y( ),
 96 |                 -pts_3[index].y( ) * pts_2[index].x( ) / pts_2[index].y( ),
 97 |                 -pts_2[index].x( ) / pts_2[index].y( ), 0, 0, 0, 0, 0, 0,
 98 |                 -pts_3[index].x( ) * pts_2[index].z( ) / pts_2[index].y( ),
 99 |                 -pts_3[index].y( ) * pts_2[index].z( ) / pts_2[index].y( ),
100 |                 -pts_2[index].z( ) / pts_2[index].y( ), pts_3[index].x( ), pts_3[index].y( ), 1;
101 |                 break;
102 |             case 2:
103 |                 //                std::cout << " z " << std::endl;
104 |                 mat_tmp << pts_3[index].x( ), pts_3[index].y( ), 1, 0, 0, 0,
105 |                 -pts_3[index].x( ) * pts_2[index].x( ) / pts_2[index].z( ),
106 |                 -pts_3[index].y( ) * pts_2[index].x( ) / pts_2[index].z( ),
107 |                 -pts_2[index].x( ) / pts_2[index].z( ), 0, 0, 0, pts_3[index].x( ),
108 |                 pts_3[index].y( ), 1, -pts_3[index].x( ) * pts_2[index].y( ) / pts_2[index].z( ),
109 |                 -pts_3[index].y( ) * pts_2[index].y( ) / pts_2[index].z( ),
110 |                 -pts_2[index].y( ) / pts_2[index].z( );
111 |                 break;
112 |             default:
113 |                 //                std::cout << " default z " << std::endl;
114 |                 mat_tmp << pts_3[index].x( ), pts_3[index].y( ), 1, 0, 0, 0,
115 |                 -pts_3[index].x( ) * pts_2[index].x( ) / pts_2[index].z( ),
116 |                 -pts_3[index].y( ) * pts_2[index].x( ) / pts_2[index].z( ),
117 |                 -pts_2[index].x( ) / pts_2[index].z( ), 0, 0, 0, pts_3[index].x( ),
118 |                 pts_3[index].y( ), 1, -pts_3[index].x( ) * pts_2[index].y( ) / pts_2[index].z( ),
119 |                 -pts_3[index].y( ) * pts_2[index].y( ) / pts_2[index].z( ),
120 |                 -pts_2[index].y( ) / pts_2[index].z( );
121 |                 break;
122 |         }
123 |         //        std::cout << " mat_tmp " << std::endl << mat_tmp << std::endl;
124 | 
125 |         M.block< 2, 9 >( 2 * index, 0 ) = mat_tmp;
126 |     }
127 |     //    std::cout << " M " << std::endl << M << std::endl;
128 | }
129 | 


--------------------------------------------------------------------------------
/src/poly_test.cpp:
--------------------------------------------------------------------------------
  1 | #include <eigen3/Eigen/Core>
  2 | #include <iostream>
  3 | 
  4 | #include <code_utils/eigen_utils.h>
  5 | #include <code_utils/math_utils/Polynomial.h>
  6 | #include <code_utils/sys_utils/printcolor.hpp>
  7 | 
  8 | using namespace std;
  9 | using namespace Eigen;
 10 | 
 11 | void
 12 | fit_test( )
 13 | {
 14 |     eigen_utils::Vector xx( 101 );
 15 |     eigen_utils::Vector out( 101 );
 16 | 
 17 |     xx << 0, 0.0261799, 0.0523599, 0.0785398, 0.10472, 0.1309, 0.15708, 0.18326, 0.20944,
 18 |     0.235619, 0.261799, 0.287979, 0.314159, 0.340339, 0.366519, 0.392699, 0.418879,
 19 |     0.445059, 0.471239, 0.497419, 0.523599, 0.549779, 0.575959, 0.602139, 0.628319,
 20 |     0.654498, 0.680678, 0.706858, 0.733038, 0.759218, 0.785398, 0.811578, 0.837758,
 21 |     0.863938, 0.890118, 0.916298, 0.942478, 0.968658, 0.994838, 1.02102, 1.0472, 1.07338,
 22 |     1.09956, 1.12574, 1.15192, 1.1781, 1.20428, 1.23046, 1.25664, 1.28282, 1.309, 1.33518,
 23 |     1.36136, 1.38754, 1.41372, 1.4399, 1.46608, 1.49226, 1.51844, 1.54462, 1.5708, 1.59698,
 24 |     1.62316, 1.64934, 1.67552, 1.7017, 1.72788, 1.75406, 1.78024, 1.80642, 1.8326, 1.85878,
 25 |     1.88496, 1.91114, 1.93732, 1.9635, 1.98968, 2.01586, 2.04204, 2.06822, 2.0944, 2.12058,
 26 |     2.14675, 2.17293, 2.19911, 2.22529, 2.25147, 2.27765, 2.30383, 2.33001, 2.35619,
 27 |     2.38237, 2.40855, 2.43473, 2.46091, 2.48709, 2.51327, 2.53945, 2.56563, 2.59181, 2.61799;
 28 | 
 29 |     out << 0, 0.0261771, 0.0523467, 0.0785066, 0.104654, 0.130789, 0.156907, 0.183008,
 30 |     0.209091, 0.235154, 0.261196, 0.287216, 0.313212, 0.339184, 0.365131, 0.391052,
 31 |     0.416945, 0.44281, 0.468646, 0.494452, 0.520226, 0.545968, 0.571676, 0.597349, 0.622986,
 32 |     0.648584, 0.674144, 0.699662, 0.725137, 0.750567, 0.775951, 0.801285, 0.826568, 0.851797,
 33 |     0.876968, 0.90208, 0.927128, 0.95211, 0.977021, 1.00186, 1.02661, 1.05129, 1.07587,
 34 |     1.10036, 1.12475, 1.14903, 1.17319, 1.19724, 1.22115, 1.24492, 1.26855, 1.29201, 1.3153,
 35 |     1.33841, 1.36132, 1.38402, 1.4065, 1.42873, 1.4507, 1.4724, 1.49379, 1.51486, 1.53558,
 36 |     1.55594, 1.57589, 1.59541, 1.61448, 1.63305, 1.65109, 1.66857, 1.68543, 1.70165,
 37 |     1.71716, 1.73193, 1.7459, 1.759, 1.77119, 1.7824, 1.79256, 1.80159, 1.80943, 1.81599,
 38 |     1.82119, 1.82494, 1.82713, 1.82767, 1.82646, 1.82338, 1.81831, 1.81112, 1.80168,
 39 |     1.78986, 1.77551, 1.75847, 1.73857, 1.71566, 1.68955, 1.66005, 1.62696, 1.59008, 1.54919;
 40 | 
 41 |     math_utils::PolynomialFit polyfit( 24, out, xx );
 42 |     math_utils::Polynomial poly = polyfit.getCoeff( );
 43 |     std::cout << "polyfit :" << endl << poly.getPolyCoeff( ).transpose( ) << std::endl;
 44 | 
 45 |     double dd = 0.02;
 46 |     for ( int i = 0; i < 100; ++i )
 47 |     {
 48 |         std::cout << dd * i << " " << poly.getValue( dd * i ) << std::endl;
 49 |     }
 50 | }
 51 | 
 52 | void
 53 | test_poly( )
 54 | {
 55 |     eigen_utils::Vector coeff( 6 );
 56 |     coeff << 1, 2, 3, 4, 5, 6;
 57 | 
 58 |     std::cout << "coeff :" << endl << coeff.transpose( ) << std::endl;
 59 | 
 60 |     math_utils::Polynomial poly( 5 );
 61 |     poly.setPolyCoeff( coeff );
 62 |     std::cout << "coeff :" << endl << poly.getPolyCoeff( ).transpose( ) << std::endl;
 63 | 
 64 |     for ( int i = 0; i < 10; i++ )
 65 |     {
 66 |         std::cout << "x: " << i << " y : " << poly.getValue( i ) << std::endl;
 67 |     }
 68 | 
 69 |     eigen_utils::Vector xx  = coeff;
 70 |     eigen_utils::Vector out = poly.getValue( xx );
 71 |     std::cout << "out :" << endl << out.transpose( ) << std::endl;
 72 | 
 73 |     std::cout << ":--PolynomialFit--------------------------------------:" << std::endl;
 74 |     math_utils::PolynomialFit polyfit( 5, xx, out );
 75 |     std::cout << "polyfit :" << endl << polyfit.getCoeff( ).transpose( ) << std::endl;
 76 | 
 77 |     math_utils::Polynomial polyfited;
 78 |     polyfited = polyfit.getPolynomial( );
 79 |     std::cout << "polyfited :" << endl
 80 |               << polyfited.getPolyCoeff( ).transpose( ) << std::endl;
 81 | 
 82 |     math_utils::Polynomial* polyfited2 = new math_utils::Polynomial( );
 83 |     *polyfited2                        = polyfit.getPolynomial( );
 84 |     std::cout << "polyfited2 :" << endl
 85 |               << polyfited2->getPolyCoeff( ).transpose( ) << std::endl;
 86 | 
 87 |     std::cout << ":----------------------------------------:" << std::endl;
 88 |     eigen_utils::Vector realroot = poly.getRealRoot( 0.0 );
 89 |     std::cout << "  Roots :" << endl << realroot.transpose( ) << std::endl;
 90 | 
 91 |     std::cout << ":----------------------------------------:" << std::endl;
 92 |     //  eigen_utils::Vector  polyn = eigen_utils::SwapSequence(coeff);
 93 |     eigen_utils::Vector polyn( 4 );
 94 |     polyn << -5, -1, 5, 1;
 95 |     std::cout << "polyn :" << endl << polyn.transpose( ) << std::endl;
 96 |     math_utils::Polynomial poly2( polyn );
 97 |     eigen_utils::Vector realroot2 = poly2.getRealRoot( 0.0 );
 98 |     std::cout << "  Roots :" << endl << realroot2.transpose( ) << std::endl;
 99 | 
100 |     std::cout << ":----------------------------------------:" << std::endl;
101 |     eigen_utils::Vector poly3( 4 );
102 |     poly3 << -5, 1, 0, 0;
103 |     std::cout << "polyn :" << endl << poly3.transpose( ) << std::endl;
104 |     math_utils::Polynomial polyn3( poly3 );
105 |     double realroot3 = polyn3.getOneRealRoot( 0.0, -100, 100 );
106 |     std::cout << " One Roots :" << endl << realroot3 << std::endl;
107 | 
108 |     std::cout << ":----------------------------------------:" << std::endl;
109 |     math_utils::Polynomial polyn4;
110 |     polyn4 = polyn3;
111 |     std::cout << "polyn3 :" << endl << polyn3.getPolyCoeff( ).transpose( ) << std::endl;
112 |     std::cout << "polyn4 :" << endl << polyn4.getPolyCoeff( ).transpose( ) << std::endl;
113 |     std::cout << "polyn4 :" << endl << polyn4.toString( ) << std::endl;
114 | }
115 | 
116 | int
117 | main( )
118 | {
119 |     eigen_utils::Vector vec( 3 );
120 |     std::cout << "size :" << endl << vec.size( ) << std::endl;
121 |     vec << 1, 2, 1;
122 |     vec = eigen_utils::pushback( vec, 22 );
123 |     std::cout << "vec :" << endl << vec.transpose( ) << std::endl;
124 | 
125 |     test_poly( );
126 |     std::cout << "    constant?" << std::endl;
127 | 
128 |     //  double q_in[4] = {1,2,3,4};
129 |     //  double q_out[4] ;
130 | 
131 |     sys_utils::print_color::PrintWarning( "warning" );
132 |     sys_utils::print_color::PrintError( "PrintError" );
133 |     sys_utils::print_color::PrintInfo( "PrintInfo" );
134 | 
135 |     std::cout << ":----------------------------------------:" << std::endl;
136 |     fit_test( );
137 | 
138 |     return 0;
139 | }
140 | 


--------------------------------------------------------------------------------
/src/cv_utils/dlt/dlt.cpp:
--------------------------------------------------------------------------------
  1 | #include <code_utils/cv_utils/dlt/dlt.h>
  2 | #include <code_utils/cv_utils/pnp/utils.h>
  3 | 
  4 | cv::DLT::DLT( const std::vector< Eigen::Vector3d >& pts_2, const std::vector< Eigen::Vector3d >& pts_3 )
  5 | {
  6 |     m_num_points = ( int )pts_2.size( );
  7 | 
  8 |     if ( m_num_points != ( int )pts_3.size( ) )
  9 |         std::cout << " [ERROR] input points size Error!" << std::endl;
 10 | 
 11 |     M.resize( 2 * m_num_points, 12 );
 12 | 
 13 |     readPointsPlanar( pts_2, pts_3 );
 14 | 
 15 |     solveDLT( );
 16 | }
 17 | 
 18 | void
 19 | cv::DLT::readPointsPlanar( const std::vector< Eigen::Vector3d >& pts_2,
 20 |                            const std::vector< Eigen::Vector3d >& pts_3 )
 21 | {
 22 | 
 23 |     for ( int i = 0; i < m_num_points; i++ )
 24 |     {
 25 | 
 26 |         int max_i = math_utils::max_index_in_three( pts_2[i].x( ), pts_2[i].y( ), pts_2[i].z( ) );
 27 | 
 28 |         switch ( max_i )
 29 |         {
 30 |             case 0:
 31 |                 M( 2 * i, 0 )      = -pts_3[i].x( ) * pts_2[i].y( ) / pts_2[i].x( );
 32 |                 M( 2 * i, 1 )      = -pts_3[i].y( ) * pts_2[i].y( ) / pts_2[i].x( );
 33 |                 M( 2 * i, 2 )      = -pts_3[i].z( ) * pts_2[i].y( ) / pts_2[i].x( );
 34 |                 M( 2 * i, 3 )      = -pts_2[i].y( ) / pts_2[i].x( );
 35 |                 M( 2 * i, 4 )      = pts_3[i].x( );
 36 |                 M( 2 * i, 5 )      = pts_3[i].y( );
 37 |                 M( 2 * i, 6 )      = pts_3[i].z( );
 38 |                 M( 2 * i, 7 )      = 1;
 39 |                 M( 2 * i, 8 )      = 0;
 40 |                 M( 2 * i, 9 )      = 0;
 41 |                 M( 2 * i, 10 )     = 0;
 42 |                 M( 2 * i, 11 )     = 0;
 43 |                 M( 2 * i + 1, 0 )  = -pts_3[i].x( ) * pts_2[i].z( ) / pts_2[i].x( );
 44 |                 M( 2 * i + 1, 1 )  = -pts_3[i].y( ) * pts_2[i].z( ) / pts_2[i].x( );
 45 |                 M( 2 * i + 1, 2 )  = -pts_3[i].z( ) * pts_2[i].z( ) / pts_2[i].x( );
 46 |                 M( 2 * i + 1, 3 )  = -pts_2[i].z( ) / pts_2[i].x( );
 47 |                 M( 2 * i + 1, 4 )  = 0;
 48 |                 M( 2 * i + 1, 5 )  = 0;
 49 |                 M( 2 * i + 1, 6 )  = 0;
 50 |                 M( 2 * i + 1, 7 )  = 0;
 51 |                 M( 2 * i + 1, 8 )  = pts_3[i].x( );
 52 |                 M( 2 * i + 1, 9 )  = pts_3[i].y( );
 53 |                 M( 2 * i + 1, 10 ) = pts_3[i].z( );
 54 |                 M( 2 * i + 1, 11 ) = 1;
 55 |                 break;
 56 |             case 1:
 57 |                 M( 2 * i, 0 )      = pts_3[i].x( );
 58 |                 M( 2 * i, 1 )      = pts_3[i].y( );
 59 |                 M( 2 * i, 2 )      = pts_3[i].z( );
 60 |                 M( 2 * i, 3 )      = 1;
 61 |                 M( 2 * i, 4 )      = -pts_3[i].x( ) * pts_2[i].x( ) / pts_2[i].y( );
 62 |                 M( 2 * i, 5 )      = -pts_3[i].y( ) * pts_2[i].x( ) / pts_2[i].y( );
 63 |                 M( 2 * i, 6 )      = -pts_3[i].z( ) * pts_2[i].x( ) / pts_2[i].y( );
 64 |                 M( 2 * i, 7 )      = -pts_2[i].x( ) / pts_2[i].y( );
 65 |                 M( 2 * i, 8 )      = 0;
 66 |                 M( 2 * i, 9 )      = 0;
 67 |                 M( 2 * i, 10 )     = 0;
 68 |                 M( 2 * i, 11 )     = 0;
 69 |                 M( 2 * i + 1, 0 )  = 0;
 70 |                 M( 2 * i + 1, 1 )  = 0;
 71 |                 M( 2 * i + 1, 2 )  = 0;
 72 |                 M( 2 * i + 1, 3 )  = 0;
 73 |                 M( 2 * i + 1, 4 )  = -pts_3[i].x( ) * pts_2[i].z( ) / pts_2[i].y( );
 74 |                 M( 2 * i + 1, 5 )  = -pts_3[i].y( ) * pts_2[i].z( ) / pts_2[i].y( );
 75 |                 M( 2 * i + 1, 6 )  = -pts_3[i].z( ) * pts_2[i].z( ) / pts_2[i].y( );
 76 |                 M( 2 * i + 1, 7 )  = -pts_2[i].z( ) / pts_2[i].y( );
 77 |                 M( 2 * i + 1, 8 )  = pts_3[i].x( );
 78 |                 M( 2 * i + 1, 9 )  = pts_3[i].y( );
 79 |                 M( 2 * i + 1, 10 ) = pts_3[i].z( );
 80 |                 M( 2 * i + 1, 11 ) = 1;
 81 |                 break;
 82 |             case 2:
 83 |                 // Update matrix A using eq. 5
 84 |                 M( 2 * i, 0 )      = pts_3[i].x( );
 85 |                 M( 2 * i, 1 )      = pts_3[i].y( );
 86 |                 M( 2 * i, 2 )      = pts_3[i].z( );
 87 |                 M( 2 * i, 3 )      = 1;
 88 |                 M( 2 * i, 4 )      = 0;
 89 |                 M( 2 * i, 5 )      = 0;
 90 |                 M( 2 * i, 6 )      = 0;
 91 |                 M( 2 * i, 7 )      = 0;
 92 |                 M( 2 * i, 8 )      = -pts_2[i].x( ) / pts_2[i].z( ) * pts_3[i].x( );
 93 |                 M( 2 * i, 9 )      = -pts_2[i].x( ) / pts_2[i].z( ) * pts_3[i].y( );
 94 |                 M( 2 * i, 10 )     = -pts_2[i].x( ) / pts_2[i].z( ) * pts_3[i].z( );
 95 |                 M( 2 * i, 11 )     = -pts_2[i].x( ) / pts_2[i].z( );
 96 |                 M( 2 * i + 1, 0 )  = 0;
 97 |                 M( 2 * i + 1, 1 )  = 0;
 98 |                 M( 2 * i + 1, 2 )  = 0;
 99 |                 M( 2 * i + 1, 3 )  = 0;
100 |                 M( 2 * i + 1, 4 )  = pts_3[i].x( );
101 |                 M( 2 * i + 1, 5 )  = pts_3[i].y( );
102 |                 M( 2 * i + 1, 6 )  = pts_3[i].z( );
103 |                 M( 2 * i + 1, 7 )  = 1;
104 |                 M( 2 * i + 1, 8 )  = -pts_2[i].y( ) / pts_2[i].z( ) * pts_3[i].x( );
105 |                 M( 2 * i + 1, 9 )  = -pts_2[i].y( ) / pts_2[i].z( ) * pts_3[i].y( );
106 |                 M( 2 * i + 1, 10 ) = -pts_2[i].y( ) / pts_2[i].z( ) * pts_3[i].z( );
107 |                 M( 2 * i + 1, 11 ) = -pts_2[i].y( ) / pts_2[i].z( );
108 |                 break;
109 |         }
110 |     }
111 | }
112 | 
113 | void
114 | cv::DLT::solveDLT( )
115 | {
116 |     //        std::cout << " M " << std::endl << M << std::endl;
117 | 
118 |     Eigen::JacobiSVD< Eigen::MatrixXd > svd( M, Eigen::ComputeThinV );
119 |     //    std::cout << " singularValues " << std::endl << svd.singularValues( ) <<
120 |     //    std::endl;
121 | 
122 |     Eigen::MatrixXd h = svd.matrixV( ).block< 12, 1 >( 0, 11 ).transpose( );
123 | 
124 |     //        std::cout << " svd.matrixV( ) " << std::endl << svd.matrixV( ) << std::endl;
125 | 
126 |     for ( int i = 0; i < 3; i++ )
127 |     {
128 |         T( i ) = h( 0, 4 * i + 3 ); // Translation
129 | 
130 |         for ( int j = 0; j < 3; j++ )
131 |             RR( i, j ) = h( 0, 4 * i + j ); // Rotation
132 |     }
133 | 
134 |     //    std::cout << " h " << std::endl << h << std::endl;
135 |     double norm = sqrt( h( 0, 8 ) * h( 0, 8 ) + h( 0, 9 ) * h( 0, 9 ) + h( 0, 10 ) * h( 0, 10 ) );
136 | 
137 |     if ( RR.determinant( ) < 0 ) // tz < 0
138 |     {
139 |         T /= -norm;
140 |         RR = RR / -norm;
141 |     }
142 |     else
143 |     {
144 |         T /= norm;
145 |         RR = RR / norm;
146 |     }
147 | 
148 |     Eigen::JacobiSVD< Eigen::MatrixXd > svd2( RR, Eigen::ComputeThinU | Eigen::ComputeThinV );
149 | 
150 |     R = svd2.matrixU( ) * svd2.matrixV( ).transpose( );
151 | }
152 | 


--------------------------------------------------------------------------------
/src/math_utils/Polynomial.cpp:
--------------------------------------------------------------------------------
  1 | #include <code_utils/math_utils/Polynomial.h>
  2 | #include <iostream>
  3 | 
  4 | using namespace math_utils;
  5 | 
  6 | Polynomial::Polynomial( ) {}
  7 | 
  8 | Polynomial::Polynomial( const int _order )
  9 | {
 10 |     m_order = _order;
 11 |     m_coeff = eigen_utils::Vector::Zero( m_order + 1 );
 12 | }
 13 | 
 14 | Polynomial::Polynomial( const eigen_utils::Vector _coeff )
 15 | {
 16 |     m_order = _coeff.size( ) - 1;
 17 |     m_coeff = _coeff;
 18 | }
 19 | 
 20 | double
 21 | Polynomial::getValue( const double in )
 22 | {
 23 |     return Evaluate( in );
 24 | }
 25 | 
 26 | eigen_utils::Vector
 27 | Polynomial::getRealRoot( double _y )
 28 | {
 29 |     eigen_utils::Vector realRoots, imagRoots;
 30 | 
 31 |     FindRoots( _y, m_coeff, realRoots, imagRoots );
 32 | 
 33 |     eigen_utils::Vector realRoot;
 34 |     for ( int i = 0; i < realRoots.size( ); i++ )
 35 |     {
 36 | 
 37 |         if ( imagRoots( i ) == 0 )
 38 |             realRoot = eigen_utils::pushback( realRoot, realRoots( i ) );
 39 |     }
 40 |     // std::cout<<"real roots: "<<realRoot<<std::endl;
 41 |     return realRoot;
 42 | }
 43 | 
 44 | eigen_utils::Vector
 45 | Polynomial::getRealRoot( double _y, double x_min, double x_max )
 46 | {
 47 |     eigen_utils::Vector realRoots, imagRoots;
 48 | 
 49 |     FindRoots( _y, m_coeff, realRoots, imagRoots );
 50 | 
 51 |     eigen_utils::Vector realRoot;
 52 |     for ( int i = 0; i < realRoots.size( ); i++ )
 53 |     {
 54 |         if ( imagRoots( i ) == 0 && realRoots( i ) >= x_min && realRoots( i ) <= x_max )
 55 |             realRoot = eigen_utils::pushback( realRoot, realRoots( i ) );
 56 |     }
 57 |     std::cout << "real roots: " << realRoot << std::endl;
 58 |     return realRoot;
 59 | }
 60 | 
 61 | double
 62 | Polynomial::getOneRealRoot( double _y, double x_min, double x_max )
 63 | {
 64 |     eigen_utils::Vector realRoots, imagRoots;
 65 | 
 66 |     FindRoots( _y, m_coeff, realRoots, imagRoots );
 67 | 
 68 |     // eigen_utils::Vector realRoot;
 69 |     //  std::cout << " max angle " << x_max ;
 70 |     for ( int i = 0; i < realRoots.size( ); ++i )
 71 |     {
 72 |         if ( imagRoots( i ) == 0 )
 73 |         {
 74 |             //      std::cout <<  " r " << realRoots(i);
 75 |             if ( realRoots( i ) >= x_min && realRoots( i ) <= x_max )
 76 |             {
 77 |                 return realRoots( i );
 78 |                 // realRoot = eigen_utils::pushback(realRoot, realRoots(i));
 79 |             }
 80 |         }
 81 |         if ( i == realRoots.size( ) - 1 )
 82 |             return 0;
 83 |     }
 84 |     //  std::cout << std::endl;
 85 | 
 86 |     // TODO:method to get one value
 87 |     // if (realRoot.size() >= 1)
 88 |     //    return realRoot(0);
 89 |     // else
 90 |     //    return 0;
 91 | }
 92 | 
 93 | eigen_utils::Vector
 94 | Polynomial::getValue( const eigen_utils::Vector& in )
 95 | {
 96 |     eigen_utils::Vector out( in.size( ) );
 97 | 
 98 |     for ( int i = in.size( ) - 1; i >= 0; --i )
 99 |     {
100 |         out( i ) = Evaluate( in( i ) );
101 |     }
102 | 
103 |     return out;
104 | }
105 | 
106 | void
107 | Polynomial::setPolyOrder( int _order )
108 | {
109 |     m_order = _order;
110 |     m_coeff.resize( m_order );
111 | }
112 | 
113 | int
114 | Polynomial::getPolyOrder( ) const
115 | {
116 |     return m_order;
117 | }
118 | 
119 | void
120 | Polynomial::setPolyCoeff( const eigen_utils::Vector& value )
121 | {
122 |     m_coeff = value;
123 | }
124 | 
125 | void
126 | Polynomial::setPolyCoeff( int order_index, double value )
127 | {
128 |     m_coeff( order_index ) = value;
129 | }
130 | 
131 | eigen_utils::Vector
132 | Polynomial::getPolyCoeff( ) const
133 | {
134 |     return m_coeff;
135 | }
136 | 
137 | double
138 | Polynomial::getPolyCoeff( int order_index ) const
139 | {
140 |     return m_coeff( order_index );
141 | }
142 | 
143 | Polynomial&
144 | Polynomial::operator=( const Polynomial& other )
145 | {
146 |     if ( this != &other )
147 |     {
148 |         m_order = other.m_order;
149 |         m_coeff = other.m_coeff;
150 |     }
151 | 
152 |     return *this;
153 | }
154 | 
155 | string
156 | Polynomial::toString( ) const
157 | {
158 |     std::ostringstream oss;
159 |     oss << "Polynomial :" << std::endl;
160 |     oss << "|   order|" << getPolyOrder( ) << std::endl;
161 |     oss << "|   coeff|" << getPolyCoeff( ).transpose( ) << std::endl;
162 |     return oss.str( );
163 | }
164 | 
165 | ostream&
166 | operator<<( ostream& out, const Polynomial& poly )
167 | {
168 |     out << "Polynomial :" << std::endl;
169 |     out << "|   order|" << poly.getPolyOrder( ) << std::endl;
170 |     out << "|   coeff|" << poly.getPolyCoeff( ).transpose( ) << std::endl;
171 |     return out;
172 | }
173 | 
174 | double
175 | Polynomial::Evaluate( double _x )
176 | {
177 |     double value_out = 0;
178 | 
179 |     for ( int i = m_order; i >= 0; --i )
180 |     {
181 |         value_out += m_coeff( i ) * pow( _x, i );
182 |     }
183 | 
184 |     return value_out;
185 | }
186 | 
187 | bool
188 | Polynomial::FindRoots( const double y,
189 |                        const eigen_utils::Vector& polynomial_in,
190 |                        eigen_utils::Vector& real,
191 |                        eigen_utils::Vector& imaginary )
192 | {
193 |     if ( polynomial_in.size( ) == 0 )
194 |         return false;
195 | 
196 |     int degree = polynomial_in.size( ) - 1;
197 | 
198 |     // count high order zero coefferent
199 |     int zero_num = 0;
200 |     for ( int i = degree; i >= 0; --i )
201 |     {
202 |         if ( polynomial_in( i ) == 0.0 )
203 |             zero_num++;
204 |         else
205 |             break;
206 |     }
207 | 
208 |     degree -= zero_num;
209 |     eigen_utils::Vector polynomial_coeff( degree + 1 );
210 |     polynomial_coeff = polynomial_in.segment( 0, degree + 1 );
211 | 
212 |     polynomial_coeff( 0 ) -= y;
213 | 
214 |     if ( degree == 0 )
215 |     {
216 |         std::cout << " Is the polynomial constant?" << std::endl;
217 |         return false;
218 |     }
219 |     // Linear
220 |     if ( degree == 1 )
221 |     {
222 |         FindLinearPolynomialRoots( polynomial_coeff, real, imaginary );
223 |         return true;
224 |     }
225 |     if ( degree == 2 )
226 |     {
227 |         FindQuadraticPolynomialRoots( polynomial_coeff, real, imaginary );
228 |         return true;
229 |     }
230 |     else if ( degree > 2 )
231 |     {
232 |         // The degree is now known to be at least 3. For cubic or higher
233 |         // roots we use the method of companion matrices.
234 | 
235 |         // Divide by leading term
236 |         const double leading_term = polynomial_coeff( degree );
237 |         //    std::cout<< " polynomial_in: " << polynomial_in.transpose() <<
238 |         //    std::endl;
239 | 
240 |         polynomial_coeff /= leading_term;
241 | 
242 |         // Build and balance the companion matrix to the polynomial.
243 |         eigen_utils::Matrix companion_matrix( degree, degree );
244 |         BuildCompanionMatrix( polynomial_coeff, &companion_matrix );
245 |         // BalanceCompanionMatrix(&companion_matrix);
246 |         // Find its (complex) eigenvalues.
247 |         Eigen::EigenSolver< eigen_utils::Matrix > solver( companion_matrix, false );
248 |         if ( solver.info( ) != Eigen::Success )
249 |         {
250 |             return false;
251 |         }
252 |         else
253 |         {
254 |             real      = solver.eigenvalues( ).real( );
255 |             imaginary = solver.eigenvalues( ).imag( );
256 | 
257 |             return true;
258 |         }
259 |     }
260 |     else
261 |         return false;
262 | }
263 | 
264 | void
265 | Polynomial::FindLinearPolynomialRoots( const eigen_utils::Vector& polynomial,
266 |                                        eigen_utils::Vector& real,
267 |                                        eigen_utils::Vector& imag )
268 | {
269 |     real.resize( 1 );
270 |     imag.resize( 1 );
271 | 
272 |     real( 0 ) = -polynomial( 0 ) / polynomial( 1 );
273 |     imag( 0 ) = 0;
274 | }
275 | 
276 | void
277 | Polynomial::FindQuadraticPolynomialRoots( const eigen_utils::Vector& polynomial,
278 |                                           eigen_utils::Vector& real,
279 |                                           eigen_utils::Vector& imag )
280 | {
281 |     const double a      = polynomial( 2 );
282 |     const double b      = polynomial( 1 );
283 |     const double c      = polynomial( 0 );
284 |     const double D      = b * b - 4 * a * c;
285 |     const double sqrt_D = sqrt( fabs( D ) );
286 | 
287 |     real.resize( 2 );
288 |     imag.resize( 2 );
289 | 
290 |     // Real roots.
291 |     if ( D >= 0 )
292 |     {
293 |         // Stable quadratic roots according to BKP Horn.
294 |         // http://people.csail.mit.edu/bkph/articles/Quadratics.pdf
295 |         if ( b >= 0 )
296 |         {
297 |             real( 0 ) = ( -b - sqrt_D ) / ( 2.0 * a );
298 |             imag( 0 ) = 0;
299 | 
300 |             real( 1 ) = ( 2.0 * c ) / ( -b - sqrt_D );
301 |             imag( 1 ) = 0;
302 |             return;
303 |         }
304 |         else
305 |         {
306 |             real( 0 ) = ( 2.0 * c ) / ( -b + sqrt_D );
307 |             imag( 0 ) = 0;
308 | 
309 |             real( 1 ) = ( -b + sqrt_D ) / ( 2.0 * a );
310 |             imag( 1 ) = 0;
311 |             return;
312 |         }
313 |     }
314 |     else
315 |     {
316 |         // Use the normal quadratic formula for the complex case.
317 |         real( 0 ) = -b / ( 2.0 * a );
318 |         imag( 0 ) = sqrt_D / ( 2.0 * a );
319 | 
320 |         real( 1 ) = -b / ( 2.0 * a );
321 |         imag( 1 ) = -sqrt_D / ( 2.0 * a );
322 |         return;
323 |     }
324 | }
325 | 
326 | void
327 | Polynomial::BuildCompanionMatrix( const eigen_utils::Vector& polynomial, eigen_utils::Matrix* companion_matrix_ptr )
328 | {
329 |     eigen_utils::Matrix& companion_matrix = *companion_matrix_ptr;
330 | 
331 |     const int degree = polynomial.size( ) - 1;
332 | 
333 |     // companion_matrix.resize(degree, degree);
334 |     companion_matrix.setZero( );
335 |     companion_matrix.diagonal( -1 ).setOnes( );
336 |     companion_matrix.col( degree - 1 ) = -polynomial.head( degree );
337 | }
338 | 
339 | void
340 | Polynomial::BalanceCompanionMatrix( eigen_utils::Matrix* companion_matrix_ptr )
341 | {
342 |     eigen_utils::Matrix& companion_matrix            = *companion_matrix_ptr;
343 |     eigen_utils::Matrix companion_matrix_offdiagonal = companion_matrix;
344 |     companion_matrix_offdiagonal.diagonal( ).setZero( );
345 | 
346 |     const int degree = companion_matrix.rows( );
347 | 
348 |     // gamma <= 1 controls how much a change in the scaling has to
349 |     // lower the 1-norm of the companion matrix to be accepted.
350 |     //
351 |     // gamma = 1 seems to lead to cycles (numerical issues?), so
352 |     // we set it slightly lower.
353 |     const double gamma = 0.9;
354 | 
355 |     // Greedily scale row/column pairs until there is no change.
356 |     bool scaling_has_changed;
357 |     do
358 |     {
359 |         scaling_has_changed = false;
360 | 
361 |         for ( int i = 0; i < degree; ++i )
362 |         {
363 |             const double row_norm = companion_matrix_offdiagonal.row( i ).lpNorm< 1 >( );
364 |             const double col_norm = companion_matrix_offdiagonal.col( i ).lpNorm< 1 >( );
365 | 
366 |             // Decompose row_norm/col_norm into mantissa * 2^exponent,
367 |             // where 0.5 <= mantissa < 1. Discard mantissa (return value
368 |             // of frexp), as only the exponent is needed.
369 |             int exponent = 0;
370 |             std::frexp( row_norm / col_norm, &exponent );
371 |             exponent /= 2;
372 | 
373 |             if ( exponent != 0 )
374 |             {
375 |                 const double scaled_col_norm = std::ldexp( col_norm, exponent );
376 |                 const double scaled_row_norm = std::ldexp( row_norm, -exponent );
377 |                 if ( scaled_col_norm + scaled_row_norm < gamma * ( col_norm + row_norm ) )
378 |                 {
379 |                     // Accept the new scaling. (Multiplication by powers of 2
380 |                     // should not
381 |                     // introduce rounding errors (ignoring non-normalized
382 |                     // numbers and
383 |                     // over- or underflow))
384 |                     scaling_has_changed = true;
385 |                     companion_matrix_offdiagonal.row( i ) *= std::ldexp( 1.0, -exponent );
386 |                     companion_matrix_offdiagonal.col( i ) *= std::ldexp( 1.0, exponent );
387 |                 }
388 |             }
389 |         }
390 |     } while ( scaling_has_changed );
391 | 
392 |     companion_matrix_offdiagonal.diagonal( ) = companion_matrix.diagonal( );
393 |     companion_matrix                         = companion_matrix_offdiagonal;
394 | }
395 | 
396 | PolynomialFit::PolynomialFit( int _order )
397 | : Polynomial( _order )
398 | {
399 |     poly = new Polynomial( _order );
400 |     samples.clear( );
401 |     data_size = 0;
402 | }
403 | 
404 | PolynomialFit::PolynomialFit( int _order, eigen_utils::Vector _x, eigen_utils::Vector _y )
405 | : Polynomial( _order )
406 | {
407 |     poly = new Polynomial( _order );
408 |     samples.clear( );
409 |     data_size = 0;
410 | 
411 |     loadSamples( _x, _y );
412 | }
413 | 
414 | void
415 | PolynomialFit::loadSamples( const eigen_utils::Vector& x, const eigen_utils::Vector& y )
416 | {
417 |     if ( x.size( ) != y.size( ) )
418 |         return;
419 | 
420 |     for ( int i = x.size( ) - 1; i >= 0; --i )
421 |     {
422 |         Sample sample;
423 |         sample.x = x( i );
424 |         sample.y = y( i );
425 |         samples.push_back( sample );
426 |         data_size++;
427 |     }
428 | }
429 | 
430 | void
431 | PolynomialFit::loadSample( const Sample sample )
432 | {
433 |     samples.push_back( sample );
434 |     data_size++;
435 | }
436 | 
437 | void
438 | PolynomialFit::clearSamples( )
439 | {
440 |     samples.clear( );
441 |     data_size = 0;
442 | }
443 | 
444 | eigen_utils::Vector
445 | PolynomialFit::getCoeff( )
446 | {
447 |     coeff = this->Fit( );
448 |     poly->setPolyCoeff( coeff );
449 | 
450 |     return coeff;
451 | }
452 | 
453 | Polynomial&
454 | PolynomialFit::getPolynomial( )
455 | {
456 |     return *poly;
457 | }
458 | 
459 | eigen_utils::Vector
460 | PolynomialFit::Fit( )
461 | {
462 |     int num_constraints = data_size;
463 | 
464 |     const int degree = this->getPolyOrder( );
465 | 
466 |     eigen_utils::Matrix lhs = eigen_utils::Matrix::Zero( num_constraints, num_constraints );
467 |     eigen_utils::Vector rhs = eigen_utils::Vector::Zero( num_constraints );
468 | 
469 |     int row = 0;
470 | 
471 |     for ( int i = 0; i < data_size; ++i )
472 |     {
473 |         const Sample& sample = samples[i];
474 | 
475 |         for ( int j = 0; j <= degree; ++j )
476 |         {
477 |             lhs( row, j ) = pow( sample.x, degree - j );
478 |         }
479 | 
480 |         rhs( row ) = sample.y;
481 | 
482 |         ++row;
483 |     }
484 | 
485 |     return ( lhs.fullPivLu( ).solve( rhs ).segment( 0, degree + 1 ) ).reverse( );
486 | }
487 | 


--------------------------------------------------------------------------------
/include/code_utils/ros_utils.h:
--------------------------------------------------------------------------------
  1 | #ifndef ROS_UTLS_H
  2 | #define ROS_UTLS_H
  3 | 
  4 | #include <cv_bridge/cv_bridge.h>
  5 | #include <eigen3/Eigen/Eigen>
  6 | #include <message_filters/subscriber.h>
  7 | #include <message_filters/sync_policies/approximate_time.h>
  8 | #include <opencv2/opencv.hpp>
  9 | #include <ros/ros.h>
 10 | #include <sensor_msgs/PointCloud2.h>
 11 | #include <sensor_msgs/image_encodings.h>
 12 | 
 13 | namespace ros_utils
 14 | {
 15 | 
 16 | typedef message_filters::Subscriber< sensor_msgs::Image > ImageSubscriber;
 17 | 
 18 | typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, //
 19 |                                                          sensor_msgs::Image >
 20 | AppSync2Images;
 21 | 
 22 | typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, //
 23 |                                                          sensor_msgs::Image,
 24 |                                                          sensor_msgs::Image >
 25 | AppSync3Images;
 26 | 
 27 | typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, //
 28 |                                                          sensor_msgs::Image,
 29 |                                                          sensor_msgs::Image,
 30 |                                                          sensor_msgs::Image >
 31 | AppSync4Images;
 32 | 
 33 | typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, //
 34 |                                                          sensor_msgs::Image,
 35 |                                                          sensor_msgs::Image,
 36 |                                                          sensor_msgs::Image,
 37 |                                                          sensor_msgs::Image >
 38 | AppSync5Images;
 39 | 
 40 | typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, //
 41 |                                                          sensor_msgs::Image,
 42 |                                                          sensor_msgs::Image,
 43 |                                                          sensor_msgs::Image,
 44 |                                                          sensor_msgs::Image,
 45 |                                                          sensor_msgs::Image >
 46 | AppSync6Images;
 47 | 
 48 | typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, //
 49 |                                                          sensor_msgs::Image,
 50 |                                                          sensor_msgs::Image,
 51 |                                                          sensor_msgs::Image,
 52 |                                                          sensor_msgs::Image,
 53 |                                                          sensor_msgs::Image,
 54 |                                                          sensor_msgs::Image,
 55 |                                                          sensor_msgs::Image >
 56 | AppSync8Images;
 57 | 
 58 | typedef message_filters::Synchronizer< AppSync2Images > App2ImgSynchronizer;
 59 | typedef message_filters::Synchronizer< AppSync3Images > App3ImgSynchronizer;
 60 | typedef message_filters::Synchronizer< AppSync4Images > App4ImgSynchronizer;
 61 | typedef message_filters::Synchronizer< AppSync5Images > App5ImgSynchronizer;
 62 | typedef message_filters::Synchronizer< AppSync6Images > App6ImgSynchronizer;
 63 | typedef message_filters::Synchronizer< AppSync8Images > App8ImgSynchronizer;
 64 | 
 65 | template< typename T >
 66 | T
 67 | readParam( ros::NodeHandle& n, std::string name )
 68 | {
 69 |     T ans;
 70 |     if ( n.getParam( name, ans ) )
 71 |     {
 72 |         ROS_INFO_STREAM( "Loaded " << name << ": " << ans );
 73 |     }
 74 |     else
 75 |     {
 76 |         ROS_ERROR_STREAM( "Failed to load " << name );
 77 |         n.shutdown( );
 78 |     }
 79 |     return ans;
 80 | }
 81 | 
 82 | inline void
 83 | sendDepthImage( ros::Publisher& pub, const ros::Time timeStamp, const std::string frame_id, const cv::Mat& depth )
 84 | {
 85 |     cv_bridge::CvImage out_msg;
 86 | 
 87 |     out_msg.header.stamp    = timeStamp;
 88 |     out_msg.header.frame_id = frame_id;
 89 |     out_msg.encoding        = sensor_msgs::image_encodings::TYPE_32FC1;
 90 |     out_msg.image           = depth.clone( );
 91 | 
 92 |     pub.publish( out_msg.toImageMsg( ) );
 93 | }
 94 | 
 95 | inline void
 96 | sendCloud( ros::Publisher& pub,
 97 |            const ros::Time timestamp,
 98 |            const cv::Mat& dense_points_,
 99 |            const cv::Mat& un_img_l0,
100 |            Eigen::Matrix3f K1,
101 |            Eigen::Matrix3f R_wc )
102 | {
103 | #define DOWNSAMPLE 0
104 |     const float DEP_INF = 1000.0f;
105 | 
106 |     sensor_msgs::PointCloud2Ptr points( new sensor_msgs::PointCloud2 );
107 |     points->header.stamp = timestamp;
108 |     ; // key_frame_data.header;
109 |     // TODO: since no publish tf, frame_id need to be modify
110 |     points->header.frame_id = "ref_frame";
111 | 
112 |     points->height = dense_points_.rows;
113 |     points->width  = dense_points_.cols;
114 |     points->fields.resize( 4 );
115 |     points->fields[0].name     = "x";
116 |     points->fields[0].offset   = 0;
117 |     points->fields[0].count    = 1;
118 |     points->fields[0].datatype = sensor_msgs::PointField::FLOAT32;
119 |     points->fields[1].name     = "y";
120 |     points->fields[1].offset   = 4;
121 |     points->fields[1].count    = 1;
122 |     points->fields[1].datatype = sensor_msgs::PointField::FLOAT32;
123 |     points->fields[2].name     = "z";
124 |     points->fields[2].offset   = 8;
125 |     points->fields[2].count    = 1;
126 |     points->fields[2].datatype = sensor_msgs::PointField::FLOAT32;
127 |     points->fields[3].name     = "rgb";
128 |     points->fields[3].offset   = 12;
129 |     points->fields[3].count    = 1;
130 |     points->fields[3].datatype = sensor_msgs::PointField::FLOAT32;
131 |     // points.is_bigendian = false; ???
132 |     points->point_step = 16;
133 |     points->row_step   = points->point_step * points->width;
134 |     points->data.resize( points->row_step * points->height );
135 |     points->is_dense = false; // there may be invalid points
136 | 
137 |     float fx = K1( 0, 0 );
138 |     float fy = K1( 1, 1 );
139 |     float cx = K1( 0, 2 );
140 |     float cy = K1( 1, 2 );
141 |     //    std::cout<<" fx "<< fx <<std::endl;
142 |     //    std::cout<<" fy "<< fy <<std::endl;
143 |     //    std::cout<<" cx "<< cx <<std::endl;
144 |     //    std::cout<<" cy "<< cy <<std::endl;
145 | 
146 |     float bad_point = std::numeric_limits< float >::quiet_NaN( );
147 |     int i           = 0;
148 |     //        std::cout<<" dense_points_.rows "<< dense_points_.rows <<std::endl;
149 |     //        std::cout<<" dense_points_.cols "<< dense_points_.cols <<std::endl;
150 |     //        std::cout<<" un_img_l0.rows "<< un_img_l0.rows <<std::endl;
151 |     //        std::cout<<" un_img_l0.cols "<< un_img_l0.cols <<std::endl;
152 |     for ( int32_t u = 0; u < dense_points_.rows; ++u )
153 |     {
154 |         for ( int32_t v = 0; v < dense_points_.cols; ++v, ++i )
155 |         {
156 |             float dep = dense_points_.at< float >( u, v );
157 |             // TODO: not correct here, since use desire focal length
158 |             // float x = dep * (v - K1.at<double>(0, 2) /*/ (1 << DOWNSAMPLE)*/) /
159 |             // (K1.at<double>(0, 0) /*/ (1
160 |             // << DOWNSAMPLE)*/);
161 |             // float y = dep * (u - K1.at<double>(1, 2) /*/ (1 << DOWNSAMPLE)*/) /
162 |             // (K1.at<double>(1, 1) /*/ (1
163 |             // << DOWNSAMPLE)*/);
164 | 
165 |             Eigen::Vector3f Point_c( dep * ( v - cx ) / fx, dep * ( u - cy ) / fy, dep );
166 |             Eigen::Vector3f Point_w = R_wc * Point_c;
167 | 
168 |             if ( dep > 5 )
169 |                 continue;
170 |             if ( dep < 0.95 )
171 |                 continue;
172 |             if ( Point_w( 2 ) > 0.5 )
173 |                 continue;
174 | 
175 |             if ( dep < 0 )
176 |                 continue;
177 |             if ( dep < DEP_INF )
178 |             {
179 |                 uint8_t g   = un_img_l0.at< uint8_t >( u, v );
180 |                 int32_t rgb = ( g << 16 ) | ( g << 8 ) | g;
181 |                 memcpy( &points->data[i * points->point_step + 0], &Point_w( 0 ), sizeof( float ) );
182 |                 memcpy( &points->data[i * points->point_step + 4], &Point_w( 1 ), sizeof( float ) );
183 |                 memcpy( &points->data[i * points->point_step + 8], &Point_w( 2 ), sizeof( float ) );
184 |                 memcpy( &points->data[i * points->point_step + 12], &rgb, sizeof( int32_t ) );
185 |             }
186 |             else
187 |             {
188 |                 memcpy( &points->data[i * points->point_step + 0], &bad_point, sizeof( float ) );
189 |                 memcpy( &points->data[i * points->point_step + 4], &bad_point, sizeof( float ) );
190 |                 memcpy( &points->data[i * points->point_step + 8], &bad_point, sizeof( float ) );
191 |                 memcpy( &points->data[i * points->point_step + 12], &bad_point, sizeof( float ) );
192 |             }
193 |         }
194 |     }
195 |     pub.publish( points );
196 | }
197 | 
198 | inline void
199 | sendCloud( ros::Publisher& pub,
200 |            const ros::Time timestamp,
201 |            const cv::Mat& dense_points_,
202 |            const cv::Mat& un_img_l0,
203 |            Eigen::Matrix3f K1,
204 |            Eigen::Matrix3f R_wc,
205 |            Eigen::Vector3f T_w )
206 | {
207 | #define DOWNSAMPLE 0
208 |     const float DEP_INF = 1000.0f;
209 | 
210 |     sensor_msgs::PointCloud2Ptr points( new sensor_msgs::PointCloud2 );
211 |     points->header.stamp = timestamp;
212 |     ; // key_frame_data.header;
213 |     // TODO: since no publish tf, frame_id need to be modify
214 |     points->header.frame_id = "ref_frame";
215 | 
216 |     points->height = dense_points_.rows;
217 |     points->width  = dense_points_.cols;
218 |     points->fields.resize( 4 );
219 |     points->fields[0].name     = "x";
220 |     points->fields[0].offset   = 0;
221 |     points->fields[0].count    = 1;
222 |     points->fields[0].datatype = sensor_msgs::PointField::FLOAT32;
223 |     points->fields[1].name     = "y";
224 |     points->fields[1].offset   = 4;
225 |     points->fields[1].count    = 1;
226 |     points->fields[1].datatype = sensor_msgs::PointField::FLOAT32;
227 |     points->fields[2].name     = "z";
228 |     points->fields[2].offset   = 8;
229 |     points->fields[2].count    = 1;
230 |     points->fields[2].datatype = sensor_msgs::PointField::FLOAT32;
231 |     points->fields[3].name     = "rgb";
232 |     points->fields[3].offset   = 12;
233 |     points->fields[3].count    = 1;
234 |     points->fields[3].datatype = sensor_msgs::PointField::FLOAT32;
235 |     points->is_bigendian       = false;
236 |     points->point_step         = sizeof( float ) * 4;
237 |     points->row_step           = points->point_step * points->width;
238 |     points->data.resize( points->row_step * points->height );
239 |     points->is_dense = true;
240 | 
241 |     float fx = K1( 0, 0 );
242 |     float fy = K1( 1, 1 );
243 |     float cx = K1( 0, 2 );
244 |     float cy = K1( 1, 2 );
245 |     //    std::cout<<" fx "<< fx <<std::endl;
246 |     //    std::cout<<" fy "<< fy <<std::endl;
247 |     //    std::cout<<" cx "<< cx <<std::endl;
248 |     //    std::cout<<" cy "<< cy <<std::endl;
249 | 
250 |     float bad_point = std::numeric_limits< float >::quiet_NaN( );
251 |     int i           = 0;
252 |     //        std::cout<<" dense_points_.rows "<< dense_points_.rows <<std::endl;
253 |     //        std::cout<<" dense_points_.cols "<< dense_points_.cols <<std::endl;
254 |     //        std::cout<<" un_img_l0.rows "<< un_img_l0.rows <<std::endl;
255 |     //        std::cout<<" un_img_l0.cols "<< un_img_l0.cols <<std::endl;
256 |     for ( int32_t u = 0; u < dense_points_.rows; ++u )
257 |     {
258 |         for ( int32_t v = 0; v < dense_points_.cols; ++v, ++i )
259 |         {
260 |             float dep = dense_points_.at< float >( u, v );
261 |             // TODO: not correct here, since use desire focal length
262 |             // float x = dep * (v - K1.at<double>(0, 2) /*/ (1 << DOWNSAMPLE)*/) /
263 |             // (K1.at<double>(0, 0) /*/ (1
264 |             // << DOWNSAMPLE)*/);
265 |             // float y = dep * (u - K1.at<double>(1, 2) /*/ (1 << DOWNSAMPLE)*/) /
266 |             // (K1.at<double>(1, 1) /*/ (1
267 |             // << DOWNSAMPLE)*/);
268 | 
269 |             Eigen::Vector3f Point_c( dep * ( v - cx ) / fx, dep * ( u - cy ) / fy, dep );
270 |             Eigen::Vector3f Point_w = R_wc * Point_c + T_w;
271 | 
272 |             // if ( dep > 5 )
273 |             //     continue;
274 |             // if ( dep < 0.95 )
275 |             //     continue;
276 |             // if ( Point_w( 2 ) > 0.5 )
277 |             //     continue;
278 | 
279 |             if ( dep < 0 )
280 |                 continue;
281 |             if ( dep < DEP_INF )
282 |             {
283 |                 uint8_t g   = un_img_l0.at< uint8_t >( u, v );
284 |                 int32_t rgb = ( g << 16 ) | ( g << 8 ) | g;
285 |                 memcpy( &points->data[i * points->point_step + 0], &Point_w( 0 ), sizeof( float ) );
286 |                 memcpy( &points->data[i * points->point_step + 4], &Point_w( 1 ), sizeof( float ) );
287 |                 memcpy( &points->data[i * points->point_step + 8], &Point_w( 2 ), sizeof( float ) );
288 |                 memcpy( &points->data[i * points->point_step + 12], &rgb, sizeof( int32_t ) );
289 |             }
290 |             else
291 |             {
292 |                 memcpy( &points->data[i * points->point_step + 0], &bad_point, sizeof( float ) );
293 |                 memcpy( &points->data[i * points->point_step + 4], &bad_point, sizeof( float ) );
294 |                 memcpy( &points->data[i * points->point_step + 8], &bad_point, sizeof( float ) );
295 |                 memcpy( &points->data[i * points->point_step + 12], &bad_point, sizeof( float ) );
296 |             }
297 |         }
298 |     }
299 |     pub.publish( points );
300 | }
301 | 
302 | inline void
303 | pointCloudPub( ros::Publisher& pub, cv::Mat depth, cv::Mat image )
304 | {
305 |     int w, h;
306 |     w = ( int )( depth.cols );
307 |     h = ( int )( depth.rows );
308 | 
309 |     sensor_msgs::PointCloud2 imagePoint;
310 |     imagePoint.header.stamp    = ros::Time::now( );
311 |     imagePoint.header.frame_id = "world";
312 |     imagePoint.height          = h;
313 |     imagePoint.width           = w;
314 |     imagePoint.fields.resize( 4 );
315 |     imagePoint.fields[0].name     = "x";
316 |     imagePoint.fields[0].offset   = 0;
317 |     imagePoint.fields[0].count    = 1;
318 |     imagePoint.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
319 |     imagePoint.fields[1].name     = "y";
320 |     imagePoint.fields[1].offset   = 4;
321 |     imagePoint.fields[1].count    = 1;
322 |     imagePoint.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
323 |     imagePoint.fields[2].name     = "z";
324 |     imagePoint.fields[2].offset   = 8;
325 |     imagePoint.fields[2].count    = 1;
326 |     imagePoint.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
327 |     imagePoint.fields[3].name     = "rgb";
328 |     imagePoint.fields[3].offset   = 12;
329 |     imagePoint.fields[3].count    = 1;
330 |     imagePoint.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
331 |     imagePoint.is_bigendian       = false;
332 |     imagePoint.point_step         = sizeof( float ) * 4;
333 |     imagePoint.row_step           = imagePoint.point_step * imagePoint.width;
334 |     imagePoint.data.resize( imagePoint.row_step * imagePoint.height );
335 |     imagePoint.is_dense = true;
336 | 
337 |     int i = 0;
338 |     for ( int row_index = 0; row_index < depth.rows; ++row_index )
339 |     {
340 |         for ( int col_index = 0; col_index < depth.cols; ++col_index, ++i )
341 |         {
342 |             float z = depth.at< float >( row_index, col_index );
343 |             float x = ( float )col_index * z;
344 |             float y = ( float )row_index * z;
345 | 
346 |             uint g;
347 |             int32_t rgb;
348 | 
349 |             g   = ( uchar )image.at< uchar >( row_index, col_index );
350 |             rgb = ( g << 16 ) | ( g << 8 ) | g;
351 | 
352 |             memcpy( &imagePoint.data[i * imagePoint.point_step + 0], &x, sizeof( float ) );
353 |             memcpy( &imagePoint.data[i * imagePoint.point_step + 4], &y, sizeof( float ) );
354 |             memcpy( &imagePoint.data[i * imagePoint.point_step + 8], &z, sizeof( float ) );
355 |             memcpy( &imagePoint.data[i * imagePoint.point_step + 12], &rgb, sizeof( int32_t ) );
356 |         }
357 |     }
358 | 
359 |     pub.publish( imagePoint );
360 | }
361 | }
362 | #endif // ROS_UTLS_H
363 | 


--------------------------------------------------------------------------------
/include/code_utils/backward.hpp:
--------------------------------------------------------------------------------
   1 | /*
   2 |  * backward.hpp
   3 |  * Copyright 2013 Google Inc. All Rights Reserved.
   4 |  *
   5 |  * Permission is hereby granted, free of charge, to any person obtaining a copy
   6 |  * of this software and associated documentation files (the "Software"), to deal
   7 |  * in the Software without restriction, including without limitation the rights
   8 |  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   9 |  * copies of the Software, and to permit persons to whom the Software is
  10 |  * furnished to do so, subject to the following conditions:
  11 |  *
  12 |  * The above copyright notice and this permission notice shall be included in
  13 |  * all copies or substantial portions of the Software.
  14 |  *
  15 |  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  16 |  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  17 |  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  18 |  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  19 |  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  20 |  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  21 |  * SOFTWARE.
  22 |  */
  23 | 
  24 | #ifndef H_6B9572DA_A64B_49E6_B234_051480991C89
  25 | #define H_6B9572DA_A64B_49E6_B234_051480991C89
  26 | 
  27 | #ifndef __cplusplus
  28 | #	error "It's not going to compile without a C++ compiler..."
  29 | #endif
  30 | 
  31 | #if	  defined(BACKWARD_CXX11)
  32 | #elif defined(BACKWARD_CXX98)
  33 | #else
  34 | #	if __cplusplus >= 201103L
  35 | #		define BACKWARD_CXX11
  36 | #		define BACKWARD_ATLEAST_CXX11
  37 | #		define BACKWARD_ATLEAST_CXX98
  38 | #	else
  39 | #		define BACKWARD_CXX98
  40 | #		define BACKWARD_ATLEAST_CXX98
  41 | #	endif
  42 | #endif
  43 | 
  44 | // You can define one of the following (or leave it to the auto-detection):
  45 | //
  46 | // #define BACKWARD_SYSTEM_LINUX
  47 | //	- specialization for linux
  48 | //
  49 | // #define BACKWARD_SYSTEM_DARWIN
  50 | //	- specialization for Mac OS X 10.5 and later.
  51 | //
  52 | // #define BACKWARD_SYSTEM_UNKNOWN
  53 | //	- placebo implementation, does nothing.
  54 | //
  55 | #if   defined(BACKWARD_SYSTEM_LINUX)
  56 | #elif defined(BACKWARD_SYSTEM_DARWIN)
  57 | #elif defined(BACKWARD_SYSTEM_UNKNOWN)
  58 | #else
  59 | #	if defined(__linux) || defined(__linux__)
  60 | #		define BACKWARD_SYSTEM_LINUX
  61 | #	elif defined(__APPLE__)
  62 | #		define BACKWARD_SYSTEM_DARWIN
  63 | #	else
  64 | #		define BACKWARD_SYSTEM_UNKNOWN
  65 | #	endif
  66 | #endif
  67 | 
  68 | #include <algorithm>
  69 | #include <cctype>
  70 | #include <cstdio>
  71 | #include <cstdlib>
  72 | #include <cstring>
  73 | #include <fstream>
  74 | #include <iomanip>
  75 | #include <iostream>
  76 | #include <new>
  77 | #include <sstream>
  78 | #include <streambuf>
  79 | #include <string>
  80 | #include <vector>
  81 | #include <limits>
  82 | 
  83 | #if defined(BACKWARD_SYSTEM_LINUX)
  84 | 
  85 | // On linux, backtrace can back-trace or "walk" the stack using the following
  86 | // libraries:
  87 | //
  88 | // #define BACKWARD_HAS_UNWIND 1
  89 | //  - unwind comes from libgcc, but I saw an equivalent inside clang itself.
  90 | //  - with unwind, the stacktrace is as accurate as it can possibly be, since
  91 | //  this is used by the C++ runtine in gcc/clang for stack unwinding on
  92 | //  exception.
  93 | //  - normally libgcc is already linked to your program by default.
  94 | //
  95 | // #define BACKWARD_HAS_BACKTRACE == 1
  96 | //  - backtrace seems to be a little bit more portable than libunwind, but on
  97 | //  linux, it uses unwind anyway, but abstract away a tiny information that is
  98 | //  sadly really important in order to get perfectly accurate stack traces.
  99 | //  - backtrace is part of the (e)glib library.
 100 | //
 101 | // The default is:
 102 | // #define BACKWARD_HAS_UNWIND == 1
 103 | //
 104 | // Note that only one of the define should be set to 1 at a time.
 105 | //
 106 | #	if   BACKWARD_HAS_UNWIND == 1
 107 | #	elif BACKWARD_HAS_BACKTRACE == 1
 108 | #	else
 109 | #		undef  BACKWARD_HAS_UNWIND
 110 | #		define BACKWARD_HAS_UNWIND 1
 111 | #		undef  BACKWARD_HAS_BACKTRACE
 112 | #		define BACKWARD_HAS_BACKTRACE 0
 113 | #	endif
 114 | 
 115 | // On linux, backward can extract detailed information about a stack trace
 116 | // using one of the following libraries:
 117 | //
 118 | // #define BACKWARD_HAS_DW 1
 119 | //  - libdw gives you the most juicy details out of your stack traces:
 120 | //    - object filename
 121 | //    - function name
 122 | //    - source filename
 123 | //	  - line and column numbers
 124 | //	  - source code snippet (assuming the file is accessible)
 125 | //	  - variables name and values (if not optimized out)
 126 | //  - You need to link with the lib "dw":
 127 | //    - apt-get install libdw-dev
 128 | //    - g++/clang++ -ldw ...
 129 | //
 130 | // #define BACKWARD_HAS_BFD 1
 131 | //  - With libbfd, you get a fair amount of details:
 132 | //    - object filename
 133 | //    - function name
 134 | //    - source filename
 135 | //	  - line numbers
 136 | //	  - source code snippet (assuming the file is accessible)
 137 | //  - You need to link with the lib "bfd":
 138 | //    - apt-get install binutils-dev
 139 | //    - g++/clang++ -lbfd ...
 140 | //
 141 | // #define BACKWARD_HAS_DWARF 1
 142 | //  - libdwarf gives you the most juicy details out of your stack traces:
 143 | //    - object filename
 144 | //    - function name
 145 | //    - source filename
 146 | //    - line and column numbers
 147 | //    - source code snippet (assuming the file is accessible)
 148 | //    - variables name and values (if not optimized out)
 149 | //  - You need to link with the lib "dwarf":
 150 | //    - apt-get install libdwarf-dev
 151 | //    - g++/clang++ -ldwarf ...
 152 | //
 153 | // #define BACKWARD_HAS_BACKTRACE_SYMBOL 1
 154 | //  - backtrace provides minimal details for a stack trace:
 155 | //    - object filename
 156 | //    - function name
 157 | //  - backtrace is part of the (e)glib library.
 158 | //
 159 | // The default is:
 160 | // #define BACKWARD_HAS_BACKTRACE_SYMBOL == 1
 161 | //
 162 | // Note that only one of the define should be set to 1 at a time.
 163 | //
 164 | #	if   BACKWARD_HAS_DW == 1
 165 | #	elif BACKWARD_HAS_BFD == 1
 166 | #   elif BACKWARD_HAS_DWARF == 1
 167 | #	elif BACKWARD_HAS_BACKTRACE_SYMBOL == 1
 168 | #	else
 169 | #		undef  BACKWARD_HAS_DW
 170 | #		define BACKWARD_HAS_DW 0
 171 | #		undef  BACKWARD_HAS_BFD
 172 | #		define BACKWARD_HAS_BFD 0
 173 | #		undef  BACKWARD_HAS_DWARF
 174 | #		define BACKWARD_HAS_DWARF 0
 175 | #		undef  BACKWARD_HAS_BACKTRACE_SYMBOL
 176 | #		define BACKWARD_HAS_BACKTRACE_SYMBOL 1
 177 | #	endif
 178 | 
 179 | #	include <cxxabi.h>
 180 | #	include <fcntl.h>
 181 | #	ifdef __ANDROID__
 182 | //		Old Android API levels define _Unwind_Ptr in both link.h and unwind.h
 183 | //		Rename the one in link.h as we are not going to be using it
 184 | #		define _Unwind_Ptr _Unwind_Ptr_Custom
 185 | #	include <link.h>
 186 | #		undef _Unwind_Ptr
 187 | #	else
 188 | #	    include <link.h>
 189 | #	endif
 190 | #	include <sys/stat.h>
 191 | #	include <syscall.h>
 192 | #	include <unistd.h>
 193 | #	include <signal.h>
 194 | 
 195 | #	if BACKWARD_HAS_BFD == 1
 196 | //              NOTE: defining PACKAGE{,_VERSION} is required before including
 197 | //                    bfd.h on some platforms, see also:
 198 | //                    https://sourceware.org/bugzilla/show_bug.cgi?id=14243
 199 | #               ifndef PACKAGE
 200 | #                       define PACKAGE
 201 | #               endif
 202 | #               ifndef PACKAGE_VERSION
 203 | #                       define PACKAGE_VERSION
 204 | #               endif
 205 | #		include <bfd.h>
 206 | #		ifndef _GNU_SOURCE
 207 | #			define _GNU_SOURCE
 208 | #			include <dlfcn.h>
 209 | #			undef _GNU_SOURCE
 210 | #		else
 211 | #			include <dlfcn.h>
 212 | #		endif
 213 | #	endif
 214 | 
 215 | #	if BACKWARD_HAS_DW == 1
 216 | #		include <elfutils/libdw.h>
 217 | #		include <elfutils/libdwfl.h>
 218 | #		include <dwarf.h>
 219 | #	endif
 220 | 
 221 | #	if BACKWARD_HAS_DWARF == 1
 222 | #		include <libelf.h>
 223 | #		include <dwarf.h>
 224 | #		include <libdwarf.h>
 225 | #		include <map>
 226 | #		include <algorithm>
 227 | #		ifndef _GNU_SOURCE
 228 | #			define _GNU_SOURCE
 229 | #			include <dlfcn.h>
 230 | #			undef _GNU_SOURCE
 231 | #		else
 232 | #			include <dlfcn.h>
 233 | #		endif
 234 | #	endif
 235 | 
 236 | #	if (BACKWARD_HAS_BACKTRACE == 1) || (BACKWARD_HAS_BACKTRACE_SYMBOL == 1)
 237 | 		// then we shall rely on backtrace
 238 | #		include <execinfo.h>
 239 | #	endif
 240 | 
 241 | #endif // defined(BACKWARD_SYSTEM_LINUX)
 242 | 
 243 | #if defined(BACKWARD_SYSTEM_DARWIN)
 244 | // On Darwin, backtrace can back-trace or "walk" the stack using the following
 245 | // libraries:
 246 | //
 247 | // #define BACKWARD_HAS_UNWIND 1
 248 | //  - unwind comes from libgcc, but I saw an equivalent inside clang itself.
 249 | //  - with unwind, the stacktrace is as accurate as it can possibly be, since
 250 | //  this is used by the C++ runtine in gcc/clang for stack unwinding on
 251 | //  exception.
 252 | //  - normally libgcc is already linked to your program by default.
 253 | //
 254 | // #define BACKWARD_HAS_BACKTRACE == 1
 255 | //  - backtrace is available by default, though it does not produce as much information
 256 | //  as another library might.
 257 | //
 258 | // The default is:
 259 | // #define BACKWARD_HAS_UNWIND == 1
 260 | //
 261 | // Note that only one of the define should be set to 1 at a time.
 262 | //
 263 | #	if   BACKWARD_HAS_UNWIND == 1
 264 | #	elif BACKWARD_HAS_BACKTRACE == 1
 265 | #	else
 266 | #		undef  BACKWARD_HAS_UNWIND
 267 | #		define BACKWARD_HAS_UNWIND 1
 268 | #		undef  BACKWARD_HAS_BACKTRACE
 269 | #		define BACKWARD_HAS_BACKTRACE 0
 270 | #	endif
 271 | 
 272 | // On Darwin, backward can extract detailed information about a stack trace
 273 | // using one of the following libraries:
 274 | //
 275 | // #define BACKWARD_HAS_BACKTRACE_SYMBOL 1
 276 | //  - backtrace provides minimal details for a stack trace:
 277 | //    - object filename
 278 | //    - function name
 279 | //
 280 | // The default is:
 281 | // #define BACKWARD_HAS_BACKTRACE_SYMBOL == 1
 282 | //
 283 | #	if BACKWARD_HAS_BACKTRACE_SYMBOL == 1
 284 | #	else
 285 | #		undef  BACKWARD_HAS_BACKTRACE_SYMBOL
 286 | #		define BACKWARD_HAS_BACKTRACE_SYMBOL 1
 287 | #	endif
 288 | 
 289 | #	include <cxxabi.h>
 290 | #	include <fcntl.h>
 291 | #	include <pthread.h>
 292 | #	include <sys/stat.h>
 293 | #	include <unistd.h>
 294 | #	include <signal.h>
 295 | 
 296 | #	if (BACKWARD_HAS_BACKTRACE == 1) || (BACKWARD_HAS_BACKTRACE_SYMBOL == 1)
 297 | #		include <execinfo.h>
 298 | #	endif
 299 | #endif // defined(BACKWARD_SYSTEM_DARWIN)
 300 | 
 301 | #if BACKWARD_HAS_UNWIND == 1
 302 | 
 303 | #	include <unwind.h>
 304 | // while gcc's unwind.h defines something like that:
 305 | //  extern _Unwind_Ptr _Unwind_GetIP (struct _Unwind_Context *);
 306 | //  extern _Unwind_Ptr _Unwind_GetIPInfo (struct _Unwind_Context *, int *);
 307 | //
 308 | // clang's unwind.h defines something like this:
 309 | //  uintptr_t _Unwind_GetIP(struct _Unwind_Context* __context);
 310 | //
 311 | // Even if the _Unwind_GetIPInfo can be linked to, it is not declared, worse we
 312 | // cannot just redeclare it because clang's unwind.h doesn't define _Unwind_Ptr
 313 | // anyway.
 314 | //
 315 | // Luckily we can play on the fact that the guard macros have a different name:
 316 | #ifdef __CLANG_UNWIND_H
 317 | // In fact, this function still comes from libgcc (on my different linux boxes,
 318 | // clang links against libgcc).
 319 | #	include <inttypes.h>
 320 | extern "C" uintptr_t _Unwind_GetIPInfo(_Unwind_Context*, int*);
 321 | #endif
 322 | 
 323 | #endif // BACKWARD_HAS_UNWIND == 1
 324 | 
 325 | #ifdef BACKWARD_ATLEAST_CXX11
 326 | #	include <unordered_map>
 327 | #	include <utility> // for std::swap
 328 | 	namespace backward {
 329 | 	namespace details {
 330 | 		template <typename K, typename V>
 331 | 		struct hashtable {
 332 | 			typedef std::unordered_map<K, V> type;
 333 | 		};
 334 | 		using std::move;
 335 | 	} // namespace details
 336 | 	} // namespace backward
 337 | #else // NOT BACKWARD_ATLEAST_CXX11
 338 | #	define override
 339 | #	include <map>
 340 | 	namespace backward {
 341 | 	namespace details {
 342 | 		template <typename K, typename V>
 343 | 		struct hashtable {
 344 | 			typedef std::map<K, V> type;
 345 | 		};
 346 | 		template <typename T>
 347 | 			const T& move(const T& v) { return v; }
 348 | 		template <typename T>
 349 | 			T& move(T& v) { return v; }
 350 | 	} // namespace details
 351 | 	} // namespace backward
 352 | #endif // BACKWARD_ATLEAST_CXX11
 353 | 
 354 | namespace backward {
 355 | 
 356 | namespace system_tag {
 357 | 	struct linux_tag; // seems that I cannot call that "linux" because the name
 358 | 	// is already defined... so I am adding _tag everywhere.
 359 | 	struct darwin_tag;
 360 | 	struct unknown_tag;
 361 | 
 362 | #if   defined(BACKWARD_SYSTEM_LINUX)
 363 | 	typedef linux_tag current_tag;
 364 | #elif defined(BACKWARD_SYSTEM_DARWIN)
 365 | 	typedef darwin_tag current_tag;
 366 | #elif defined(BACKWARD_SYSTEM_UNKNOWN)
 367 | 	typedef unknown_tag current_tag;
 368 | #else
 369 | #	error "May I please get my system defines?"
 370 | #endif
 371 | } // namespace system_tag
 372 | 
 373 | 
 374 | namespace trace_resolver_tag {
 375 | #if defined(BACKWARD_SYSTEM_LINUX)
 376 | 	struct libdw;
 377 | 	struct libbfd;
 378 | 	struct libdwarf;
 379 | 	struct backtrace_symbol;
 380 | 
 381 | #	if   BACKWARD_HAS_DW == 1
 382 | 		typedef libdw current;
 383 | #	elif BACKWARD_HAS_BFD == 1
 384 | 		typedef libbfd current;
 385 | #	elif BACKWARD_HAS_DWARF == 1
 386 | 		typedef libdwarf current;
 387 | #	elif BACKWARD_HAS_BACKTRACE_SYMBOL == 1
 388 | 		typedef backtrace_symbol current;
 389 | #	else
 390 | #		error "You shall not pass, until you know what you want."
 391 | #	endif
 392 | #elif defined(BACKWARD_SYSTEM_DARWIN)
 393 | 	struct backtrace_symbol;
 394 | 
 395 | #	if BACKWARD_HAS_BACKTRACE_SYMBOL == 1
 396 | 		typedef backtrace_symbol current;
 397 | #	else
 398 | #		error "You shall not pass, until you know what you want."
 399 | #	endif
 400 | #endif
 401 | } // namespace trace_resolver_tag
 402 | 
 403 | 
 404 | namespace details {
 405 | 
 406 | template <typename T>
 407 | 	struct rm_ptr { typedef T type; };
 408 | 
 409 | template <typename T>
 410 | 	struct rm_ptr<T*> { typedef T type; };
 411 | 
 412 | template <typename T>
 413 | 	struct rm_ptr<const T*> { typedef const T type; };
 414 | 
 415 | template <typename R, typename T, R (*F)(T)>
 416 | struct deleter {
 417 | 	template <typename U>
 418 | 		void operator()(U& ptr) const {
 419 | 			(*F)(ptr);
 420 | 		}
 421 | };
 422 | 
 423 | template <typename T>
 424 | struct default_delete {
 425 | 	void operator()(T& ptr) const {
 426 | 		delete ptr;
 427 | 	}
 428 | };
 429 | 
 430 | template <typename T, typename Deleter = deleter<void, void*, &::free> >
 431 | class handle {
 432 | 	struct dummy;
 433 | 	T    _val;
 434 | 	bool _empty;
 435 | 
 436 | #ifdef BACKWARD_ATLEAST_CXX11
 437 | 	handle(const handle&) = delete;
 438 | 	handle& operator=(const handle&) = delete;
 439 | #endif
 440 | 
 441 | public:
 442 | 	~handle() {
 443 | 		if (!_empty) {
 444 | 			Deleter()(_val);
 445 | 		}
 446 | 	}
 447 | 
 448 | 	explicit handle(): _val(), _empty(true) {}
 449 | 	explicit handle(T val): _val(val), _empty(false) { if(!_val) _empty = true; }
 450 | 
 451 | #ifdef BACKWARD_ATLEAST_CXX11
 452 | 	handle(handle&& from): _empty(true) {
 453 | 		swap(from);
 454 | 	}
 455 | 	handle& operator=(handle&& from) {
 456 | 		swap(from); return *this;
 457 | 	}
 458 | #else
 459 | 	explicit handle(const handle& from): _empty(true) {
 460 | 		// some sort of poor man's move semantic.
 461 | 		swap(const_cast<handle&>(from));
 462 | 	}
 463 | 	handle& operator=(const handle& from) {
 464 | 		// some sort of poor man's move semantic.
 465 | 		swap(const_cast<handle&>(from)); return *this;
 466 | 	}
 467 | #endif
 468 | 
 469 | 	void reset(T new_val) {
 470 | 		handle tmp(new_val);
 471 | 		swap(tmp);
 472 | 	}
 473 | 	operator const dummy*() const {
 474 | 		if (_empty) {
 475 | 			return 0;
 476 | 		}
 477 | 		return reinterpret_cast<const dummy*>(_val);
 478 | 	}
 479 | 	T get() {
 480 | 		return _val;
 481 | 	}
 482 | 	T release() {
 483 | 		_empty = true;
 484 | 		return _val;
 485 | 	}
 486 | 	void swap(handle& b) {
 487 | 		using std::swap;
 488 | 		swap(b._val, _val); // can throw, we are safe here.
 489 | 		swap(b._empty, _empty); // should not throw: if you cannot swap two
 490 | 		// bools without throwing... It's a lost cause anyway!
 491 | 	}
 492 | 
 493 | 	T operator->() { return _val; }
 494 | 	const T operator->() const { return _val; }
 495 | 
 496 | 	typedef typename rm_ptr<T>::type& ref_t;
 497 | 	typedef const typename rm_ptr<T>::type& const_ref_t;
 498 | 	ref_t operator*() { return *_val; }
 499 | 	const_ref_t operator*() const { return *_val; }
 500 | 	ref_t operator[](size_t idx) { return _val[idx]; }
 501 | 
 502 | 	// Watch out, we've got a badass over here
 503 | 	T* operator&() {
 504 | 		_empty = false;
 505 | 		return &_val;
 506 | 	}
 507 | };
 508 | 
 509 | // Default demangler implementation (do nothing).
 510 | template <typename TAG>
 511 | struct demangler_impl {
 512 | 	static std::string demangle(const char* funcname) {
 513 | 		return funcname;
 514 | 	}
 515 | };
 516 | 
 517 | #if defined(BACKWARD_SYSTEM_LINUX) || defined(BACKWARD_SYSTEM_DARWIN)
 518 | 
 519 | template <>
 520 | struct demangler_impl<system_tag::current_tag> {
 521 | 	demangler_impl(): _demangle_buffer_length(0) {}
 522 | 
 523 | 	std::string demangle(const char* funcname) {
 524 | 		using namespace details;
 525 | 		char* result = abi::__cxa_demangle(funcname,
 526 | 			_demangle_buffer.release(), &_demangle_buffer_length, 0);
 527 | 		if(result) {
 528 | 			_demangle_buffer.reset(result);
 529 | 			return result;
 530 | 		}
 531 | 		return funcname;
 532 | 	}
 533 | 
 534 | private:
 535 | 	details::handle<char*> _demangle_buffer;
 536 | 	size_t                 _demangle_buffer_length;
 537 | };
 538 | 
 539 | #endif // BACKWARD_SYSTEM_LINUX || BACKWARD_SYSTEM_DARWIN
 540 | 
 541 | struct demangler:
 542 | 	public demangler_impl<system_tag::current_tag> {};
 543 | 
 544 | } // namespace details
 545 | 
 546 | /*************** A TRACE ***************/
 547 | 
 548 | struct Trace {
 549 | 	void*    addr;
 550 | 	size_t   idx;
 551 | 
 552 | 	Trace():
 553 | 		addr(0), idx(0) {}
 554 | 
 555 | 	explicit Trace(void* _addr, size_t _idx):
 556 | 		addr(_addr), idx(_idx) {}
 557 | };
 558 | 
 559 | struct ResolvedTrace: public Trace {
 560 | 
 561 | 	struct SourceLoc {
 562 | 		std::string function;
 563 | 		std::string filename;
 564 | 		unsigned    line;
 565 | 		unsigned    col;
 566 | 
 567 | 		SourceLoc(): line(0), col(0) {}
 568 | 
 569 | 		bool operator==(const SourceLoc& b) const {
 570 | 			return function == b.function
 571 | 				&& filename == b.filename
 572 | 				&& line == b.line
 573 | 				&& col == b.col;
 574 | 		}
 575 | 
 576 | 		bool operator!=(const SourceLoc& b) const {
 577 | 			return !(*this == b);
 578 | 		}
 579 | 	};
 580 | 
 581 | 	// In which binary object this trace is located.
 582 | 	std::string                    object_filename;
 583 | 
 584 | 	// The function in the object that contain the trace. This is not the same
 585 | 	// as source.function which can be an function inlined in object_function.
 586 | 	std::string                    object_function;
 587 | 
 588 | 	// The source location of this trace. It is possible for filename to be
 589 | 	// empty and for line/col to be invalid (value 0) if this information
 590 | 	// couldn't be deduced, for example if there is no debug information in the
 591 | 	// binary object.
 592 | 	SourceLoc                      source;
 593 | 
 594 | 	// An optionals list of "inliners". All the successive sources location
 595 | 	// from where the source location of the trace (the attribute right above)
 596 | 	// is inlined. It is especially useful when you compiled with optimization.
 597 | 	typedef std::vector<SourceLoc> source_locs_t;
 598 | 	source_locs_t                  inliners;
 599 | 
 600 | 	ResolvedTrace():
 601 | 		Trace() {}
 602 | 	ResolvedTrace(const Trace& mini_trace):
 603 | 		Trace(mini_trace) {}
 604 | };
 605 | 
 606 | /*************** STACK TRACE ***************/
 607 | 
 608 | // default implemention.
 609 | template <typename TAG>
 610 | class StackTraceImpl {
 611 | public:
 612 | 	size_t size() const { return 0; }
 613 | 	Trace operator[](size_t) { return Trace(); }
 614 | 	size_t load_here(size_t=0) { return 0; }
 615 | 	size_t load_from(void*, size_t=0) { return 0; }
 616 | 	size_t thread_id() const { return 0; }
 617 | 	void skip_n_firsts(size_t) { }
 618 | };
 619 | 
 620 | class StackTraceImplBase {
 621 | public:
 622 | 	StackTraceImplBase(): _thread_id(0), _skip(0) {}
 623 | 
 624 | 	size_t thread_id() const {
 625 | 		return _thread_id;
 626 | 	}
 627 | 
 628 | 	void skip_n_firsts(size_t n) { _skip = n; }
 629 | 
 630 | protected:
 631 | 	void load_thread_info() {
 632 | #ifdef BACKWARD_SYSTEM_LINUX
 633 | #ifndef __ANDROID__
 634 | 		_thread_id = (size_t)syscall(SYS_gettid);
 635 | #else
 636 | 		_thread_id = (size_t)gettid();
 637 | #endif
 638 | 		if (_thread_id == (size_t) getpid()) {
 639 | 			// If the thread is the main one, let's hide that.
 640 | 			// I like to keep little secret sometimes.
 641 | 			_thread_id = 0;
 642 | 		}
 643 | #elif defined(BACKWARD_SYSTEM_DARWIN)
 644 | 		_thread_id = reinterpret_cast<size_t>(pthread_self());
 645 | 		if (pthread_main_np() == 1) {
 646 | 			// If the thread is the main one, let's hide that.
 647 | 			_thread_id = 0;
 648 | 		}
 649 | #endif
 650 | 	}
 651 | 
 652 | 	size_t skip_n_firsts() const { return _skip; }
 653 | 
 654 | private:
 655 | 	size_t _thread_id;
 656 | 	size_t _skip;
 657 | };
 658 | 
 659 | class StackTraceImplHolder: public StackTraceImplBase {
 660 | public:
 661 | 	size_t size() const {
 662 | 		return _stacktrace.size() ? _stacktrace.size() - skip_n_firsts() : 0;
 663 | 	}
 664 | 	Trace operator[](size_t idx) const {
 665 | 		if (idx >= size()) {
 666 | 			return Trace();
 667 | 		}
 668 | 		return Trace(_stacktrace[idx + skip_n_firsts()], idx);
 669 | 	}
 670 | 	void* const* begin() const {
 671 | 		if (size()) {
 672 | 			return &_stacktrace[skip_n_firsts()];
 673 | 		}
 674 | 		return 0;
 675 | 	}
 676 | 
 677 | protected:
 678 | 	std::vector<void*> _stacktrace;
 679 | };
 680 | 
 681 | 
 682 | #if BACKWARD_HAS_UNWIND == 1
 683 | 
 684 | namespace details {
 685 | 
 686 | template <typename F>
 687 | class Unwinder {
 688 | public:
 689 | 	size_t operator()(F& f, size_t depth) {
 690 | 		_f = &f;
 691 | 		_index = -1;
 692 | 		_depth = depth;
 693 | 		_Unwind_Backtrace(&this->backtrace_trampoline, this);
 694 | 		return _index;
 695 | 	}
 696 | 
 697 | private:
 698 | 	F*      _f;
 699 | 	ssize_t _index;
 700 | 	size_t  _depth;
 701 | 
 702 | 	static _Unwind_Reason_Code backtrace_trampoline(
 703 | 			_Unwind_Context* ctx, void *self) {
 704 | 		return ((Unwinder*)self)->backtrace(ctx);
 705 | 	}
 706 | 
 707 | 	_Unwind_Reason_Code backtrace(_Unwind_Context* ctx) {
 708 | 		if (_index >= 0 && static_cast<size_t>(_index) >= _depth)
 709 | 			return _URC_END_OF_STACK;
 710 | 
 711 | 		int ip_before_instruction = 0;
 712 | 		uintptr_t ip = _Unwind_GetIPInfo(ctx, &ip_before_instruction);
 713 | 
 714 | 		if (!ip_before_instruction) {
 715 | 			// calculating 0-1 for unsigned, looks like a possible bug to sanitiziers, so let's do it explicitly:
 716 | 			if (ip==0) {
 717 | 				ip = std::numeric_limits<uintptr_t>::max(); // set it to 0xffff... (as from casting 0-1)
 718 | 			} else {
 719 | 				ip -= 1; // else just normally decrement it (no overflow/underflow will happen)
 720 | 			}
 721 | 		}
 722 | 
 723 | 		if (_index >= 0) { // ignore first frame.
 724 | 			(*_f)(_index, (void*)ip);
 725 | 		}
 726 | 		_index += 1;
 727 | 		return _URC_NO_REASON;
 728 | 	}
 729 | };
 730 | 
 731 | template <typename F>
 732 | size_t unwind(F f, size_t depth) {
 733 | 	Unwinder<F> unwinder;
 734 | 	return unwinder(f, depth);
 735 | }
 736 | 
 737 | } // namespace details
 738 | 
 739 | 
 740 | template <>
 741 | class StackTraceImpl<system_tag::current_tag>: public StackTraceImplHolder {
 742 | public:
 743 | 	__attribute__ ((noinline)) // TODO use some macro
 744 | 	size_t load_here(size_t depth=32) {
 745 | 		load_thread_info();
 746 | 		if (depth == 0) {
 747 | 			return 0;
 748 | 		}
 749 | 		_stacktrace.resize(depth);
 750 | 		size_t trace_cnt = details::unwind(callback(*this), depth);
 751 | 		_stacktrace.resize(trace_cnt);
 752 | 		skip_n_firsts(0);
 753 | 		return size();
 754 | 	}
 755 | 	size_t load_from(void* addr, size_t depth=32) {
 756 | 		load_here(depth + 8);
 757 | 
 758 | 		for (size_t i = 0; i < _stacktrace.size(); ++i) {
 759 | 			if (_stacktrace[i] == addr) {
 760 | 				skip_n_firsts(i);
 761 | 				break;
 762 | 			}
 763 | 		}
 764 | 
 765 | 		_stacktrace.resize(std::min(_stacktrace.size(),
 766 | 					skip_n_firsts() + depth));
 767 | 		return size();
 768 | 	}
 769 | 
 770 | private:
 771 | 	struct callback {
 772 | 		StackTraceImpl& self;
 773 | 		callback(StackTraceImpl& _self): self(_self) {}
 774 | 
 775 | 		void operator()(size_t idx, void* addr) {
 776 | 			self._stacktrace[idx] = addr;
 777 | 		}
 778 | 	};
 779 | };
 780 | 
 781 | 
 782 | #else // BACKWARD_HAS_UNWIND == 0
 783 | 
 784 | template <>
 785 | class StackTraceImpl<system_tag::current_tag>: public StackTraceImplHolder {
 786 | public:
 787 | 	__attribute__ ((noinline)) // TODO use some macro
 788 | 	size_t load_here(size_t depth=32) {
 789 | 		load_thread_info();
 790 | 		if (depth == 0) {
 791 | 			return 0;
 792 | 		}
 793 | 		_stacktrace.resize(depth + 1);
 794 | 		size_t trace_cnt = backtrace(&_stacktrace[0], _stacktrace.size());
 795 | 		_stacktrace.resize(trace_cnt);
 796 | 		skip_n_firsts(1);
 797 | 		return size();
 798 | 	}
 799 | 
 800 | 	size_t load_from(void* addr, size_t depth=32) {
 801 | 		load_here(depth + 8);
 802 | 
 803 | 		for (size_t i = 0; i < _stacktrace.size(); ++i) {
 804 | 			if (_stacktrace[i] == addr) {
 805 | 				skip_n_firsts(i);
 806 | 				_stacktrace[i] = (void*)( (uintptr_t)_stacktrace[i] + 1);
 807 | 				break;
 808 | 			}
 809 | 		}
 810 | 
 811 | 		_stacktrace.resize(std::min(_stacktrace.size(),
 812 | 					skip_n_firsts() + depth));
 813 | 		return size();
 814 | 	}
 815 | };
 816 | 
 817 | #endif // BACKWARD_HAS_UNWIND
 818 | 
 819 | class StackTrace:
 820 | 	public StackTraceImpl<system_tag::current_tag> {};
 821 | 
 822 | /*************** TRACE RESOLVER ***************/
 823 | 
 824 | template <typename TAG>
 825 | class TraceResolverImpl;
 826 | 
 827 | #ifdef BACKWARD_SYSTEM_UNKNOWN
 828 | 
 829 | template <>
 830 | class TraceResolverImpl<system_tag::unknown_tag> {
 831 | public:
 832 | 	template <class ST>
 833 | 		void load_stacktrace(ST&) {}
 834 | 	ResolvedTrace resolve(ResolvedTrace t) {
 835 | 		return t;
 836 | 	}
 837 | };
 838 | 
 839 | #endif
 840 | 
 841 | class TraceResolverImplBase {
 842 | protected:
 843 | 	std::string demangle(const char* funcname) {
 844 | 		return _demangler.demangle(funcname);
 845 | 	}
 846 | 
 847 | private:
 848 | 	details::demangler _demangler;
 849 | };
 850 | 
 851 | #ifdef BACKWARD_SYSTEM_LINUX
 852 | 
 853 | template <typename STACKTRACE_TAG>
 854 | class TraceResolverLinuxImpl;
 855 | 
 856 | #if BACKWARD_HAS_BACKTRACE_SYMBOL == 1
 857 | 
 858 | template <>
 859 | class TraceResolverLinuxImpl<trace_resolver_tag::backtrace_symbol>:
 860 | 	public TraceResolverImplBase {
 861 | public:
 862 | 	template <class ST>
 863 | 		void load_stacktrace(ST& st) {
 864 | 			using namespace details;
 865 | 			if (st.size() == 0) {
 866 | 				return;
 867 | 			}
 868 | 			_symbols.reset(
 869 | 					backtrace_symbols(st.begin(), (int)st.size())
 870 | 					);
 871 | 		}
 872 | 
 873 | 	ResolvedTrace resolve(ResolvedTrace trace) {
 874 | 		char* filename = _symbols[trace.idx];
 875 | 		char* funcname = filename;
 876 | 		while (*funcname && *funcname != '(') {
 877 | 			funcname += 1;
 878 | 		}
 879 | 		trace.object_filename.assign(filename, funcname); // ok even if funcname is the ending \0 (then we assign entire string)
 880 | 
 881 | 		if (*funcname) { // if it's not end of string (e.g. from last frame ip==0)
 882 | 			funcname += 1;
 883 | 			char* funcname_end = funcname;
 884 | 			while (*funcname_end && *funcname_end != ')' && *funcname_end != '+') {
 885 | 				funcname_end += 1;
 886 | 			}
 887 | 			*funcname_end = '\0';
 888 | 			trace.object_function = this->demangle(funcname);
 889 | 			trace.source.function = trace.object_function; // we cannot do better.
 890 | 		}
 891 | 		return trace;
 892 | 	}
 893 | 
 894 | private:
 895 | 	details::handle<char**> _symbols;
 896 | };
 897 | 
 898 | #endif // BACKWARD_HAS_BACKTRACE_SYMBOL == 1
 899 | 
 900 | #if BACKWARD_HAS_BFD == 1
 901 | 
 902 | template <>
 903 | class TraceResolverLinuxImpl<trace_resolver_tag::libbfd>:
 904 | 	public TraceResolverImplBase {
 905 | 	static std::string read_symlink(std::string const & symlink_path) {
 906 | 		std::string path;
 907 | 		path.resize(100);
 908 | 
 909 | 		while(true) {
 910 | 			ssize_t len = ::readlink(symlink_path.c_str(), &*path.begin(), path.size());
 911 | 			if(len < 0) {
 912 | 				return "";
 913 | 			}
 914 | 			if ((size_t)len == path.size()) {
 915 | 				path.resize(path.size() * 2);
 916 | 			}
 917 | 			else {
 918 | 				path.resize(len);
 919 | 				break;
 920 | 			}
 921 | 		}
 922 | 
 923 | 		return path;
 924 | 	}
 925 | public:
 926 | 	TraceResolverLinuxImpl(): _bfd_loaded(false) {}
 927 | 
 928 | 	template <class ST>
 929 | 		void load_stacktrace(ST&) {}
 930 | 
 931 | 	ResolvedTrace resolve(ResolvedTrace trace) {
 932 | 		Dl_info symbol_info;
 933 | 
 934 | 		// trace.addr is a virtual address in memory pointing to some code.
 935 | 		// Let's try to find from which loaded object it comes from.
 936 | 		// The loaded object can be yourself btw.
 937 | 		if (!dladdr(trace.addr, &symbol_info)) {
 938 | 			return trace; // dat broken trace...
 939 | 		}
 940 | 
 941 | 		std::string argv0;
 942 | 		{
 943 | 			std::ifstream ifs("/proc/self/cmdline");
 944 | 			std::getline(ifs, argv0, '\0');
 945 | 		}
 946 | 		std::string tmp;
 947 | 		if(symbol_info.dli_fname == argv0) {
 948 | 			tmp = read_symlink("/proc/self/exe");
 949 | 			symbol_info.dli_fname = tmp.c_str();
 950 | 		}
 951 | 
 952 | 		// Now we get in symbol_info:
 953 | 		// .dli_fname:
 954 | 		//		pathname of the shared object that contains the address.
 955 | 		// .dli_fbase:
 956 | 		//		where the object is loaded in memory.
 957 | 		// .dli_sname:
 958 | 		//		the name of the nearest symbol to trace.addr, we expect a
 959 | 		//		function name.
 960 | 		// .dli_saddr:
 961 | 		//		the exact address corresponding to .dli_sname.
 962 | 
 963 | 		if (symbol_info.dli_sname) {
 964 | 			trace.object_function = demangle(symbol_info.dli_sname);
 965 | 		}
 966 | 
 967 | 		if (!symbol_info.dli_fname) {
 968 | 			return trace;
 969 | 		}
 970 | 
 971 | 		trace.object_filename = symbol_info.dli_fname;
 972 | 		bfd_fileobject& fobj = load_object_with_bfd(symbol_info.dli_fname);
 973 | 		if (!fobj.handle) {
 974 | 			return trace; // sad, we couldn't load the object :(
 975 | 		}
 976 | 
 977 | 
 978 | 		find_sym_result* details_selected; // to be filled.
 979 | 
 980 | 		// trace.addr is the next instruction to be executed after returning
 981 | 		// from the nested stack frame. In C++ this usually relate to the next
 982 | 		// statement right after the function call that leaded to a new stack
 983 | 		// frame. This is not usually what you want to see when printing out a
 984 | 		// stacktrace...
 985 | 		find_sym_result details_call_site = find_symbol_details(fobj,
 986 | 				trace.addr, symbol_info.dli_fbase);
 987 | 		details_selected = &details_call_site;
 988 | 
 989 | #if BACKWARD_HAS_UNWIND == 0
 990 | 		// ...this is why we also try to resolve the symbol that is right
 991 | 		// before the return address. If we are lucky enough, we will get the
 992 | 		// line of the function that was called. But if the code is optimized,
 993 | 		// we might get something absolutely not related since the compiler
 994 | 		// can reschedule the return address with inline functions and
 995 | 		// tail-call optimisation (among other things that I don't even know
 996 | 		// or cannot even dream about with my tiny limited brain).
 997 | 		find_sym_result details_adjusted_call_site = find_symbol_details(fobj,
 998 | 				(void*) (uintptr_t(trace.addr) - 1),
 999 | 				symbol_info.dli_fbase);
1000 | 
1001 | 		// In debug mode, we should always get the right thing(TM).
1002 | 		if (details_call_site.found && details_adjusted_call_site.found) {
1003 | 			// Ok, we assume that details_adjusted_call_site is a better estimation.
1004 | 			details_selected = &details_adjusted_call_site;
1005 | 			trace.addr = (void*) (uintptr_t(trace.addr) - 1);
1006 | 		}
1007 | 
1008 | 		if (details_selected == &details_call_site && details_call_site.found) {
1009 | 			// we have to re-resolve the symbol in order to reset some
1010 | 			// internal state in BFD... so we can call backtrace_inliners
1011 | 			// thereafter...
1012 | 			details_call_site = find_symbol_details(fobj, trace.addr,
1013 | 					symbol_info.dli_fbase);
1014 | 		}
1015 | #endif // BACKWARD_HAS_UNWIND
1016 | 
1017 | 		if (details_selected->found) {
1018 | 			if (details_selected->filename) {
1019 | 				trace.source.filename = details_selected->filename;
1020 | 			}
1021 | 			trace.source.line = details_selected->line;
1022 | 
1023 | 			if (details_selected->funcname) {
1024 | 				// this time we get the name of the function where the code is
1025 | 				// located, instead of the function were the address is
1026 | 				// located. In short, if the code was inlined, we get the
1027 | 				// function correspoding to the code. Else we already got in
1028 | 				// trace.function.
1029 | 				trace.source.function = demangle(details_selected->funcname);
1030 | 
1031 | 				if (!symbol_info.dli_sname) {
1032 | 					// for the case dladdr failed to find the symbol name of
1033 | 					// the function, we might as well try to put something
1034 | 					// here.
1035 | 					trace.object_function = trace.source.function;
1036 | 				}
1037 | 			}
1038 | 
1039 | 			// Maybe the source of the trace got inlined inside the function
1040 | 			// (trace.source.function). Let's see if we can get all the inlined
1041 | 			// calls along the way up to the initial call site.
1042 | 			trace.inliners = backtrace_inliners(fobj, *details_selected);
1043 | 
1044 | #if 0
1045 | 			if (trace.inliners.size() == 0) {
1046 | 				// Maybe the trace was not inlined... or maybe it was and we
1047 | 				// are lacking the debug information. Let's try to make the
1048 | 				// world better and see if we can get the line number of the
1049 | 				// function (trace.source.function) now.
1050 | 				//
1051 | 				// We will get the location of where the function start (to be
1052 | 				// exact: the first instruction that really start the
1053 | 				// function), not where the name of the function is defined.
1054 | 				// This can be quite far away from the name of the function
1055 | 				// btw.
1056 | 				//
1057 | 				// If the source of the function is the same as the source of
1058 | 				// the trace, we cannot say if the trace was really inlined or
1059 | 				// not.  However, if the filename of the source is different
1060 | 				// between the function and the trace... we can declare it as
1061 | 				// an inliner.  This is not 100% accurate, but better than
1062 | 				// nothing.
1063 | 
1064 | 				if (symbol_info.dli_saddr) {
1065 | 					find_sym_result details = find_symbol_details(fobj,
1066 | 							symbol_info.dli_saddr,
1067 | 							symbol_info.dli_fbase);
1068 | 
1069 | 					if (details.found) {
1070 | 						ResolvedTrace::SourceLoc diy_inliner;
1071 | 						diy_inliner.line = details.line;
1072 | 						if (details.filename) {
1073 | 							diy_inliner.filename = details.filename;
1074 | 						}
1075 | 						if (details.funcname) {
1076 | 							diy_inliner.function = demangle(details.funcname);
1077 | 						} else {
1078 | 							diy_inliner.function = trace.source.function;
1079 | 						}
1080 | 						if (diy_inliner != trace.source) {
1081 | 							trace.inliners.push_back(diy_inliner);
1082 | 						}
1083 | 					}
1084 | 				}
1085 | 			}
1086 | #endif
1087 | 		}
1088 | 
1089 | 		return trace;
1090 | 	}
1091 | 
1092 | private:
1093 | 	bool                _bfd_loaded;
1094 | 
1095 | 	typedef details::handle<bfd*,
1096 | 			details::deleter<bfd_boolean, bfd*, &bfd_close>
1097 | 				> bfd_handle_t;
1098 | 
1099 | 	typedef details::handle<asymbol**> bfd_symtab_t;
1100 | 
1101 | 
1102 | 	struct bfd_fileobject {
1103 | 		bfd_handle_t handle;
1104 | 		bfd_vma      base_addr;
1105 | 		bfd_symtab_t symtab;
1106 | 		bfd_symtab_t dynamic_symtab;
1107 | 	};
1108 | 
1109 | 	typedef details::hashtable<std::string, bfd_fileobject>::type
1110 | 		fobj_bfd_map_t;
1111 | 	fobj_bfd_map_t      _fobj_bfd_map;
1112 | 
1113 | 	bfd_fileobject& load_object_with_bfd(const std::string& filename_object) {
1114 | 		using namespace details;
1115 | 
1116 | 		if (!_bfd_loaded) {
1117 | 			using namespace details;
1118 | 			bfd_init();
1119 | 			_bfd_loaded = true;
1120 | 		}
1121 | 
1122 | 		fobj_bfd_map_t::iterator it =
1123 | 			_fobj_bfd_map.find(filename_object);
1124 | 		if (it != _fobj_bfd_map.end()) {
1125 | 			return it->second;
1126 | 		}
1127 | 
1128 | 		// this new object is empty for now.
1129 | 		bfd_fileobject& r = _fobj_bfd_map[filename_object];
1130 | 
1131 | 		// we do the work temporary in this one;
1132 | 		bfd_handle_t bfd_handle;
1133 | 
1134 | 		int fd = open(filename_object.c_str(), O_RDONLY);
1135 | 		bfd_handle.reset(
1136 | 				bfd_fdopenr(filename_object.c_str(), "default", fd)
1137 | 				);
1138 | 		if (!bfd_handle) {
1139 | 			close(fd);
1140 | 			return r;
1141 | 		}
1142 | 
1143 | 		if (!bfd_check_format(bfd_handle.get(), bfd_object)) {
1144 | 			return r; // not an object? You lose.
1145 | 		}
1146 | 
1147 | 		if ((bfd_get_file_flags(bfd_handle.get()) & HAS_SYMS) == 0) {
1148 | 			return r; // that's what happen when you forget to compile in debug.
1149 | 		}
1150 | 
1151 | 		ssize_t symtab_storage_size =
1152 | 			bfd_get_symtab_upper_bound(bfd_handle.get());
1153 | 
1154 | 		ssize_t dyn_symtab_storage_size =
1155 | 			bfd_get_dynamic_symtab_upper_bound(bfd_handle.get());
1156 | 
1157 | 		if (symtab_storage_size <= 0 && dyn_symtab_storage_size <= 0) {
1158 | 			return r; // weird, is the file is corrupted?
1159 | 		}
1160 | 
1161 | 		bfd_symtab_t symtab, dynamic_symtab;
1162 | 		ssize_t symcount = 0, dyn_symcount = 0;
1163 | 
1164 | 		if (symtab_storage_size > 0) {
1165 | 			symtab.reset(
1166 | 					(bfd_symbol**) malloc(symtab_storage_size)
1167 | 					);
1168 | 			symcount = bfd_canonicalize_symtab(
1169 | 					bfd_handle.get(), symtab.get()
1170 | 					);
1171 | 		}
1172 | 
1173 | 		if (dyn_symtab_storage_size > 0) {
1174 | 			dynamic_symtab.reset(
1175 | 					(bfd_symbol**) malloc(dyn_symtab_storage_size)
1176 | 					);
1177 | 			dyn_symcount = bfd_canonicalize_dynamic_symtab(
1178 | 					bfd_handle.get(), dynamic_symtab.get()
1179 | 					);
1180 | 		}
1181 | 
1182 | 
1183 | 		if (symcount <= 0 && dyn_symcount <= 0) {
1184 | 			return r; // damned, that's a stripped file that you got there!
1185 | 		}
1186 | 
1187 | 		r.handle = move(bfd_handle);
1188 | 		r.symtab = move(symtab);
1189 | 		r.dynamic_symtab = move(dynamic_symtab);
1190 | 		return r;
1191 | 	}
1192 | 
1193 | 	struct find_sym_result {
1194 | 		bool found;
1195 | 		const char* filename;
1196 | 		const char* funcname;
1197 | 		unsigned int line;
1198 | 	};
1199 | 
1200 | 	struct find_sym_context {
1201 | 		TraceResolverLinuxImpl* self;
1202 | 		bfd_fileobject* fobj;
1203 | 		void* addr;
1204 | 		void* base_addr;
1205 | 		find_sym_result result;
1206 | 	};
1207 | 
1208 | 	find_sym_result find_symbol_details(bfd_fileobject& fobj, void* addr,
1209 | 			void* base_addr) {
1210 | 		find_sym_context context;
1211 | 		context.self = this;
1212 | 		context.fobj = &fobj;
1213 | 		context.addr = addr;
1214 | 		context.base_addr = base_addr;
1215 | 		context.result.found = false;
1216 | 		bfd_map_over_sections(fobj.handle.get(), &find_in_section_trampoline,
1217 | 				(void*)&context);
1218 | 		return context.result;
1219 | 	}
1220 | 
1221 | 	static void find_in_section_trampoline(bfd*, asection* section,
1222 | 			void* data) {
1223 | 		find_sym_context* context = static_cast<find_sym_context*>(data);
1224 | 		context->self->find_in_section(
1225 | 				reinterpret_cast<bfd_vma>(context->addr),
1226 | 				reinterpret_cast<bfd_vma>(context->base_addr),
1227 | 				*context->fobj,
1228 | 				section, context->result
1229 | 				);
1230 | 	}
1231 | 
1232 | 	void find_in_section(bfd_vma addr, bfd_vma base_addr,
1233 | 			bfd_fileobject& fobj, asection* section, find_sym_result& result)
1234 | 	{
1235 | 		if (result.found) return;
1236 | 
1237 | 		if ((bfd_get_section_flags(fobj.handle.get(), section)
1238 | 					& SEC_ALLOC) == 0)
1239 | 			return; // a debug section is never loaded automatically.
1240 | 
1241 | 		bfd_vma sec_addr = bfd_get_section_vma(fobj.handle.get(), section);
1242 | 		bfd_size_type size = bfd_get_section_size(section);
1243 | 
1244 | 		// are we in the boundaries of the section?
1245 | 		if (addr < sec_addr || addr >= sec_addr + size) {
1246 | 			addr -= base_addr; // oups, a relocated object, lets try again...
1247 | 			if (addr < sec_addr || addr >= sec_addr + size) {
1248 | 				return;
1249 | 			}
1250 | 		}
1251 | 
1252 | 		if (!result.found && fobj.symtab) {
1253 | 			result.found = bfd_find_nearest_line(fobj.handle.get(), section,
1254 | 					fobj.symtab.get(), addr - sec_addr, &result.filename,
1255 | 					&result.funcname, &result.line);
1256 | 		}
1257 | 
1258 | 		if (!result.found && fobj.dynamic_symtab) {
1259 | 			result.found = bfd_find_nearest_line(fobj.handle.get(), section,
1260 | 					fobj.dynamic_symtab.get(), addr - sec_addr,
1261 | 					&result.filename, &result.funcname, &result.line);
1262 | 		}
1263 | 
1264 | 	}
1265 | 
1266 | 	ResolvedTrace::source_locs_t backtrace_inliners(bfd_fileobject& fobj,
1267 | 			find_sym_result previous_result) {
1268 | 		// This function can be called ONLY after a SUCCESSFUL call to
1269 | 		// find_symbol_details. The state is global to the bfd_handle.
1270 | 		ResolvedTrace::source_locs_t results;
1271 | 		while (previous_result.found) {
1272 | 			find_sym_result result;
1273 | 			result.found = bfd_find_inliner_info(fobj.handle.get(),
1274 | 					&result.filename, &result.funcname, &result.line);
1275 | 
1276 | 			if (result.found) /* and not (
1277 | 						cstrings_eq(previous_result.filename, result.filename)
1278 | 						and cstrings_eq(previous_result.funcname, result.funcname)
1279 | 						and result.line == previous_result.line
1280 | 						)) */ {
1281 | 				ResolvedTrace::SourceLoc src_loc;
1282 | 				src_loc.line = result.line;
1283 | 				if (result.filename) {
1284 | 					src_loc.filename = result.filename;
1285 | 				}
1286 | 				if (result.funcname) {
1287 | 					src_loc.function = demangle(result.funcname);
1288 | 				}
1289 | 				results.push_back(src_loc);
1290 | 			}
1291 | 			previous_result = result;
1292 | 		}
1293 | 		return results;
1294 | 	}
1295 | 
1296 | 	bool cstrings_eq(const char* a, const char* b) {
1297 | 		if (!a || !b) {
1298 | 			return false;
1299 | 		}
1300 | 		return strcmp(a, b) == 0;
1301 | 	}
1302 | 
1303 | };
1304 | #endif // BACKWARD_HAS_BFD == 1
1305 | 
1306 | #if BACKWARD_HAS_DW == 1
1307 | 
1308 | template <>
1309 | class TraceResolverLinuxImpl<trace_resolver_tag::libdw>:
1310 | 	public TraceResolverImplBase {
1311 | public:
1312 | 	TraceResolverLinuxImpl(): _dwfl_handle_initialized(false) {}
1313 | 
1314 | 	template <class ST>
1315 | 		void load_stacktrace(ST&) {}
1316 | 
1317 | 	ResolvedTrace resolve(ResolvedTrace trace) {
1318 | 		using namespace details;
1319 | 
1320 | 		Dwarf_Addr trace_addr = (Dwarf_Addr) trace.addr;
1321 | 
1322 | 		if (!_dwfl_handle_initialized) {
1323 | 			// initialize dwfl...
1324 | 			_dwfl_cb.reset(new Dwfl_Callbacks);
1325 | 			_dwfl_cb->find_elf = &dwfl_linux_proc_find_elf;
1326 | 			_dwfl_cb->find_debuginfo = &dwfl_standard_find_debuginfo;
1327 | 			_dwfl_cb->debuginfo_path = 0;
1328 | 
1329 | 			_dwfl_handle.reset(dwfl_begin(_dwfl_cb.get()));
1330 | 			_dwfl_handle_initialized = true;
1331 | 
1332 | 			if (!_dwfl_handle) {
1333 | 				return trace;
1334 | 			}
1335 | 
1336 | 			// ...from the current process.
1337 | 			dwfl_report_begin(_dwfl_handle.get());
1338 | 			int r = dwfl_linux_proc_report (_dwfl_handle.get(), getpid());
1339 | 			dwfl_report_end(_dwfl_handle.get(), NULL, NULL);
1340 | 			if (r < 0) {
1341 | 				return trace;
1342 | 			}
1343 | 		}
1344 | 
1345 | 		if (!_dwfl_handle) {
1346 | 			return trace;
1347 | 		}
1348 | 
1349 | 		// find the module (binary object) that contains the trace's address.
1350 | 		// This is not using any debug information, but the addresses ranges of
1351 | 		// all the currently loaded binary object.
1352 | 		Dwfl_Module* mod = dwfl_addrmodule(_dwfl_handle.get(), trace_addr);
1353 | 		if (mod) {
1354 | 			// now that we found it, lets get the name of it, this will be the
1355 | 			// full path to the running binary or one of the loaded library.
1356 | 			const char* module_name = dwfl_module_info (mod,
1357 | 					0, 0, 0, 0, 0, 0, 0);
1358 | 			if (module_name) {
1359 | 				trace.object_filename = module_name;
1360 | 			}
1361 | 			// We also look after the name of the symbol, equal or before this
1362 | 			// address. This is found by walking the symtab. We should get the
1363 | 			// symbol corresponding to the function (mangled) containing the
1364 | 			// address. If the code corresponding to the address was inlined,
1365 | 			// this is the name of the out-most inliner function.
1366 | 			const char* sym_name = dwfl_module_addrname(mod, trace_addr);
1367 | 			if (sym_name) {
1368 | 				trace.object_function = demangle(sym_name);
1369 | 			}
1370 | 		}
1371 | 
1372 | 		// now let's get serious, and find out the source location (file and
1373 | 		// line number) of the address.
1374 | 
1375 | 		// This function will look in .debug_aranges for the address and map it
1376 | 		// to the location of the compilation unit DIE in .debug_info and
1377 | 		// return it.
1378 | 		Dwarf_Addr mod_bias = 0;
1379 | 		Dwarf_Die* cudie = dwfl_module_addrdie(mod, trace_addr, &mod_bias);
1380 | 
1381 | #if 1
1382 | 		if (!cudie) {
1383 | 			// Sadly clang does not generate the section .debug_aranges, thus
1384 | 			// dwfl_module_addrdie will fail early. Clang doesn't either set
1385 | 			// the lowpc/highpc/range info for every compilation unit.
1386 | 			//
1387 | 			// So in order to save the world:
1388 | 			// for every compilation unit, we will iterate over every single
1389 | 			// DIEs. Normally functions should have a lowpc/highpc/range, which
1390 | 			// we will use to infer the compilation unit.
1391 | 
1392 | 			// note that this is probably badly inefficient.
1393 | 			while ((cudie = dwfl_module_nextcu(mod, cudie, &mod_bias))) {
1394 | 				Dwarf_Die die_mem;
1395 | 				Dwarf_Die* fundie = find_fundie_by_pc(cudie,
1396 | 						trace_addr - mod_bias, &die_mem);
1397 | 				if (fundie) {
1398 | 					break;
1399 | 				}
1400 | 			}
1401 | 		}
1402 | #endif
1403 | 
1404 | //#define BACKWARD_I_DO_NOT_RECOMMEND_TO_ENABLE_THIS_HORRIBLE_PIECE_OF_CODE
1405 | #ifdef BACKWARD_I_DO_NOT_RECOMMEND_TO_ENABLE_THIS_HORRIBLE_PIECE_OF_CODE
1406 | 		if (!cudie) {
1407 | 			// If it's still not enough, lets dive deeper in the shit, and try
1408 | 			// to save the world again: for every compilation unit, we will
1409 | 			// load the corresponding .debug_line section, and see if we can
1410 | 			// find our address in it.
1411 | 
1412 | 			Dwarf_Addr cfi_bias;
1413 | 			Dwarf_CFI* cfi_cache = dwfl_module_eh_cfi(mod, &cfi_bias);
1414 | 
1415 | 			Dwarf_Addr bias;
1416 | 			while ((cudie = dwfl_module_nextcu(mod, cudie, &bias))) {
1417 | 				if (dwarf_getsrc_die(cudie, trace_addr - bias)) {
1418 | 
1419 | 					// ...but if we get a match, it might be a false positive
1420 | 					// because our (address - bias) might as well be valid in a
1421 | 					// different compilation unit. So we throw our last card on
1422 | 					// the table and lookup for the address into the .eh_frame
1423 | 					// section.
1424 | 
1425 | 					handle<Dwarf_Frame*> frame;
1426 | 					dwarf_cfi_addrframe(cfi_cache, trace_addr - cfi_bias, &frame);
1427 | 					if (frame) {
1428 | 						break;
1429 | 					}
1430 | 				}
1431 | 			}
1432 | 		}
1433 | #endif
1434 | 
1435 | 		if (!cudie) {
1436 | 			return trace; // this time we lost the game :/
1437 | 		}
1438 | 
1439 | 		// Now that we have a compilation unit DIE, this function will be able
1440 | 		// to load the corresponding section in .debug_line (if not already
1441 | 		// loaded) and hopefully find the source location mapped to our
1442 | 		// address.
1443 | 		Dwarf_Line* srcloc = dwarf_getsrc_die(cudie, trace_addr - mod_bias);
1444 | 
1445 | 		if (srcloc) {
1446 | 			const char* srcfile = dwarf_linesrc(srcloc, 0, 0);
1447 | 			if (srcfile) {
1448 | 				trace.source.filename = srcfile;
1449 | 			}
1450 | 			int line = 0, col = 0;
1451 | 			dwarf_lineno(srcloc, &line);
1452 | 			dwarf_linecol(srcloc, &col);
1453 | 			trace.source.line = line;
1454 | 			trace.source.col = col;
1455 | 		}
1456 | 
1457 | 		deep_first_search_by_pc(cudie, trace_addr - mod_bias,
1458 | 				inliners_search_cb(trace));
1459 | 		if (trace.source.function.size() == 0) {
1460 | 			// fallback.
1461 | 			trace.source.function = trace.object_function;
1462 | 		}
1463 | 
1464 | 		return trace;
1465 | 	}
1466 | 
1467 | private:
1468 | 	typedef details::handle<Dwfl*, details::deleter<void, Dwfl*, &dwfl_end> >
1469 | 		dwfl_handle_t;
1470 | 	details::handle<Dwfl_Callbacks*, details::default_delete<Dwfl_Callbacks*> >
1471 | 		           _dwfl_cb;
1472 | 	dwfl_handle_t  _dwfl_handle;
1473 | 	bool           _dwfl_handle_initialized;
1474 | 
1475 | 	// defined here because in C++98, template function cannot take locally
1476 | 	// defined types... grrr.
1477 | 	struct inliners_search_cb {
1478 | 		void operator()(Dwarf_Die* die) {
1479 | 			switch (dwarf_tag(die)) {
1480 | 				const char* name;
1481 | 				case DW_TAG_subprogram:
1482 | 					if ((name = dwarf_diename(die))) {
1483 | 						trace.source.function = name;
1484 | 					}
1485 | 					break;
1486 | 
1487 | 				case DW_TAG_inlined_subroutine:
1488 | 					ResolvedTrace::SourceLoc sloc;
1489 | 					Dwarf_Attribute attr_mem;
1490 | 
1491 | 					if ((name = dwarf_diename(die))) {
1492 | 						sloc.function = name;
1493 | 					}
1494 | 					if ((name = die_call_file(die))) {
1495 | 						sloc.filename = name;
1496 | 					}
1497 | 
1498 | 					Dwarf_Word line = 0, col = 0;
1499 | 					dwarf_formudata(dwarf_attr(die, DW_AT_call_line,
1500 | 								&attr_mem), &line);
1501 | 					dwarf_formudata(dwarf_attr(die, DW_AT_call_column,
1502 | 								&attr_mem), &col);
1503 | 					sloc.line = (unsigned)line;
1504 | 					sloc.col = (unsigned)col;
1505 | 
1506 | 					trace.inliners.push_back(sloc);
1507 | 					break;
1508 | 			};
1509 | 		}
1510 | 		ResolvedTrace& trace;
1511 | 		inliners_search_cb(ResolvedTrace& t): trace(t) {}
1512 | 	};
1513 | 
1514 | 
1515 | 	static bool die_has_pc(Dwarf_Die* die, Dwarf_Addr pc) {
1516 | 		Dwarf_Addr low, high;
1517 | 
1518 | 		// continuous range
1519 | 		if (dwarf_hasattr(die, DW_AT_low_pc) &&
1520 | 							dwarf_hasattr(die, DW_AT_high_pc)) {
1521 | 			if (dwarf_lowpc(die, &low) != 0) {
1522 | 				return false;
1523 | 			}
1524 | 			if (dwarf_highpc(die, &high) != 0) {
1525 | 				Dwarf_Attribute attr_mem;
1526 | 				Dwarf_Attribute* attr = dwarf_attr(die, DW_AT_high_pc, &attr_mem);
1527 | 				Dwarf_Word value;
1528 | 				if (dwarf_formudata(attr, &value) != 0) {
1529 | 					return false;
1530 | 				}
1531 | 				high = low + value;
1532 | 			}
1533 | 			return pc >= low && pc < high;
1534 | 		}
1535 | 
1536 | 		// non-continuous range.
1537 | 		Dwarf_Addr base;
1538 | 		ptrdiff_t offset = 0;
1539 | 		while ((offset = dwarf_ranges(die, offset, &base, &low, &high)) > 0) {
1540 | 			if (pc >= low && pc < high) {
1541 | 				return true;
1542 | 			}
1543 | 		}
1544 | 		return false;
1545 | 	}
1546 | 
1547 | 	static Dwarf_Die* find_fundie_by_pc(Dwarf_Die* parent_die, Dwarf_Addr pc,
1548 | 			Dwarf_Die* result) {
1549 | 		if (dwarf_child(parent_die, result) != 0) {
1550 | 			return 0;
1551 | 		}
1552 | 
1553 | 		Dwarf_Die* die = result;
1554 | 		do {
1555 | 			switch (dwarf_tag(die)) {
1556 | 				case DW_TAG_subprogram:
1557 | 				case DW_TAG_inlined_subroutine:
1558 | 					if (die_has_pc(die, pc)) {
1559 | 						return result;
1560 | 					}
1561 | 			};
1562 | 			bool declaration = false;
1563 | 			Dwarf_Attribute attr_mem;
1564 | 			dwarf_formflag(dwarf_attr(die, DW_AT_declaration,
1565 | 						&attr_mem), &declaration);
1566 | 			if (!declaration) {
1567 | 				// let's be curious and look deeper in the tree,
1568 | 				// function are not necessarily at the first level, but
1569 | 				// might be nested inside a namespace, structure etc.
1570 | 				Dwarf_Die die_mem;
1571 | 				Dwarf_Die* indie = find_fundie_by_pc(die, pc, &die_mem);
1572 | 				if (indie) {
1573 | 					*result = die_mem;
1574 | 					return result;
1575 | 				}
1576 | 			}
1577 | 		} while (dwarf_siblingof(die, result) == 0);
1578 | 		return 0;
1579 | 	}
1580 | 
1581 | 	template <typename CB>
1582 | 		static bool deep_first_search_by_pc(Dwarf_Die* parent_die,
1583 | 				Dwarf_Addr pc, CB cb) {
1584 | 		Dwarf_Die die_mem;
1585 | 		if (dwarf_child(parent_die, &die_mem) != 0) {
1586 | 			return false;
1587 | 		}
1588 | 
1589 | 		bool branch_has_pc = false;
1590 | 		Dwarf_Die* die = &die_mem;
1591 | 		do {
1592 | 			bool declaration = false;
1593 | 			Dwarf_Attribute attr_mem;
1594 | 			dwarf_formflag(dwarf_attr(die, DW_AT_declaration, &attr_mem), &declaration);
1595 | 			if (!declaration) {
1596 | 				// let's be curious and look deeper in the tree, function are
1597 | 				// not necessarily at the first level, but might be nested
1598 | 				// inside a namespace, structure, a function, an inlined
1599 | 				// function etc.
1600 | 				branch_has_pc = deep_first_search_by_pc(die, pc, cb);
1601 | 			}
1602 | 			if (!branch_has_pc) {
1603 | 				branch_has_pc = die_has_pc(die, pc);
1604 | 			}
1605 | 			if (branch_has_pc) {
1606 | 				cb(die);
1607 | 			}
1608 | 		} while (dwarf_siblingof(die, &die_mem) == 0);
1609 | 		return branch_has_pc;
1610 | 	}
1611 | 
1612 | 	static const char* die_call_file(Dwarf_Die *die) {
1613 | 		Dwarf_Attribute attr_mem;
1614 | 		Dwarf_Sword file_idx = 0;
1615 | 
1616 | 		dwarf_formsdata(dwarf_attr(die, DW_AT_call_file, &attr_mem),
1617 | 				&file_idx);
1618 | 
1619 | 		if (file_idx == 0) {
1620 | 			return 0;
1621 | 		}
1622 | 
1623 | 		Dwarf_Die die_mem;
1624 | 		Dwarf_Die* cudie = dwarf_diecu(die, &die_mem, 0, 0);
1625 | 		if (!cudie) {
1626 | 			return 0;
1627 | 		}
1628 | 
1629 | 		Dwarf_Files* files = 0;
1630 | 		size_t nfiles;
1631 | 		dwarf_getsrcfiles(cudie, &files, &nfiles);
1632 | 		if (!files) {
1633 | 			return 0;
1634 | 		}
1635 | 
1636 | 		return dwarf_filesrc(files, file_idx, 0, 0);
1637 | 	}
1638 | 
1639 | };
1640 | #endif // BACKWARD_HAS_DW == 1
1641 | 
1642 | #if BACKWARD_HAS_DWARF == 1
1643 | 
1644 | template <>
1645 | class TraceResolverLinuxImpl<trace_resolver_tag::libdwarf>:
1646 | 	public TraceResolverImplBase {
1647 | 	static std::string read_symlink(std::string const & symlink_path) {
1648 | 		std::string path;
1649 | 		path.resize(100);
1650 | 
1651 | 		while(true) {
1652 | 			ssize_t len = ::readlink(symlink_path.c_str(),
1653 | 									&*path.begin(), path.size());
1654 | 			if(len < 0) {
1655 | 				return "";
1656 | 			}
1657 | 			if ((size_t)len == path.size()) {
1658 | 				path.resize(path.size() * 2);
1659 | 			}
1660 | 			else {
1661 | 				path.resize(len);
1662 | 				break;
1663 | 			}
1664 | 		}
1665 | 
1666 | 		return path;
1667 | 	}
1668 | public:
1669 | 	TraceResolverLinuxImpl(): _dwarf_loaded(false) {}
1670 | 
1671 | 	template <class ST>
1672 | 		void load_stacktrace(ST&) {}
1673 | 
1674 | 	ResolvedTrace resolve(ResolvedTrace trace) {
1675 | 		// trace.addr is a virtual address in memory pointing to some code.
1676 | 		// Let's try to find from which loaded object it comes from.
1677 | 		// The loaded object can be yourself btw.
1678 | 
1679 | 		Dl_info symbol_info;
1680 | 		int dladdr_result = 0;
1681 | #ifndef __ANDROID__
1682 | 		link_map *link_map;
1683 | 		// We request the link map so we can get information about offsets
1684 | 		dladdr_result = dladdr1(trace.addr, &symbol_info,
1685 | 				reinterpret_cast<void**>(&link_map), RTLD_DL_LINKMAP);
1686 | #else
1687 | 		// Android doesn't have dladdr1. Don't use the linker map.
1688 | 		dladdr_result = dladdr(trace.addr, &symbol_info);
1689 | #endif
1690 | 		if (!dladdr_result) {
1691 | 			return trace; // dat broken trace...
1692 | 		}
1693 | 
1694 | 		std::string argv0;
1695 | 		{
1696 | 			std::ifstream ifs("/proc/self/cmdline");
1697 | 			std::getline(ifs, argv0, '\0');
1698 | 		}
1699 | 		std::string tmp;
1700 | 		if(symbol_info.dli_fname == argv0) {
1701 | 			tmp = read_symlink("/proc/self/exe");
1702 | 			symbol_info.dli_fname = tmp.c_str();
1703 | 		}
1704 | 
1705 | 		// Now we get in symbol_info:
1706 | 		// .dli_fname:
1707 | 		//      pathname of the shared object that contains the address.
1708 | 		// .dli_fbase:
1709 | 		//      where the object is loaded in memory.
1710 | 		// .dli_sname:
1711 | 		//      the name of the nearest symbol to trace.addr, we expect a
1712 | 		//      function name.
1713 | 		// .dli_saddr:
1714 | 		//      the exact address corresponding to .dli_sname.
1715 | 		//
1716 | 		// And in link_map:
1717 | 		// .l_addr:
1718 | 		//      difference between the address in the ELF file and the address
1719 | 		//      in memory
1720 | 		// l_name:
1721 | 		//      absolute pathname where the object was found
1722 | 
1723 | 		if (symbol_info.dli_sname) {
1724 | 			trace.object_function = demangle(symbol_info.dli_sname);
1725 | 		}
1726 | 
1727 | 		if (!symbol_info.dli_fname) {
1728 | 			return trace;
1729 | 		}
1730 | 
1731 | 		trace.object_filename = symbol_info.dli_fname;
1732 | 		dwarf_fileobject& fobj = load_object_with_dwarf(symbol_info.dli_fname);
1733 | 		if (!fobj.dwarf_handle) {
1734 | 			return trace; // sad, we couldn't load the object :(
1735 | 		}
1736 | 
1737 | #ifndef __ANDROID__
1738 | 		// Convert the address to a module relative one by looking at
1739 | 		// the module's loading address in the link map
1740 | 		Dwarf_Addr address = reinterpret_cast<uintptr_t>(trace.addr) -
1741 | 				reinterpret_cast<uintptr_t>(link_map->l_addr);
1742 | #else
1743 | 		Dwarf_Addr address = reinterpret_cast<uintptr_t>(trace.addr);
1744 | #endif
1745 | 
1746 | 		if (trace.object_function.empty()) {
1747 | 			symbol_cache_t::iterator it =
1748 | 					fobj.symbol_cache.lower_bound(address);
1749 | 
1750 | 			if (it != fobj.symbol_cache.end()) {
1751 | 				if (it->first != address) {
1752 | 					if (it != fobj.symbol_cache.begin()) {
1753 | 						--it;
1754 | 					}
1755 | 				}
1756 | 				trace.object_function = demangle(it->second.c_str());
1757 | 			}
1758 | 		}
1759 | 
1760 | 		// Get the Compilation Unit DIE for the address
1761 | 		Dwarf_Die die = find_die(fobj, address);
1762 | 
1763 | 		if (!die) {
1764 | 			return trace; // this time we lost the game :/
1765 | 		}
1766 | 
1767 | 		// libdwarf doesn't give us direct access to its objects, it always
1768 | 		// allocates a copy for the caller. We keep that copy alive in a cache
1769 | 		// and we deallocate it later when it's no longer required.
1770 | 		die_cache_entry& die_object = get_die_cache(fobj, die);
1771 | 		if (die_object.isEmpty())
1772 | 			return trace;  // We have no line section for this DIE
1773 | 
1774 | 		die_linemap_t::iterator it =
1775 | 				die_object.line_section.lower_bound(address);
1776 | 
1777 | 		if (it != die_object.line_section.end()) {
1778 | 			if (it->first != address) {
1779 | 				if (it == die_object.line_section.begin()) {
1780 | 					// If we are on the first item of the line section
1781 | 					// but the address does not match it means that
1782 | 					// the address is below the range of the DIE. Give up.
1783 | 					return trace;
1784 | 				} else {
1785 | 					--it;
1786 | 				}
1787 | 			}
1788 | 		} else {
1789 | 			return trace; // We didn't find the address.
1790 | 		}
1791 | 
1792 | 		// Get the Dwarf_Line that the address points to and call libdwarf
1793 | 		// to get source file, line and column info.
1794 | 		Dwarf_Line line = die_object.line_buffer[it->second];
1795 | 		Dwarf_Error error = DW_DLE_NE;
1796 | 
1797 | 		char* filename;
1798 | 		if (dwarf_linesrc(line, &filename, &error)
1799 | 				== DW_DLV_OK) {
1800 | 			trace.source.filename = std::string(filename);
1801 | 			dwarf_dealloc(fobj.dwarf_handle.get(), filename, DW_DLA_STRING);
1802 | 		}
1803 | 
1804 | 		Dwarf_Unsigned number = 0;
1805 | 		if (dwarf_lineno(line, &number, &error) == DW_DLV_OK) {
1806 | 			trace.source.line = number;
1807 | 		} else {
1808 | 			trace.source.line = 0;
1809 | 		}
1810 | 
1811 | 		if (dwarf_lineoff_b(line, &number, &error) == DW_DLV_OK) {
1812 | 			trace.source.col = number;
1813 | 		} else {
1814 | 			trace.source.col = 0;
1815 | 		}
1816 | 
1817 | 		std::vector<std::string> namespace_stack;
1818 | 		deep_first_search_by_pc(fobj, die, address, namespace_stack,
1819 | 				inliners_search_cb(trace, fobj, die));
1820 | 
1821 | 		dwarf_dealloc(fobj.dwarf_handle.get(), die, DW_DLA_DIE);
1822 | 
1823 | 		return trace;
1824 | 	}
1825 | 
1826 | public:
1827 | 	static int close_dwarf(Dwarf_Debug dwarf) {
1828 | 		return dwarf_finish(dwarf, NULL);
1829 | 	}
1830 | 
1831 | private:
1832 | 	bool                _dwarf_loaded;
1833 | 
1834 | 	typedef details::handle<int,
1835 | 					details::deleter<int, int, &::close>
1836 | 							> dwarf_file_t;
1837 | 
1838 | 	typedef details::handle<Elf*,
1839 | 					details::deleter<int, Elf*, &elf_end>
1840 | 							> dwarf_elf_t;
1841 | 
1842 | 	typedef details::handle<Dwarf_Debug,
1843 | 					details::deleter<int, Dwarf_Debug, &close_dwarf>
1844 | 							> dwarf_handle_t;
1845 | 
1846 | 	typedef std::map<Dwarf_Addr, int>		die_linemap_t;
1847 | 
1848 | 	typedef std::map<Dwarf_Off, Dwarf_Off>	die_specmap_t;
1849 | 
1850 | 	struct die_cache_entry {
1851 | 		die_specmap_t			spec_section;
1852 | 		die_linemap_t			line_section;
1853 | 		Dwarf_Line*				line_buffer;
1854 | 		Dwarf_Signed			line_count;
1855 | 		Dwarf_Line_Context		line_context;
1856 | 
1857 | 		inline bool isEmpty() {
1858 | 			return  line_buffer == NULL ||
1859 | 					line_count == 0 ||
1860 | 					line_context == NULL ||
1861 | 					line_section.empty();
1862 | 		}
1863 | 
1864 | 		die_cache_entry() :
1865 | 			line_buffer(0), line_count(0), line_context(0) {}
1866 | 
1867 | 		~die_cache_entry()
1868 | 		{
1869 | 			if (line_context) {
1870 | 				dwarf_srclines_dealloc_b(line_context);
1871 | 			}
1872 | 		}
1873 | 	};
1874 | 
1875 | 	typedef std::map<Dwarf_Off, die_cache_entry> die_cache_t;
1876 | 
1877 | 	typedef std::map<uintptr_t, std::string>     symbol_cache_t;
1878 | 
1879 | 	struct dwarf_fileobject {
1880 | 		dwarf_file_t		file_handle;
1881 | 		dwarf_elf_t			elf_handle;
1882 | 		dwarf_handle_t		dwarf_handle;
1883 | 		symbol_cache_t		symbol_cache;
1884 | 
1885 | 		// Die cache
1886 | 		die_cache_t     	die_cache;
1887 | 		die_cache_entry*	current_cu;
1888 | 	};
1889 | 
1890 | 	typedef details::hashtable<std::string, dwarf_fileobject>::type
1891 | 			fobj_dwarf_map_t;
1892 | 	fobj_dwarf_map_t    _fobj_dwarf_map;
1893 | 
1894 | 	static bool cstrings_eq(const char* a, const char* b) {
1895 | 		if (!a || !b) {
1896 | 			return false;
1897 | 		}
1898 | 		return strcmp(a, b) == 0;
1899 | 	}
1900 | 
1901 | 	dwarf_fileobject& load_object_with_dwarf(
1902 | 			const std::string filename_object) {
1903 | 
1904 | 		if (!_dwarf_loaded) {
1905 | 			// Set the ELF library operating version
1906 | 			// If that fails there's nothing we can do
1907 | 			_dwarf_loaded = elf_version(EV_CURRENT) != EV_NONE;
1908 | 		}
1909 | 
1910 | 		fobj_dwarf_map_t::iterator it =
1911 | 				_fobj_dwarf_map.find(filename_object);
1912 | 		if (it != _fobj_dwarf_map.end()) {
1913 | 				return it->second;
1914 | 		}
1915 | 
1916 | 		// this new object is empty for now
1917 | 		dwarf_fileobject& r = _fobj_dwarf_map[filename_object];
1918 | 
1919 | 		dwarf_file_t file_handle;
1920 | 		file_handle.reset(open(filename_object.c_str(), O_RDONLY));
1921 | 		if (file_handle < 0) {
1922 | 			return r;
1923 | 		}
1924 | 
1925 | 		// Try to get an ELF handle. We need to read the ELF sections
1926 | 		// because we want to see if there is a .gnu_debuglink section
1927 | 		// that points to a split debug file
1928 | 		dwarf_elf_t elf_handle;
1929 | 		elf_handle.reset(elf_begin(file_handle.get(), ELF_C_READ, NULL));
1930 | 		if (!elf_handle) {
1931 | 			return r;
1932 | 		}
1933 | 
1934 | 		const char* e_ident = elf_getident(elf_handle.get(), 0);
1935 | 		if (!e_ident) {
1936 | 			return r;
1937 | 		}
1938 | 
1939 | 		// Get the number of sections
1940 | 		// We use the new APIs as elf_getshnum is deprecated
1941 | 		size_t shdrnum = 0;
1942 | 		if (elf_getshdrnum(elf_handle.get(), &shdrnum) == -1) {
1943 | 			return r;
1944 | 		}
1945 | 
1946 | 		// Get the index to the string section
1947 | 		size_t shdrstrndx = 0;
1948 | 		if (elf_getshdrstrndx (elf_handle.get(), &shdrstrndx) == -1) {
1949 | 			return r;
1950 | 		}
1951 | 
1952 | 		std::string debuglink;
1953 | 		// Iterate through the ELF sections to try to get a gnu_debuglink
1954 | 		// note and also to cache the symbol table.
1955 | 		// We go the preprocessor way to avoid having to create templated
1956 | 		// classes or using gelf (which might throw a compiler error if 64 bit
1957 | 		// is not supported
1958 | #define ELF_GET_DATA(ARCH)                                                   \
1959 | 		Elf_Scn *elf_section = 0;                                            \
1960 | 		Elf_Data *elf_data = 0;                                              \
1961 | 		Elf##ARCH##_Shdr* section_header = 0;                                \
1962 | 		Elf_Scn *symbol_section = 0;                                         \
1963 | 		size_t symbol_count = 0;                                             \
1964 | 		size_t symbol_strings = 0;                                           \
1965 | 		Elf##ARCH##_Sym *symbol = 0;                                         \
1966 | 		const char* section_name = 0;                                        \
1967 | 		                                                                     \
1968 | 		while ((elf_section = elf_nextscn(elf_handle.get(), elf_section))    \
1969 | 				!= NULL) {                                                   \
1970 | 			section_header = elf##ARCH##_getshdr(elf_section);               \
1971 | 			if (section_header == NULL) {                                    \
1972 | 				return r;                                                    \
1973 | 			}                                                                \
1974 | 		                                                                     \
1975 | 			if ((section_name = elf_strptr(                                  \
1976 | 								elf_handle.get(), shdrstrndx,                \
1977 | 								section_header->sh_name)) == NULL) {         \
1978 | 				return r;                                                    \
1979 | 			}                                                                \
1980 | 		                                                                     \
1981 | 			if (cstrings_eq(section_name, ".gnu_debuglink")) {               \
1982 | 				elf_data = elf_getdata(elf_section, NULL);                   \
1983 | 				if (elf_data && elf_data->d_size > 0) {                      \
1984 | 					debuglink = std::string(                                 \
1985 | 							reinterpret_cast<const char*>(elf_data->d_buf)); \
1986 | 				}                                                            \
1987 | 			}                                                                \
1988 | 		                                                                     \
1989 | 			switch(section_header->sh_type) {                                \
1990 | 				case SHT_SYMTAB:                                             \
1991 | 					symbol_section = elf_section;                            \
1992 | 					symbol_count   = section_header->sh_size /               \
1993 | 									section_header->sh_entsize;              \
1994 | 					symbol_strings = section_header->sh_link;                \
1995 | 					break;                                                   \
1996 | 		                                                                     \
1997 | 				/* We use .dynsyms as a last resort, we prefer .symtab */    \
1998 | 				case SHT_DYNSYM:                                             \
1999 | 					if (!symbol_section) {                                   \
2000 | 						symbol_section = elf_section;                        \
2001 | 						symbol_count   = section_header->sh_size /           \
2002 | 										section_header->sh_entsize;          \
2003 | 						symbol_strings = section_header->sh_link;            \
2004 | 					}                                                        \
2005 | 					break;                                                   \
2006 | 			}                                                                \
2007 | 		}                                                                    \
2008 | 		                                                                     \
2009 | 		if (symbol_section && symbol_count && symbol_strings) {              \
2010 | 			elf_data = elf_getdata(symbol_section, NULL);                    \
2011 | 			symbol = reinterpret_cast<Elf##ARCH##_Sym*>(elf_data->d_buf);    \
2012 | 			for (size_t i = 0; i < symbol_count; ++i) {                      \
2013 | 				int type = ELF##ARCH##_ST_TYPE(symbol->st_info);             \
2014 | 				if (type == STT_FUNC && symbol->st_value > 0) {              \
2015 | 					r.symbol_cache[symbol->st_value] = std::string(          \
2016 | 							elf_strptr(elf_handle.get(),                     \
2017 | 							symbol_strings, symbol->st_name));               \
2018 | 				}                                                            \
2019 | 				++symbol;                                                    \
2020 | 			}                                                                \
2021 | 		}                                                                    \
2022 | 
2023 | 
2024 | 		if (e_ident[EI_CLASS] == ELFCLASS32) {
2025 | 			ELF_GET_DATA(32)
2026 | 		} else if (e_ident[EI_CLASS] == ELFCLASS64) {
2027 | 		// libelf might have been built without 64 bit support
2028 | #if __LIBELF64
2029 | 			ELF_GET_DATA(64)
2030 | #endif
2031 | 		}
2032 | 
2033 | 		if (!debuglink.empty()) {
2034 | 			// We have a debuglink section! Open an elf instance on that
2035 | 			// file instead. If we can't open the file, then return
2036 | 			// the elf handle we had already opened.
2037 | 			dwarf_file_t debuglink_file;
2038 | 			debuglink_file.reset(open(debuglink.c_str(), O_RDONLY));
2039 | 			if (debuglink_file.get() > 0) {
2040 | 				dwarf_elf_t debuglink_elf;
2041 | 				debuglink_elf.reset(
2042 | 					elf_begin(debuglink_file.get(),ELF_C_READ, NULL)
2043 | 				);
2044 | 
2045 | 				// If we have a valid elf handle, return the new elf handle
2046 | 				// and file handle and discard the original ones
2047 | 				if (debuglink_elf) {
2048 | 					elf_handle = move(debuglink_elf);
2049 | 					file_handle = move(debuglink_file);
2050 | 				}
2051 | 			}
2052 | 		}
2053 | 
2054 | 		// Ok, we have a valid ELF handle, let's try to get debug symbols
2055 | 		Dwarf_Debug dwarf_debug;
2056 | 		Dwarf_Error error = DW_DLE_NE;
2057 | 		dwarf_handle_t dwarf_handle;
2058 | 
2059 | 		int dwarf_result = dwarf_elf_init(elf_handle.get(),
2060 | 						DW_DLC_READ, NULL, NULL, &dwarf_debug, &error);
2061 | 
2062 | 		// We don't do any special handling for DW_DLV_NO_ENTRY specially.
2063 | 		// If we get an error, or the file doesn't have debug information
2064 | 		// we just return.
2065 | 		if (dwarf_result != DW_DLV_OK) {
2066 | 			return r;
2067 | 		}
2068 | 
2069 | 		dwarf_handle.reset(dwarf_debug);
2070 | 
2071 | 		r.file_handle = move(file_handle);
2072 | 		r.elf_handle = move(elf_handle);
2073 | 		r.dwarf_handle = move(dwarf_handle);
2074 | 
2075 | 		return r;
2076 | 	}
2077 | 
2078 | 	die_cache_entry& get_die_cache(dwarf_fileobject& fobj, Dwarf_Die die)
2079 | 	{
2080 | 		Dwarf_Error error = DW_DLE_NE;
2081 | 
2082 | 		// Get the die offset, we use it as the cache key
2083 | 		Dwarf_Off die_offset;
2084 | 		if (dwarf_dieoffset(die, &die_offset, &error) != DW_DLV_OK) {
2085 | 			die_offset = 0;
2086 | 		}
2087 | 
2088 | 		die_cache_t::iterator it = fobj.die_cache.find(die_offset);
2089 | 
2090 | 		if (it != fobj.die_cache.end()) {
2091 | 			fobj.current_cu = &it->second;
2092 | 			return it->second;
2093 | 		}
2094 | 
2095 | 		die_cache_entry& de = fobj.die_cache[die_offset];
2096 | 		fobj.current_cu = &de;
2097 | 
2098 | 		Dwarf_Addr line_addr;
2099 | 		Dwarf_Small table_count;
2100 | 
2101 | 		// The addresses in the line section are not fully sorted (they might
2102 | 		// be sorted by block of code belonging to the same file), which makes
2103 | 		// it necessary to do so before searching is possible.
2104 | 		//
2105 | 		// As libdwarf allocates a copy of everything, let's get the contents
2106 | 		// of the line section and keep it around. We also create a map of
2107 | 		// program counter to line table indices so we can search by address
2108 | 		// and get the line buffer index.
2109 | 		//
2110 | 		// To make things more difficult, the same address can span more than
2111 | 		// one line, so we need to keep the index pointing to the first line
2112 | 		// by using insert instead of the map's [ operator.
2113 | 
2114 | 		// Get the line context for the DIE
2115 | 		if (dwarf_srclines_b(die, 0, &table_count, &de.line_context, &error)
2116 | 				== DW_DLV_OK) {
2117 | 			// Get the source lines for this line context, to be deallocated
2118 | 			// later
2119 | 			if (dwarf_srclines_from_linecontext(
2120 | 					de.line_context, &de.line_buffer, &de.line_count, &error)
2121 | 						== DW_DLV_OK) {
2122 | 
2123 | 				// Add all the addresses to our map
2124 | 				for (int i = 0; i < de.line_count; i++) {
2125 | 					if (dwarf_lineaddr(de.line_buffer[i], &line_addr, &error)
2126 | 							!= DW_DLV_OK) {
2127 | 						line_addr = 0;
2128 | 					}
2129 | 					de.line_section.insert(
2130 | 							std::pair<Dwarf_Addr, int>(line_addr, i));
2131 | 				}
2132 | 			}
2133 | 		}
2134 | 
2135 | 		// For each CU, cache the function DIEs that contain the
2136 | 		// DW_AT_specification attribute. When building with -g3 the function
2137 | 		// DIEs are separated in declaration and specification, with the
2138 | 		// declaration containing only the name and parameters and the
2139 | 		// specification the low/high pc and other compiler attributes.
2140 | 		//
2141 | 		// We cache those specifications so we don't skip over the declarations,
2142 | 		// because they have no pc, and we can do namespace resolution for
2143 | 		// DWARF function names.
2144 | 		Dwarf_Debug dwarf = fobj.dwarf_handle.get();
2145 | 		Dwarf_Die current_die = 0;
2146 | 		if (dwarf_child(die, &current_die, &error) == DW_DLV_OK) {
2147 | 			for(;;) {
2148 | 				Dwarf_Die sibling_die = 0;
2149 | 
2150 | 				Dwarf_Half tag_value;
2151 | 				dwarf_tag(current_die, &tag_value, &error);
2152 | 
2153 | 				if (tag_value == DW_TAG_subprogram ||
2154 | 					tag_value == DW_TAG_inlined_subroutine) {
2155 | 
2156 | 					Dwarf_Bool has_attr = 0;
2157 | 					if (dwarf_hasattr(current_die, DW_AT_specification,
2158 | 									  &has_attr, &error) == DW_DLV_OK) {
2159 | 						if (has_attr) {
2160 | 							Dwarf_Attribute attr_mem;
2161 | 							if (dwarf_attr(current_die, DW_AT_specification,
2162 | 											&attr_mem, &error) == DW_DLV_OK) {
2163 | 								Dwarf_Off spec_offset = 0;
2164 | 								if (dwarf_formref(attr_mem,
2165 | 										&spec_offset, &error) == DW_DLV_OK) {
2166 | 									Dwarf_Off spec_die_offset;
2167 | 									if (dwarf_dieoffset(current_die,
2168 | 											&spec_die_offset, &error)
2169 | 											== DW_DLV_OK) {
2170 | 										de.spec_section[spec_offset] =
2171 | 												spec_die_offset;
2172 | 									}
2173 | 								}
2174 | 							}
2175 | 							dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2176 | 						}
2177 | 					}
2178 | 				}
2179 | 
2180 | 				int result = dwarf_siblingof(
2181 | 						dwarf, current_die, &sibling_die, &error);
2182 | 				if (result == DW_DLV_ERROR) {
2183 | 					break;
2184 | 				} else if (result == DW_DLV_NO_ENTRY) {
2185 | 					break;
2186 | 				}
2187 | 
2188 | 				if (current_die != die) {
2189 | 					dwarf_dealloc(dwarf, current_die, DW_DLA_DIE);
2190 | 					current_die = 0;
2191 | 				}
2192 | 
2193 | 				current_die = sibling_die;
2194 | 			}
2195 | 		}
2196 | 		return de;
2197 | 	}
2198 | 
2199 | 	static Dwarf_Die get_referenced_die(
2200 | 			Dwarf_Debug dwarf, Dwarf_Die die, Dwarf_Half attr, bool global) {
2201 | 		Dwarf_Error error = DW_DLE_NE;
2202 | 		Dwarf_Attribute attr_mem;
2203 | 
2204 | 		Dwarf_Die found_die = NULL;
2205 | 		if (dwarf_attr(die, attr, &attr_mem, &error) == DW_DLV_OK) {
2206 | 			Dwarf_Off offset;
2207 | 			int result = 0;
2208 | 			if (global) {
2209 | 				result = dwarf_global_formref(attr_mem, &offset, &error);
2210 | 			} else {
2211 | 				result = dwarf_formref(attr_mem, &offset, &error);
2212 | 			}
2213 | 
2214 | 			if (result == DW_DLV_OK) {
2215 | 				if (dwarf_offdie(dwarf, offset, &found_die, &error)
2216 | 						!= DW_DLV_OK) {
2217 | 					found_die = NULL;
2218 | 				}
2219 | 			}
2220 | 			dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2221 | 		}
2222 | 		return found_die;
2223 | 	}
2224 | 
2225 | 	static std::string get_referenced_die_name(
2226 | 			Dwarf_Debug dwarf, Dwarf_Die die, Dwarf_Half attr, bool global) {
2227 | 		Dwarf_Error error = DW_DLE_NE;
2228 | 		std::string value;
2229 | 
2230 | 		Dwarf_Die found_die = get_referenced_die(dwarf, die, attr, global);
2231 | 
2232 | 		if (found_die) {
2233 | 			char *name;
2234 | 			if (dwarf_diename(found_die, &name, &error) == DW_DLV_OK) {
2235 | 				if (name) {
2236 | 					value = std::string(name);
2237 | 				}
2238 | 				dwarf_dealloc(dwarf, name, DW_DLA_STRING);
2239 | 			}
2240 | 			dwarf_dealloc(dwarf, found_die, DW_DLA_DIE);
2241 | 		}
2242 | 
2243 | 		return value;
2244 | 	}
2245 | 
2246 | 	// Returns a spec DIE linked to the passed one. The caller should
2247 | 	// deallocate the DIE
2248 | 	static Dwarf_Die get_spec_die(dwarf_fileobject& fobj, Dwarf_Die die) {
2249 | 		Dwarf_Debug dwarf = fobj.dwarf_handle.get();
2250 | 		Dwarf_Error error = DW_DLE_NE;
2251 | 		Dwarf_Off die_offset;
2252 | 		if (fobj.current_cu && dwarf_die_CU_offset(die, &die_offset, &error)
2253 | 				== DW_DLV_OK) {
2254 | 			die_specmap_t::iterator it =
2255 | 					fobj.current_cu->spec_section.find(die_offset);
2256 | 
2257 | 			// If we have a DIE that completes the current one, check if
2258 | 			// that one has the pc we are looking for
2259 | 			if (it != fobj.current_cu->spec_section.end()) {
2260 | 				Dwarf_Die spec_die = 0;
2261 | 				if (dwarf_offdie(dwarf, it->second, &spec_die, &error)
2262 | 						== DW_DLV_OK) {
2263 | 					return spec_die;
2264 | 				}
2265 | 			}
2266 | 		}
2267 | 
2268 | 		// Maybe we have an abstract origin DIE with the function information?
2269 | 		return get_referenced_die(
2270 | 				fobj.dwarf_handle.get(), die, DW_AT_abstract_origin, true);
2271 | 
2272 | 	}
2273 | 
2274 | 	static bool die_has_pc(dwarf_fileobject& fobj, Dwarf_Die die, Dwarf_Addr pc)
2275 | 	{
2276 | 		Dwarf_Addr low_pc = 0, high_pc = 0;
2277 | 		Dwarf_Half high_pc_form = 0;
2278 | 		Dwarf_Form_Class return_class;
2279 | 		Dwarf_Error error = DW_DLE_NE;
2280 | 		Dwarf_Debug dwarf = fobj.dwarf_handle.get();
2281 | 		bool has_lowpc = false;
2282 | 		bool has_highpc = false;
2283 | 		bool has_ranges = false;
2284 | 
2285 | 		if (dwarf_lowpc(die, &low_pc, &error) == DW_DLV_OK) {
2286 | 			// If we have a low_pc check if there is a high pc.
2287 | 			// If we don't have a high pc this might mean we have a base
2288 | 			// address for the ranges list or just an address.
2289 | 			has_lowpc = true;
2290 | 
2291 | 			if (dwarf_highpc_b(
2292 | 					die, &high_pc, &high_pc_form, &return_class, &error)
2293 | 					== DW_DLV_OK) {
2294 | 				// We do have a high pc. In DWARF 4+ this is an offset from the
2295 | 				// low pc, but in earlier versions it's an absolute address.
2296 | 
2297 | 				has_highpc = true;
2298 | 				// In DWARF 2/3 this would be a DW_FORM_CLASS_ADDRESS
2299 | 				if (return_class == DW_FORM_CLASS_CONSTANT) {
2300 | 					high_pc = low_pc + high_pc;
2301 | 				}
2302 | 
2303 | 				// We have low and high pc, check if our address
2304 | 				// is in that range
2305 | 				return pc >= low_pc && pc < high_pc;
2306 | 			}
2307 | 		} else {
2308 | 			// Reset the low_pc, in case dwarf_lowpc failing set it to some
2309 | 			// undefined value.
2310 | 			low_pc = 0;
2311 | 		}
2312 | 
2313 | 		// Check if DW_AT_ranges is present and search for the PC in the
2314 | 		// returned ranges list. We always add the low_pc, as it not set it will
2315 | 		// be 0, in case we had a DW_AT_low_pc and DW_AT_ranges pair
2316 | 		bool result = false;
2317 | 
2318 | 		Dwarf_Attribute attr;
2319 | 		if (dwarf_attr(die, DW_AT_ranges, &attr, &error) == DW_DLV_OK) {
2320 | 
2321 | 			Dwarf_Off offset;
2322 | 			if (dwarf_global_formref(attr, &offset, &error) == DW_DLV_OK) {
2323 | 				Dwarf_Ranges *ranges;
2324 | 				Dwarf_Signed ranges_count = 0;
2325 | 				Dwarf_Unsigned byte_count = 0;
2326 | 
2327 | 				if (dwarf_get_ranges_a(dwarf, offset, die, &ranges,
2328 | 						&ranges_count, &byte_count, &error) == DW_DLV_OK) {
2329 | 					has_ranges = ranges_count != 0;
2330 | 					for (int i = 0; i < ranges_count; i++) {
2331 | 						if (pc >= ranges[i].dwr_addr1 + low_pc &&
2332 | 							pc < ranges[i].dwr_addr2 + low_pc) {
2333 | 							result = true;
2334 | 							break;
2335 | 						}
2336 | 					}
2337 | 					dwarf_ranges_dealloc(dwarf, ranges, ranges_count);
2338 | 				}
2339 | 			}
2340 | 		}
2341 | 
2342 | 		// Last attempt. We might have a single address set as low_pc.
2343 | 		if (!result && low_pc != 0 && pc == low_pc) {
2344 | 			result = true;
2345 | 		}
2346 | 
2347 | 		// If we don't have lowpc, highpc and ranges maybe this DIE is a
2348 | 		// declaration that relies on a DW_AT_specification DIE that happens
2349 | 		// later. Use the specification cache we filled when we loaded this CU.
2350 | 		if (!result && (!has_lowpc && !has_highpc && !has_ranges)) {
2351 | 			Dwarf_Die spec_die = get_spec_die(fobj, die);
2352 | 			if (spec_die) {
2353 | 				result = die_has_pc(fobj, spec_die, pc);
2354 | 				dwarf_dealloc(dwarf, spec_die, DW_DLA_DIE);
2355 | 			}
2356 | 		}
2357 | 
2358 | 		return result;
2359 | 	}
2360 | 
2361 | 	static void get_type(Dwarf_Debug dwarf, Dwarf_Die die, std::string& type) {
2362 | 		Dwarf_Error error = DW_DLE_NE;
2363 | 
2364 | 		Dwarf_Die child = 0;
2365 | 		if (dwarf_child(die, &child, &error) == DW_DLV_OK) {
2366 | 			get_type(dwarf, child, type);
2367 | 		}
2368 | 
2369 | 		if (child) {
2370 | 			type.insert(0, "::");
2371 | 			dwarf_dealloc(dwarf, child, DW_DLA_DIE);
2372 | 		}
2373 | 
2374 | 		char *name;
2375 | 		if (dwarf_diename(die, &name, &error) == DW_DLV_OK) {
2376 | 			type.insert(0, std::string(name));
2377 | 			dwarf_dealloc(dwarf, name, DW_DLA_STRING);
2378 | 		} else {
2379 | 			type.insert(0,"<unknown>");
2380 | 		}
2381 | 	}
2382 | 
2383 | 	static std::string get_type_by_signature(Dwarf_Debug dwarf, Dwarf_Die die) {
2384 | 		Dwarf_Error error = DW_DLE_NE;
2385 | 
2386 | 		Dwarf_Sig8 signature;
2387 | 		Dwarf_Bool has_attr = 0;
2388 | 		if (dwarf_hasattr(die, DW_AT_signature,
2389 | 						  &has_attr, &error) == DW_DLV_OK) {
2390 | 			if (has_attr) {
2391 | 				Dwarf_Attribute attr_mem;
2392 | 				if (dwarf_attr(die, DW_AT_signature,
2393 | 								&attr_mem, &error) == DW_DLV_OK) {
2394 | 					if (dwarf_formsig8(attr_mem, &signature, &error)
2395 | 							!= DW_DLV_OK) {
2396 | 						return std::string("<no type signature>");
2397 | 					}
2398 | 				}
2399 | 				dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2400 | 			}
2401 | 		}
2402 | 
2403 | 		Dwarf_Unsigned next_cu_header;
2404 | 		Dwarf_Sig8 tu_signature;
2405 | 		std::string result;
2406 | 		bool found = false;
2407 | 
2408 | 		while (dwarf_next_cu_header_d(dwarf, 0, 0, 0, 0, 0, 0, 0, &tu_signature,
2409 | 				0, &next_cu_header, 0, &error) == DW_DLV_OK) {
2410 | 
2411 | 			if (strncmp(signature.signature, tu_signature.signature, 8) == 0) {
2412 | 				Dwarf_Die type_cu_die = 0;
2413 | 				if (dwarf_siblingof_b(dwarf, 0, 0, &type_cu_die, &error)
2414 | 						== DW_DLV_OK) {
2415 | 					Dwarf_Die child_die = 0;
2416 | 					if (dwarf_child(type_cu_die, &child_die, &error)
2417 | 							== DW_DLV_OK) {
2418 | 						get_type(dwarf, child_die, result);
2419 | 						found = !result.empty();
2420 | 						dwarf_dealloc(dwarf, child_die, DW_DLA_DIE);
2421 | 					}
2422 | 					dwarf_dealloc(dwarf, type_cu_die, DW_DLA_DIE);
2423 | 				}
2424 | 			}
2425 | 		}
2426 | 
2427 | 		if (found) {
2428 | 			while (dwarf_next_cu_header_d(dwarf, 0, 0, 0, 0, 0, 0, 0, 0, 0,
2429 | 					&next_cu_header, 0, &error) == DW_DLV_OK) {
2430 | 				// Reset the cu header state. Unfortunately, libdwarf's
2431 | 				// next_cu_header API keeps its own iterator per Dwarf_Debug that
2432 | 				// can't be reset. We need to keep fetching elements until the end.
2433 | 			}
2434 | 		} else {
2435 | 			// If we couldn't resolve the type just print out the signature
2436 | 			std::ostringstream string_stream;
2437 | 			string_stream << "<0x" <<
2438 | 					std::hex << std::setfill('0');
2439 | 			for (int i = 0; i < 8; ++i) {
2440 | 				string_stream << std::setw(2) << std::hex << (int)(unsigned char)(signature.signature[i]);
2441 | 			}
2442 | 			string_stream << ">";
2443 | 			result = string_stream.str();
2444 | 		}
2445 | 		return result;
2446 | 	}
2447 | 
2448 | 	struct type_context_t {
2449 | 		bool is_const;
2450 | 		bool is_typedef;
2451 | 		bool has_type;
2452 | 		bool has_name;
2453 | 		std::string text;
2454 | 
2455 | 		type_context_t() :
2456 | 			is_const(false), is_typedef(false),
2457 | 			has_type(false), has_name(false) {}
2458 | 	};
2459 | 
2460 | 	// Types are resolved from right to left: we get the variable name first
2461 | 	// and then all specifiers (like const or pointer) in a chain of DW_AT_type
2462 | 	// DIEs. Call this function recursively until we get a complete type
2463 | 	// string.
2464 | 	static void set_parameter_string(
2465 | 			dwarf_fileobject& fobj, Dwarf_Die die, type_context_t &context) {
2466 | 		char *name;
2467 | 		Dwarf_Error error = DW_DLE_NE;
2468 | 
2469 | 		// typedefs contain also the base type, so we skip it and only
2470 | 		// print the typedef name
2471 | 		if (!context.is_typedef) {
2472 | 			if (dwarf_diename(die, &name, &error) == DW_DLV_OK) {
2473 | 				if (!context.text.empty()) {
2474 | 					context.text.insert(0, " ");
2475 | 				}
2476 | 				context.text.insert(0, std::string(name));
2477 | 				dwarf_dealloc(fobj.dwarf_handle.get(), name, DW_DLA_STRING);
2478 | 			}
2479 | 		} else {
2480 | 			context.is_typedef = false;
2481 | 			context.has_type = true;
2482 | 			if (context.is_const) {
2483 | 				context.text.insert(0, "const ");
2484 | 				context.is_const = false;
2485 | 			}
2486 | 		}
2487 | 
2488 | 		bool next_type_is_const = false;
2489 | 		bool is_keyword = true;
2490 | 
2491 | 		Dwarf_Half tag = 0;
2492 | 		Dwarf_Bool has_attr = 0;
2493 | 		if (dwarf_tag(die, &tag, &error) == DW_DLV_OK) {
2494 | 			switch(tag) {
2495 | 			case DW_TAG_structure_type:
2496 | 			case DW_TAG_union_type:
2497 | 			case DW_TAG_class_type:
2498 | 			case DW_TAG_enumeration_type:
2499 | 				context.has_type = true;
2500 | 				if (dwarf_hasattr(die, DW_AT_signature,
2501 | 								  &has_attr, &error) == DW_DLV_OK) {
2502 | 					// If we have a signature it means the type is defined
2503 | 					// in .debug_types, so we need to load the DIE pointed
2504 | 					// at by the signature and resolve it
2505 | 					if (has_attr) {
2506 | 						std::string type =
2507 | 							get_type_by_signature(fobj.dwarf_handle.get(), die);
2508 | 						if (context.is_const)
2509 | 							type.insert(0, "const ");
2510 | 
2511 | 						if (!context.text.empty())
2512 | 							context.text.insert(0, " ");
2513 | 						context.text.insert(0, type);
2514 | 					}
2515 | 
2516 | 					// Treat enums like typedefs, and skip printing its
2517 | 					// base type
2518 | 					context.is_typedef = (tag == DW_TAG_enumeration_type);
2519 | 				}
2520 | 				break;
2521 | 			case DW_TAG_const_type:
2522 | 				next_type_is_const = true;
2523 | 				break;
2524 | 			case DW_TAG_pointer_type:
2525 | 				context.text.insert(0, "*");
2526 | 				break;
2527 | 			case DW_TAG_reference_type:
2528 | 				context.text.insert(0, "&");
2529 | 				break;
2530 | 			case DW_TAG_restrict_type:
2531 | 				context.text.insert(0, "restrict ");
2532 | 				break;
2533 | 			case DW_TAG_rvalue_reference_type:
2534 | 				context.text.insert(0, "&&");
2535 | 				break;
2536 | 			case DW_TAG_volatile_type:
2537 | 				context.text.insert(0, "volatile ");
2538 | 				break;
2539 | 			case DW_TAG_typedef:
2540 | 				// Propagate the const-ness to the next type
2541 | 				// as typedefs are linked to its base type
2542 | 				next_type_is_const = context.is_const;
2543 | 				context.is_typedef = true;
2544 | 				context.has_type = true;
2545 | 				break;
2546 | 			case DW_TAG_base_type:
2547 | 				context.has_type = true;
2548 | 				break;
2549 | 			case DW_TAG_formal_parameter:
2550 | 				context.has_name = true;
2551 | 				break;
2552 | 			default:
2553 | 				is_keyword = false;
2554 | 				break;
2555 | 			}
2556 | 		}
2557 | 
2558 | 		if (!is_keyword && context.is_const) {
2559 | 			context.text.insert(0, "const ");
2560 | 		}
2561 | 
2562 | 		context.is_const = next_type_is_const;
2563 | 
2564 | 		Dwarf_Die ref = get_referenced_die(fobj.dwarf_handle.get(), die, DW_AT_type, true);
2565 | 		if (ref) {
2566 | 			set_parameter_string(fobj, ref, context);
2567 | 			dwarf_dealloc(fobj.dwarf_handle.get(), ref, DW_DLA_DIE);
2568 | 		}
2569 | 
2570 | 		if (!context.has_type && context.has_name) {
2571 | 			context.text.insert(0, "void ");
2572 | 			context.has_type = true;
2573 | 		}
2574 | 	}
2575 | 
2576 | 	// Resolve the function return type and parameters
2577 | 	static void set_function_parameters(std::string& function_name,
2578 | 										std::vector<std::string>& ns,
2579 | 										dwarf_fileobject& fobj, Dwarf_Die die) {
2580 | 		Dwarf_Debug dwarf = fobj.dwarf_handle.get();
2581 | 		Dwarf_Error error = DW_DLE_NE;
2582 | 		Dwarf_Die current_die = 0;
2583 | 		std::string parameters;
2584 | 		bool has_spec = true;
2585 | 		// Check if we have a spec DIE. If we do we use it as it contains
2586 | 		// more information, like parameter names.
2587 | 		Dwarf_Die spec_die = get_spec_die(fobj, die);
2588 | 		if (!spec_die) {
2589 | 			has_spec = false;
2590 | 			spec_die = die;
2591 | 		}
2592 | 
2593 | 		std::vector<std::string>::const_iterator it = ns.begin();
2594 | 		std::string ns_name;
2595 | 		for (it = ns.begin(); it < ns.end(); ++it) {
2596 | 			ns_name.append(*it).append("::");
2597 | 		}
2598 | 
2599 | 		if (!ns_name.empty()) {
2600 | 			function_name.insert(0, ns_name);
2601 | 		}
2602 | 
2603 | 		// See if we have a function return type. It can be either on the
2604 | 		// current die or in its spec one (usually true for inlined functions)
2605 | 		std::string return_type =
2606 | 				get_referenced_die_name(dwarf, die, DW_AT_type, true);
2607 | 		if (return_type.empty()) {
2608 | 			return_type =
2609 | 				get_referenced_die_name(dwarf, spec_die, DW_AT_type, true);
2610 | 		}
2611 | 		if (!return_type.empty()) {
2612 | 			return_type.append(" ");
2613 | 			function_name.insert(0, return_type);
2614 | 		}
2615 | 
2616 | 		if (dwarf_child(spec_die, &current_die, &error) == DW_DLV_OK) {
2617 | 			for(;;) {
2618 | 				Dwarf_Die sibling_die = 0;
2619 | 
2620 | 				Dwarf_Half tag_value;
2621 | 				dwarf_tag(current_die, &tag_value, &error);
2622 | 
2623 | 				if (tag_value == DW_TAG_formal_parameter) {
2624 | 					// Ignore artificial (ie, compiler generated) parameters
2625 | 					bool is_artificial = false;
2626 | 					Dwarf_Attribute attr_mem;
2627 | 					if (dwarf_attr(
2628 | 							current_die, DW_AT_artificial, &attr_mem, &error)
2629 | 							== DW_DLV_OK) {
2630 | 						Dwarf_Bool flag = 0;
2631 | 						if (dwarf_formflag(attr_mem, &flag, &error)
2632 | 								== DW_DLV_OK) {
2633 | 							is_artificial = flag != 0;
2634 | 						}
2635 | 						dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2636 | 					}
2637 | 
2638 | 					if (!is_artificial) {
2639 | 						type_context_t context;
2640 | 						set_parameter_string(fobj, current_die, context);
2641 | 
2642 | 						if (parameters.empty()) {
2643 | 							parameters.append("(");
2644 | 						} else {
2645 | 							parameters.append(", ");
2646 | 						}
2647 | 						parameters.append(context.text);
2648 | 					}
2649 | 				}
2650 | 
2651 | 				int result = dwarf_siblingof(
2652 | 						dwarf, current_die, &sibling_die, &error);
2653 | 				if (result == DW_DLV_ERROR) {
2654 | 					break;
2655 | 				} else if (result == DW_DLV_NO_ENTRY) {
2656 | 					break;
2657 | 				}
2658 | 
2659 | 				if (current_die != die) {
2660 | 					dwarf_dealloc(dwarf, current_die, DW_DLA_DIE);
2661 | 					current_die = 0;
2662 | 				}
2663 | 
2664 | 				current_die = sibling_die;
2665 | 			}
2666 | 		}
2667 | 		if (parameters.empty())
2668 | 			parameters = "(";
2669 | 		parameters.append(")");
2670 | 
2671 | 		// If we got a spec DIE we need to deallocate it
2672 | 		if (has_spec)
2673 | 			dwarf_dealloc(dwarf, spec_die, DW_DLA_DIE);
2674 | 
2675 | 		function_name.append(parameters);
2676 | 	}
2677 | 
2678 | 	// defined here because in C++98, template function cannot take locally
2679 | 	// defined types... grrr.
2680 | 	struct inliners_search_cb {
2681 | 		void operator()(Dwarf_Die die, std::vector<std::string>& ns) {
2682 | 			Dwarf_Error error = DW_DLE_NE;
2683 | 			Dwarf_Half tag_value;
2684 | 			Dwarf_Attribute attr_mem;
2685 | 			Dwarf_Debug dwarf = fobj.dwarf_handle.get();
2686 | 
2687 | 			dwarf_tag(die, &tag_value, &error);
2688 | 
2689 | 			switch (tag_value) {
2690 | 				char* name;
2691 | 				case DW_TAG_subprogram:
2692 | 					if (!trace.source.function.empty())
2693 | 						break;
2694 | 					if (dwarf_diename(die, &name, &error) == DW_DLV_OK) {
2695 | 						trace.source.function = std::string(name);
2696 | 						dwarf_dealloc(dwarf, name, DW_DLA_STRING);
2697 | 					} else {
2698 | 						// We don't have a function name in this DIE.
2699 | 						// Check if there is a referenced non-defining
2700 | 						// declaration.
2701 | 						trace.source.function = get_referenced_die_name(
2702 | 								dwarf, die, DW_AT_abstract_origin, true);
2703 | 						if (trace.source.function.empty()) {
2704 | 							trace.source.function = get_referenced_die_name(
2705 | 									dwarf, die, DW_AT_specification, true);
2706 | 						}
2707 | 					}
2708 | 
2709 | 					// Append the function parameters, if available
2710 | 					set_function_parameters(
2711 | 							trace.source.function, ns, fobj, die);
2712 | 
2713 | 					// If the object function name is empty, it's possible that
2714 | 					// there is no dynamic symbol table (maybe the executable
2715 | 					// was stripped or not built with -rdynamic). See if we have
2716 | 					// a DWARF linkage name to use instead. We try both
2717 | 					// linkage_name and MIPS_linkage_name because the MIPS tag
2718 | 					// was the unofficial one until it was adopted in DWARF4.
2719 | 					// Old gcc versions generate MIPS_linkage_name
2720 | 					if (trace.object_function.empty()) {
2721 | 						details::demangler demangler;
2722 | 
2723 | 						if (dwarf_attr(die, DW_AT_linkage_name,
2724 | 								&attr_mem, &error) != DW_DLV_OK) {
2725 | 							if (dwarf_attr(die, DW_AT_MIPS_linkage_name,
2726 | 									&attr_mem, &error) != DW_DLV_OK) {
2727 | 								break;
2728 | 							}
2729 | 						}
2730 | 
2731 | 						char* linkage;
2732 | 						if (dwarf_formstring(attr_mem, &linkage, &error)
2733 | 								== DW_DLV_OK) {
2734 | 							trace.object_function = demangler.demangle(linkage);
2735 | 							dwarf_dealloc(dwarf, linkage, DW_DLA_STRING);
2736 | 						}
2737 | 						dwarf_dealloc(dwarf, name, DW_DLA_ATTR);
2738 | 					}
2739 | 					break;
2740 | 
2741 | 				case DW_TAG_inlined_subroutine:
2742 | 					ResolvedTrace::SourceLoc sloc;
2743 | 
2744 | 					if (dwarf_diename(die, &name, &error) == DW_DLV_OK) {
2745 | 						sloc.function = std::string(name);
2746 | 						dwarf_dealloc(dwarf, name, DW_DLA_STRING);
2747 | 					} else {
2748 | 						// We don't have a name for this inlined DIE, it could
2749 | 						// be that there is an abstract origin instead.
2750 | 						// Get the DW_AT_abstract_origin value, which is a
2751 | 						// reference to the source DIE and try to get its name
2752 | 						sloc.function = get_referenced_die_name(
2753 | 								dwarf, die, DW_AT_abstract_origin, true);
2754 | 					}
2755 | 
2756 | 					set_function_parameters(sloc.function, ns, fobj, die);
2757 | 
2758 | 					std::string file = die_call_file(dwarf, die, cu_die);
2759 | 					if (!file.empty())
2760 | 						sloc.filename = file;
2761 | 
2762 | 					Dwarf_Unsigned number = 0;
2763 | 					if (dwarf_attr(die, DW_AT_call_line, &attr_mem, &error)
2764 | 							== DW_DLV_OK) {
2765 | 						if (dwarf_formudata(attr_mem, &number, &error)
2766 | 								== DW_DLV_OK) {
2767 | 							sloc.line = number;
2768 | 						}
2769 | 						dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2770 | 					}
2771 | 
2772 | 					if (dwarf_attr(die, DW_AT_call_column, &attr_mem, &error)
2773 | 							== DW_DLV_OK) {
2774 | 						if (dwarf_formudata(attr_mem, &number, &error)
2775 | 								== DW_DLV_OK) {
2776 | 							sloc.col = number;
2777 | 						}
2778 | 						dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2779 | 					}
2780 | 
2781 | 					trace.inliners.push_back(sloc);
2782 | 					break;
2783 | 			};
2784 | 		}
2785 | 		ResolvedTrace& trace;
2786 | 		dwarf_fileobject& fobj;
2787 | 		Dwarf_Die cu_die;
2788 | 		inliners_search_cb(ResolvedTrace& t, dwarf_fileobject& f, Dwarf_Die c)
2789 | 			: trace(t), fobj(f), cu_die(c) {}
2790 | 	};
2791 | 
2792 | 	static Dwarf_Die find_fundie_by_pc(dwarf_fileobject& fobj,
2793 | 					Dwarf_Die parent_die, Dwarf_Addr pc, Dwarf_Die result) {
2794 | 		Dwarf_Die current_die = 0;
2795 | 		Dwarf_Error error = DW_DLE_NE;
2796 | 		Dwarf_Debug dwarf = fobj.dwarf_handle.get();
2797 | 
2798 | 		if (dwarf_child(parent_die, &current_die, &error) != DW_DLV_OK) {
2799 | 			return NULL;
2800 | 		}
2801 | 
2802 | 		for(;;) {
2803 | 			Dwarf_Die sibling_die = 0;
2804 | 			Dwarf_Half tag_value;
2805 | 			dwarf_tag(current_die, &tag_value, &error);
2806 | 
2807 | 			switch (tag_value) {
2808 | 				case DW_TAG_subprogram:
2809 | 				case DW_TAG_inlined_subroutine:
2810 | 					if (die_has_pc(fobj, current_die, pc)) {
2811 | 						return current_die;
2812 | 					}
2813 | 			};
2814 | 			bool declaration = false;
2815 | 			Dwarf_Attribute attr_mem;
2816 | 			if (dwarf_attr(current_die, DW_AT_declaration, &attr_mem, &error)
2817 | 					== DW_DLV_OK) {
2818 | 				Dwarf_Bool flag = 0;
2819 | 				if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) {
2820 | 					declaration = flag != 0;
2821 | 				}
2822 | 				dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2823 | 			}
2824 | 
2825 | 			if (!declaration) {
2826 | 				// let's be curious and look deeper in the tree, functions are
2827 | 				// not necessarily at the first level, but might be nested
2828 | 				// inside a namespace, structure, a function, an inlined
2829 | 				// function etc.
2830 | 				Dwarf_Die die_mem = 0;
2831 | 				Dwarf_Die indie = find_fundie_by_pc(
2832 | 						fobj, current_die, pc, die_mem);
2833 | 				if (indie) {
2834 | 					result = die_mem;
2835 | 					return result;
2836 | 				}
2837 | 			}
2838 | 
2839 | 			int res = dwarf_siblingof(
2840 | 					dwarf, current_die, &sibling_die, &error);
2841 | 			if (res == DW_DLV_ERROR) {
2842 | 				return NULL;
2843 | 			} else if (res == DW_DLV_NO_ENTRY) {
2844 | 				break;
2845 | 			}
2846 | 
2847 | 			if (current_die != parent_die) {
2848 | 				dwarf_dealloc(dwarf, current_die, DW_DLA_DIE);
2849 | 				current_die = 0;
2850 | 			}
2851 | 
2852 | 			current_die = sibling_die;
2853 | 		}
2854 | 		return NULL;
2855 | 	}
2856 | 
2857 | 	template <typename CB>
2858 | 		static bool deep_first_search_by_pc(dwarf_fileobject& fobj,
2859 | 						Dwarf_Die parent_die, Dwarf_Addr pc,
2860 | 						std::vector<std::string>& ns, CB cb) {
2861 | 		Dwarf_Die current_die = 0;
2862 | 		Dwarf_Debug dwarf = fobj.dwarf_handle.get();
2863 | 		Dwarf_Error error = DW_DLE_NE;
2864 | 
2865 | 		if (dwarf_child(parent_die, &current_die, &error) != DW_DLV_OK) {
2866 | 			return false;
2867 | 		}
2868 | 
2869 | 		bool branch_has_pc = false;
2870 | 		bool has_namespace = false;
2871 | 		for(;;) {
2872 | 			Dwarf_Die sibling_die = 0;
2873 | 
2874 | 			Dwarf_Half tag;
2875 | 			if (dwarf_tag(current_die, &tag, &error) == DW_DLV_OK) {
2876 | 				if (tag == DW_TAG_namespace || tag == DW_TAG_class_type) {
2877 | 					char* ns_name = NULL;
2878 | 					if (dwarf_diename(current_die, &ns_name, &error)
2879 | 							== DW_DLV_OK) {
2880 | 						if (ns_name) {
2881 | 							ns.push_back(std::string(ns_name));
2882 | 						} else {
2883 | 							ns.push_back("<unknown>");
2884 | 						}
2885 | 						dwarf_dealloc(dwarf, ns_name,  DW_DLA_STRING);
2886 | 					} else {
2887 | 						ns.push_back("<unknown>");
2888 | 					}
2889 | 					has_namespace = true;
2890 | 				}
2891 | 			}
2892 | 
2893 | 			bool declaration = false;
2894 | 			Dwarf_Attribute attr_mem;
2895 | 			if (tag != DW_TAG_class_type &&
2896 | 				dwarf_attr(current_die, DW_AT_declaration, &attr_mem, &error)
2897 | 					== DW_DLV_OK) {
2898 | 				Dwarf_Bool flag = 0;
2899 | 				if (dwarf_formflag(attr_mem, &flag, &error) == DW_DLV_OK) {
2900 | 					declaration = flag != 0;
2901 | 				}
2902 | 				dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2903 | 			}
2904 | 
2905 | 			if (!declaration) {
2906 | 				// let's be curious and look deeper in the tree, function are
2907 | 				// not necessarily at the first level, but might be nested
2908 | 				// inside a namespace, structure, a function, an inlined
2909 | 				// function etc.
2910 | 				branch_has_pc = deep_first_search_by_pc(
2911 | 												fobj, current_die, pc, ns, cb);
2912 | 			}
2913 | 
2914 | 			if (!branch_has_pc) {
2915 | 				branch_has_pc = die_has_pc(fobj, current_die, pc);
2916 | 			}
2917 | 
2918 | 			if (branch_has_pc) {
2919 | 				cb(current_die, ns);
2920 | 			}
2921 | 
2922 | 			int result = dwarf_siblingof(
2923 | 					dwarf, current_die, &sibling_die, &error);
2924 | 			if (result == DW_DLV_ERROR) {
2925 | 				return false;
2926 | 			} else if (result == DW_DLV_NO_ENTRY) {
2927 | 				break;
2928 | 			}
2929 | 
2930 | 			if (current_die != parent_die) {
2931 | 				dwarf_dealloc(dwarf, current_die, DW_DLA_DIE);
2932 | 				current_die = 0;
2933 | 			}
2934 | 
2935 | 			if (has_namespace) {
2936 | 				has_namespace = false;
2937 | 				ns.pop_back();
2938 | 			}
2939 | 			current_die = sibling_die;
2940 | 		}
2941 | 
2942 | 		if (has_namespace) {
2943 | 			ns.pop_back();
2944 | 		}
2945 | 		return branch_has_pc;
2946 | 	}
2947 | 
2948 | 	static std::string die_call_file(
2949 | 			Dwarf_Debug dwarf, Dwarf_Die die, Dwarf_Die cu_die) {
2950 | 		Dwarf_Attribute attr_mem;
2951 | 		Dwarf_Error error = DW_DLE_NE;
2952 | 		Dwarf_Signed file_index;
2953 | 
2954 | 		std::string file;
2955 | 
2956 | 		if (dwarf_attr(die, DW_AT_call_file, &attr_mem, &error) == DW_DLV_OK) {
2957 | 			if (dwarf_formsdata(attr_mem, &file_index, &error) != DW_DLV_OK) {
2958 | 				file_index = 0;
2959 | 			}
2960 | 			dwarf_dealloc(dwarf, attr_mem, DW_DLA_ATTR);
2961 | 
2962 | 			if (file_index == 0) {
2963 | 				return file;
2964 | 			}
2965 | 
2966 | 			char **srcfiles = 0;
2967 | 			Dwarf_Signed file_count = 0;
2968 | 			if (dwarf_srcfiles(cu_die, &srcfiles, &file_count, &error)
2969 | 					== DW_DLV_OK) {
2970 | 				if (file_index <= file_count)
2971 | 					file = std::string(srcfiles[file_index - 1]);
2972 | 
2973 | 				// Deallocate all strings!
2974 | 				for (int i = 0; i < file_count; ++i) {
2975 | 					dwarf_dealloc(dwarf, srcfiles[i], DW_DLA_STRING);
2976 | 				}
2977 | 				dwarf_dealloc(dwarf, srcfiles, DW_DLA_LIST);
2978 | 			}
2979 | 		}
2980 | 		return file;
2981 | 	}
2982 | 
2983 | 
2984 | 	Dwarf_Die find_die(dwarf_fileobject& fobj, Dwarf_Addr addr)
2985 | 	{
2986 | 		// Let's get to work! First see if we have a debug_aranges section so
2987 | 		// we can speed up the search
2988 | 
2989 | 		Dwarf_Debug dwarf = fobj.dwarf_handle.get();
2990 | 		Dwarf_Error error = DW_DLE_NE;
2991 | 		Dwarf_Arange *aranges;
2992 | 		Dwarf_Signed arange_count;
2993 | 
2994 | 		Dwarf_Die returnDie;
2995 | 		bool found = false;
2996 | 		if (dwarf_get_aranges(
2997 | 				dwarf, &aranges, &arange_count, &error) != DW_DLV_OK) {
2998 | 			aranges = NULL;
2999 | 		}
3000 | 
3001 | 		if (aranges) {
3002 | 			// We have aranges. Get the one where our address is.
3003 | 			Dwarf_Arange arange;
3004 | 			if (dwarf_get_arange(
3005 | 					aranges, arange_count, addr, &arange, &error)
3006 | 						== DW_DLV_OK) {
3007 | 
3008 | 				// We found our address. Get the compilation-unit DIE offset
3009 | 				// represented by the given address range.
3010 | 				Dwarf_Off cu_die_offset;
3011 | 				if (dwarf_get_cu_die_offset(arange, &cu_die_offset, &error)
3012 | 						== DW_DLV_OK) {
3013 | 					// Get the DIE at the offset returned by the aranges search.
3014 | 					// We set is_info to 1 to specify that the offset is from
3015 | 					// the .debug_info section (and not .debug_types)
3016 | 					int dwarf_result = dwarf_offdie_b(
3017 | 							dwarf, cu_die_offset, 1, &returnDie, &error);
3018 | 
3019 | 					found = dwarf_result == DW_DLV_OK;
3020 | 				}
3021 | 				dwarf_dealloc(dwarf, arange, DW_DLA_ARANGE);
3022 | 			}
3023 | 		}
3024 | 
3025 | 		if (found)
3026 | 			return returnDie; // The caller is responsible for freeing the die
3027 | 
3028 | 		// The search for aranges failed. Try to find our address by scanning
3029 | 		// all compilation units.
3030 | 		Dwarf_Unsigned next_cu_header;
3031 | 		while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0,
3032 | 				&next_cu_header, 0, &error) == DW_DLV_OK) {
3033 | 			if (dwarf_siblingof(dwarf, 0, &returnDie, &error) == DW_DLV_OK) {
3034 | 				if (die_has_pc(fobj, returnDie, addr)) {
3035 | 					found = true;
3036 | 					break;
3037 | 				}
3038 | 				dwarf_dealloc(dwarf, returnDie, DW_DLA_DIE);
3039 | 			}
3040 | 		}
3041 | 
3042 | 		while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0,
3043 | 				&next_cu_header, 0, &error) == DW_DLV_OK) {
3044 | 			// Reset the cu header state. Unfortunately, libdwarf's
3045 | 			// next_cu_header API keeps its own iterator per Dwarf_Debug that
3046 | 			// can't be reset. We need to keep fetching elements until the end.
3047 | 		}
3048 | 
3049 | 		if (found)
3050 | 			return returnDie;
3051 | 
3052 | 
3053 | 		// We couldn't find any compilation units with ranges or a high/low pc.
3054 | 		// Try again by looking at all DIEs in all compilation units.
3055 | 		Dwarf_Die cudie;
3056 | 		while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0,
3057 | 				&next_cu_header, 0, &error) == DW_DLV_OK) {
3058 | 			if (dwarf_siblingof(dwarf, 0, &cudie, &error) == DW_DLV_OK) {
3059 | 				Dwarf_Die die_mem = 0;
3060 | 				Dwarf_Die resultDie = find_fundie_by_pc(
3061 | 						fobj, cudie, addr, die_mem);
3062 | 
3063 | 				if (resultDie) {
3064 | 					found = true;
3065 | 					break;
3066 | 				}
3067 | 			}
3068 | 		}
3069 | 
3070 | 		while (dwarf_next_cu_header_d(dwarf, 1, 0, 0, 0, 0, 0, 0, 0, 0,
3071 | 				&next_cu_header, 0, &error) == DW_DLV_OK) {
3072 | 			// Reset the cu header state. Unfortunately, libdwarf's
3073 | 			// next_cu_header API keeps its own iterator per Dwarf_Debug that
3074 | 			// can't be reset. We need to keep fetching elements until the end.
3075 | 		}
3076 | 
3077 | 		if (found)
3078 | 			return cudie;
3079 | 
3080 | 		// We failed.
3081 | 		return NULL;
3082 | 	}
3083 | };
3084 | #endif // BACKWARD_HAS_DWARF == 1
3085 | 
3086 | template<>
3087 | class TraceResolverImpl<system_tag::linux_tag>:
3088 | 	public TraceResolverLinuxImpl<trace_resolver_tag::current> {};
3089 | 
3090 | #endif // BACKWARD_SYSTEM_LINUX
3091 | 
3092 | #ifdef BACKWARD_SYSTEM_DARWIN
3093 | 
3094 | template <typename STACKTRACE_TAG>
3095 | class TraceResolverDarwinImpl;
3096 | 
3097 | template <>
3098 | class TraceResolverDarwinImpl<trace_resolver_tag::backtrace_symbol>:
3099 | 	public TraceResolverImplBase {
3100 | public:
3101 | 	template <class ST>
3102 | 		void load_stacktrace(ST& st) {
3103 | 			using namespace details;
3104 | 			if (st.size() == 0) {
3105 | 				return;
3106 | 			}
3107 | 			_symbols.reset(
3108 | 					backtrace_symbols(st.begin(), st.size())
3109 | 					);
3110 | 		}
3111 | 
3112 | 	ResolvedTrace resolve(ResolvedTrace trace) {
3113 | 		// parse:
3114 | 		// <n>  <file>  <addr>  <mangled-name> + <offset>
3115 | 		char* filename = _symbols[trace.idx];
3116 | 
3117 | 		// skip "<n>  "
3118 | 		while(*filename && *filename != ' ') filename++;
3119 | 		while(*filename == ' ') filename++;
3120 | 
3121 | 		// find start of <mangled-name> from end (<file> may contain a space)
3122 | 		char* p = filename + strlen(filename) - 1;
3123 | 		// skip to start of " + <offset>"
3124 | 		while(p > filename && *p != ' ') p--;
3125 | 		while(p > filename && *p == ' ') p--;
3126 | 		while(p > filename && *p != ' ') p--;
3127 | 		while(p > filename && *p == ' ') p--;
3128 | 		char *funcname_end = p + 1;
3129 | 
3130 | 		// skip to start of "<manged-name>"
3131 | 		while(p > filename && *p != ' ') p--;
3132 | 		char *funcname = p + 1;
3133 | 
3134 | 		// skip to start of "  <addr>  "
3135 | 		while(p > filename && *p == ' ') p--;
3136 | 		while(p > filename && *p != ' ') p--;
3137 | 		while(p > filename && *p == ' ') p--;
3138 | 
3139 | 		// skip "<file>", handling the case where it contains a
3140 | 		char* filename_end = p + 1;
3141 | 		if (p == filename) {
3142 | 			// something went wrong, give up
3143 | 			filename_end = filename + strlen(filename);
3144 | 			funcname = filename_end;
3145 | 		}
3146 | 		trace.object_filename.assign(filename, filename_end); // ok even if filename_end is the ending \0 (then we assign entire string)
3147 | 
3148 | 		if (*funcname) { // if it's not end of string
3149 | 			*funcname_end = '\0';
3150 | 
3151 | 			trace.object_function = this->demangle(funcname);
3152 | 			trace.object_function += " ";
3153 | 			trace.object_function += (funcname_end + 1);
3154 | 			trace.source.function = trace.object_function; // we cannot do better.
3155 | 		}
3156 | 		return trace;
3157 | 	}
3158 | 
3159 | private:
3160 | 	details::handle<char**> _symbols;
3161 | };
3162 | 
3163 | template<>
3164 | class TraceResolverImpl<system_tag::darwin_tag>:
3165 | 	public TraceResolverDarwinImpl<trace_resolver_tag::current> {};
3166 | 
3167 | #endif // BACKWARD_SYSTEM_DARWIN
3168 | 
3169 | class TraceResolver:
3170 | 	public TraceResolverImpl<system_tag::current_tag> {};
3171 | 
3172 | /*************** CODE SNIPPET ***************/
3173 | 
3174 | class SourceFile {
3175 | public:
3176 | 	typedef std::vector<std::pair<unsigned, std::string> > lines_t;
3177 | 
3178 | 	SourceFile() {}
3179 | 	SourceFile(const std::string& path): _file(new std::ifstream(path.c_str())) {}
3180 | 	bool is_open() const { return _file->is_open(); }
3181 | 
3182 | 	lines_t& get_lines(unsigned line_start, unsigned line_count, lines_t& lines) {
3183 | 		using namespace std;
3184 | 		// This function make uses of the dumbest algo ever:
3185 | 		//	1) seek(0)
3186 | 		//	2) read lines one by one and discard until line_start
3187 | 		//	3) read line one by one until line_start + line_count
3188 | 		//
3189 | 		// If you are getting snippets many time from the same file, it is
3190 | 		// somewhat a waste of CPU, feel free to benchmark and propose a
3191 | 		// better solution ;)
3192 | 
3193 | 		_file->clear();
3194 | 		_file->seekg(0);
3195 | 		string line;
3196 | 		unsigned line_idx;
3197 | 
3198 | 		for (line_idx = 1; line_idx < line_start; ++line_idx) {
3199 | 			std::getline(*_file, line);
3200 | 			if (!*_file) {
3201 | 				return lines;
3202 | 			}
3203 | 		}
3204 | 
3205 | 		// think of it like a lambda in C++98 ;)
3206 | 		// but look, I will reuse it two times!
3207 | 		// What a good boy am I.
3208 | 		struct isspace {
3209 | 			bool operator()(char c) {
3210 | 				return std::isspace(c);
3211 | 			}
3212 | 		};
3213 | 
3214 | 		bool started = false;
3215 | 		for (; line_idx < line_start + line_count; ++line_idx) {
3216 | 			getline(*_file, line);
3217 | 			if (!*_file) {
3218 | 				return lines;
3219 | 			}
3220 | 			if (!started) {
3221 | 				if (std::find_if(line.begin(), line.end(),
3222 | 							not_isspace()) == line.end())
3223 | 					continue;
3224 | 				started = true;
3225 | 			}
3226 | 			lines.push_back(make_pair(line_idx, line));
3227 | 		}
3228 | 
3229 | 		lines.erase(
3230 | 				std::find_if(lines.rbegin(), lines.rend(),
3231 | 					not_isempty()).base(), lines.end()
3232 | 				);
3233 | 		return lines;
3234 | 	}
3235 | 
3236 | 	lines_t get_lines(unsigned line_start, unsigned line_count) {
3237 | 		lines_t lines;
3238 | 		return get_lines(line_start, line_count, lines);
3239 | 	}
3240 | 
3241 | 	// there is no find_if_not in C++98, lets do something crappy to
3242 | 	// workaround.
3243 | 	struct not_isspace {
3244 | 		bool operator()(char c) {
3245 | 			return !std::isspace(c);
3246 | 		}
3247 | 	};
3248 | 	// and define this one here because C++98 is not happy with local defined
3249 | 	// struct passed to template functions, fuuuu.
3250 | 	struct not_isempty {
3251 | 		bool operator()(const lines_t::value_type& p) {
3252 | 			return !(std::find_if(p.second.begin(), p.second.end(),
3253 | 						not_isspace()) == p.second.end());
3254 | 		}
3255 | 	};
3256 | 
3257 | 	void swap(SourceFile& b) {
3258 | 		_file.swap(b._file);
3259 | 	}
3260 | 
3261 | #ifdef BACKWARD_ATLEAST_CXX11
3262 | 	SourceFile(SourceFile&& from): _file(0) {
3263 | 		swap(from);
3264 | 	}
3265 | 	SourceFile& operator=(SourceFile&& from) {
3266 | 		swap(from); return *this;
3267 | 	}
3268 | #else
3269 | 	explicit SourceFile(const SourceFile& from) {
3270 | 		// some sort of poor man's move semantic.
3271 | 		swap(const_cast<SourceFile&>(from));
3272 | 	}
3273 | 	SourceFile& operator=(const SourceFile& from) {
3274 | 		// some sort of poor man's move semantic.
3275 | 		swap(const_cast<SourceFile&>(from)); return *this;
3276 | 	}
3277 | #endif
3278 | 
3279 | private:
3280 | 	details::handle<std::ifstream*,
3281 | 		details::default_delete<std::ifstream*>
3282 | 			> _file;
3283 | 
3284 | #ifdef BACKWARD_ATLEAST_CXX11
3285 | 	SourceFile(const SourceFile&) = delete;
3286 | 	SourceFile& operator=(const SourceFile&) = delete;
3287 | #endif
3288 | };
3289 | 
3290 | class SnippetFactory {
3291 | public:
3292 | 	typedef SourceFile::lines_t lines_t;
3293 | 
3294 | 	lines_t get_snippet(const std::string& filename,
3295 | 			unsigned line_start, unsigned context_size) {
3296 | 
3297 | 		SourceFile& src_file = get_src_file(filename);
3298 | 		unsigned start = line_start - context_size / 2;
3299 | 		return src_file.get_lines(start, context_size);
3300 | 	}
3301 | 
3302 | 	lines_t get_combined_snippet(
3303 | 			const std::string& filename_a, unsigned line_a,
3304 | 			const std::string& filename_b, unsigned line_b,
3305 | 			unsigned context_size) {
3306 | 		SourceFile& src_file_a = get_src_file(filename_a);
3307 | 		SourceFile& src_file_b = get_src_file(filename_b);
3308 | 
3309 | 		lines_t lines = src_file_a.get_lines(line_a - context_size / 4,
3310 | 				context_size / 2);
3311 | 		src_file_b.get_lines(line_b - context_size / 4, context_size / 2,
3312 | 				lines);
3313 | 		return lines;
3314 | 	}
3315 | 
3316 | 	lines_t get_coalesced_snippet(const std::string& filename,
3317 | 			unsigned line_a, unsigned line_b, unsigned context_size) {
3318 | 		SourceFile& src_file = get_src_file(filename);
3319 | 
3320 | 		using std::min; using std::max;
3321 | 		unsigned a = min(line_a, line_b);
3322 | 		unsigned b = max(line_a, line_b);
3323 | 
3324 | 		if ((b - a) < (context_size / 3)) {
3325 | 			return src_file.get_lines((a + b - context_size + 1) / 2,
3326 | 					context_size);
3327 | 		}
3328 | 
3329 | 		lines_t lines = src_file.get_lines(a - context_size / 4,
3330 | 				context_size / 2);
3331 | 		src_file.get_lines(b - context_size / 4, context_size / 2, lines);
3332 | 		return lines;
3333 | 	}
3334 | 
3335 | 
3336 | private:
3337 | 	typedef details::hashtable<std::string, SourceFile>::type src_files_t;
3338 | 	src_files_t _src_files;
3339 | 
3340 | 	SourceFile& get_src_file(const std::string& filename) {
3341 | 		src_files_t::iterator it = _src_files.find(filename);
3342 | 		if (it != _src_files.end()) {
3343 | 			return it->second;
3344 | 		}
3345 | 		SourceFile& new_src_file = _src_files[filename];
3346 | 		new_src_file = SourceFile(filename);
3347 | 		return new_src_file;
3348 | 	}
3349 | };
3350 | 
3351 | /*************** PRINTER ***************/
3352 | 
3353 | namespace ColorMode {
3354 | 	enum type {
3355 | 		automatic,
3356 | 		never,
3357 | 		always
3358 | 	};
3359 | }
3360 | 
3361 | class cfile_streambuf: public std::streambuf {
3362 | public:
3363 | 	cfile_streambuf(FILE *_sink): sink(_sink) {}
3364 | 	int_type underflow() override { return traits_type::eof(); }
3365 | 	int_type overflow(int_type ch) override {
3366 | 		if (traits_type::not_eof(ch) && fwrite(&ch, sizeof ch, 1, sink) == 1) {
3367 | 				return ch;
3368 | 		}
3369 | 		return traits_type::eof();
3370 | 	}
3371 | 
3372 | 	std::streamsize xsputn(const char_type* s, std::streamsize count) override {
3373 | 		return fwrite(s, sizeof *s, count, sink);
3374 | 	}
3375 | 
3376 | #ifdef BACKWARD_ATLEAST_CXX11
3377 | public:
3378 | 	cfile_streambuf(const cfile_streambuf&) = delete;
3379 | 	cfile_streambuf& operator=(const cfile_streambuf&) = delete;
3380 | #else
3381 | private:
3382 | 	cfile_streambuf(const cfile_streambuf &);
3383 | 	cfile_streambuf &operator= (const cfile_streambuf &);
3384 | #endif
3385 | 
3386 | private:
3387 | 	FILE *sink;
3388 | 	std::vector<char> buffer;
3389 | };
3390 | 
3391 | #ifdef BACKWARD_SYSTEM_LINUX
3392 | 
3393 | namespace Color {
3394 | 	enum type {
3395 | 		yellow = 33,
3396 | 		purple = 35,
3397 | 		reset  = 39
3398 | 	};
3399 | } // namespace Color
3400 | 
3401 | class Colorize {
3402 | public:
3403 | 	Colorize(std::ostream& os):
3404 | 		_os(os), _reset(false), _enabled(false) {}
3405 | 
3406 | 	void activate(ColorMode::type mode) {
3407 | 		_enabled = mode == ColorMode::always;
3408 | 	}
3409 | 
3410 | 	void activate(ColorMode::type mode, FILE* fp) {
3411 | 		activate(mode, fileno(fp));
3412 | 	}
3413 | 
3414 | 	void set_color(Color::type ccode) {
3415 | 		if (!_enabled) return;
3416 | 
3417 | 		// I assume that the terminal can handle basic colors. Seriously I
3418 | 		// don't want to deal with all the termcap shit.
3419 | 		_os << "\033[" << static_cast<int>(ccode) << "m";
3420 | 		_reset = (ccode != Color::reset);
3421 | 	}
3422 | 
3423 | 	~Colorize() {
3424 | 		if (_reset) {
3425 | 			set_color(Color::reset);
3426 | 		}
3427 | 	}
3428 | 
3429 | private:
3430 | 	void activate(ColorMode::type mode, int fd) {
3431 | 		activate(mode == ColorMode::automatic && isatty(fd) ? ColorMode::always : mode);
3432 | 	}
3433 | 
3434 | 	std::ostream& _os;
3435 | 	bool          _reset;
3436 | 	bool          _enabled;
3437 | };
3438 | 
3439 | #else // ndef BACKWARD_SYSTEM_LINUX
3440 | 
3441 | namespace Color {
3442 | 	enum type {
3443 | 		yellow = 0,
3444 | 		purple = 0,
3445 | 		reset  = 0
3446 | 	};
3447 | } // namespace Color
3448 | 
3449 | class Colorize {
3450 | public:
3451 | 	Colorize(std::ostream&) {}
3452 | 	void activate(ColorMode::type) {}
3453 | 	void activate(ColorMode::type, FILE*) {}
3454 | 	void set_color(Color::type) {}
3455 | };
3456 | 
3457 | #endif // BACKWARD_SYSTEM_LINUX
3458 | 
3459 | class Printer {
3460 | public:
3461 | 
3462 | 	bool snippet;
3463 | 	ColorMode::type color_mode;
3464 | 	bool address;
3465 | 	bool object;
3466 | 	int inliner_context_size;
3467 | 	int trace_context_size;
3468 | 
3469 | 	Printer():
3470 | 		snippet(true),
3471 | 		color_mode(ColorMode::automatic),
3472 | 		address(false),
3473 | 		object(false),
3474 | 		inliner_context_size(5),
3475 | 		trace_context_size(7)
3476 | 		{}
3477 | 
3478 | 	template <typename ST>
3479 | 		FILE* print(ST& st, FILE* fp = stderr) {
3480 | 			cfile_streambuf obuf(fp);
3481 | 			std::ostream os(&obuf);
3482 | 			Colorize colorize(os);
3483 | 			colorize.activate(color_mode, fp);
3484 | 			print_stacktrace(st, os, colorize);
3485 | 			return fp;
3486 | 		}
3487 | 
3488 | 	template <typename ST>
3489 | 		std::ostream& print(ST& st, std::ostream& os) {
3490 | 			Colorize colorize(os);
3491 | 			colorize.activate(color_mode);
3492 | 			print_stacktrace(st, os, colorize);
3493 | 			return os;
3494 | 		}
3495 | 
3496 | 	template <typename IT>
3497 | 		FILE* print(IT begin, IT end, FILE* fp = stderr, size_t thread_id = 0) {
3498 | 			cfile_streambuf obuf(fp);
3499 | 			std::ostream os(&obuf);
3500 | 			Colorize colorize(os);
3501 | 			colorize.activate(color_mode, fp);
3502 | 			print_stacktrace(begin, end, os, thread_id, colorize);
3503 | 			return fp;
3504 | 		}
3505 | 
3506 | 	template <typename IT>
3507 | 		std::ostream& print(IT begin, IT end, std::ostream& os, size_t thread_id = 0) {
3508 | 			Colorize colorize(os);
3509 | 			colorize.activate(color_mode);
3510 | 			print_stacktrace(begin, end, os, thread_id, colorize);
3511 | 			return os;
3512 | 		}
3513 | 
3514 | private:
3515 | 	TraceResolver  _resolver;
3516 | 	SnippetFactory _snippets;
3517 | 
3518 | 	template <typename ST>
3519 | 		void print_stacktrace(ST& st, std::ostream& os, Colorize& colorize) {
3520 | 			print_header(os, st.thread_id());
3521 | 			_resolver.load_stacktrace(st);
3522 | 			for (size_t trace_idx = st.size(); trace_idx > 0; --trace_idx) {
3523 | 				print_trace(os, _resolver.resolve(st[trace_idx-1]), colorize);
3524 | 			}
3525 | 		}
3526 | 
3527 | 	template <typename IT>
3528 | 		void print_stacktrace(IT begin, IT end, std::ostream& os, size_t thread_id, Colorize& colorize) {
3529 | 			print_header(os, thread_id);
3530 | 			for (; begin != end; ++begin) {
3531 | 				print_trace(os, *begin, colorize);
3532 | 			}
3533 | 		}
3534 | 
3535 | 	void print_header(std::ostream& os, size_t thread_id) {
3536 | 		os << "Stack trace (most recent call last)";
3537 | 		if (thread_id) {
3538 | 			os << " in thread " << thread_id;
3539 | 		}
3540 | 		os << ":\n";
3541 | 	}
3542 | 
3543 | 	void print_trace(std::ostream& os, const ResolvedTrace& trace,
3544 | 			Colorize& colorize) {
3545 | 		os << "#"
3546 | 		   << std::left << std::setw(2) << trace.idx
3547 | 		   << std::right;
3548 | 		bool already_indented = true;
3549 | 
3550 | 		if (!trace.source.filename.size() || object) {
3551 | 			os << "   Object \""
3552 | 			   << trace.object_filename
3553 | 			   << "\", at "
3554 | 			   << trace.addr
3555 | 			   << ", in "
3556 | 			   << trace.object_function
3557 | 			   << "\n";
3558 | 			already_indented = false;
3559 | 		}
3560 | 
3561 | 		for (size_t inliner_idx = trace.inliners.size();
3562 | 				inliner_idx > 0; --inliner_idx) {
3563 | 			if (!already_indented) {
3564 | 				os << "   ";
3565 | 			}
3566 | 			const ResolvedTrace::SourceLoc& inliner_loc
3567 | 				= trace.inliners[inliner_idx-1];
3568 | 			print_source_loc(os, " | ", inliner_loc);
3569 | 			if (snippet) {
3570 | 				print_snippet(os, "    | ", inliner_loc,
3571 | 						colorize, Color::purple, inliner_context_size);
3572 | 			}
3573 | 			already_indented = false;
3574 | 		}
3575 | 
3576 | 		if (trace.source.filename.size()) {
3577 | 			if (!already_indented) {
3578 | 				os << "   ";
3579 | 			}
3580 | 			print_source_loc(os, "   ", trace.source, trace.addr);
3581 | 			if (snippet) {
3582 | 				print_snippet(os, "      ", trace.source,
3583 | 						colorize, Color::yellow, trace_context_size);
3584 | 			}
3585 | 		}
3586 | 	}
3587 | 
3588 | 	void print_snippet(std::ostream& os, const char* indent,
3589 | 			const ResolvedTrace::SourceLoc& source_loc,
3590 | 			Colorize& colorize, Color::type color_code,
3591 | 			int context_size)
3592 | 	{
3593 | 		using namespace std;
3594 | 		typedef SnippetFactory::lines_t lines_t;
3595 | 
3596 | 		lines_t lines = _snippets.get_snippet(source_loc.filename,
3597 | 				source_loc.line, context_size);
3598 | 
3599 | 		for (lines_t::const_iterator it = lines.begin();
3600 | 				it != lines.end(); ++it) {
3601 | 			if (it-> first == source_loc.line) {
3602 | 				colorize.set_color(color_code);
3603 | 				os << indent << ">";
3604 | 			} else {
3605 | 				os << indent << " ";
3606 | 			}
3607 | 			os << std::setw(4) << it->first
3608 | 			   << ": "
3609 | 			   << it->second
3610 | 			   << "\n";
3611 | 			if (it-> first == source_loc.line) {
3612 | 				colorize.set_color(Color::reset);
3613 | 			}
3614 | 		}
3615 | 	}
3616 | 
3617 | 	void print_source_loc(std::ostream& os, const char* indent,
3618 | 			const ResolvedTrace::SourceLoc& source_loc,
3619 | 			void* addr=0) {
3620 | 		os << indent
3621 | 		   << "Source \""
3622 | 		   << source_loc.filename
3623 | 		   << "\", line "
3624 | 		   << source_loc.line
3625 | 		   << ", in "
3626 | 		   << source_loc.function;
3627 | 
3628 | 		if (address && addr != 0) {
3629 | 			os << " [" << addr << "]";
3630 | 		}
3631 | 		os << "\n";
3632 | 	}
3633 | };
3634 | 
3635 | /*************** SIGNALS HANDLING ***************/
3636 | 
3637 | #if defined(BACKWARD_SYSTEM_LINUX) || defined(BACKWARD_SYSTEM_DARWIN)
3638 | 
3639 | 
3640 | class SignalHandling {
3641 | public:
3642 |    static std::vector<int> make_default_signals() {
3643 |        const int posix_signals[] = {
3644 | 		// Signals for which the default action is "Core".
3645 | 		SIGABRT,    // Abort signal from abort(3)
3646 | 		SIGBUS,     // Bus error (bad memory access)
3647 | 		SIGFPE,     // Floating point exception
3648 | 		SIGILL,     // Illegal Instruction
3649 | 		SIGIOT,     // IOT trap. A synonym for SIGABRT
3650 | 		SIGQUIT,    // Quit from keyboard
3651 | 		SIGSEGV,    // Invalid memory reference
3652 | 		SIGSYS,     // Bad argument to routine (SVr4)
3653 | 		SIGTRAP,    // Trace/breakpoint trap
3654 | 		SIGXCPU,    // CPU time limit exceeded (4.2BSD)
3655 | 		SIGXFSZ,    // File size limit exceeded (4.2BSD)
3656 | #if defined(BACKWARD_SYSTEM_DARWIN)
3657 | 		SIGEMT,     // emulation instruction executed
3658 | #endif
3659 | 	};
3660 |         return std::vector<int>(posix_signals, posix_signals + sizeof posix_signals / sizeof posix_signals[0] );
3661 |    }
3662 | 
3663 |   SignalHandling(const std::vector<int>& posix_signals = make_default_signals()):
3664 | 	  _loaded(false) {
3665 | 		bool success = true;
3666 | 
3667 | 		const size_t stack_size = 1024 * 1024 * 8;
3668 | 		_stack_content.reset((char*)malloc(stack_size));
3669 | 		if (_stack_content) {
3670 | 			stack_t ss;
3671 | 			ss.ss_sp = _stack_content.get();
3672 | 			ss.ss_size = stack_size;
3673 | 			ss.ss_flags = 0;
3674 | 			if (sigaltstack(&ss, 0) < 0) {
3675 | 				success = false;
3676 | 			}
3677 | 		} else {
3678 | 			success = false;
3679 | 		}
3680 | 
3681 | 		for (size_t i = 0; i < posix_signals.size(); ++i) {
3682 | 			struct sigaction action;
3683 | 			memset(&action, 0, sizeof action);
3684 | 			action.sa_flags = (SA_SIGINFO | SA_ONSTACK | SA_NODEFER |
3685 | 					SA_RESETHAND);
3686 | 			sigfillset(&action.sa_mask);
3687 | 			sigdelset(&action.sa_mask, posix_signals[i]);
3688 | 			action.sa_sigaction = &sig_handler;
3689 | 
3690 | 			int r = sigaction(posix_signals[i], &action, 0);
3691 | 			if (r < 0) success = false;
3692 | 		}
3693 | 
3694 | 		_loaded = success;
3695 | 	}
3696 | 
3697 | 	bool loaded() const { return _loaded; }
3698 | 
3699 | 	static void handleSignal(int, siginfo_t* info, void* _ctx) {
3700 | 		ucontext_t *uctx = (ucontext_t*) _ctx;
3701 | 
3702 | 		StackTrace st;
3703 | 		void* error_addr = 0;
3704 | #ifdef REG_RIP // x86_64
3705 | 		error_addr = reinterpret_cast<void*>(uctx->uc_mcontext.gregs[REG_RIP]);
3706 | #elif defined(REG_EIP) // x86_32
3707 | 		error_addr = reinterpret_cast<void*>(uctx->uc_mcontext.gregs[REG_EIP]);
3708 | #elif defined(__arm__)
3709 | 		error_addr = reinterpret_cast<void*>(uctx->uc_mcontext.arm_pc);
3710 | #elif defined(__aarch64__)
3711 | 		error_addr = reinterpret_cast<void*>(uctx->uc_mcontext.pc);
3712 | #elif defined(__ppc__) || defined(__powerpc) || defined(__powerpc__) || defined(__POWERPC__)
3713 | 		error_addr = reinterpret_cast<void*>(uctx->uc_mcontext.regs->nip);
3714 | #elif defined(__s390x__)
3715 |                 error_addr = reinterpret_cast<void*>(uctx->uc_mcontext.psw.addr);
3716 | #elif defined(__APPLE__) && defined(__x86_64__)
3717 | 		error_addr = reinterpret_cast<void*>(uctx->uc_mcontext->__ss.__rip);
3718 | #elif defined(__APPLE__)
3719 | 		error_addr = reinterpret_cast<void*>(uctx->uc_mcontext->__ss.__eip);
3720 | #else
3721 | #	warning ":/ sorry, ain't know no nothing none not of your architecture!"
3722 | #endif
3723 | 		if (error_addr) {
3724 | 			st.load_from(error_addr, 32);
3725 | 		} else {
3726 | 			st.load_here(32);
3727 | 		}
3728 | 
3729 | 		Printer printer;
3730 | 		printer.address = true;
3731 | 		printer.print(st, stderr);
3732 | 
3733 | #if _XOPEN_SOURCE >= 700 || _POSIX_C_SOURCE >= 200809L
3734 | 		psiginfo(info, 0);
3735 | #else
3736 | 		(void)info;
3737 | #endif
3738 | 	}
3739 | 
3740 | private:
3741 | 	details::handle<char*> _stack_content;
3742 | 	bool                   _loaded;
3743 | 
3744 | #ifdef __GNUC__
3745 | 	__attribute__((noreturn))
3746 | #endif
3747 | 	static void sig_handler(int signo, siginfo_t* info, void* _ctx) {
3748 | 		handleSignal(signo, info, _ctx);
3749 | 
3750 | 		// try to forward the signal.
3751 | 		raise(info->si_signo);
3752 | 
3753 | 		// terminate the process immediately.
3754 | 		puts("watf? exit");
3755 | 		_exit(EXIT_FAILURE);
3756 | 	}
3757 | };
3758 | 
3759 | #endif // BACKWARD_SYSTEM_LINUX || BACKWARD_SYSTEM_DARWIN
3760 | 
3761 | #ifdef BACKWARD_SYSTEM_UNKNOWN
3762 | 
3763 | class SignalHandling {
3764 | public:
3765 | 	SignalHandling(const std::vector<int>& = std::vector<int>()) {}
3766 | 	bool init() { return false; }
3767 | 	bool loaded() { return false; }
3768 | };
3769 | 
3770 | #endif // BACKWARD_SYSTEM_UNKNOWN
3771 | 
3772 | } // namespace backward
3773 | 
3774 | #endif /* H_GUARD */
3775 | 


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