COLMAP的Spatial Matching增添位姿信息

Spatial Matching

考虑使用colmap的Spatial Matching匹配来避免含有相似场景的特征匹配,在获取的数据集中,包含图片信息和位姿信息.其中位姿信息为旋转和平移信息.根据官方文档介绍:

Spatial Matching: This matching mode matches every image against its spatial nearest neighbors. Spatial locations can be manually set in the database management. By default, COLMAP also extracts GPS information from EXIF and uses it for spatial nearest neighbor search. If accurate prior location information is available, this is the recommended matching mode.

允许在database management中添加位姿信息,(无EXIF格式文件忽略掉)但是手动添加几百行数据的所有位姿太麻烦了.在exe目录下写了一个add_pose小程序直接通过命令行来将位姿信息导入.修改CmakeList.txt:

COLMAP_ADD_EXECUTABLE(colmap_exe colmap.cc)
set_target_properties(colmap_exe  PROPERTIES OUTPUT_NAME colmap)
COLMAP_ADD_EXECUTABLE(add_pose_exe add_pose.cpp)
set_target_properties(add_pose_exe  PROPERTIES OUTPUT_NAME add_pose)

加两行即可,增加一个add_pose.cpp文件(至于头文件和help之类的可以舍去很多,自行修改吧):

#include "base/similarity_transform.h"
#include "controllers/bundle_adjustment.h"
#include "controllers/hierarchical_mapper.h"
#include "estimators/coordinate_frame.h"
#include "feature/extraction.h"
#include "feature/matching.h"
#include "feature/utils.h"
#include "retrieval/visual_index.h"
#include "util/opengl_utils.h"
#include "util/version.h"
#include 
#include 
#include 
#include 
#include 
#include 
using namespace colmap;

void GetFileNames(std::string path,std::vector<std::string>& filenames)
{
    DIR *pDir;
    struct dirent* ptr;
    if(!(pDir = opendir(path.c_str()))){
        std::cout<<"Folder doesn't Exist!"<<std::endl;
        return;
    }
    //int i=0;
    while(1){
        ptr = readdir(pDir);//每次使用readdir后，readdir会读到下一个文件，readdir是依次读出目录中的所有文件，每次只能读一个
        if(ptr!=NULL){
            if(strcmp(ptr->d_name, ".") ==0||strcmp(ptr->d_name, "..") ==0){
                continue;
            }else{
            filenames.push_back(path + "/" + ptr->d_name);
            }
        } else
            break;
    }
    sort(filenames.begin(),filenames.end());

    return;
}
int Runaddpose(int argc, char** argv){
    std::string input_path;
    //std::string output_path;

    OptionManager options;
    options.AddDatabaseOptions();//database的路径读取(后面得加上保存)
    //options.AddRequiredOption("input_path", &input_path);传入读取txt的路径
    options.Parse(argc, argv);
    Database database(*options.database_path);//AddDatabaseOptions()读取路径

    //std::cout << "*options.database_path: " << *options.database_path << std::endl;

    std::vector<Image> images = database.ReadAllImages();

    //读取txt文件
    std::vector<std::string> file_name;

    //获取当前路径的绝对路径
    char result[PATH_MAX];
    ssize_t count = readlink( "/proc/self/exe", result, PATH_MAX );
    std::string path = std::string( result, (count > 0) ? count-9 : 0 );//返回当前执行的绝对路径并且去掉add_pose路径,9为add_pose字符数


    path+=argv[3];//传入txt路径


    GetFileNames(path,file_name);
    std::string line;
    std::string item;

    for(size_t i= 0;i < file_name.size(); ++i) {
        Image& image = images[i];
        std::ifstream file(file_name[i]);
        std::getline(file, line);//获取第i个文件的行
        std::stringstream line_stream1(line);
        std::getline(line_stream1, item, ' ');
        auto q0 = std::stold(item);
        std::getline(line_stream1, item, ' ');
        auto q1 = std::stold(item);
        std::getline(line_stream1, item, ' ');
        auto q2 = std::stold(item);
        std::getline(line_stream1, item, ' ');
        auto q3 = std::stold(item);
        Eigen::Vector4d QvecPrior(q0, q1, q2, q3);
        std::getline(line_stream1, item, ' ');
        auto t0 = std::stold(item);
        std::getline(line_stream1, item, ' ');
        auto t1 = std::stold(item);
        std::getline(line_stream1, item, ' ');
        auto t2 = std::stold(item);
        Eigen::Vector3d TvecPrior(t0, t1, t2);
        image.SetTvecPrior(TvecPrior);
        image.SetQvecPrior(QvecPrior);
        database.UpdateImage(image);
    }
    database.Close();
    return 0;
}


