slambook-master.tar.gz

#include 
#include 
#include “ceres/ceres.h“

#include “SnavelyReprojectionerror.h“
#include “common/BALProblem.h“
#include “common/BundleParams.h“


using namespace ceres;

void SetLinearSolver（ceres::Solver::Options* options const BundleParams& params）
{
    CHECK（ceres::StringToLinearSolverType（params.linear_solver &options->linear_solver_type））;
    CHECK（ceres::StringToSparseLinearAlgebraLibraryType（params.sparse_linear_algebra_library &options->sparse_linear_algebra_library_type））;
    CHECK（ceres::StringToDenseLinearAlgebraLibraryType（params.dense_linear_algebra_library &options->dense_linear_algebra_library_type））;
    options->num_linear_solver_threads = params.num_threads;

}


void SetOrdering（BALProblem* bal_problem ceres::Solver::Options* options const BundleParams& params）
{
    const int num_points = bal_problem->num_points（）;
    const int point_block_size = bal_problem->point_block_size（）;
    double* points = bal_problem->mutable_points（）;

    const int num_cameras = bal_problem->num_cameras（）;
    const int camera_block_size = bal_problem->camera_block_size（）;
    double* cameras = bal_problem->mutable_cameras（）;


    if （params.ordering == “automatic“）
        return;

    ceres::ParameterBlockOrdering* ordering = new ceres::ParameterBlockOrdering;

    // The points come before the cameras
    for（int i = 0; i < num_points; ++i）
       ordering->AddElementToGroup（points + point_block_size * i 0）;
       
    
    for（int i = 0; i < num_cameras; ++i）
        ordering->AddElementToGroup（cameras + camera_block_size * i 1）;

    options->linear_solver_ordering.reset（ordering）;

}

void SetMinimizerOptions（Solver::Options* options const BundleParams& params）{
    options->max_num_iterations = params.num_iterations;
    options->minimizer_progress_to_stdout = true;
    options->num_threads = params.num_threads;
    // options->eta = params.eta;
    // options->max_solver_time_in_seconds = params.max_solver_time;
    
    CHECK（StringToTrustRegionStrategyType（params.trust_region_strategy
                                        &options->trust_region_strategy_type））;
    
}

void SetSolverOptionsFromFlags（BALProblem* bal_problem
                               const BundleParams& params Solver::Options* options）{
    SetMinimizerOptions（optionsparams）;
    SetLinearSolver（optionsparams）;
    SetOrdering（bal_problem optionsparams）;
}

void BuildProblem（BALProblem* bal_problem Problem* problem const BundleParams& params）
{
    const int point_block_size = bal_problem->point_block_size（）;
    const int camera_block_size = bal_problem->camera_block_size（）;
    double* points = bal_problem->mutable_points（）;
    double* cameras = bal_problem->mutable_cameras（）;

    // Observations is 2 * num_observations long array observations
    // [u_1 u_2 ... u_n] where each u_i is two dimensional the x 
    // and y position of the observation. 
    const double* observations = bal_problem->observations（）;