void plot_best_plane(const pcl::PointCloud<pcl::PointXYZ>& points, const Eigen::Vector4f plane, double threshold)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inlier_cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZ>::Ptr outlier_cloud(new pcl::PointCloud<pcl::PointXYZ>());
for (int i = 0; i < points.size(); ++i) {
if (is_inlier(points[i].getVector3fMap(), plane, threshold)) {
inlier_cloud->push_back(points[i]);
}
else {
outlier_cloud->push_back(points[i]);
}
}
pcl::visualization::PCLVisualizer viewer("3D Viewer");
viewer.setBackgroundColor(0, 0, 0);
viewer.addCoordinateSystem(1.0);
viewer.initCameraParameters();
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> inlier_color_handler(inlier_cloud, 255, 0, 0);
viewer.addPointCloud(inlier_cloud, inlier_color_handler, "inliers");
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outlier_color_handler(outlier_cloud, 0, 0, 255);
viewer.addPointCloud(outlier_cloud, outlier_color_handler, "outliers");
while (!viewer.wasStopped())
{
viewer.spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
ROS_INFO("Got a pointcloud, calibrating...");
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg(*msg, cloud);
int nbr = cloud.size();
int max = 1000; // ransac iterations
double threshold = 0.02; // threshold for plane inliers
Eigen::Vector4f best_plane; // best plane parameters found
int best_inliers = -1; // best number of inliers
int inds[3];
Eigen::Vector4f plane;
int inliers;
for (int i = 0; i < max; ++i) {
for (int j = 0; j < 3; ++j) {
inds[j] = rand() % nbr; // get a random point
}
// check that the points aren't the same
if (inds[0] == inds[1] || inds[0] == inds[2] || inds[1] == inds[2]) {
continue;
}
compute_plane(plane, cloud, inds);
inliers = 0;
for (int j = 0; j < nbr; j += 30) { // count number of inliers
if (is_inlier(cloud[j].getVector3fMap(), plane, threshold)) {
++inliers;
}
}
if (inliers > best_inliers) {
best_plane = plane;
best_inliers = inliers;
}
}
extract_height_and_angle(best_plane); // find parameters and feed them to datacentre
plot_best_plane(cloud, best_plane, threshold); // visually evaluate plane fit
exit(0);
}
TEX_NAMESPACE_BEGIN
/**
* Dampens the quality of all views in which the face's projection
* has a much different color than in the majority of views.
* Returns whether the outlier removal was successfull.
*
* @param infos contains information about one face seen from several views
* @param settings runtime configuration.
*/
bool
photometric_outlier_detection(std::vector<ProjectedFaceInfo> * infos, Settings const & settings) {
if (infos->size() == 0) return true;
/* Configuration variables. */
double const gauss_rejection_threshold = 6e-3;
/* If all covariances drop below this we stop outlier detection. */
double const minimal_covariance = 5e-4;
int const outlier_detection_iterations = 10;
int const minimal_num_inliers = 4;
float outlier_removal_factor = std::numeric_limits<float>::signaling_NaN();
switch (settings.outlier_removal) {
case NONE: return true;
case GAUSS_CLAMPING:
outlier_removal_factor = 1.0f;
break;
case GAUSS_DAMPING:
outlier_removal_factor = 0.2f;
break;
}
Eigen::MatrixX3d inliers(infos->size(), 3);
std::vector<std::uint32_t> is_inlier(infos->size(), 1);
for (std::size_t row = 0; row < infos->size(); ++row) {
inliers.row(row) = mve_to_eigen(infos->at(row).mean_color).cast<double>();
}
Eigen::RowVector3d var_mean;
Eigen::Matrix3d covariance;
Eigen::Matrix3d covariance_inv;
for (int i = 0; i < outlier_detection_iterations; ++i) {
if (inliers.rows() < minimal_num_inliers) {
return false;
}
/* Calculate the inliers' mean color and color covariance. */
var_mean = inliers.colwise().mean();
Eigen::MatrixX3d centered = inliers.rowwise() - var_mean;
covariance = (centered.adjoint() * centered) / double(inliers.rows() - 1);
/* If all covariances are very small we stop outlier detection
* and only keep the inliers (set quality of outliers to zero). */
if (covariance.array().abs().maxCoeff() < minimal_covariance) {
for (std::size_t row = 0; row < infos->size(); ++row) {
if (!is_inlier[row]) infos->at(row).quality = 0.0f;
}
return true;
}
/* Invert the covariance. FullPivLU is not the fastest way but
* it gives feedback about numerical stability during inversion. */
Eigen::FullPivLU<Eigen::Matrix3d> lu(covariance);
if (!lu.isInvertible()) {
return false;
}
covariance_inv = lu.inverse();
/* Compute new number of inliers (all views with a gauss value above a threshold). */
for (std::size_t row = 0; row < infos->size(); ++row) {
Eigen::RowVector3d color = mve_to_eigen(infos->at(row).mean_color).cast<double>();
double gauss_value = multi_gauss_unnormalized(color, var_mean, covariance_inv);
is_inlier[row] = (gauss_value >= gauss_rejection_threshold ? 1 : 0);
}
/* Resize Eigen matrix accordingly and fill with new inliers. */
inliers.resize(std::accumulate(is_inlier.begin(), is_inlier.end(), 0), Eigen::NoChange);
for (std::size_t row = 0, inlier_row = 0; row < infos->size(); ++row) {
if (is_inlier[row]) {
inliers.row(inlier_row++) = mve_to_eigen(infos->at(row).mean_color).cast<double>();
}
}
}
covariance_inv *= outlier_removal_factor;
for (ProjectedFaceInfo & info : *infos) {
Eigen::RowVector3d color = mve_to_eigen(info.mean_color).cast<double>();
double gauss_value = multi_gauss_unnormalized(color, var_mean, covariance_inv);
assert(0.0 <= gauss_value && gauss_value <= 1.0);
switch(settings.outlier_removal) {
case NONE: return true;
case GAUSS_DAMPING:
info.quality *= gauss_value;
break;
case GAUSS_CLAMPING:
if (gauss_value < gauss_rejection_threshold) info.quality = 0.0f;
break;
}
}
return true;
}
