std::vector<std::vector<cv::Point2f> > OutlierDetector::fitSubspace(const std::vector<std::vector<cv::Point2f> > &trajectories, std::vector<cv::Point2f> &outlier_points, int num_motions, double sigma)
{
bool print = false;
int subspace_dimensions = trajectories[0].size() * 2; // n
int num_trajectories = trajectories.size();
// each column in data represents one trajectory
// even rows are x coordinates, odd rows are y coordinates
Eigen::MatrixXf data(subspace_dimensions, num_trajectories);
if (print) std::cout << "fill matrix " << std::endl;
fillMatrix(trajectories, data);
if (print) std::cout << "mean subtract " << std::endl;
meanSubtract(data);
int num_sample_points = 4 * num_motions; // d
int num_iterations = 50;
Eigen::VectorXf final_residual;
std::vector<int> final_columns;
int max_points = 0;
if (print) std::cout << " start iterations " << std::endl;
for (int i = 0; i < num_iterations; i++)
{
Eigen::MatrixXf subset(subspace_dimensions, num_sample_points);
if (print) std::cout << "file subset " << std::endl;
std::vector<int> column_indices;
column_indices = fillSubset(data, subset, num_sample_points);
if (print) std::cout << "svd calculation " << std::endl;
// Eigen::JacobiSVD<Eigen::MatrixXf> svd(subset, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::JacobiSVD<Eigen::MatrixXf, Eigen::FullPivHouseholderQRPreconditioner> svd(subset, Eigen::ComputeFullU | Eigen::ComputeFullV);
if (print) std::cout << "init Pnd " << std::endl;
Eigen::MatrixXf Pnd = Eigen::MatrixXf::Zero(subspace_dimensions, subspace_dimensions);
//std::cout << "U" << std::endl;
//std::cout << svd.matrixU() << std::endl;
//std::cout << svd.singularValues() << std::endl;
if (print) std::cout << "calc M " << std::endl;
for (int idx = 0; idx < num_sample_points; idx++)
{
Eigen::VectorXf u = svd.matrixU().col(idx);
Eigen::MatrixXf M = u*u.transpose();
Pnd = Pnd + M;
}
if (print) std::cout << "calc Pnd " << std::endl;
Pnd = Eigen::MatrixXf::Identity(subspace_dimensions, subspace_dimensions) - Pnd;
Eigen::MatrixXf data_T = data.transpose();
if (print) std::cout << "calc residual " << std::endl;
Eigen::VectorXf residual = (data_T * (Pnd * data)).diagonal();
if (print) std::cout << "residual : " << std::endl;
if (print) std::cout << residual << std::endl;
residual = residual.cwiseAbs();
int num_points = 0;
for (int idx = 0; idx < residual.size(); idx++)
{
if (print) std::cout << "threshold " << (subspace_dimensions - num_sample_points) * sigma * sigma << std::endl;
if (residual(idx) < (subspace_dimensions - num_sample_points) * sigma * sigma)
{
if (print) std::cout << "adding " << std::endl;
num_points++;
}
}
if (num_points > max_points)
{
if (print) std::cout << num_points << " to " << max_points << std::endl;
if (print) std::cout << "copy residual" << residual << std::endl;
max_points = num_points;
final_residual = residual;
final_columns = column_indices;
if (print) std::cout << "copy final residual " << final_residual << std::endl;
}
}
if (print) std::cout << "final residual " << std::endl;
if (print) std::cout << final_residual << std::endl;
double residual_threshold = 0.2;
if (subspace_dimensions - num_sample_points < 11 && subspace_dimensions - num_sample_points > 0)
{
residual_threshold = sigma * sigma * chi_square_table.at(0).at(subspace_dimensions - num_sample_points);
std::cout << "residual threshold: " << residual_threshold << std::endl;
}
for (int idx = 0; idx < final_residual.size(); idx++)
{
if (final_residual(idx) > residual_threshold)
{
outlier_points.push_back(trajectories.at(idx).at(trajectories.at(idx).size() - 2));
}
}
std::vector<std::vector<cv::Point2f> > trajectory_subspace_vectors;
for (int i = 0; i < final_columns.size(); i++)
{
trajectory_subspace_vectors.push_back(trajectories.at(final_columns.at(i)));
}
return trajectory_subspace_vectors;
}
