void Masseuse::Relax() {
if(!graph->size() || !values->size()){
std::cerr << "Pose graph not loaded. Load poses using 'LoadPosesFromFile'" <<
" before calling Relax()" << std::endl;
throw runtime_error("Initial values or graph empty. Nothing to relax.");
}
// Global problem
ceres::Problem problem;
// Build the problem with the given pose graph
if(options.enable_prior_at_origin && !options.fix_first_pose){
// Add a prior at the origin:
Point3 p = origin.head<3>();
Rot3 rot(origin[3], origin[4], origin[5]);
Pose3 orig(rot, p);
Eigen::Vector6d cov_vec;
cov_vec << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4;
Eigen::MatrixXd m = cov_vec.asDiagonal();
Matrix sqrt_information_matrix = inverseSqrt(m);
ceres::CostFunction* prior_cost_function =
new ceres::AutoDiffCostFunction<PriorPoseCostFunctor<double>,
Sophus::SE3::DoF,
Sophus::SE3::num_parameters>
(new PriorPoseCostFunctor<double>(orig, sqrt_information_matrix));
problem.AddResidualBlock(prior_cost_function, NULL,
values->begin()->second.data());
}
// Now add a binary constraint for all relative and loop closure constraints
for(Factor& f : *graph){
Pose3& T_a = values->at(f.id1);
Pose3& T_b = values->at(f.id2);
Matrix sqrt_information_matrix = inverseSqrt(f.cov);
if(options.enable_switchable_constraints && f.isLCC){
// Use switchable constraints to selectively disable bad LCC's
// during the optimization, see:
// 'Switchable Constraints for Robust Pose Graph SLAM'
ceres::CostFunction* binary_cost_function =
new ceres::AutoDiffCostFunction<SwitchableBinaryPoseCostFunctor
<double>, Sophus::SE3::DoF,
Sophus::SE3::num_parameters,
Sophus::SE3::num_parameters,
1>
(new SwitchableBinaryPoseCostFunctor<double>(f.rel_pose,
sqrt_information_matrix));
HuberLoss* loss = new HuberLoss(options.huber_loss_delta);
problem.AddResidualBlock(binary_cost_function, loss,
T_a.data(),
T_b.data(),
&f.switch_variable);
// Constrain the switch variable to be between 0 and 1
problem.SetParameterLowerBound(&f.switch_variable, 0, 0.0);
problem.SetParameterUpperBound(&f.switch_variable, 0, 1.0);
// Add a prior to anchor the switch variable at its initial value
ceres::CostFunction* prior_cost_function =
new ceres::AutoDiffCostFunction<PriorCostFunctor<double>,
1, 1>(new PriorCostFunctor<double>(f.switch_variable,
options.switch_variable_prior_cov,
0));
problem.AddResidualBlock(prior_cost_function, NULL,
&f.switch_variable);
}else{
Matrix sqrt_information_matrix = inverseSqrt(f.cov);
ceres::CostFunction* binary_cost_function =
new ceres::AutoDiffCostFunction<BinaryPoseCostFunctor
<double>, Sophus::SE3::DoF,
Sophus::SE3::num_parameters,
Sophus::SE3::num_parameters>
(new BinaryPoseCostFunctor<double>(f.rel_pose, sqrt_information_matrix));
HuberLoss* loss = new HuberLoss(options.huber_loss_delta);
problem.AddResidualBlock(binary_cost_function, loss,
T_a.data(),
T_b.data());
}
if(options.enable_z_prior){
// Add a prior on z so that it anchors the height to the initial value
// this assumes roughly planar motion to avoid the z drift.
double initial_z = origin[2]; // first three elements are x, y, z
// The last parameter is the index of the z in the SO3 data structure
// It is [i j k w x y z]
ceres::CostFunction* prior_cost_function =
new ceres::AutoDiffCostFunction<PriorCostFunctor<double>,
1, 7>(new PriorCostFunctor<double>(initial_z,
options.cov_z_prior,
6));
problem.AddResidualBlock(prior_cost_function, NULL,
T_b.data());
}
}
if(options.fix_first_pose && problem.HasParameterBlock(values->begin()->second.data())){
problem.SetParameterBlockConstant(values->begin()->second.data());
}
ceres::LocalParameterization* local_param =
new ceres::AutoDiffLocalParameterization
<Sophus::masseuse::AutoDiffLocalParamSE3, 7, 6>;
for(auto &pair : *values){
if(problem.HasParameterBlock(pair.second.data())){
problem.SetParameterization(pair.second.data(),
local_param);
}
}
ceres::Solver::Options ceres_options;
ceres_options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
ceres_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
ceres_options.minimizer_progress_to_stdout = options.print_minimizer_progress;
ceres_options.max_num_iterations = options.num_iterations;
ceres_options.update_state_every_iteration =
options.update_state_every_iteration;
ceres_options.check_gradients = options.check_gradients;
ceres_options.function_tolerance = options.function_tolearnce;
ceres_options.parameter_tolerance = options.parameter_tolerance;
ceres_options.gradient_tolerance = options.gradient_tolerance;
if(options.print_error_statistics){
std::cerr << "BEFORE RELAXATION:" << std::endl;
PrintErrorStatistics(*values);
std::cerr << std::endl;
// if(options.enable_switchable_constraints){
// std::cerr << "switch variables BEFORE optimizing: " << std::endl;
// for(const Factor& f : *graph){
// if(f.isLCC){
// fprintf(stderr, "%1.3f ",
// f.switch_variable);
// }
// }
// std::cerr << std::endl;
// }
// double initial_cost = 0.0;
// std::vector<double> residuals(problem.NumResiduals());
// problem.Evaluate(Problem::EvaluateOptions(), &initial_cost, &residuals
// , NULL, NULL);
// std::cerr << "num residual blocks: " << problem.NumResidualBlocks() <<
// std::endl;
// std::cerr << "Cost BEFORE optimizing: " << initial_cost << std::endl;
}
ceres::Solver::Summary summary;
std::cerr << "Relaxing graph...." << std::endl;
double ceres_time = masseuse::Tic();
ceres::Solve(ceres_options, &problem, &summary);
ceres_time = masseuse::Toc(ceres_time);
fprintf(stderr, "Optimization finished in %fs \n",
ceres_time);
if(options.print_full_report){
std::cerr << summary.FullReport() << std::endl;
}
if(options.print_brief_report){
std::cerr << summary.BriefReport() << std::endl;
}
if(options.print_error_statistics){
std::cerr << std::endl << "AFTER REALXATION:" << std::endl;
PrintErrorStatistics(*values);
// if(options.enable_switchable_constraints){
// std::cerr << "switch variables AFTER optimizing: " << std::endl;
// for(const Factor& f : *graph){
// if(f.isLCC){
// fprintf(stderr, "%1.3f ",
// f.switch_variable);
// }
// }
// std::cerr << std::endl;
// }
// double final_cost = 0.0;
// std::vector<double> residuals(problem.NumResiduals());
// problem.Evaluate(Problem::EvaluateOptions(), &final_cost, &residuals,
// NULL, NULL);
//std::cerr << "Cost AFTER optimizing: " << final_cost << std::endl;
// Eigen::Map<Eigen::VectorXd> vec_residuals(residuals.data(), residuals.size());
// std::cerr << "Residuals: " << vec_residuals << std::endl;
problem.NumResiduals();
}
// Save optimization result in a binary file
if(options.save_results_binary){
output_abs_poses.clear();
Eigen::Vector6d absPoseVec;
ceres::Covariance::Options cov_options;
ceres::Covariance covariance(cov_options);
vector<pair<const double*, const double*> > covariance_blocks;
for(const auto& kvp : *values){
const Pose3& pose = kvp.second;
Point3 p = pose.translation();
Rot3 R = pose.so3();
absPoseVec.head<3>() = p;
// unpack Euler angles: roll, pitch, yaw
absPoseVec.tail<3>() = R.matrix().eulerAngles(0, 1, 2);
// Now get the covariance for this pose
covariance_blocks.clear();
covariance_blocks.push_back(std::make_pair(pose.data(), pose.data()));
CHECK(covariance.Compute(covariance_blocks, &problem));
double cov[6*6];
covariance.GetCovarianceBlockInTangentSpace(
pose.data(),
pose.data(),
cov);
// convert to an Eigen matrix
Eigen::Map < Eigen::Matrix<double, 6, 6, Eigen::RowMajor> > cov_mat(
cov);
AbsPose absPose(kvp.first, absPoseVec, cov_mat);
output_abs_poses.push_back(absPose);
}
SaveAbsPG(options.binary_output_path);
}
}
CovarianceSet* init_covariance_set( Cokriging_type type,
const Covariance<Location>& C11,
CokrigTagMap& tags_map,
const Parameters_handler* parameters,
Error_messages_handler* errors,
CokrigDefaultsMap defaults ) {
CovarianceSet* cov_set = 0;
switch( type ) {
case geostat_utils::FULL : // full cokriging
{
std::string tag_string = tags_map[ geostat_utils::FULL ];
std::vector<std::string> tags =
String_Op::decompose_string( tag_string, " ", false );
// we need 2 tags: one for C12 and one for C22
if( tags.size() != 2 ) return 0;
Covariance<Location> C12;
geostat_utils::initialize_covariance( &C12, tags[0],
parameters, errors );
Covariance<Location> C22;
geostat_utils::initialize_covariance( &C22, tags[1],
parameters, errors );
TNT::Matrix< Covariance<Location> > cov_matrix(2,2);
cov_matrix(1,1) = C11;
cov_matrix(2,2) = C22;
cov_matrix(1,2) = C12;
cov_matrix(2,1) = C12;
typedef LMC_covariance< Covariance<Location> > LmcCovariance;
LmcCovariance* tmp_covset = new LmcCovariance( cov_matrix, 2 );
cov_set = new CovarianceSet( tmp_covset );
delete tmp_covset;
break;
}
case geostat_utils::MM1 : // colocated cokriging with Markov Model 1
{
std::string tag_string = tags_map[ geostat_utils::MM1 ];
std::vector<std::string> tags =
String_Op::decompose_string( tag_string, " ", false );
// we need 2 tags: one for C12(0) and one for C22(0)
if( tags.size() != 2 ) return 0;
TNT::Matrix<double> cov_mat(2,2);
double C22;
CokrigDefaultsMap::const_iterator defaults_it =
defaults.find( geostat_utils::MM1 );
if( defaults_it != defaults.end() )
C22 = String_Op::to_number<double>( defaults_it->second );
else
C22 = String_Op::to_number<double>( parameters->value(tags[1]) );
cov_mat(1,1) = C11.compute( EuclideanVector( 0, 0, 0 ) );
cov_mat(2,2) = C22;
double correl = String_Op::to_number<double>( parameters->value(tags[0]) );
cov_mat(1,2) = correl * std::sqrt( cov_mat(1,1) * cov_mat( 2,2 ) );
cov_mat(2,1) = cov_mat(1,2);
typedef MM1_covariance< Covariance<Location> > Mm1Covariance;
Mm1Covariance* tmp_mm1 = new Mm1Covariance( C11, cov_mat, 2 );
cov_set = new CovarianceSet( tmp_mm1 );
delete tmp_mm1;
break;
}
case geostat_utils::MM2 : // colocated cokriging with Markov Model 2
{
std::string tag_string = tags_map[ geostat_utils::MM2 ];
std::vector<std::string> tags =
String_Op::decompose_string( tag_string, " ", false );
// we need 2 tags: one for C12(0) and one for C22(h)
if( tags.size() != 2 ) return 0;
Covariance<Location> C22;
geostat_utils::initialize_covariance( &C22, tags[1],
parameters, errors );
TNT::Matrix<double> cov_mat(2,2);
cov_mat(1,1) = C11.compute( EuclideanVector( 0, 0, 0 ) );
cov_mat(2,2) = C22.compute( EuclideanVector( 0, 0, 0 ) );
double correl = String_Op::to_number<double>( parameters->value(tags[0]) );
cov_mat(1,2) = correl * cov_mat(1,1) * cov_mat( 2,2 );
cov_mat(2,1) = cov_mat(1,2);
std::vector< Covariance<Location> > cov_vector(2);
cov_vector[0] = C11;
cov_vector[1] = C22;
typedef MM2_covariance< Covariance<Location> > Mm2Covariance;
Mm2Covariance* tmp_mm2 =
new Mm2Covariance( cov_vector.begin(), cov_vector.end(), cov_mat, 2 );
cov_set = new CovarianceSet( tmp_mm2 );
delete tmp_mm2;
break;
}
} // end switch
return cov_set;
}
