Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
59 lines (51 sloc) 1.53 KB
#ifndef PLANEMODEL_H
#define PLANEMODEL_H
#include <Eigen/Core>
#include <Eigen/Dense>
#include <array>
#include <vector>
struct PlaneModel
{
static const int ModelSize = 3;
Eigen::Vector3d n;
double d;
void compute(const std::vector<Eigen::Vector3d>& data, const std::array<size_t, 3>& indices)
{
Eigen::Vector3d a = data[indices[1]] - data[indices[0]];
Eigen::Vector3d b = data[indices[2]] - data[indices[0]];
n = a.cross(b).normalized();
d = n.dot(data[indices[0]]);
}
int computeInliers(const std::vector<Eigen::Vector3d>& data, double threshold)
{
int inliers = 0;
for (size_t i = 0; i < data.size(); i++)
{
if (fabs(n.dot(data[i]) - d) < threshold)
inliers++;
}
return inliers;
}
void refine(const std::vector<Eigen::Vector3d>& data, double threshold)
{
Eigen::Vector3d Ex = Eigen::Vector3d::Zero();
Eigen::Matrix3d Exsqr = Eigen::Matrix3d::Zero();
int inliers = 0;
for (size_t i = 0; i < data.size(); i++)
{
if (fabs(n.dot(data[i]) - d) < threshold)
{
inliers++;
Ex += data[i];
Exsqr += data[i] * data[i].transpose();
}
}
Ex /= inliers;
Exsqr /= inliers;
Eigen::Matrix3d cov = Exsqr - Ex * Ex.transpose();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eig(cov);
n = eig.eigenvectors().col(0);
d = n.dot(Ex);
}
};
#endif
You can’t perform that action at this time.