data=importdata(data_path);

filename = 'euroc_ground_truth.txt';
delimiterIn = ' ';
headerlinesIn = 1;
A = importdata(filename,delimiterIn,headerlinesIn);

data_pattern='%f,%f,%f,%f,%f,%f,%f,%f';

[timestamp, tx, ty, tz , qw , qx , qy , qz] = ...
 textread(data_path,data_pattern,'delimiter', '\n','headerlines', 1);

fid = fopen(data_path);
tline = fgets(fid);    %读取第一行无用数据
data_pattern = '%u64,%f,%f,%f,%f,%f,%f,%f';
data_cell = textscan(fid,data_pattern) ;
fclose(fid);

images = read_images(images.txt);

% 定义读取SFM图像文本函数
function images = read_images(path)
    images = containers.Map('KeyType', 'int64', 'ValueType', 'any');
    fid = fopen(path);
    tline = fgets(fid);    %读取一行数据
    while ischar(tline)  %判断是否是文件结尾
        elems = strsplit(tline,' '); %分割以' '为间隔的字符，返回元胞数组
        if numel(elems) < 4 || strcmp(elems(1), '#') %略过非数据行
            tline = fgets(fid);
            continue
        end
        if mod(images.Count, 10) == 0 %打印读取信息
            fprintf('Reading image %d\n', images.length);
        end
        image = struct;
        image.image_id = str2num(elems{1});
        qw = str2double(elems{2});
        qx = str2double(elems{3});
        qy = str2double(elems{4});
        qz = str2double(elems{5});
        image.R = quat2rotmat([qw, qx, qy, qz]);
        tx = str2double(elems{6});
        ty = str2double(elems{7});
        tz = str2double(elems{8});
        image.t = [tx; ty; tz];
        image.camera_id = str2num(elems{9});
        image.name = elems{10};

tline = fgets(fid);
        elems = sscanf(tline, '%f'); % 读取图像特点
        elems = reshape(elems, [3, numel(elems) / 3]);
        image.xys = elems(1:2,:)';
        image.point3D_ids = elems(3,:)';
        images(image.image_id) = image;
        tline = fgets(fid);
    end
    fclose(fid);
end
%四元数转旋转矩阵
function rotmat = quat2rotmat(qvec)
    rotmat = [1 - 2 * qvec(3).^2 - 2 * qvec(4).^2, ...
            2 * qvec(2) * qvec(3) - 2 * qvec(1) * qvec(4), ...
            2 * qvec(4) * qvec(2) + 2 * qvec(1) * qvec(3); ...

2 * qvec(2) * qvec(3) + 2 * qvec(1) * qvec(4), ...
            1 - 2 * qvec(2).^2 - 2 * qvec(4).^2, ...
            2 * qvec(3) * qvec(4) - 2 * qvec(1) * qvec(2); ...

2 * qvec(4) * qvec(2) - 2 * qvec(1) * qvec(3), ...
            2 * qvec(3) * qvec(4) + 2 * qvec(1) * qvec(2), ...
            1 - 2 * qvec(2).^2 - 2 * qvec(3).^2];
end

function write_ply(path, xyz, normals, rgb)
    % Write point cloud to PLY text file.
    file = fopen(path, 'W');
    fprintf(file,'ply\n');
    fprintf(file,'format ascii 1.0\n');
    fprintf(file,'element vertex %d\n',size(xyz,1));
    fprintf(file,'property float x\n');
    fprintf(file,'property float y\n');
    fprintf(file,'property float z\n');
    fprintf(file,'property float nx\n');
    fprintf(file,'property float ny\n');
    fprintf(file,'property float nz\n');
    fprintf(file,'property uchar diffuse_red\n');
    fprintf(file,'property uchar diffuse_green\n');
    fprintf(file,'property uchar diffuse_blue\n');
    fprintf(file,'end_header\n');
    for i = 1:size(xyz, 1)
        fprintf(file, '%f %f %f %f %f %f %d %d %d\n', ...
            xyz(i,1), xyz(i,2), xyz(i,3), ...
            normals(i,1), normals(i,2), normals(i,3), ...
            uint8(rgb(i,1)), uint8(rgb(i,2)), uint8(rgb(i,3)));
    end
    fclose(file);
