From fd08cd36793f3f0ddeb7dadc716645b0ad671997 Mon Sep 17 00:00:00 2001 From: jvallve <jvallve@224674b8-e365-4e73-a4a8-558dbbfec58c> Date: Tue, 7 Apr 2015 09:25:52 +0000 Subject: [PATCH] minor changes --- src/capture_laser_2D.cpp | 2 +- src/ceres_wrapper/ceres_manager.cpp | 3 ++- src/examples/test_ceres_2_lasers.cpp | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp index d6255c7e2..d6ebf8595 100644 --- a/src/capture_laser_2D.cpp +++ b/src/capture_laser_2D.cpp @@ -284,7 +284,7 @@ WolfScalar CaptureLaser2D::computeMahalanobisDistance(const FeatureBase* _featur // Feature-Landmark Mahalanobis distance // covariance: Eigen::Matrix3s S_m = J * Sigma.selfadjointView<Eigen::Upper>() * J.transpose() + _feature_ptr->getMeasurementCovariance().topLeftCorner<3,3>(); WolfScalar mahalanobis_distance = d_euclidean.transpose() * (J.transpose() * Sigma.selfadjointView<Eigen::Upper>() * J + _feature_ptr->getMeasurementCovariance().topLeftCorner<3,3>()).inverse() * d_euclidean; - std::cout << "mahalanobis_distance = " << mahalanobis_distance << std::endl; + //std::cout << "mahalanobis_distance = " << mahalanobis_distance << std::endl; return mahalanobis_distance; } else diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp index 190eff4d0..dfee1b0c0 100644 --- a/src/ceres_wrapper/ceres_manager.cpp +++ b/src/ceres_wrapper/ceres_manager.cpp @@ -4,7 +4,8 @@ CeresManager::CeresManager(ceres::Problem::Options _options) : ceres_problem_(new ceres::Problem(_options)) { ceres::Covariance::Options covariance_options; - covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD; + //covariance_options.algorithm_type = ceres::SUITE_SPARSE_QR;//ceres::DENSE_SVD; + covariance_options.num_threads = 8;//ceres::DENSE_SVD; //covariance_options.null_space_rank = -1; covariance_ = new ceres::Covariance(covariance_options); } diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index bfaa57f1e..8aca178a0 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -116,7 +116,7 @@ int main(int argc, char** argv) // Ceres initialization ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::LINE_SEARCH; //ceres::TRUST_REGION; + 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; -- GitLab