├── README.md ├── .gitignore ├── CurveFitting ├── GenerateRawData.py ├── CMakeLists.txt └── main.cpp ├── RobustCurveFitting ├── plot.py ├── CMakeLists.txt ├── data.csv └── main.cpp ├── PowellOptimization ├── CMakeLists.txt └── main.cpp ├── SimpleOptimizationSample ├── CMakeLists.txt └── main.cpp ├── OptimizationWithConstraint ├── CMakeLists.txt └── main.cpp ├── Robot2DOptimization ├── CMakeLists.txt ├── csvparser.h ├── GenerateData.py ├── main.cpp └── matplotlibcpp.h └── LICENSE /README.md: -------------------------------------------------------------------------------- 1 | # CeresSamples 2 | Sample codes for Ceres optimizer developed by Google 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | *.smod 19 | 20 | # Compiled Static libraries 21 | *.lai 22 | *.la 23 | *.a 24 | *.lib 25 | 26 | # Executables 27 | *.exe 28 | *.out 29 | *.app 30 | -------------------------------------------------------------------------------- /CurveFitting/GenerateRawData.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # author:Atsushi Sakai 4 | 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | 8 | x=np.arange(0,10,0.1) 9 | y=[np.exp(0.3*tx+0.1)+np.random.normal(0.0,0.2) for tx in x] 10 | 11 | plt.plot(x,y,'+r') 12 | plt.grid(True) 13 | plt.show() 14 | 15 | f=open('data.csv', 'w') 16 | for (x,y) in zip(x,y): 17 | f.write(str(x)+","+str(y)+"\n") 18 | f.close() 19 | 20 | 21 | -------------------------------------------------------------------------------- /RobustCurveFitting/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # author:Atsushi Sakai 4 | 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | import pandas as pd 8 | 9 | x=np.arange(0,10,0.1) 10 | ty=[np.exp(0.3*tx+0.1) for tx in x] 11 | ey=[np.exp(0.280248*tx+0.2698163) for tx in x] 12 | data=pd.read_csv("data.csv") 13 | 14 | plt.plot(data["x"],data["y"],'+r') 15 | plt.plot(x,ty,'-b') 16 | plt.plot(x,ey,'-g') 17 | plt.grid(True) 18 | plt.show() 19 | 20 | -------------------------------------------------------------------------------- /PowellOptimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(helloworld) 4 | 5 | # find_package(Eigen3 REQUIRED) 6 | #ubuntu 7 | include_directories(/usr/include/eigen3) 8 | #mac 9 | # include_directories(/usr/local/include/eigen3) 10 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 11 | # add_definitions(${EIGEN_DEFINITIONS}) 12 | 13 | find_package(Ceres REQUIRED) 14 | include_directories(${CERESS_INCLUDE_DIRS}) 15 | 16 | add_executable(PowellOptimization main.cpp) 17 | target_link_libraries(PowellOptimization ${CERES_LIBRARIES}) 18 | -------------------------------------------------------------------------------- /SimpleOptimizationSample/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(helloworld) 4 | 5 | # find_package(Eigen3 REQUIRED) 6 | #ubuntu 7 | include_directories(/usr/include/eigen3) 8 | #mac 9 | # include_directories(/usr/local/include/eigen3) 10 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 11 | # add_definitions(${EIGEN_DEFINITIONS}) 12 | 13 | find_package(Ceres REQUIRED) 14 | include_directories(${CERESS_INCLUDE_DIRS}) 15 | 16 | add_executable(SimpleOptimizationSample main.cpp) 17 | target_link_libraries(SimpleOptimizationSample ${CERES_LIBRARIES}) 18 | -------------------------------------------------------------------------------- /OptimizationWithConstraint/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(helloworld) 4 | 5 | # find_package(Eigen3 REQUIRED) 6 | #ubuntu 7 | include_directories(/usr/include/eigen3) 8 | #mac 9 | # include_directories(/usr/local/include/eigen3) 10 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 11 | # add_definitions(${EIGEN_DEFINITIONS}) 12 | 13 | find_package(Ceres REQUIRED) 14 | include_directories(${CERESS_INCLUDE_DIRS}) 15 | 16 | add_executable(SimpleOptimizationSample main.cpp) 17 | target_link_libraries(SimpleOptimizationSample ${CERES_LIBRARIES}) 18 | -------------------------------------------------------------------------------- /CurveFitting/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(helloworld) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") 6 | 7 | # find_package(Eigen3 REQUIRED) 8 | #ubuntu 9 | include_directories(/usr/include/eigen3) 10 | #mac 11 | # include_directories(/usr/local/include/eigen3) 12 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 13 | # add_definitions(${EIGEN_DEFINITIONS}) 14 | 15 | find_package(Ceres REQUIRED) 16 | include_directories(${CERESS_INCLUDE_DIRS}) 17 | 18 | add_executable(CurveFitting main.cpp) 19 | target_link_libraries(CurveFitting ${CERES_LIBRARIES}) 20 | -------------------------------------------------------------------------------- /RobustCurveFitting/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(helloworld) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") 6 | 7 | # find_package(Eigen3 REQUIRED) 8 | #ubuntu 9 | include_directories(/usr/include/eigen3) 10 | #mac 11 | # include_directories(/usr/local/include/eigen3) 12 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 13 | # add_definitions(${EIGEN_DEFINITIONS}) 14 | 15 | find_package(Ceres REQUIRED) 16 | include_directories(${CERESS_INCLUDE_DIRS}) 17 | 18 | add_executable(RobustCurveFitting main.cpp) 19 | target_link_libraries(RobustCurveFitting ${CERES_LIBRARIES}) 20 | -------------------------------------------------------------------------------- /Robot2DOptimization/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | 3 | project(helloworld) 4 | 5 | set(CMAKE_CXX_FLAGS "-std=c++0x ${CMAKE_CXX_FLAGS}") 6 | 7 | find_package(PythonLibs REQUIRED) 8 | 9 | #ubuntu 10 | include_directories(/usr/include/eigen3) 11 | #mac 12 | # include_directories(/usr/local/include/eigen3) 13 | # set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3") 14 | # add_definitions(${EIGEN_DEFINITIONS}) 15 | 16 | find_package(Ceres REQUIRED) 17 | include_directories(${CERESS_INCLUDE_DIRS} ${PYTHON_INCLUDE_DIRS}) 18 | 19 | add_executable(Robot2DOptimization main.cpp) 20 | target_link_libraries(Robot2DOptimization ${CERES_LIBRARIES} ${PYTHON_LIBRARIES}) 21 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 Atsushi Sakai 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 all 13 | 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 | -------------------------------------------------------------------------------- /SimpleOptimizationSample/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Ceres Optimimization Sample 3 | * 4 | * @author Atsushi Sakai 5 | * 6 | **/ 7 | 8 | #include "ceres/ceres.h" 9 | #include "glog/logging.h" 10 | 11 | using ceres::AutoDiffCostFunction; 12 | using ceres::CostFunction; 13 | using ceres::Problem; 14 | using ceres::Solver; 15 | using ceres::Solve; 16 | 17 | /** 18 | * @brief コスト関数 19 | **/ 20 | struct CostFunctor{ 21 | template 22 | bool operator()(const T* const x, T* residual) const{ 23 | residual[0]=T(10.0)-x[0]; 24 | return true; 25 | } 26 | }; 27 | 28 | 29 | int main(int argc, char** argv){ 30 | google::InitGoogleLogging(argv[0]); 31 | 32 | //最適化問題を解く変数と初期値の設定 33 | double initial_x=15.0; 34 | double x=initial_x; 35 | 36 | //最適化問題を解く用のオブジェクトの生成 37 | Problem problem; 38 | 39 | //コスト関数の設定 40 | //AutoDiffCostFunctionを使うことで、自動的にヤコビ行列を設定できる 41 | CostFunction* cost_function=new AutoDiffCostFunction(new CostFunctor); 42 | 43 | //最適化問題に残差項と変数を設定 44 | problem.AddResidualBlock(cost_function,NULL,&x); 45 | 46 | //最適化の実行 47 | Solver::Options options;//最適化のオプション設定用構造体 48 | options.linear_solver_type=ceres::DENSE_QR; 49 | options.minimizer_progress_to_stdout=true;//最適化の結果を標準出力に表示する。 50 | Solver::Summary summary;//最適化の結果を格納するよう構造体 51 | Solve(options,&problem,&summary);//最適化の実行 52 | 53 | //結果の表示 54 | std::cout<"< 22 | bool operator()(const T* const x, T* residual) const{ 23 | residual[0]=T(10.0)-x[0]; 24 | return true; 25 | } 26 | }; 27 | 28 | 29 | int main(int argc, char** argv){ 30 | google::InitGoogleLogging(argv[0]); 31 | 32 | //最適化問題を解く変数と初期値の設定 33 | double initial_x=15.0; 34 | double x=initial_x; 35 | 36 | //最適化問題を解く用のオブジェクトの生成 37 | Problem problem; 38 | 39 | //コスト関数の設定 40 | //AutoDiffCostFunctionを使うことで、自動的にヤコビ行列を設定できる 41 | CostFunction* cost_function=new AutoDiffCostFunction(new CostFunctor); 42 | 43 | //最適化問題に残差項と変数を設定 44 | problem.AddResidualBlock(cost_function,NULL,&x); 45 | 46 | //制約設定 47 | problem.SetParameterLowerBound(&x,0,12.0); 48 | problem.SetParameterUpperBound(&x,0,14.0); 49 | 50 | //最適化の実行 51 | Solver::Options options;//最適化のオプション設定用構造体 52 | options.linear_solver_type=ceres::DENSE_QR; 53 | options.minimizer_progress_to_stdout=true;//最適化の結果を標準出力に表示する。 54 | Solver::Summary summary;//最適化の結果を格納するよう構造体 55 | Solve(options,&problem,&summary);//最適化の実行 56 | 57 | //結果の表示 58 | std::cout<"< 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | using namespace std; 19 | 20 | class CSVParser{ 21 | public: 22 | CSVParser(const string &filename, bool withheader=true){ 23 | 24 | ncol_=nrow_=0.0; 25 | 26 | cout<<"file name is "< > data_; 40 | uint32_t ncol_; 41 | uint32_t nrow_; 42 | 43 | private: 44 | 45 | void ReadContents(ifstream &ifs, bool withheader){ 46 | //Read each line of csv file 47 | string str; 48 | if(withheader) getline(ifs,str); //remove header 49 | 50 | 51 | while(getline(ifs,str)){ 52 | string token; 53 | istringstream stream(str); 54 | 55 | vector line; 56 | 57 | while(getline(stream,token,',')){ 58 | // cout< 23 | bool operator()( 24 | const T* const x1, 25 | const T* const x2, 26 | T* residual) const{ 27 | residual[0]=x1[0]+T(10.0)*x2[0]; 28 | return true; 29 | } 30 | }; 31 | 32 | struct F2{ 33 | template 34 | bool operator()( 35 | const T* const x3, 36 | const T* const x4, 37 | T* residual) const{ 38 | residual[0]=T(sqrt(5.0))*(x3[0]-x4[0]); 39 | return true; 40 | } 41 | }; 42 | 43 | struct F3{ 44 | template 45 | bool operator()( 46 | const T* const x2, 47 | const T* const x3, 48 | T* residual) const{ 49 | residual[0]=(x2[0]-T(2.0)*x3[0])*(x2[0]-T(2.0)*x3[0]); 50 | return true; 51 | } 52 | }; 53 | 54 | struct F4{ 55 | template 56 | bool operator()( 57 | const T* const x1, 58 | const T* const x4, 59 | T* residual) const{ 60 | residual[0]=T(sqrt(10.0))*(x1[0]-x4[0])*(x1[0]-x4[0]); 61 | return true; 62 | } 63 | }; 64 | 65 | 66 | int main(int argc, char** argv){ 67 | google::InitGoogleLogging(argv[0]); 68 | 69 | //最適化問題を解く変数と初期値の設定 70 | double x1= 3.0; 71 | double x2=-1.0; 72 | double x3= 0.0; 73 | double x4= 1.0; 74 | 75 | 76 | //最適化問題を解く用のオブジェクトの生成 77 | Problem problem; 78 | 79 | //最適化問題に残差項と変数を設定 80 | problem.AddResidualBlock( 81 | new AutoDiffCostFunction(new F1),NULL,&x1,&x2); 82 | problem.AddResidualBlock( 83 | new AutoDiffCostFunction(new F2),NULL,&x3,&x4); 84 | problem.AddResidualBlock( 85 | new AutoDiffCostFunction(new F3),NULL,&x2,&x3); 86 | problem.AddResidualBlock( 87 | new AutoDiffCostFunction(new F4),NULL,&x1,&x4); 88 | 89 | 90 | //最適化の実行 91 | Solver::Options options;//最適化のオプション設定用構造体 92 | options.linear_solver_type=ceres::DENSE_QR; 93 | options.minimizer_progress_to_stdout=true;//最適化の結果を標準出力に表示する。 94 | Solver::Summary summary;//最適化の結果を格納するよう構造体 95 | Solve(options,&problem,&summary);//最適化の実行 96 | 97 | //結果の表示 98 | std::cout< 0.1: 29 | return z 30 | 31 | z[0]=xTrue[0]+np.random.randn()*R[0] 32 | z[1]=xTrue[1]+np.random.randn()*R[0] 33 | z[2]=R[0] 34 | 35 | return z 36 | 37 | xTrue=[0,0,0]# state x,y,yaw 38 | x=[0,0,0]# state x,y,yaw 39 | u=[1.0,0.1]# input v[m/s],omega[rad/s] 40 | Q=[0.5,0.1] #input noise 41 | R=[0.1] #observation noise 42 | z=[0,0]# state x,y,yaw 43 | dt=0.1 #[s] 44 | zdt=4.0 #[s] 45 | time=0.0 46 | SimTime=50.0 47 | 48 | true_x = [] 49 | true_y = [] 50 | true_yaw = [] 51 | odo_x = [] 52 | odo_y = [] 53 | odo_yaw = [] 54 | z_x = [] 55 | z_y = [] 56 | z_n = [] 57 | u_dl = [] 58 | u_dtheta = [] 59 | u_dl_n = [] 60 | u_dtheta_n = [] 61 | 62 | plt.grid(True) 63 | plt.axis("equal") 64 | 65 | while time 13 | 14 | using namespace std; 15 | 16 | namespace plt = matplotlibcpp; 17 | 18 | using ceres::AutoDiffCostFunction; 19 | using ceres::CostFunction; 20 | using ceres::CauchyLoss; 21 | using ceres::Problem; 22 | using ceres::Solver; 23 | using ceres::Solve; 24 | 25 | struct GPSConstraint{ 26 | GPSConstraint(double x, double y, double n) 27 | :x_(x), y_(y), n_(n) {} 28 | 29 | template 30 | bool operator()( 31 | const T* const x, 32 | const T* const y, 33 | T* residual) const { 34 | residual[0]=(x[0]-T(x_))/n_; 35 | residual[1]=(y[0]-T(y_))/n_; 36 | return true; 37 | } 38 | 39 | static ceres::CostFunction* Create( 40 | const double gx, 41 | const double gy, 42 | const double gn 43 | ){ 44 | return (new ceres::AutoDiffCostFunction( 45 | new GPSConstraint(gx,gy,gn))); 46 | } 47 | 48 | private: 49 | const double x_;//gps position x 50 | const double y_;//gps position y 51 | const double n_;//gps xy accuracy 52 | }; 53 | 54 | struct OdometryConstraint{ 55 | OdometryConstraint(double dl, double dtheta, double dl_n, double dtheta_n) 56 | :dl_(dl), dtheta_(dtheta), dl_n_(dl_n), dtheta_n_(dtheta_n) {} 57 | 58 | template 59 | bool operator()( 60 | const T* const cx, 61 | const T* const cy, 62 | const T* const cyaw, 63 | const T* const nx, 64 | const T* const ny, 65 | const T* const nyaw, 66 | T* residual) const { 67 | residual[0]=(nx[0]-(cx[0]+dl_*cos(cyaw[0])))/dl_n_; 68 | residual[1]=(ny[0]-(cy[0]+dl_*sin(cyaw[0])))/dl_n_; 69 | residual[2]=(nyaw[0]-(cyaw[0]+dtheta_))/dtheta_n_; 70 | return true; 71 | } 72 | 73 | static ceres::CostFunction* Create( 74 | const double dl, 75 | const double dtheta, 76 | const double dl_n, 77 | const double dtheta_n 78 | ){ 79 | return (new ceres::AutoDiffCostFunction( 80 | new OdometryConstraint(dl,dtheta,dl_n, dtheta_n))); 81 | } 82 | 83 | private: 84 | const double dl_;//move distance 85 | const double dtheta_;//change angle 86 | const double dl_n_;// move distance accuracy 87 | const double dtheta_n_;// change angle accuracy 88 | }; 89 | 90 | int main(int argc, char** argv){ 91 | cout<<"Start similation"< tx; 99 | vector ty; 100 | vector tyaw; 101 | //parameter 102 | vector x; 103 | vector y; 104 | vector yaw; 105 | //observation 106 | vector zx; 107 | vector zy; 108 | vector zn; 109 | //input 110 | vector dl; 111 | vector dtheta; 112 | vector dl_n; 113 | vector dtheta_n; 114 | 115 | for(int i=0;i ix; 133 | vector iy; 134 | vector iyaw; 135 | ix=x; 136 | iy=y; 137 | iyaw=yaw; 138 | 139 | //====Optimization===== 140 | ceres::Problem problem; 141 | 142 | for(int i=0;i=0.001){ 157 | problem.AddResidualBlock( 158 | GPSConstraint::Create(zx[i],zy[i],zn[i]), 159 | NULL, 160 | &x[i], 161 | &y[i] 162 | ); 163 | } 164 | } 165 | 166 | //Optimization 167 | Solver::Options options; 168 | options.linear_solver_type=ceres::DENSE_QR; 169 | options.minimizer_progress_to_stdout=true; 170 | Solver::Summary summary; 171 | Solve(options,&problem,&summary); 172 | 173 | plt::named_plot("Truth",tx,ty, "-b"); 174 | plt::named_plot("init",ix,iy, "-g"); 175 | plt::named_plot("Estmated",x, y, "-r"); 176 | plt::named_plot("GPS", zx, zy, "xk"); 177 | plt::legend(); 178 | plt::axis("equal"); 179 | plt::grid(true); 180 | plt::show(); 181 | 182 | return 0; 183 | } 184 | 185 | -------------------------------------------------------------------------------- /CurveFitting/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @brief Curve fitting Sample 3 | * 4 | * @author Atsushi Sakai 5 | * 6 | **/ 7 | 8 | #include "ceres/ceres.h" 9 | #include "glog/logging.h" 10 | #include "math.h" 11 | #include 12 | 13 | using namespace std; 14 | 15 | using ceres::AutoDiffCostFunction; 16 | using ceres::CostFunction; 17 | using ceres::Problem; 18 | using ceres::Solver; 19 | using ceres::Solve; 20 | 21 | vector x={ 22 | 0.0, 23 | 0.1, 24 | 0.2, 25 | 0.3, 26 | 0.4, 27 | 0.5, 28 | 0.6, 29 | 0.7, 30 | 0.8, 31 | 0.9, 32 | 1.0, 33 | 1.1, 34 | 1.2, 35 | 1.3, 36 | 1.4, 37 | 1.5, 38 | 1.6, 39 | 1.7, 40 | 1.8, 41 | 1.9, 42 | 2.0, 43 | 2.1, 44 | 2.2, 45 | 2.3, 46 | 2.4, 47 | 2.5, 48 | 2.6, 49 | 2.7, 50 | 2.8, 51 | 2.9, 52 | 3.0, 53 | 3.1, 54 | 3.2, 55 | 3.3, 56 | 3.4, 57 | 3.5, 58 | 3.6, 59 | 3.7, 60 | 3.8, 61 | 3.9, 62 | 4.0, 63 | 4.1, 64 | 4.2, 65 | 4.3, 66 | 4.4, 67 | 4.5, 68 | 4.6, 69 | 4.7, 70 | 4.8, 71 | 4.9, 72 | 5.0, 73 | 5.1, 74 | 5.2, 75 | 5.3, 76 | 5.4, 77 | 5.5, 78 | 5.6, 79 | 5.7, 80 | 5.8, 81 | 5.9, 82 | 6.0, 83 | 6.1, 84 | 6.2, 85 | 6.3, 86 | 6.4, 87 | 6.5, 88 | 6.6, 89 | 6.7, 90 | 6.8, 91 | 6.9, 92 | 7.0, 93 | 7.1, 94 | 7.2, 95 | 7.3, 96 | 7.4, 97 | 7.5, 98 | 7.6, 99 | 7.7, 100 | 7.8, 101 | 7.9, 102 | 8.0, 103 | 8.1, 104 | 8.2, 105 | 8.3, 106 | 8.4, 107 | 8.5, 108 | 8.6, 109 | 8.7, 110 | 8.8, 111 | 8.9, 112 | 9.0, 113 | 9.1, 114 | 9.2, 115 | 9.3, 116 | 9.4, 117 | 9.5, 118 | 9.6, 119 | 9.7, 120 | 9.8, 121 | 9.9 122 | }; 123 | 124 | vector y={ 125 | 1.02490416236 , 126 | 1.29305791957 , 127 | 1.21808052029 , 128 | 1.3588488308 , 129 | 1.39714914507 , 130 | 1.21248581892 , 131 | 1.43837677213 , 132 | 1.70810328393 , 133 | 1.23257688353 , 134 | 1.01403219676 , 135 | 1.50925801818 , 136 | 1.6662848504 , 137 | 1.62685479195 , 138 | 1.72943981083 , 139 | 1.6268189861 , 140 | 1.5232015196 , 141 | 1.75886813965 , 142 | 1.93519928417 , 143 | 1.9708652989 , 144 | 2.34153642459 , 145 | 2.33431720324 , 146 | 2.19900176974 , 147 | 2.41857038053 , 148 | 2.05907010769 , 149 | 2.14753269599 , 150 | 2.26971851294 , 151 | 2.67786409035 , 152 | 2.36423267435 , 153 | 2.98507375854 , 154 | 2.61935149386 , 155 | 2.61920129111 , 156 | 2.77018486544 , 157 | 2.58031060219 , 158 | 3.39765031555 , 159 | 3.00758588694 , 160 | 3.03219044437 , 161 | 3.47396275685 , 162 | 3.33445068721 , 163 | 3.35104422442 , 164 | 3.66884606242 , 165 | 3.76938172337 , 166 | 3.71002906934 , 167 | 3.99279950267 , 168 | 4.02309903921 , 169 | 4.48927062765 , 170 | 3.99157533677 , 171 | 4.39760556415 , 172 | 4.58161866676 , 173 | 4.17123107607 , 174 | 4.72860665359 , 175 | 5.098285443 , 176 | 4.92916651281 , 177 | 5.30439889148 , 178 | 5.56271822085 , 179 | 5.78117714742 , 180 | 5.74989797305 , 181 | 5.72580355915 , 182 | 5.67394357099 , 183 | 6.53751089972 , 184 | 6.52964092617 , 185 | 6.17575247141 , 186 | 6.66194065898 , 187 | 7.10096705189 , 188 | 7.13447829695 , 189 | 7.76971839588 , 190 | 8.12613000585 , 191 | 7.62828689202 , 192 | 8.38091275525 , 193 | 8.67785948187 , 194 | 8.36503603111 , 195 | 9.45888208319 , 196 | 9.34159256258 , 197 | 9.56090107074 , 198 | 9.8416453843 , 199 | 9.97227446831 , 200 | 10.1873938121 , 201 | 10.8344553317 , 202 | 11.0763825902 , 203 | 11.2842012655 , 204 | 11.9209145172 , 205 | 12.0536165764 , 206 | 12.5759989771 , 207 | 13.1364781742 , 208 | 13.3973702715 , 209 | 13.6533113094 , 210 | 14.3127362678 , 211 | 14.5541054308 , 212 | 15.0865724847 , 213 | 15.3825751 , 214 | 15.7758082945 , 215 | 16.218452986 , 216 | 16.9454721082 , 217 | 17.223654526 , 218 | 18.079015446 , 219 | 18.7208248424 , 220 | 19.4461197524 , 221 | 19.6863469253 , 222 | 20.4498195898 , 223 | 21.1118474511 , 224 | 21.4684135588 225 | }; 226 | 227 | 228 | /** 229 | * @brief コスト関数 230 | **/ 231 | struct ExponentialResidual{ 232 | ExponentialResidual(double x, double y) 233 | :x_(x), y_(y){} 234 | 235 | template 236 | bool operator()(const T* const m, const T* const c, T* residual) const { 237 | residual[0]=T(y_)-exp(m[0]*T(x_)+c[0]); 238 | return true; 239 | } 240 | 241 | private: 242 | //Observations for a sample 243 | const double x_; 244 | const double y_; 245 | }; 246 | 247 | int main(int argc, char** argv){ 248 | google::InitGoogleLogging(argv[0]); 249 | 250 | //最適化問題を解く変数と初期値の設定 251 | double m = 0.0;//exp(mx+c) 252 | double c = 0.0; 253 | 254 | //最適化問題を解く用のオブジェクトの生成 255 | Problem problem; 256 | 257 | for(int i=0; i( 259 | new ExponentialResidual(x[i],y[i])); 260 | 261 | problem.AddResidualBlock(cost_function, NULL, &m, &c); 262 | } 263 | 264 | //最適化の実行 265 | Solver::Options options;//最適化のオプション設定用構造体 266 | options.linear_solver_type=ceres::DENSE_QR; 267 | options.minimizer_progress_to_stdout=true;//最適化の結果を標準出力に表示する。 268 | Solver::Summary summary;//最適化の結果を格納するよう構造体 269 | Solve(options,&problem,&summary);//最適化の実行 270 | 271 | //結果の表示 272 | std::cout< 12 | 13 | using namespace std; 14 | 15 | using ceres::AutoDiffCostFunction; 16 | using ceres::CostFunction; 17 | using ceres::CauchyLoss; 18 | using ceres::Problem; 19 | using ceres::Solver; 20 | using ceres::Solve; 21 | 22 | vector x={ 23 | 0.0, 24 | 0.1, 25 | 0.2, 26 | 0.3, 27 | 0.4, 28 | 0.5, 29 | 0.6, 30 | 0.7, 31 | 0.8, 32 | 0.9, 33 | 1.0, 34 | 1.1, 35 | 1.2, 36 | 1.3, 37 | 1.4, 38 | 1.5, 39 | 1.6, 40 | 1.7, 41 | 1.8, 42 | 1.9, 43 | 2.0, 44 | 2.1, 45 | 2.2, 46 | 2.3, 47 | 2.4, 48 | 2.5, 49 | 2.6, 50 | 2.7, 51 | 2.8, 52 | 2.9, 53 | 3.0, 54 | 3.1, 55 | 3.2, 56 | 3.3, 57 | 3.4, 58 | 3.5, 59 | 3.6, 60 | 3.7, 61 | 3.8, 62 | 3.9, 63 | 4.0, 64 | 4.1, 65 | 4.2, 66 | 4.3, 67 | 4.4, 68 | 4.5, 69 | 4.6, 70 | 4.7, 71 | 4.8, 72 | 4.9, 73 | 5.0, 74 | 5.1, 75 | 5.2, 76 | 5.3, 77 | 5.4, 78 | 5.5, 79 | 5.6, 80 | 5.7, 81 | 5.8, 82 | 5.9, 83 | 6.0, 84 | 6.1, 85 | 6.2, 86 | 6.3, 87 | 6.4, 88 | 6.5, 89 | 6.6, 90 | 6.7, 91 | 6.8, 92 | 6.9, 93 | 7.0, 94 | 7.1, 95 | 7.2, 96 | 7.3, 97 | 7.4, 98 | 7.5, 99 | 7.6, 100 | 7.7, 101 | 7.8, 102 | 7.9, 103 | 8.0, 104 | 8.1, 105 | 8.2, 106 | 8.3, 107 | 8.4, 108 | 8.5, 109 | 8.6, 110 | 8.7, 111 | 8.8, 112 | 8.9, 113 | 9.0, 114 | 9.1, 115 | 9.2, 116 | 9.3, 117 | 9.4, 118 | 9.5, 119 | 9.6, 120 | 9.7, 121 | 9.8, 122 | 9.9, 123 | 0.5, //outlier 124 | 0.4 //outlier 125 | }; 126 | 127 | vector y={ 128 | 1.02490416236 , 129 | 1.29305791957 , 130 | 1.21808052029 , 131 | 1.3588488308 , 132 | 1.39714914507 , 133 | 1.21248581892 , 134 | 1.43837677213 , 135 | 1.70810328393 , 136 | 1.23257688353 , 137 | 1.01403219676 , 138 | 1.50925801818 , 139 | 1.6662848504 , 140 | 1.62685479195 , 141 | 1.72943981083 , 142 | 1.6268189861 , 143 | 1.5232015196 , 144 | 1.75886813965 , 145 | 1.93519928417 , 146 | 1.9708652989 , 147 | 2.34153642459 , 148 | 2.33431720324 , 149 | 2.19900176974 , 150 | 2.41857038053 , 151 | 2.05907010769 , 152 | 2.14753269599 , 153 | 2.26971851294 , 154 | 2.67786409035 , 155 | 2.36423267435 , 156 | 2.98507375854 , 157 | 2.61935149386 , 158 | 2.61920129111 , 159 | 2.77018486544 , 160 | 2.58031060219 , 161 | 3.39765031555 , 162 | 3.00758588694 , 163 | 3.03219044437 , 164 | 3.47396275685 , 165 | 3.33445068721 , 166 | 3.35104422442 , 167 | 3.66884606242 , 168 | 3.76938172337 , 169 | 3.71002906934 , 170 | 3.99279950267 , 171 | 4.02309903921 , 172 | 4.48927062765 , 173 | 3.99157533677 , 174 | 4.39760556415 , 175 | 4.58161866676 , 176 | 4.17123107607 , 177 | 4.72860665359 , 178 | 5.098285443 , 179 | 4.92916651281 , 180 | 5.30439889148 , 181 | 5.56271822085 , 182 | 5.78117714742 , 183 | 5.74989797305 , 184 | 5.72580355915 , 185 | 5.67394357099 , 186 | 6.53751089972 , 187 | 6.52964092617 , 188 | 6.17575247141 , 189 | 6.66194065898 , 190 | 7.10096705189 , 191 | 7.13447829695 , 192 | 7.76971839588 , 193 | 8.12613000585 , 194 | 7.62828689202 , 195 | 8.38091275525 , 196 | 8.67785948187 , 197 | 8.36503603111 , 198 | 9.45888208319 , 199 | 9.34159256258 , 200 | 9.56090107074 , 201 | 9.8416453843 , 202 | 9.97227446831 , 203 | 10.1873938121 , 204 | 10.8344553317 , 205 | 11.0763825902 , 206 | 11.2842012655 , 207 | 11.9209145172 , 208 | 12.0536165764 , 209 | 12.5759989771 , 210 | 13.1364781742 , 211 | 13.3973702715 , 212 | 13.6533113094 , 213 | 14.3127362678 , 214 | 14.5541054308 , 215 | 15.0865724847 , 216 | 15.3825751 , 217 | 15.7758082945 , 218 | 16.218452986 , 219 | 16.9454721082 , 220 | 17.223654526 , 221 | 18.079015446 , 222 | 18.7208248424 , 223 | 19.4461197524 , 224 | 19.6863469253 , 225 | 20.4498195898 , 226 | 21.1118474511 , 227 | 21.4684135588 , 228 | 20.0, //outlier 229 | 21.0 //outlier 230 | }; 231 | 232 | 233 | /** 234 | * @brief コスト関数 235 | **/ 236 | struct ExponentialResidual{ 237 | ExponentialResidual(double x, double y) 238 | :x_(x), y_(y){} 239 | 240 | template 241 | bool operator()(const T* const m, const T* const c, T* residual) const { 242 | residual[0]=T(y_)-exp(m[0]*T(x_)+c[0]); 243 | return true; 244 | } 245 | 246 | private: 247 | //Observations for a sample 248 | const double x_; 249 | const double y_; 250 | }; 251 | 252 | int main(int argc, char** argv){ 253 | google::InitGoogleLogging(argv[0]); 254 | 255 | //最適化問題を解く変数と初期値の設定 256 | double m = 0.0;//exp(mx+c) 257 | double c = 0.0; 258 | 259 | //最適化問題を解く用のオブジェクトの生成 260 | Problem problem; 261 | 262 | for(int i=0; i( 264 | new ExponentialResidual(x[i],y[i])); 265 | 266 | problem.AddResidualBlock( 267 | cost_function, 268 | new CauchyLoss(0.5), 269 | &m, &c); 270 | } 271 | 272 | //最適化の実行 273 | Solver::Options options;//最適化のオプション設定用構造体 274 | options.linear_solver_type=ceres::DENSE_QR; 275 | options.minimizer_progress_to_stdout=true;//最適化の結果を標準出力に表示する。 276 | Solver::Summary summary;//最適化の結果を格納するよう構造体 277 | Solve(options,&problem,&summary);//最適化の実行 278 | 279 | //結果の表示 280 | std::cout< 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #if __cplusplus > 199711L 10 | #include 11 | #endif 12 | 13 | #include 14 | 15 | namespace matplotlibcpp { 16 | 17 | namespace detail { 18 | struct _interpreter { 19 | PyObject *s_python_function_show; 20 | PyObject *s_python_function_save; 21 | PyObject *s_python_function_figure; 22 | PyObject *s_python_function_plot; 23 | PyObject *s_python_function_legend; 24 | PyObject *s_python_function_xlim; 25 | PyObject *s_python_function_ylim; 26 | PyObject *s_python_function_title; 27 | PyObject *s_python_function_axis; 28 | PyObject *s_python_function_xlabel; 29 | PyObject *s_python_function_ylabel; 30 | PyObject *s_python_function_grid; 31 | PyObject *s_python_function_pause; 32 | PyObject *s_python_empty_tuple; 33 | 34 | /* For now, _interpreter is implemented as a singleton since its currently not possible to have 35 | multiple independent embedded python interpreters without patching the python source code 36 | or starting a seperate process for each. 37 | 38 | http://bytes.com/topic/python/answers/793370-multiple-independent-python-interpreters-c-c-program 39 | */ 40 | 41 | static _interpreter& get() { 42 | static _interpreter ctx; 43 | return ctx; 44 | } 45 | 46 | private: 47 | _interpreter() { 48 | char name[] = "plotting"; // silence compiler warning abount const strings 49 | Py_SetProgramName(name); // optional but recommended 50 | Py_Initialize(); 51 | 52 | PyObject* pyplotname = PyString_FromString("matplotlib.pyplot"); 53 | PyObject* pylabname = PyString_FromString("pylab"); 54 | if(!pyplotname || !pylabname) { throw std::runtime_error("couldnt create string"); } 55 | 56 | PyObject* pymod = PyImport_Import(pyplotname); 57 | Py_DECREF(pyplotname); 58 | if(!pymod) { throw std::runtime_error("Error loading module matplotlib.pyplot!"); } 59 | 60 | PyObject* pylabmod = PyImport_Import(pylabname); 61 | Py_DECREF(pylabname); 62 | if(!pymod) { throw std::runtime_error("Error loading module pylab!"); } 63 | 64 | s_python_function_show = PyObject_GetAttrString(pymod, "show"); 65 | s_python_function_figure = PyObject_GetAttrString(pymod, "figure"); 66 | s_python_function_plot = PyObject_GetAttrString(pymod, "plot"); 67 | s_python_function_legend = PyObject_GetAttrString(pymod, "legend"); 68 | s_python_function_ylim = PyObject_GetAttrString(pymod, "ylim"); 69 | s_python_function_title = PyObject_GetAttrString(pymod, "title"); 70 | s_python_function_axis = PyObject_GetAttrString(pymod, "axis"); 71 | s_python_function_xlabel = PyObject_GetAttrString(pymod, "xlabel"); 72 | s_python_function_ylabel = PyObject_GetAttrString(pymod, "ylabel"); 73 | s_python_function_grid = PyObject_GetAttrString(pymod, "grid"); 74 | s_python_function_pause = PyObject_GetAttrString(pymod, "pause"); 75 | s_python_function_xlim = PyObject_GetAttrString(pymod, "xlim"); 76 | 77 | s_python_function_save = PyObject_GetAttrString(pylabmod, "savefig"); 78 | 79 | if(!s_python_function_show 80 | || !s_python_function_save 81 | || !s_python_function_figure 82 | || !s_python_function_plot 83 | || !s_python_function_legend 84 | || !s_python_function_xlim 85 | || !s_python_function_ylim 86 | || !s_python_function_title 87 | || !s_python_function_axis 88 | || !s_python_function_xlabel 89 | || !s_python_function_ylabel 90 | || !s_python_function_grid 91 | || !s_python_function_pause 92 | ) 93 | { throw std::runtime_error("Couldnt find required function!"); } 94 | 95 | if(!PyFunction_Check(s_python_function_show) 96 | || !PyFunction_Check(s_python_function_save) 97 | || !PyFunction_Check(s_python_function_figure) 98 | || !PyFunction_Check(s_python_function_plot) 99 | || !PyFunction_Check(s_python_function_legend) 100 | || !PyFunction_Check(s_python_function_xlim) 101 | || !PyFunction_Check(s_python_function_ylim) 102 | || !PyFunction_Check(s_python_function_title) 103 | || !PyFunction_Check(s_python_function_axis) 104 | || !PyFunction_Check(s_python_function_xlabel) 105 | || !PyFunction_Check(s_python_function_ylabel) 106 | || !PyFunction_Check(s_python_function_grid) 107 | || !PyFunction_Check(s_python_function_pause) 108 | ) 109 | { throw std::runtime_error("Python object is unexpectedly not a PyFunction."); } 110 | 111 | s_python_empty_tuple = PyTuple_New(0); 112 | } 113 | 114 | ~_interpreter() { 115 | Py_Finalize(); 116 | } 117 | }; 118 | } 119 | 120 | 121 | 122 | template 123 | bool plot(const std::vector &x, const std::vector &y, const std::map& keywords) 124 | { 125 | assert(x.size() == y.size()); 126 | 127 | // using python lists 128 | PyObject* xlist = PyList_New(x.size()); 129 | PyObject* ylist = PyList_New(y.size()); 130 | 131 | for(size_t i = 0; i < x.size(); ++i) { 132 | PyList_SetItem(xlist, i, PyFloat_FromDouble(x.at(i))); 133 | PyList_SetItem(ylist, i, PyFloat_FromDouble(y.at(i))); 134 | } 135 | 136 | // construct positional args 137 | PyObject* args = PyTuple_New(2); 138 | PyTuple_SetItem(args, 0, xlist); 139 | PyTuple_SetItem(args, 1, ylist); 140 | 141 | Py_DECREF(xlist); 142 | Py_DECREF(ylist); 143 | 144 | // construct keyword args 145 | PyObject* kwargs = PyDict_New(); 146 | for(std::map::const_iterator it = keywords.begin(); it != keywords.end(); ++it) 147 | { 148 | PyDict_SetItemString(kwargs, it->first.c_str(), PyString_FromString(it->second.c_str())); 149 | } 150 | 151 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, args, kwargs); 152 | 153 | Py_DECREF(args); 154 | Py_DECREF(kwargs); 155 | if(res) Py_DECREF(res); 156 | 157 | return res; 158 | } 159 | 160 | 161 | template 162 | bool plot(const std::vector& x, const std::vector& y, const std::string& s = "") 163 | { 164 | assert(x.size() == y.size()); 165 | 166 | PyObject* xlist = PyList_New(x.size()); 167 | PyObject* ylist = PyList_New(y.size()); 168 | PyObject* pystring = PyString_FromString(s.c_str()); 169 | 170 | for(size_t i = 0; i < x.size(); ++i) { 171 | PyList_SetItem(xlist, i, PyFloat_FromDouble(x.at(i))); 172 | PyList_SetItem(ylist, i, PyFloat_FromDouble(y.at(i))); 173 | } 174 | 175 | PyObject* plot_args = PyTuple_New(3); 176 | PyTuple_SetItem(plot_args, 0, xlist); 177 | PyTuple_SetItem(plot_args, 1, ylist); 178 | PyTuple_SetItem(plot_args, 2, pystring); 179 | 180 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); 181 | 182 | Py_DECREF(xlist); 183 | Py_DECREF(ylist); 184 | Py_DECREF(plot_args); 185 | if(res) Py_DECREF(res); 186 | 187 | return res; 188 | } 189 | 190 | 191 | template 192 | bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { 193 | PyObject* kwargs = PyDict_New(); 194 | PyDict_SetItemString(kwargs, "label", PyString_FromString(name.c_str())); 195 | 196 | PyObject* xlist = PyList_New(x.size()); 197 | PyObject* ylist = PyList_New(y.size()); 198 | PyObject* pystring = PyString_FromString(format.c_str()); 199 | 200 | for(size_t i = 0; i < x.size(); ++i) { 201 | PyList_SetItem(xlist, i, PyFloat_FromDouble(x.at(i))); 202 | PyList_SetItem(ylist, i, PyFloat_FromDouble(y.at(i))); 203 | } 204 | 205 | PyObject* plot_args = PyTuple_New(3); 206 | PyTuple_SetItem(plot_args, 0, xlist); 207 | PyTuple_SetItem(plot_args, 1, ylist); 208 | PyTuple_SetItem(plot_args, 2, pystring); 209 | 210 | PyObject* res = PyObject_Call(detail::_interpreter::get().s_python_function_plot, plot_args, kwargs); 211 | 212 | Py_DECREF(kwargs); 213 | Py_DECREF(xlist); 214 | Py_DECREF(ylist); 215 | Py_DECREF(plot_args); 216 | if(res) Py_DECREF(res); 217 | 218 | return res; 219 | } 220 | 221 | template 222 | bool plot(const std::vector& y, const std::string& format = "") 223 | { 224 | std::vector x(y.size()); 225 | for(size_t i=0; i 238 | void ylim(Numeric left, Numeric right) 239 | { 240 | PyObject* list = PyList_New(2); 241 | PyList_SetItem(list, 0, PyFloat_FromDouble(left)); 242 | PyList_SetItem(list, 1, PyFloat_FromDouble(right)); 243 | 244 | PyObject* args = PyTuple_New(1); 245 | PyTuple_SetItem(args, 0, list); 246 | 247 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylim, args); 248 | if(!res) throw std::runtime_error("Call to ylim() failed."); 249 | 250 | Py_DECREF(list); 251 | Py_DECREF(args); 252 | Py_DECREF(res); 253 | } 254 | 255 | template 256 | void xlim(Numeric left, Numeric right) 257 | { 258 | PyObject* list = PyList_New(2); 259 | PyList_SetItem(list, 0, PyFloat_FromDouble(left)); 260 | PyList_SetItem(list, 1, PyFloat_FromDouble(right)); 261 | 262 | PyObject* args = PyTuple_New(1); 263 | PyTuple_SetItem(args, 0, list); 264 | 265 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlim, args); 266 | if(!res) throw std::runtime_error("Call to xlim() failed."); 267 | 268 | Py_DECREF(list); 269 | Py_DECREF(args); 270 | Py_DECREF(res); 271 | } 272 | 273 | 274 | inline void title(const std::string &titlestr) 275 | { 276 | PyObject* pytitlestr = PyString_FromString(titlestr.c_str()); 277 | PyObject* args = PyTuple_New(1); 278 | PyTuple_SetItem(args, 0, pytitlestr); 279 | 280 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_title, args); 281 | if(!res) throw std::runtime_error("Call to title() failed."); 282 | 283 | //if PyDeCRFF, the show function doesn't wook on Mac OS 284 | } 285 | 286 | inline void axis(const std::string &axisstr) 287 | { 288 | PyObject* str = PyString_FromString(axisstr.c_str()); 289 | PyObject* args = PyTuple_New(1); 290 | PyTuple_SetItem(args, 0, str); 291 | 292 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_axis, args); 293 | if(!res) throw std::runtime_error("Call to title() failed."); 294 | 295 | //if PyDeCRFF, the show function doesn't wook on Mac OS 296 | } 297 | 298 | 299 | 300 | inline void xlabel(const std::string &str) 301 | { 302 | PyObject* pystr = PyString_FromString(str.c_str()); 303 | PyObject* args = PyTuple_New(1); 304 | PyTuple_SetItem(args, 0, pystr); 305 | 306 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_xlabel, args); 307 | if(!res) throw std::runtime_error("Call to xlabel() failed."); 308 | 309 | //if PyDeCRFF, the show function doesn't wook on Mac OS 310 | } 311 | 312 | inline void ylabel(const std::string &str) 313 | { 314 | PyObject* pystr = PyString_FromString(str.c_str()); 315 | PyObject* args = PyTuple_New(1); 316 | PyTuple_SetItem(args, 0, pystr); 317 | 318 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_ylabel, args); 319 | if(!res) throw std::runtime_error("Call to ylabel() failed."); 320 | 321 | //if PyDeCRFF, the show function doesn't wook on Mac OS 322 | } 323 | 324 | inline void grid(const bool &flag) 325 | { 326 | PyObject* pyflag; 327 | if(flag){ 328 | pyflag=Py_True; 329 | } 330 | else{ 331 | pyflag=Py_False; 332 | } 333 | PyObject* args = PyTuple_New(1); 334 | PyTuple_SetItem(args, 0, pyflag); 335 | 336 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_grid, args); 337 | if(!res) throw std::runtime_error("Call to grid() failed."); 338 | 339 | //if PyDeCRFF, the show function doesn't wook on Mac OS 340 | } 341 | 342 | void pause(double sec) 343 | { 344 | PyObject* pysec = PyFloat_FromDouble(sec); 345 | 346 | PyObject* args = PyTuple_New(1); 347 | PyTuple_SetItem(args, 0, pysec); 348 | 349 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_pause, args); 350 | if(!res) throw std::runtime_error("Call to pause() failed."); 351 | 352 | } 353 | 354 | 355 | 356 | 357 | inline void show() 358 | { 359 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_show, detail::_interpreter::get().s_python_empty_tuple); 360 | if(!res) throw std::runtime_error("Call to show() failed."); 361 | 362 | Py_DECREF(res); 363 | } 364 | 365 | inline void save(const std::string& filename) 366 | { 367 | PyObject* pyfilename = PyString_FromString(filename.c_str()); 368 | 369 | PyObject* args = PyTuple_New(1); 370 | PyTuple_SetItem(args, 0, pyfilename); 371 | 372 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_save, args); 373 | if(!res) throw std::runtime_error("Call to save() failed."); 374 | 375 | Py_DECREF(pyfilename); 376 | Py_DECREF(args); 377 | Py_DECREF(res); 378 | } 379 | 380 | #if __cplusplus > 199711L 381 | // C++11-exclusive content starts here (variadic plot() and initializer list support) 382 | 383 | namespace detail { 384 | template 385 | using is_function = typename std::is_function>>::type; 386 | 387 | template 388 | struct is_callable_impl; 389 | 390 | template 391 | struct is_callable_impl 392 | { 393 | typedef is_function type; 394 | }; // a non-object is callable iff it is a function 395 | 396 | template 397 | struct is_callable_impl 398 | { 399 | struct Fallback { void operator()(); }; 400 | struct Derived : T, Fallback { }; 401 | 402 | template struct Check; 403 | 404 | template 405 | static std::true_type test( ... ); // use a variadic function to make sure (1) it accepts everything and (2) its always the worst match 406 | 407 | template 408 | static std::false_type test( Check* ); 409 | 410 | public: 411 | typedef decltype(test(nullptr)) type; 412 | typedef decltype(&Fallback::operator()) dtype; 413 | static constexpr bool value = type::value; 414 | }; // an object is callable iff it defines operator() 415 | 416 | template 417 | struct is_callable 418 | { 419 | // dispatch to is_callable_impl or is_callable_impl depending on whether T is of class type or not 420 | typedef typename is_callable_impl::value, T>::type type; 421 | }; 422 | 423 | template 424 | struct plot_impl { }; 425 | 426 | template<> 427 | struct plot_impl 428 | { 429 | template 430 | bool operator()(const IterableX& x, const IterableY& y, const std::string& format) 431 | { 432 | // 2-phase lookup for distance, begin, end 433 | using std::distance; 434 | using std::begin; 435 | using std::end; 436 | 437 | auto xs = distance(begin(x), end(x)); 438 | auto ys = distance(begin(y), end(y)); 439 | assert(xs == ys && "x and y data must have the same number of elements!"); 440 | 441 | PyObject* xlist = PyList_New(xs); 442 | PyObject* ylist = PyList_New(ys); 443 | PyObject* pystring = PyString_FromString(format.c_str()); 444 | 445 | auto itx = begin(x), ity = begin(y); 446 | for(size_t i = 0; i < xs; ++i) { 447 | PyList_SetItem(xlist, i, PyFloat_FromDouble(*itx++)); 448 | PyList_SetItem(ylist, i, PyFloat_FromDouble(*ity++)); 449 | } 450 | 451 | PyObject* plot_args = PyTuple_New(3); 452 | PyTuple_SetItem(plot_args, 0, xlist); 453 | PyTuple_SetItem(plot_args, 1, ylist); 454 | PyTuple_SetItem(plot_args, 2, pystring); 455 | 456 | PyObject* res = PyObject_CallObject(detail::_interpreter::get().s_python_function_plot, plot_args); 457 | 458 | Py_DECREF(xlist); 459 | Py_DECREF(ylist); 460 | Py_DECREF(plot_args); 461 | if(res) Py_DECREF(res); 462 | 463 | return res; 464 | } 465 | }; 466 | 467 | template<> 468 | struct plot_impl 469 | { 470 | template 471 | bool operator()(const Iterable& ticks, const Callable& f, const std::string& format) 472 | { 473 | //std::cout << "Callable impl called" << std::endl; 474 | 475 | if(begin(ticks) == end(ticks)) return true; 476 | 477 | // We could use additional meta-programming to deduce the correct element type of y, 478 | // but all values have to be convertible to double anyways 479 | std::vector y; 480 | for(auto x : ticks) y.push_back(f(x)); 481 | return plot_impl()(ticks,y,format); 482 | } 483 | }; 484 | } 485 | 486 | // recursion stop for the above 487 | template 488 | bool plot() { return true; } 489 | 490 | template 491 | bool plot(const A& a, const B& b, const std::string& format, Args... args) 492 | { 493 | return detail::plot_impl::type>()(a,b,format) && plot(args...); 494 | } 495 | 496 | /* 497 | * This group of plot() functions is needed to support initializer lists, i.e. calling 498 | * plot( {1,2,3,4} ) 499 | */ 500 | bool plot(const std::vector& x, const std::vector& y, const std::string& format = "") { 501 | return plot(x,y,format); 502 | } 503 | 504 | bool plot(const std::vector& y, const std::string& format = "") { 505 | return plot(y,format); 506 | } 507 | 508 | bool plot(const std::vector& x, const std::vector& y, const std::map& keywords) { 509 | return plot(x,y,keywords); 510 | } 511 | 512 | bool named_plot(const std::string& name, const std::vector& x, const std::vector& y, const std::string& format = "") { 513 | return named_plot(name,x,y,format); 514 | } 515 | 516 | #endif 517 | 518 | 519 | 520 | 521 | } 522 | --------------------------------------------------------------------------------