diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp index d6255c7e2f054e373b127d4d07e8b8d41671a8e2..d6ebf859581a054b02f59403fe6a269ab3224c08 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 190eff4d0cf782073259f2b76ca82f653a1402c1..dfee1b0c0e8b309e74be9d06bdb588718d154ecc 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 bfaa57f1e42fc4a53bc98eef82a1cdac41cd50a0..8aca178a01e8bdca1c0bc6212d36a707e2681488 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;