pbyhqr72x
/
exercise_2
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'a1e351e2dc'
${ noResults }
exercise_2/colmap-dev/lib/PBA/util.h

754 lines
26 KiB
Raw Blame History

////////////////////////////////////////////////////////////////////////////
// File: util.h
// Author: Changchang Wu (ccwu@cs.washington.edu)
// Description : some utility functions for reading/writing SfM data
//
// Copyright (c) 2011 Changchang Wu (ccwu@cs.washington.edu)
// and the University of Washington at Seattle
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public
// License as published by the Free Software Foundation; either
// Version 3 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
////////////////////////////////////////////////////////////////////////////////
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <math.h>
#include <time.h>
#include <iomanip>
#include <algorithm>
using namespace std;
#include "DataInterface.h"
namespace pba {
// File loader supports .nvm format and bundler format
bool LoadModelFile(const char* name, vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<Point2D>& measurements,
vector<int>& ptidx, vector<int>& camidx,
vector<string>& names, vector<int>& ptc);
void SaveNVM(const char* filename, vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<Point2D>& measurements,
vector<int>& ptidx, vector<int>& camidx, vector<string>& names,
vector<int>& ptc);
void SaveBundlerModel(const char* filename, vector<CameraT>& camera_data,
vector<Point3D>& point_data,
vector<Point2D>& measurements, vector<int>& ptidx,
vector<int>& camidx);
//////////////////////////////////////////////////////////////////
void AddNoise(vector<CameraT>& camera_data, vector<Point3D>& point_data,
float percent);
void AddStableNoise(vector<CameraT>& camera_data, vector<Point3D>& point_data,
const vector<int>& ptidx, const vector<int>& camidx,
float percent);
bool RemoveInvisiblePoints(vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<int>& ptidx,
vector<int>& camidx, vector<Point2D>& measurements,
vector<string>& names, vector<int>& ptc);
/////////////////////////////////////////////////////////////////////////////
bool LoadNVM(ifstream& in, vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<Point2D>& measurements,
vector<int>& ptidx, vector<int>& camidx, vector<string>& names,
vector<int>& ptc) {
int rotation_parameter_num = 4;
bool format_r9t = false;
string token;
if (in.peek() == 'N') {
in >> token; // file header
if (strstr(token.c_str(), "R9T")) {
rotation_parameter_num = 9; // rotation as 3x3 matrix
format_r9t = true;
}
}
int ncam = 0, npoint = 0, nproj = 0;
// read # of cameras
in >> ncam;
if (ncam <= 1) return false;
// read the camera parameters
camera_data.resize(ncam); // allocate the camera data
names.resize(ncam);
for (int i = 0; i < ncam; ++i) {
double f, q[9], c[3], d[2];
in >> token >> f;
for (int j = 0; j < rotation_parameter_num; ++j) in >> q[j];
in >> c[0] >> c[1] >> c[2] >> d[0] >> d[1];
camera_data[i].SetFocalLength(f);
if (format_r9t) {
camera_data[i].SetMatrixRotation(q);
camera_data[i].SetTranslation(c);
} else {
// older format for compability
camera_data[i].SetQuaternionRotation(q); // quaternion from the file
camera_data[i].SetCameraCenterAfterRotation(
c); // camera center from the file
}
camera_data[i].SetNormalizedMeasurementDistortion(d[0]);
names[i] = token;
}
//////////////////////////////////////
in >> npoint;
if (npoint <= 0) return false;
// read image projections and 3D points.
point_data.resize(npoint);
for (int i = 0; i < npoint; ++i) {
float pt[3];
int cc[3], npj;
in >> pt[0] >> pt[1] >> pt[2] >> cc[0] >> cc[1] >> cc[2] >> npj;
for (int j = 0; j < npj; ++j) {
int cidx, fidx;
float imx, imy;
in >> cidx >> fidx >> imx >> imy;
camidx.push_back(cidx); // camera index
ptidx.push_back(i); // point index
// add a measurment to the vector
measurements.push_back(Point2D(imx, imy));
nproj++;
}
point_data[i].SetPoint(pt);
ptc.insert(ptc.end(), cc, cc + 3);
}
///////////////////////////////////////////////////////////////////////////////
std::cout << ncam << " cameras; " << npoint << " 3D points; " << nproj
<< " projections\n";
return true;
}
void SaveNVM(const char* filename, vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<Point2D>& measurements,
vector<int>& ptidx, vector<int>& camidx, vector<string>& names,
vector<int>& ptc) {
std::cout << "Saving model to " << filename << "...\n";
ofstream out(filename);
out << "NVM_V3_R9T\n" << camera_data.size() << '\n' << std::setprecision(12);
if (names.size() < camera_data.size())
names.resize(camera_data.size(), string("unknown"));
if (ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0);
////////////////////////////////////
for (size_t i = 0; i < camera_data.size(); ++i) {
CameraT& cam = camera_data[i];
out << names[i] << ' ' << cam.GetFocalLength() << ' ';
for (int j = 0; j < 9; ++j) out << cam.m[0][j] << ' ';
out << cam.t[0] << ' ' << cam.t[1] << ' ' << cam.t[2] << ' '
<< cam.GetNormalizedMeasurementDistortion() << " 0\n";
}
out << point_data.size() << '\n';
for (size_t i = 0, j = 0; i < point_data.size(); ++i) {
Point3D& pt = point_data[i];
int* pc = &ptc[i * 3];
out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << ' ' << pc[0]
<< ' ' << pc[1] << ' ' << pc[2] << ' ';
size_t je = j;
while (je < ptidx.size() && ptidx[je] == (int)i) je++;
out << (je - j) << ' ';
for (; j < je; ++j)
out << camidx[j] << ' ' << " 0 " << measurements[j].x << ' '
<< measurements[j].y << ' ';
out << '\n';
}
}
bool LoadBundlerOut(const char* name, ifstream& in,
vector<CameraT>& camera_data, vector<Point3D>& point_data,
vector<Point2D>& measurements, vector<int>& ptidx,
vector<int>& camidx, vector<string>& names,
vector<int>& ptc) {
int rotation_parameter_num = 9;
string token;
while (in.peek() == '#') std::getline(in, token);
char listpath[1024], filepath[1024];
strcpy(listpath, name);
char* ext = strstr(listpath, ".out");
strcpy(ext, "-list.txt\0");
///////////////////////////////////
ifstream listin(listpath);
if (!listin.is_open()) {
listin.close();
listin.clear();
char* slash = strrchr(listpath, '/');
if (slash == NULL) slash = strrchr(listpath, '\\');
slash = slash ? slash + 1 : listpath;
strcpy(slash, "image_list.txt");
listin.open(listpath);
}
if (listin) std::cout << "Using image list: " << listpath << '\n';
// read # of cameras
int ncam = 0, npoint = 0, nproj = 0;
in >> ncam >> npoint;
if (ncam <= 1 || npoint <= 1) return false;
std::cout << ncam << " cameras; " << npoint << " 3D points;\n";
// read the camera parameters
camera_data.resize(ncam); // allocate the camera data
names.resize(ncam);
bool det_checked = false;
for (int i = 0; i < ncam; ++i) {
float f, q[9], c[3], d[2];
in >> f >> d[0] >> d[1];
for (int j = 0; j < rotation_parameter_num; ++j) in >> q[j];
in >> c[0] >> c[1] >> c[2];
camera_data[i].SetFocalLength(f);
camera_data[i].SetInvertedR9T(q, c);
camera_data[i].SetProjectionDistortion(d[0]);
if (listin >> filepath && f != 0) {
char* slash = strrchr(filepath, '/');
if (slash == NULL) slash = strchr(filepath, '\\');
names[i] = (slash ? (slash + 1) : filepath);
std::getline(listin, token);
if (!det_checked) {
float det = camera_data[i].GetRotationMatrixDeterminant();
std::cout << "Check rotation matrix: " << det << '\n';
det_checked = true;
}
} else {
names[i] = "unknown";
}
}
// read image projections and 3D points.
point_data.resize(npoint);
for (int i = 0; i < npoint; ++i) {
float pt[3];
int cc[3], npj;
in >> pt[0] >> pt[1] >> pt[2] >> cc[0] >> cc[1] >> cc[2] >> npj;
for (int j = 0; j < npj; ++j) {
int cidx, fidx;
float imx, imy;
in >> cidx >> fidx >> imx >> imy;
camidx.push_back(cidx); // camera index
ptidx.push_back(i); // point index
// add a measurment to the vector
measurements.push_back(Point2D(imx, -imy));
nproj++;
}
point_data[i].SetPoint(pt[0], pt[1], pt[2]);
ptc.insert(ptc.end(), cc, cc + 3);
}
///////////////////////////////////////////////////////////////////////////////
std::cout << ncam << " cameras; " << npoint << " 3D points; " << nproj
<< " projections\n";
return true;
}
void SaveBundlerOut(const char* filename, vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<Point2D>& measurements,
vector<int>& ptidx, vector<int>& camidx,
vector<string>& names, vector<int>& ptc) {
char listpath[1024];
strcpy(listpath, filename);
char* ext = strstr(listpath, ".out");
if (ext == NULL) return;
strcpy(ext, "-list.txt\0");
ofstream out(filename);
out << "# Bundle file v0.3\n";
out << std::setprecision(12); // need enough precision
out << camera_data.size() << " " << point_data.size() << '\n';
// save camera data
for (size_t i = 0; i < camera_data.size(); ++i) {
float q[9], c[3];
CameraT& ci = camera_data[i];
out << ci.GetFocalLength() << ' ' << ci.GetProjectionDistortion() << " 0\n";
ci.GetInvertedR9T(q, c);
for (int j = 0; j < 9; ++j) out << q[j] << (((j % 3) == 2) ? '\n' : ' ');
out << c[0] << ' ' << c[1] << ' ' << c[2] << '\n';
}
///
for (size_t i = 0, j = 0; i < point_data.size(); ++i) {
int npj = 0, *ci = &ptc[i * 3];
Point3D& pt = point_data[i];
while (j + npj < point_data.size() && ptidx[j + npj] == ptidx[j]) npj++;
///////////////////////////
out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n';
out << ci[0] << ' ' << ci[1] << ' ' << ci[2] << '\n';
out << npj << ' ';
for (int k = 0; k < npj; ++k)
out << camidx[j + k] << " 0 " << measurements[j + k].x << ' '
<< -measurements[j + k].y << '\n';
out << '\n';
j += npj;
}
ofstream listout(listpath);
for (size_t i = 0; i < names.size(); ++i) listout << names[i] << '\n';
}
template <class CameraT, class Point3D>
bool LoadBundlerModel(ifstream& in, vector<CameraT>& camera_data,
vector<Point3D>& point_data,
vector<Point2D>& measurements, vector<int>& ptidx,
vector<int>& camidx) {
// read bundle data from a file
size_t ncam = 0, npt = 0, nproj = 0;
if (!(in >> ncam >> npt >> nproj)) return false;
///////////////////////////////////////////////////////////////////////////////
std::cout << ncam << " cameras; " << npt << " 3D points; " << nproj
<< " projections\n";
camera_data.resize(ncam);
point_data.resize(npt);
measurements.resize(nproj);
camidx.resize(nproj);
ptidx.resize(nproj);
for (size_t i = 0; i < nproj; ++i) {
double x, y;
int cidx, pidx;
in >> cidx >> pidx >> x >> y;
if (((size_t)pidx) == npt && camidx.size() > i) {
camidx.resize(i);
ptidx.resize(i);
measurements.resize(i);
std::cout << "Truncate measurements to " << i << '\n';
} else if (((size_t)pidx) >= npt) {
continue;
} else {
camidx[i] = cidx;
ptidx[i] = pidx;
measurements[i].SetPoint2D(x, -y);
}
}
for (size_t i = 0; i < ncam; ++i) {
double p[9];
for (int j = 0; j < 9; ++j) in >> p[j];
CameraT& cam = camera_data[i];
cam.SetFocalLength(p[6]);
cam.SetInvertedRT(p, p + 3);
cam.SetProjectionDistortion(p[7]);
}
for (size_t i = 0; i < npt; ++i) {
double pt[3];
in >> pt[0] >> pt[1] >> pt[2];
point_data[i].SetPoint(pt);
}
return true;
}
void SaveBundlerModel(const char* filename, vector<CameraT>& camera_data,
vector<Point3D>& point_data,
vector<Point2D>& measurements, vector<int>& ptidx,
vector<int>& camidx) {
std::cout << "Saving model to " << filename << "...\n";
ofstream out(filename);
out << std::setprecision(12); // need enough precision
out << camera_data.size() << ' ' << point_data.size() << ' '
<< measurements.size() << '\n';
for (size_t i = 0; i < measurements.size(); ++i) {
out << camidx[i] << ' ' << ptidx[i] << ' ' << measurements[i].x << ' '
<< -measurements[i].y << '\n';
}
for (size_t i = 0; i < camera_data.size(); ++i) {
CameraT& cam = camera_data[i];
double r[3], t[3];
cam.GetInvertedRT(r, t);
out << r[0] << ' ' << r[1] << ' ' << r[2] << ' ' << t[0] << ' ' << t[1]
<< ' ' << t[2] << ' ' << cam.f << ' ' << cam.GetProjectionDistortion()
<< " 0\n";
}
for (size_t i = 0; i < point_data.size(); ++i) {
Point3D& pt = point_data[i];
out << pt.xyz[0] << ' ' << pt.xyz[1] << ' ' << pt.xyz[2] << '\n';
}
}
bool LoadModelFile(const char* name, vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<Point2D>& measurements,
vector<int>& ptidx, vector<int>& camidx,
vector<string>& names, vector<int>& ptc) {
if (name == NULL) return false;
ifstream in(name);
std::cout << "Loading cameras/points: " << name << "\n";
if (!in.is_open()) return false;
if (strstr(name, ".nvm"))
return LoadNVM(in, camera_data, point_data, measurements, ptidx, camidx,
names, ptc);
else if (strstr(name, ".out"))
return LoadBundlerOut(name, in, camera_data, point_data, measurements,
ptidx, camidx, names, ptc);
else
return LoadBundlerModel(in, camera_data, point_data, measurements, ptidx,
camidx);
}
float random_ratio(float percent) {
return (rand() % 101 - 50) * 0.02f * percent + 1.0f;
}
void AddNoise(vector<CameraT>& camera_data, vector<Point3D>& point_data,
float percent) {
std::srand((unsigned int)time(NULL));
for (size_t i = 0; i < camera_data.size(); ++i) {
camera_data[i].f *= random_ratio(percent);
camera_data[i].t[0] *= random_ratio(percent);
camera_data[i].t[1] *= random_ratio(percent);
camera_data[i].t[2] *= random_ratio(percent);
double e[3];
camera_data[i].GetRodriguesRotation(e);
e[0] *= random_ratio(percent);
e[1] *= random_ratio(percent);
e[2] *= random_ratio(percent);
camera_data[i].SetRodriguesRotation(e);
}
for (size_t i = 0; i < point_data.size(); ++i) {
point_data[i].xyz[0] *= random_ratio(percent);
point_data[i].xyz[1] *= random_ratio(percent);
point_data[i].xyz[2] *= random_ratio(percent);
}
}
void AddStableNoise(vector<CameraT>& camera_data, vector<Point3D>& point_data,
const vector<int>& ptidx, const vector<int>& camidx,
float percent) {
///
std::srand((unsigned int)time(NULL));
// do not modify the visibility status..
vector<float> zz0(ptidx.size());
vector<CameraT> backup = camera_data;
vector<float> vx(point_data.size()), vy(point_data.size()),
vz(point_data.size());
for (size_t i = 0; i < point_data.size(); ++i) {
Point3D& pt = point_data[i];
vx[i] = pt.xyz[0];
vy[i] = pt.xyz[1];
vz[i] = pt.xyz[2];
}
// find out the median location of all the 3D points.
size_t median_idx = point_data.size() / 2;
std::nth_element(vx.begin(), vx.begin() + median_idx, vx.end());
std::nth_element(vy.begin(), vy.begin() + median_idx, vy.end());
std::nth_element(vz.begin(), vz.begin() + median_idx, vz.end());
float cx = vx[median_idx], cy = vy[median_idx], cz = vz[median_idx];
for (size_t i = 0; i < ptidx.size(); ++i) {
CameraT& cam = camera_data[camidx[i]];
Point3D& pt = point_data[ptidx[i]];
zz0[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
cam.m[2][2] * pt.xyz[2] + cam.t[2];
}
vector<float> z2 = zz0;
median_idx = ptidx.size() / 2;
std::nth_element(z2.begin(), z2.begin() + median_idx, z2.end());
float mz = z2[median_idx]; // median depth
float dist_noise_base = mz * 0.2f;
/////////////////////////////////////////////////
// modify points first..
for (size_t i = 0; i < point_data.size(); ++i) {
Point3D& pt = point_data[i];
pt.xyz[0] = pt.xyz[0] - cx + dist_noise_base * random_ratio(percent);
pt.xyz[1] = pt.xyz[1] - cy + dist_noise_base * random_ratio(percent);
pt.xyz[2] = pt.xyz[2] - cz + dist_noise_base * random_ratio(percent);
}
vector<bool> need_modification(camera_data.size(), true);
int invalid_count = 0, modify_iteration = 1;
do {
if (invalid_count)
std::cout << "NOTE" << std::setw(2) << modify_iteration << ": modify "
<< invalid_count << " camera to fix visibility\n";
//////////////////////////////////////////////////////
for (size_t i = 0; i < camera_data.size(); ++i) {
if (!need_modification[i]) continue;
CameraT& cam = camera_data[i];
double e[3], c[3];
cam = backup[i];
cam.f *= random_ratio(percent);
///////////////////////////////////////////////////////////
cam.GetCameraCenter(c);
c[0] = c[0] - cx + dist_noise_base * random_ratio(percent);
c[1] = c[1] - cy + dist_noise_base * random_ratio(percent);
c[2] = c[2] - cz + dist_noise_base * random_ratio(percent);
///////////////////////////////////////////////////////////
cam.GetRodriguesRotation(e);
e[0] *= random_ratio(percent);
e[1] *= random_ratio(percent);
e[2] *= random_ratio(percent);
///////////////////////////////////////////////////////////
cam.SetRodriguesRotation(e);
cam.SetCameraCenterAfterRotation(c);
}
vector<bool> invalidc(camera_data.size(), false);
invalid_count = 0;
for (size_t i = 0; i < ptidx.size(); ++i) {
int cid = camidx[i];
if (need_modification[cid] == false) continue;
if (invalidc[cid]) continue;
CameraT& cam = camera_data[cid];
Point3D& pt = point_data[ptidx[i]];
float z = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
cam.m[2][2] * pt.xyz[2] + cam.t[2];
if (z * zz0[i] > 0) continue;
if (zz0[i] == 0 && z > 0) continue;
invalid_count++;
invalidc[cid] = true;
}
need_modification = invalidc;
modify_iteration++;
} while (invalid_count && modify_iteration < 20);
}
void ExamineVisiblity(const char* input_filename) {
//////////////
vector<CameraD> camera_data;
vector<Point3B> point_data;
vector<int> ptidx, camidx;
vector<Point2D> measurements;
ifstream in(input_filename);
LoadBundlerModel(in, camera_data, point_data, measurements, ptidx, camidx);
////////////////
int count = 0;
double d1 = 100, d2 = 100;
std::cout << "checking visibility...\n";
vector<double> zz(ptidx.size());
for (size_t i = 0; i < ptidx.size(); ++i) {
CameraD& cam = camera_data[camidx[i]];
Point3B& pt = point_data[ptidx[i]];
double dz = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
cam.m[2][2] * pt.xyz[2] + cam.t[2];
// double dx = cam.m[0][0] * pt.xyz[0] + cam.m[0][1] * pt.xyz[1] +
// cam.m[0][2] * pt.xyz[2] + cam.t[0];
// double dy = cam.m[1][0] * pt.xyz[0] + cam.m[1][1] * pt.xyz[1] +
// cam.m[1][2] * pt.xyz[2] + cam.t[1];
////////////////////////////////////////
float c[3];
cam.GetCameraCenter(c);
CameraT camt;
camt.SetCameraT(cam);
Point3D ptt;
ptt.SetPoint(pt.xyz);
double fz = camt.m[2][0] * ptt.xyz[0] + camt.m[2][1] * ptt.xyz[1] +
camt.m[2][2] * ptt.xyz[2] + camt.t[2];
double fz2 = camt.m[2][0] * (ptt.xyz[0] - c[0]) +
camt.m[2][1] * (ptt.xyz[1] - c[1]) +
camt.m[2][2] * (ptt.xyz[2] - c[2]);
// if(dz == 0 && fz == 0) continue;
if (dz * fz <= 0 || fz == 0) {
std::cout << "cam "
<< camidx[i] //<<// "; dx = " << dx << "; dy = " << dy
<< "; double: " << dz << "; float " << fz << "; float2 " << fz2
<< "\n";
// std::cout << cam.m[2][0] << " "<<cam.m[2][1]<< " " << cam.m[2][2] << "
// "<<cam.t[2] << "\n";
// std::cout << camt.m[2][0] << " "<<camt.m[2][1]<< " " << camt.m[2][2]
// << " "<<camt.t[2] << "\n";
// std::cout << cam.m[2][0] - camt.m[2][0] << " " <<cam.m[2][1] -
// camt.m[2][1]<< " "
// << cam.m[2][2] - camt.m[2][2] << " " <<cam.t[2] - camt.t[2]<<
// "\n";
}
zz[i] = dz;
d1 = std::min(fabs(dz), d1);
d2 = std::min(fabs(fz), d2);
}
std::cout << count << " points moved to wrong side " << d1 << ", " << d2
<< "\n";
}
bool RemoveInvisiblePoints(vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<int>& ptidx,
vector<int>& camidx, vector<Point2D>& measurements,
vector<string>& names, vector<int>& ptc) {
vector<float> zz(ptidx.size());
for (size_t i = 0; i < ptidx.size(); ++i) {
CameraT& cam = camera_data[camidx[i]];
Point3D& pt = point_data[ptidx[i]];
zz[i] = cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
cam.m[2][2] * pt.xyz[2] + cam.t[2];
}
size_t median_idx = ptidx.size() / 2;
std::nth_element(zz.begin(), zz.begin() + median_idx, zz.end());
float dist_threshold = zz[median_idx] * 0.001f;
// keep removing 3D points. until all of them are infront of the cameras..
vector<bool> pmask(point_data.size(), true);
int points_removed = 0;
for (size_t i = 0; i < ptidx.size(); ++i) {
int cid = camidx[i], pid = ptidx[i];
if (!pmask[pid]) continue;
CameraT& cam = camera_data[cid];
Point3D& pt = point_data[pid];
bool visible = (cam.m[2][0] * pt.xyz[0] + cam.m[2][1] * pt.xyz[1] +
cam.m[2][2] * pt.xyz[2] + cam.t[2] >
dist_threshold);
pmask[pid] = visible; // this point should be removed
if (!visible) points_removed++;
}
if (points_removed == 0) return false;
vector<int> cv(camera_data.size(), 0);
// should any cameras be removed ?
int min_observation = 20; // cameras should see at leat 20 points
do {
// count visible points for each camera
std::fill(cv.begin(), cv.end(), 0);
for (size_t i = 0; i < ptidx.size(); ++i) {
int cid = camidx[i], pid = ptidx[i];
if (pmask[pid]) cv[cid]++;
}
// check if any more points should be removed
vector<int> pv(point_data.size(), 0);
for (size_t i = 0; i < ptidx.size(); ++i) {
int cid = camidx[i], pid = ptidx[i];
if (!pmask[pid]) continue; // point already removed
if (cv[cid] < min_observation) // this camera shall be removed.
{
///
} else {
pv[pid]++;
}
}
points_removed = 0;
for (size_t i = 0; i < point_data.size(); ++i) {
if (pmask[i] == false) continue;
if (pv[i] >= 2) continue;
pmask[i] = false;
points_removed++;
}
} while (points_removed > 0);
////////////////////////////////////
vector<bool> cmask(camera_data.size(), true);
for (size_t i = 0; i < camera_data.size(); ++i)
cmask[i] = cv[i] >= min_observation;
////////////////////////////////////////////////////////
vector<int> cidx(camera_data.size());
vector<int> pidx(point_data.size());
/// modified model.
vector<CameraT> camera_data2;
vector<Point3D> point_data2;
vector<int> ptidx2;
vector<int> camidx2;
vector<Point2D> measurements2;
vector<string> names2;
vector<int> ptc2;
//
if (names.size() < camera_data.size())
names.resize(camera_data.size(), string("unknown"));
if (ptc.size() < 3 * point_data.size()) ptc.resize(point_data.size() * 3, 0);
//////////////////////////////
int new_camera_count = 0, new_point_count = 0;
for (size_t i = 0; i < camera_data.size(); ++i) {
if (!cmask[i]) continue;
camera_data2.push_back(camera_data[i]);
names2.push_back(names[i]);
cidx[i] = new_camera_count++;
}
for (size_t i = 0; i < point_data.size(); ++i) {
if (!pmask[i]) continue;
point_data2.push_back(point_data[i]);
ptc.push_back(ptc[i]);
pidx[i] = new_point_count++;
}
int new_observation_count = 0;
for (size_t i = 0; i < ptidx.size(); ++i) {
int pid = ptidx[i], cid = camidx[i];
if (!pmask[pid] || !cmask[cid]) continue;
ptidx2.push_back(pidx[pid]);
camidx2.push_back(cidx[cid]);
measurements2.push_back(measurements[i]);
new_observation_count++;
}
std::cout << "NOTE: removing " << (camera_data.size() - new_camera_count)
<< " cameras; " << (point_data.size() - new_point_count)
<< " 3D Points; " << (measurements.size() - new_observation_count)
<< " Observations;\n";
camera_data2.swap(camera_data);
names2.swap(names);
point_data2.swap(point_data);
ptc2.swap(ptc);
ptidx2.swap(ptidx);
camidx2.swap(camidx);
measurements2.swap(measurements);
return true;
}
void SaveModelFile(const char* outpath, vector<CameraT>& camera_data,
vector<Point3D>& point_data, vector<Point2D>& measurements,
vector<int>& ptidx, vector<int>& camidx,
vector<string>& names, vector<int>& ptc) {
if (outpath == NULL) return;
if (strstr(outpath, ".nvm"))
SaveNVM(outpath, camera_data, point_data, measurements, ptidx, camidx,
names, ptc);
else if (strstr(outpath, ".out"))
SaveBundlerOut(outpath, camera_data, point_data, measurements, ptidx,
camidx, names, ptc);
else
SaveBundlerModel(outpath, camera_data, point_data, measurements, ptidx,
camidx);
}
} // namespace pba
Reference in new issue View Git Blame Copy Permalink