From 3d772b1de0cc7290077fc62acb5362a6769dff7f Mon Sep 17 00:00:00 2001 From: joanvallve <jvallve@iri.upc.edu> Date: Mon, 15 Jun 2020 08:44:46 +0200 Subject: [PATCH] tests/demos adapted to SolverCeres --- demos/demo_2_lasers_offline.cpp | 13 ++++++------- demos/demo_ceres_2_lasers.cpp | 14 +++++++------- demos/demo_ceres_2_lasers_polylines.cpp | 14 +++++++------- test/gtest_processor_odom_icp.cpp | 4 ++-- 4 files changed, 22 insertions(+), 23 deletions(-) diff --git a/demos/demo_2_lasers_offline.cpp b/demos/demo_2_lasers_offline.cpp index 40ebda489..597dfd71f 100644 --- a/demos/demo_2_lasers_offline.cpp +++ b/demos/demo_2_lasers_offline.cpp @@ -171,15 +171,14 @@ int main(int argc, char** argv) problem.setPrior(ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1, ts, 0.1); // Ceres wrapper - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - // ceres_options.minimizer_progress_to_stdout = false; - // ceres_options.line_search_direction_type = ceres::LBFGS; - // ceres_options.max_num_iterations = 100; + SolverCeres ceres_manager(&problem); + ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3; + // ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false; + // ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS; + // ceres_manager.getSolverOptions().max_num_iterations = 100; google::InitGoogleLogging(argv[0]); - CeresManager ceres_manager(&problem, ceres_options); std::ofstream log_file, landmark_file; //output file std::cout << "START TRAJECTORY..." << std::endl; diff --git a/demos/demo_ceres_2_lasers.cpp b/demos/demo_ceres_2_lasers.cpp index 050f47f22..bc3c6fb52 100644 --- a/demos/demo_ceres_2_lasers.cpp +++ b/demos/demo_ceres_2_lasers.cpp @@ -257,15 +257,15 @@ int main(int argc, char** argv) odom_processor->setOrigin(origin_frame); // Ceres wrapper - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - // ceres_options.minimizer_progress_to_stdout = false; - // ceres_options.line_search_direction_type = ceres::LBFGS; - // ceres_options.max_num_iterations = 100; + SolverCeres ceres_manager(&problem); + ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3; + // ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false; + // ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS; + // ceres_manager.getSolverOptions().max_num_iterations = 100; google::InitGoogleLogging(argv[0]); - CeresManager ceres_manager(&problem, ceres_options); + SolverCeres ceres_manager(&problem, ceres_options); std::ofstream log_file, landmark_file; //output file //std::cout << "START TRAJECTORY..." << std::endl; diff --git a/demos/demo_ceres_2_lasers_polylines.cpp b/demos/demo_ceres_2_lasers_polylines.cpp index 3bc78c0a4..a8a93d9fa 100644 --- a/demos/demo_ceres_2_lasers_polylines.cpp +++ b/demos/demo_ceres_2_lasers_polylines.cpp @@ -264,15 +264,15 @@ int main(int argc, char** argv) odom_processor->setOrigin(origin_frame); // Ceres wrapper - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH - ceres_options.max_line_search_step_contraction = 1e-3; - // ceres_options.minimizer_progress_to_stdout = false; - // ceres_options.line_search_direction_type = ceres::LBFGS; - // ceres_options.max_num_iterations = 100; + SolverCeres ceres_manager(&problem); + ceres_manager.getSolverOptions().minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH + ceres_manager.getSolverOptions().max_line_search_step_contraction = 1e-3; + // ceres_manager.getSolverOptions().minimizer_progress_to_stdout = false; + // ceres_manager.getSolverOptions().line_search_direction_type = ceres::LBFGS; + // ceres_manager.getSolverOptions().max_num_iterations = 100; google::InitGoogleLogging(argv[0]); - CeresManager ceres_manager(&problem, ceres_options); + SolverCeres ceres_manager(&problem, ceres_options); std::ofstream log_file, landmark_file; //output file //std::cout << "START TRAJECTORY..." << std::endl; diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp index 8b8d0fbb3..cc51f3302 100644 --- a/test/gtest_processor_odom_icp.cpp +++ b/test/gtest_processor_odom_icp.cpp @@ -37,7 +37,7 @@ class ProcessorOdomIcp_Test : public testing::Test { public: ProblemPtr problem; - CeresManagerPtr solver; + SolverCeresPtr solver; SensorLaser2dPtr lidar; ProcessorOdomIcpPublicPtr processor; @@ -51,7 +51,7 @@ class ProcessorOdomIcp_Test : public testing::Test void SetUp() override { problem = Problem::create("PO", 2); - solver = std::make_shared<CeresManager>(problem); + solver = std::make_shared<SolverCeres>(problem); auto sen = problem->installSensor("SensorLaser2d", "lidar", Eigen::Vector3d(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2d.yaml"); lidar = std::static_pointer_cast<SensorLaser2d>(sen); -- GitLab