end

// 使用stringstream分割字符，注意引入头文件#include <sstream>
void read_images_text(const string& path)
{
    std::ifstream file(path);//文件到流缓冲，即内存
    std::string line;
    std::string item;
    long double qw, qx, qy, qz, tx, ty, tz;
    unsigned long camera_id;
    string image_name;
    while (std::getline(file, line)) 
    {
        if (line.empty() || line[0] == '#') {
            continue;
        }
        std::stringstream line_stream1(line);
        line_stream1 >> item; qw = std::stold(item);
        line_stream1 >> item; qx = std::stold(item);
        line_stream1 >> item; qy = std::stold(item);
        line_stream1 >> item; qz = std::stold(item);
        line_stream1 >> item; tx = std::stold(item);
        line_stream1 >> item; ty = std::stold(item);
        line_stream1 >> item; tz = std::stold(item);
        line_stream1 >> item; qw = std::stold(item);
        line_stream1 >> item; camera_id = std::stoul(item);
        line_stream1 >> item; image_name = item;

// POINTS2D
        if (!std::getline(file, line)) {
            break;
        }
        std::cout <<"processing : " << image_name << std::endl;

std::stringstream line_stream2(line);
        long double x,y;
        long long point3D_id;
        if (!line.empty())
        {
            while (!line_stream2.eof())
            {
                line_stream2 >> item; x = std::stold(item);
                line_stream2 >> item; y = std::stold(item);
                line_stream2 >> item; point3D_id = std::stoll(item);
                // cout <<x <<" " << y << " " << point3D_id<<" ";
            }
        }
    }
    file.close();
}

// 使用getline分割字符，需指定间隔字符
void read_images_text(const string& path)
{
    std::ifstream file(path);
    std::string line;
    std::string item;
    while (std::getline(file, line)) {
        StringTrim(&line);
        if (line.empty() || line[0] == '#') continue;
        std::stringstream line_stream1(line);
        // ID
        std::getline(line_stream1, item, ' ');
        // QVEC (qw, qx, qy, qz)
        std::getline(line_stream1, item, ' ');long double qw = std::stold(item);
        std::getline(line_stream1, item, ' ');long double qx = std::stold(item);
        std::getline(line_stream1, item, ' ');long double qy = std::stold(item);
        std::getline(line_stream1, item, ' ');long double qz = std::stold(item);
        // TVEC
        std::getline(line_stream1, item, ' ');long double tx = std::stold(item);
        std::getline(line_stream1, item, ' ');long double ty = std::stold(item);
        std::getline(line_stream1, item, ' ');long double tz = std::stold(item);
        // CAMERA_ID
        std::getline(line_stream1, item, ' ');unsigned long camera_id = std::stoul(item);
        // NAME
        std::getline(line_stream1, item, ' ');std::string image_name = item;
        // POINTS2D
        if (!std::getline(file, line)) break;
        StringTrim(&line);
        std::stringstream line_stream2(line);
        if (!line.empty())
        {
            while (!line_stream2.eof())
            {
                std::getline(line_stream2, item, ' ');double x = std::stold(item);
                std::getline(line_stream2, item, ' ');double y = std::stold(item);
                std::getline(line_stream2, item, ' ');
                long long point3D_id;
                if (item == "-1") {
                    std::cout <<"2D point has no 3D points" <<std::endl;
                    point3D_id = -1;
                }
                else {
                    std::cout <<"2D point ok!!!" <<std::endl;
                    point3D_id = std::stoll(item);
                }
            }
        }
    }
    file.close();
}

