TEST (RandomSample, Filters)
{
// Test the PointCloud<PointT> method
// Randomly sample 10 points from cloud
RandomSample<PointXYZ> sample (true); // Extract removed indices
sample.setInputCloud (cloud_walls);
sample.setSample (10);
// Indices
std::vector<int> indices;
sample.filter (indices);
EXPECT_EQ (int (indices.size ()), 10);
// Cloud
PointCloud<PointXYZ> cloud_out;
sample.filter(cloud_out);
EXPECT_EQ (int (cloud_out.width), 10);
EXPECT_EQ (int (indices.size ()), int (cloud_out.size ()));
for (size_t i = 0; i < indices.size () - 1; ++i)
{
// Check that indices are sorted
EXPECT_LT (indices[i], indices[i+1]);
// Compare original points with sampled indices against sampled points
EXPECT_NEAR (cloud_walls->points[indices[i]].x, cloud_out.points[i].x, 1e-4);
EXPECT_NEAR (cloud_walls->points[indices[i]].y, cloud_out.points[i].y, 1e-4);
EXPECT_NEAR (cloud_walls->points[indices[i]].z, cloud_out.points[i].z, 1e-4);
}
IndicesConstPtr removed = sample.getRemovedIndices ();
EXPECT_EQ (removed->size (), cloud_walls->size () - 10);
// Organized
// (sdmiller) Removing for now, to debug the Linux 32-bit segfault offline
sample.setKeepOrganized (true);
sample.filter(cloud_out);
removed = sample.getRemovedIndices ();
EXPECT_EQ (int (removed->size ()), cloud_walls->size () - 10);
for (size_t i = 0; i < removed->size (); ++i)
{
EXPECT_TRUE (pcl_isnan (cloud_out.at ((*removed)[i]).x));
EXPECT_TRUE (pcl_isnan (cloud_out.at ((*removed)[i]).y));
EXPECT_TRUE (pcl_isnan (cloud_out.at ((*removed)[i]).z));
}
EXPECT_EQ (cloud_out.width, cloud_walls->width);
EXPECT_EQ (cloud_out.height, cloud_walls->height);
// Negative
sample.setKeepOrganized (false);
sample.setNegative (true);
sample.filter(cloud_out);
removed = sample.getRemovedIndices ();
EXPECT_EQ (int (removed->size ()), 10);
EXPECT_EQ (int (cloud_out.size ()), int (cloud_walls->size () - 10));
// Make sure sampling >N works
sample.setSample (static_cast<unsigned int> (cloud_walls->size ()+10));
sample.setNegative (false);
sample.filter (cloud_out);
EXPECT_EQ (cloud_out.size (), cloud_walls->size ());
removed = sample.getRemovedIndices ();
EXPECT_TRUE (removed->empty ());
// Test the pcl::PCLPointCloud2 method
// Randomly sample 10 points from cloud
pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2 ());
toPCLPointCloud2 (*cloud_walls, *cloud_blob);
RandomSample<pcl::PCLPointCloud2> sample2;
sample2.setInputCloud (cloud_blob);
sample2.setSample (10);
// Indices
std::vector<int> indices2;
sample2.filter (indices2);
EXPECT_EQ (int (indices2.size ()), 10);
// Cloud
pcl::PCLPointCloud2 output_blob;
sample2.filter (output_blob);
fromPCLPointCloud2 (output_blob, cloud_out);
EXPECT_EQ (int (cloud_out.width), 10);
EXPECT_EQ (int (indices2.size ()), int (cloud_out.size ()));
for (size_t i = 0; i < indices2.size () - 1; ++i)
{
// Check that indices are sorted
EXPECT_LT (indices2[i], indices2[i+1]);
// Compare original points with sampled indices against sampled points
EXPECT_NEAR (cloud_walls->points[indices2[i]].x, cloud_out.points[i].x, 1e-4);
EXPECT_NEAR (cloud_walls->points[indices2[i]].y, cloud_out.points[i].y, 1e-4);
EXPECT_NEAR (cloud_walls->points[indices2[i]].z, cloud_out.points[i].z, 1e-4);
}
}
int
PatchFit::fit()
{
// first check whether cloud subsampling is required
RandomSample<PointXYZ> *rs;
rs = new RandomSample<PointXYZ>();
if (this->ssmax_)
{
rs->setInputCloud(cloud_);
rs->setSample(ssmax_);
rs->setSeed(rand());
rs->filter(*cloud_);
}
delete (rs);
//remove NAN points from the cloud
std::vector<int> indices;
removeNaNFromPointCloud(*cloud_, *cloud_, indices); //is_dense should be false
// Step 1: fit plane by lls
Vector4d centroid;
compute3DCentroid(*cloud_, centroid);
p_->setC(centroid.head(3));
cloud_vec.resize(cloud_->points.size(),3);
fit_cloud_vec.resize(cloud_->points.size(),3);
int vec_counter = 0;
for (size_t i = 0; i<cloud_->points.size (); i++)
{
cloud_vec.row(vec_counter) = cloud_->points[i].getVector3fMap ().cast<double>();
// for fitting sub the centroid
fit_cloud_vec.row(vec_counter) = cloud_vec.row(vec_counter) - centroid.head(3).transpose();
vec_counter++;
}
cloud_vec.conservativeResize(vec_counter-1, 3); //normal resize does not keep old values
VectorXd b(fit_cloud_vec.rows());
b.fill(0.0);
Vector3d zl = lls(fit_cloud_vec,b);
p_->setR(zh.cross(zl));
double ss = p_->getR().norm();
double cc = zh.adjoint()*zl;
double t = atan2(ss,cc);
double th = sqrt(sqrt(EPS))*10.0;
double aa;
if (t>th)
aa = t/ss;
else
aa = 6.0/(6.0-t*t);
p_->setR(aa*p_->getR());
// Save the zl and the centroid p.c, for patch center constraint's use
plane_n = zl;
plane_c = p_->getC();
// Step 2: continue surface fitting if not plane
VectorXd a;
if (st_a) // general paraboloid
{
if (ccon_)
{
a.resize(6);
a << 0.0, 0.0, p_->getR(), 0.0;
}
else
{
a.resize(8);
a << 0.0, 0.0, p_->getR(), p_->getC();
}
a = wlm(cloud_vec, a);
// update the patch
p_->setR(a.segment(2,3));
if (ccon_)
p_->setC(plane_c + a(5)*plane_n);
else
p_->setC(a.segment(5,3));
Vector2d k;
k << a(0), a(1);
// refine into a specific paraboloid type
if (k.cwiseAbs().maxCoeff() < this->ktol_)
{
// fitting planar paraboloid
p_->setName("planar paraboloid");
p_->setS(Patch::s_p);
Matrix<double,1,1> k_p;
k_p << 0.0;
p_->setK(k_p);
p_->setR(p_->rcanon2(p_->getR(),2,3)); //TBD not updating correctly
}
else if (k.cwiseAbs().minCoeff() < this->ktol_)
{
// fitting cylindric paraboloid
p_->setName("cylindric paraboloid");
p_->setS(Patch::s_y);
p_->setB(Patch::b_r);
kx = k(0);
ky = k(1);
if (fabs(kx) > fabs(ky))
{
// swap curvatures
kx = k(1);
ky = k(0);
Matrix3d ww;
ww << yh, -xh, zh;
Matrix3d rr;
rr = p_->rexp(p_->getR());
rr = rr*ww;
p_->setR(p_->rlog(rr));
}
Eigen::Matrix<double,1,1> k_c_p;
k_c_p << ky;
p_->setK(k_c_p);
}
else if (fabs(k(0)-k(1)) < this->ktol_)
{
// fitting circular paraboloid
p_->setName("circular paraboloid");
p_->setS(Patch::s_o);
p_->setB(Patch::b_c);
Matrix<double,1,1> k_c_p;
k_c_p << k.mean();
p_->setK(k_c_p);
p_->setR(p_->rcanon2(p_->getR(),2,3));
}
else
{
if (sign(k(0),k(1)))
{
// fitting elliptic paraboloid
p_->setName("elliptic paraboloid");
p_->setS(Patch::s_e);
}
else
{
// fitting hyperbolic paraboloid
p_->setName("hyperbolic paraboloid");
p_->setS(Patch::s_h);
}
p_->setB(Patch::b_e);
p_->setK(k);
kx = k(0);
ky = k(1);
if (fabs(kx) > fabs(ky))
{
// swap curvatures
Vector2d k_swap;
k_swap << ky, kx;
p_->setK(k_swap);
Matrix3d ww;
ww << yh, -xh, zh;
Matrix3d rr;
rr = p_->rexp(p_->getR());
rr = rr*ww;
p_->setR(p_->rlog(rr));
}
}
}
else
{
// plane fitting
p_->setS(Patch::s_p);
p_->setB(this->bt_);
Matrix<double,1,1> k_p;
k_p << 0.0;
p_->setK(k_p);
}
// Step 5: fit boundary; project to local frame XY plane
Xform3 proj;
Matrix3d rrinv;
rrinv = p_->rexp(-p_->getR());
MatrixXd ux(cloud_vec.rows(),1), uy(cloud_vec.rows(),1), uz(cloud_vec.rows(),1);
ux = cloud_vec.col(0);
uy = cloud_vec.col(1);
uz = cloud_vec.col(2);
proj.transform(ux,uy,uz,p_->rexp(-p_->getR()), (-rrinv*p_->getC()),0,0);
// (normalized) moments
double mx = ux.mean();
double my = uy.mean();
double vx = (ux.array().square()).mean();
double vy = (uy.array().square()).mean();
double vxy = (ux.array() * uy.array()).mean();
// Step 5
lambda = sqrt(2.0) * boost::math::erf_inv(this->bcp_);
//STEP 6: cylindric parab: aa rect bound. This step also sets p.c as the 1D
// data centroid along the local frame x axis, which is the symmetry axis of
// the cylinder.
if (p_->getS() == Patch::s_y)
{
// fitting boundary: cyl parab, aa rect
VectorXd d(2);
d << lambda * sqrt(vx-mx*mx) , lambda * (sqrt(vy));
p_->setD(d);
p_->setC(p_->rexp(p_->getR())*mx*xh + p_->getC());
}
// STEP 7: circ parab: circ bound
if (p_->getS() == Patch::s_o)
{
// fitting boundary: cir parab, circ
VectorXd d(1);
d << lambda * max(sqrt(vx),sqrt(vy));
p_->setD(d);
}
// STEP 8: ell or hyp parab: ell bound
if (p_->getS() == Patch::s_e || p_->getS() == Patch::s_h)
{
// fitting boundary: ell/hyb parab, ell
VectorXd d(2);
d << lambda * sqrt(vx), lambda * sqrt(vy);
p_->setD(d);
}
// STEP 9: plane bounds
if (p_->getS() == Patch::s_p)
{
// fitting boundary: plane
Matrix3d rr = p_->rexp(p_->getR());
p_->setC(rr*(mx*xh+my*yh) + p_->getC());
double a = vx-mx*mx;
double b = 2.0*(vxy-mx*my);
double c = vy-my*my;
lambda = -log(1.0-this->bcp_);
double d = sqrt(b*b+(a-c)*(a-c));
double wp = a+c+d;
double wn = a+c-d;
Vector2d l;
l << sqrt(lambda*wp), sqrt(lambda*wn);
VectorXd d_p(1);
switch (p_->getB())
{
case Patch::b_c:
d_p << l.maxCoeff();
p_->setD(d_p);
break;
case Patch::b_e:
p_->setD(l);
break;
case Patch::b_r:
p_->setD(l);
break;
default:
cerr << "Invalid pach boundary for plane" << endl;
}
if (p_->getB() != Patch::b_c)
{
double t = 0.5 * atan2(b,a-c);
Matrix3d rr = p_->rexp(p_->getR());
Vector3d xl = rr.col(0);
Vector3d yl = rr.col(1);
Vector3d zl = rr.col(2);
xl = (cos(t)*xl) + (sin(t)*yl);
yl = zl.cross(xl);
Matrix3d xyzl;
xyzl << xl, yl, zl;
p_->setR(p_->rlog(xyzl));
}
} //plane boundary
return (1);
}
