| 32 | #include "App_Robust_Models_2D.h" |
| 33 | |
| 34 | int CreateGraphAndSolve(std::vector<std::string> &Arguments, |
| 35 | libRSF::FactorGraphConfig &Config, |
| 36 | libRSF::StateDataSet &CostSurfaceData, |
| 37 | libRSF::StateDataSet &PreOptimizationData, |
| 38 | libRSF::StateDataSet &PostOptimizationData, |
| 39 | libRSF::StateDataSet &SolverData) |
| 40 | { |
| 41 | /** parse testing parameter */ |
| 42 | const int NumberPoints = std::stoi(Arguments.at(3)); |
| 43 | const double Range = std::stod(Arguments.at(4)); |
| 44 | const string ErrorModel = Arguments.at(5); |
| 45 | |
| 46 | /** parse GMM parameter */ |
| 47 | libRSF::Vector2 Mean1, Mean2; |
| 48 | Mean1 << std::stod(Arguments.at(6)), std::stod(Arguments.at(7)); |
| 49 | Mean2 << std::stod(Arguments.at(8)), std::stod(Arguments.at(9)); |
| 50 | |
| 51 | libRSF::Matrix22 StdDev1, StdDev2; |
| 52 | StdDev1 << std::stod(Arguments.at(10)), std::stod(Arguments.at(11)), |
| 53 | std::stod(Arguments.at(12)), std::stod(Arguments.at(13)); |
| 54 | StdDev2 << std::stod(Arguments.at(14)), std::stod(Arguments.at(15)), |
| 55 | std::stod(Arguments.at(16)), std::stod(Arguments.at(17)); |
| 56 | |
| 57 | libRSF::Vector1 Weight1, Weight2; |
| 58 | Weight1 << std::stod(Arguments.at(18)); |
| 59 | Weight2 << std::stod(Arguments.at(19)); |
| 60 | |
| 61 | /** estimate DCS parameter */ |
| 62 | const double ScalingDCS = std::pow(10, Mean2(0)); |
| 63 | |
| 64 | /** create our own graph object */ |
| 65 | libRSF::FactorGraph SimpleGraph; |
| 66 | |
| 67 | /** set the solver options for ceres */ |
| 68 | ceres::Solver::Options SolverOptions; |
| 69 | SolverOptions.linear_solver_type = ceres::LinearSolverType::DENSE_QR; |
| 70 | SolverOptions.minimizer_type = ceres::MinimizerType::TRUST_REGION; |
| 71 | SolverOptions.trust_region_strategy_type = ceres::TrustRegionStrategyType::LEVENBERG_MARQUARDT; |
| 72 | SolverOptions.num_threads = 1; |
| 73 | SolverOptions.max_num_iterations = 100; |
| 74 | SolverOptions.max_solver_time_in_seconds = 1.0; |
| 75 | SolverOptions.minimizer_progress_to_stdout = false; |
| 76 | |
| 77 | /** configure Gaussian error model */ |
| 78 | libRSF::GaussianFull<2> Noise; |
| 79 | Noise.setCovarianceMatrix(StdDev1.transpose()*StdDev1); |
| 80 | |
| 81 | /** configure Gaussian Identity model for cDCE */ |
| 82 | libRSF::GaussianDiagonal<2> NoiseIdentity; |
| 83 | NoiseIdentity.setStdDevSharedDiagonal(1.0); |
| 84 | |
| 85 | /** create Gaussian mixture object */ |
| 86 | libRSF::GaussianComponent<2> Gaussian; |
| 87 | libRSF::GaussianMixture<2> GMM; |
| 88 | /** component 1 */ |
| 89 | Gaussian.setParamsCovariance(StdDev1.transpose()*StdDev1, Mean1, Weight1); |
| 90 | GMM.addComponent(Gaussian); |
| 91 | /** component 2*/ |
no test coverage detected