// 此处给出colmap中WriteImagesText函数
void Reconstruction::WriteImagesText(const std::string& path) const {
  std::ofstream file(path, std::ios::trunc);
  CHECK(file.is_open()) << path;

// Ensure that we don't loose any precision by storing in text.
  file.precision(17);

file << "# Image list with two lines of data per image:" << std::endl;
  file << "#   IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, "
          "NAME"
       << std::endl;
  file << "#   POINTS2D[] as (X, Y, POINT3D_ID)" << std::endl;
  file << "# Number of images: " << reg_image_ids_.size()
       << ", mean observations per image: "
       << ComputeMeanObservationsPerRegImage() << std::endl;

for (const auto& image : images_) {
    if (!image.second.IsRegistered()) {
      continue;
    }

std::ostringstream line; //接收一行数据，通过'<<'接收
    std::string line_string;

line << image.first << " ";

// QVEC (qw, qx, qy, qz)
    const Eigen::Vector4d normalized_qvec =
        NormalizeQuaternion(image.second.Qvec());
    line << normalized_qvec(0) << " ";
    line << normalized_qvec(1) << " ";
    line << normalized_qvec(2) << " ";
    line << normalized_qvec(3) << " ";

// TVEC
    line << image.second.Tvec(0) << " ";
    line << image.second.Tvec(1) << " ";
    line << image.second.Tvec(2) << " ";
    line << image.second.CameraId() << " ";
    line << image.second.Name();
    file << line.str() << std::endl;
    line.str("");
    line.clear();

for (const Point2D& point2D : image.second.Points2D()) {
      line << point2D.X() << " ";
      line << point2D.Y() << " ";
      if (point2D.HasPoint3D()) {
        line << point2D.Point3DId() << " ";
      } else {
        line << -1 << " ";
      }
    }
    line_string = line.str();
    line_string = line_string.substr(0, line_string.size() - 1);
    file << line_string << std::endl;
  }
}

//string to number
template <typename Type>
Type tonumber(string str)
{
    std::stringstream ss;
    ss << str;
    Type ret = 0;
    ss >> ret;
    return ret;
}
//number to str
template <typename Type>
string tostring(Type number)
{
    std::stringstream ss;
    ss << number;
    string ret;
    ss >> ret;
    return ret;
}
// 调用时：
string str = "1403636580838555648";
uint64_t number = 1403636580838555648;
num_convert = tonumber<uint64_t >(str);
str_convert = tostring<uint64_t >(number);

def read_images_text(path):
    images = {}
    with open(path, "r") as fid:
        while True:
            line = fid.readline()#读取一行数据
            if not line:
                break
            line = line.strip() #移除字符串头尾指定的字符（默认为空格或换行符）或字符序列
            if len(line) > 0 and line[0] != "#":
                elems = line.split()  #分割字符
                image_id = int(elems[0])
                qvec = np.array(tuple(map(float, elems[1:5])))
                tvec = np.array(tuple(map(float, elems[5:8])))
                camera_id = int(elems[8])
                image_name = elems[9]
                elems = fid.readline().split()
                xys = np.column_stack([tuple(map(float, elems[0::3])),
                                       tuple(map(float, elems[1::3]))])
                point3D_ids = np.array(tuple(map(int, elems[2::3])))
                images[image_id] = Image(
                    id=image_id, qvec=qvec, tvec=tvec,
                    camera_id=camera_id, name=image_name,
                    xys=xys, point3D_ids=point3D_ids)
    return images

pairs = []
for query, indices in zip(query_names, topk):
    for i in indices:
        pair = (query, db_names[i])
        pairs.append(pair)

logging.info(f'Found {len(pairs)} pairs.')
with open(output, 'w') as f:
    f.write('\n'.join(' '.join([i, j]) for i, j in pairs))

# 写
def write_images_text(images, path):
    if len(images) == 0:
        mean_observations = 0
    else:
        mean_observations = sum((len(img.point3D_ids) \
         for _, img in images.items()))/len(images)
    HEADER = "# Image list with two lines of data per image:\n"
    "#   IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME\n"
    "#   POINTS2D[] as (X, Y, POINT3D_ID)\n"
    "# Number of images: {}, mean observations per image: {}\n"\
    .format(len(images),mean_observations)

with open(path, "w") as fid:
        fid.write(HEADER)
        for _, img in images.items():
            #写图像ID/位姿/相机ID/图像名
            image_header = [img.id, *img.qvec, *img.tvec, img.camera_id, img.name]
            first_line = " ".join(map(str, image_header))
            fid.write(first_line + "\n")
            #写2D点
            points_strings = []
            for xy, point3D_id in zip(img.xys, img.point3D_ids):
                points_strings.append(" ".join(map(str, [*xy, point3D_id])))
            fid.write(" ".join(points_strings) + "\n")