typedef std::function<int(int, char**)> command_func_t;
int ShowHelp(
        const std::vector<std::pair<std::string, command_func_t>>& commands) {
    std::cout << StringPrintf(
            "%s -- Structure-from-Motion and Multi-View Stereo\n"
            "              (%s)",
            GetVersionInfo().c_str(), GetBuildInfo().c_str())
              << std::endl
              << std::endl;

    std::cout << "Usage:" << std::endl;
    std::cout << "  colmap [command] [options]" << std::endl << std::endl;

    std::cout << "Documentation:" << std::endl;
    std::cout << "  https://colmap.github.io/" << std::endl << std::endl;

    std::cout << "Example usage:" << std::endl;
    std::cout << "  colmap help [ -h, --help ]" << std::endl;
    std::cout << "  colmap gui" << std::endl;
    std::cout << "  colmap gui -h [ --help ]" << std::endl;
    std::cout << "  colmap automatic_reconstructor -h [ --help ]" << std::endl;
    std::cout << "  colmap automatic_reconstructor --image_path IMAGES "
                 "--workspace_path WORKSPACE"
              << std::endl;
    std::cout << "  colmap feature_extractor --image_path IMAGES --database_path "
                 "DATABASE"
              << std::endl;
    std::cout << "  colmap exhaustive_matcher --database_path DATABASE"
              << std::endl;
    std::cout << "  colmap mapper --image_path IMAGES --database_path DATABASE "
                 "--output_path MODEL"
              << std::endl;
    std::cout << "  ..." << std::endl << std::endl;

    std::cout << "Available commands:" << std::endl;
    std::cout << "  help" << std::endl;
    for (const auto& command : commands) {
        std::cout << "  " << command.first << std::endl;
    }
    std::cout << std::endl;

    return EXIT_SUCCESS;
}
int main(int argc, char** argv) {
    InitializeGlog(argv);

    std::vector<std::pair<std::string, command_func_t>> commands;
    commands.emplace_back("add_pose",&Runaddpose);
    if (argc == 1) {
        return ShowHelp(commands);
    }

    const std::string command = argv[1];
    if (command == "help" || command == "-h" || command == "--help") {
        return ShowHelp(commands);
    } else {
        command_func_t matched_command_func = nullptr;
        for (const auto& command_func : commands) {
            if (command == command_func.first) {
                matched_command_func = command_func.second;
                break;
            }
        }
        if (matched_command_func == nullptr) {
            std::cerr << StringPrintf(
                    "ERROR: Command `%s` not recognized. To list the "
                    "available commands, run `colmap help`.",
                    command.c_str())
                      << std::endl;
            return EXIT_FAILURE;
        } else {
            int command_argc = argc - 1;
            char** command_argv = &argv[1];
            command_argv[0] = argv[0];
            return matched_command_func(command_argc, command_argv);
        }
    }
}

编译之后,会在~/colmap/build/src/exe目录下生成add_pose可执行文件,在该路径下加入images图片对应的txt文件夹,注意需要将特征提取生成的database.db文件和包含位姿的txt文件夹放在exe目录下.参考下图:

然后在该目录下的终端运行:./add_pose add_pose --database_path database.db /txts
运行前在gui界面的database文件为(不含位姿):

运行后:

就将对应位姿信息全部传入到350张图片了!然后可以在修改下相机的内参信息,进行Spatial Matching了!

注意!!!

应该是将pose取逆之后再导入到database中,但是效果还是不好,位姿差太多也匹配上了,需要修改knn参数…把全部问题解决了,再改博客吧.