Skip to content
Snippets Groups Projects
Commit fd08cd36 authored by jvallve's avatar jvallve
Browse files

minor changes

parent d010c5d2
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
......@@ -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);
}
......
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment