int
main (int argc, char** argv)
{
if (argc < 2)
{
std::cerr << "Error: Please specify a PCD file (rosrun cob_3d_features profile_ne test.pcd)." << std::endl;
return -1;
}
PointCloud<PointXYZ>::Ptr p (new PointCloud<PointXYZ>);
PointCloud<Normal>::Ptr n (new PointCloud<Normal>);
PointCloud<InterestPoint>::Ptr ip (new PointCloud<InterestPoint>);
pcl::PointCloud<pcl::PointNormal> p_n;
PrecisionStopWatch t;
std::string file_ = argv[1];
PCDReader r;
if (r.read (file_, *p) == -1)
return -1;
Eigen::Vector3f normal;
determinePlaneNormal (p, normal);
//std::cout << normal << std::endl;
cob_3d_features::OrganizedNormalEstimation<PointXYZ, Normal, PointLabel> ne;
ne.setPixelSearchRadius (8, 2, 2);
//ne.setSkipDistantPointThreshold(8);
ne.setInputCloud (p);
PointCloud<PointLabel>::Ptr labels (new PointCloud<PointLabel>);
ne.setOutputLabels (labels);
t.precisionStart ();
ne.compute (*n);
std::cout << t.precisionStop () << "s\t for organized normal estimation" << std::endl;
cob_3d_features::OrganizedNormalEstimationOMP<PointXYZ, Normal, PointLabel> ne_omp;
ne_omp.setPixelSearchRadius (8, 2, 2);
//ne.setSkipDistantPointThreshold(8);
ne_omp.setInputCloud (p);
//PointCloud<PointLabel>::Ptr labels(new PointCloud<PointLabel>);
ne_omp.setOutputLabels (labels);
t.precisionStart ();
ne_omp.compute (*n);
std::cout << t.precisionStop () << "s\t for organized normal estimation (OMP)" << std::endl;
concatenateFields (*p, *n, p_n);
io::savePCDFileASCII ("normals_organized.pcd", p_n);
double good_thr = 0.97;
unsigned int ctr = 0, nan_ctr = 0;
double d_sum = 0;
for (unsigned int i = 0; i < p->size (); i++)
{
if (pcl_isnan(n->points[i].normal[0]))
{
nan_ctr++;
continue;
}
double d = normal.dot (n->points[i].getNormalVector3fMap ());
d_sum += fabs (1 - fabs (d));
if (fabs (d) > good_thr)
ctr++;
}
std::cout << "Average error: " << d_sum / p->size () << std::endl;
std::cout << "Ratio of good normals: " << (double)ctr / p->size () << std::endl;
std::cout << "Invalid normals: " << nan_ctr << std::endl;
IntegralImageNormalEstimation<PointXYZ, Normal> ne2;
ne2.setNormalEstimationMethod (ne2.COVARIANCE_MATRIX);
ne2.setMaxDepthChangeFactor (0.02f);
ne2.setNormalSmoothingSize (10.0f);
ne2.setDepthDependentSmoothing (true);
ne2.setInputCloud (p);
t.precisionStart ();
ne2.compute (*n);
std::cout << t.precisionStop () << "s\t for integral image normal estimation" << std::endl;
concatenateFields (*p, *n, p_n);
io::savePCDFileASCII ("normals_integral.pcd", p_n);
ctr = 0;
nan_ctr = 0;
d_sum = 0;
for (unsigned int i = 0; i < p->size (); i++)
{
if (pcl_isnan(n->points[i].normal[0]))
{
nan_ctr++;
continue;
}
double d = normal.dot (n->points[i].getNormalVector3fMap ());
d_sum += fabs (1 - fabs (d));
if (fabs (d) > good_thr)
ctr++;
}
std::cout << "Average error: " << d_sum / p->size () << std::endl;
std::cout << "Ratio of good normals: " << (double)ctr / p->size () << std::endl;
std::cout << "Invalid normals: " << nan_ctr << std::endl;
NormalEstimationOMP<PointXYZ, Normal> ne3;
ne3.setInputCloud (p);
ne3.setNumberOfThreads (4);
ne3.setKSearch (256);
//ne3.setRadiusSearch(0.01);
t.precisionStart ();
ne3.compute (*n);
std::cout << t.precisionStop () << "s\t for vanilla normal estimation" << std::endl;
concatenateFields (*p, *n, p_n);
io::savePCDFileASCII ("normals_vanilla.pcd", p_n);
ctr = 0;
nan_ctr = 0;
d_sum = 0;
for (unsigned int i = 0; i < p->size (); i++)
{
if (pcl_isnan(n->points[i].normal[0]))
{
nan_ctr++;
continue;
}
double d = normal.dot (n->points[i].getNormalVector3fMap ());
d_sum += fabs (1 - fabs (d));
if (fabs (d) > good_thr)
ctr++;
}
std::cout << "Average error: " << d_sum / p->size () << std::endl;
std::cout << "Ratio of good normals: " << (double)ctr / p->size () << std::endl;
std::cout << "Invalid normals: " << nan_ctr << std::endl;
return 0;
}
int test_eigen(int argc, char *argv[])
{
int rc = 0;
warnx("testing eigen");
{
Eigen::Vector2f v;
Eigen::Vector2f v1(1.0f, 2.0f);
Eigen::Vector2f v2(1.0f, -1.0f);
float data[2] = {1.0f, 2.0f};
TEST_OP("Constructor Vector2f()", Eigen::Vector2f v3);
TEST_OP_VERIFY("Constructor Vector2f(Vector2f)", Eigen::Vector2f v3(v1), v3.isApprox(v1));
TEST_OP_VERIFY("Constructor Vector2f(float[])", Eigen::Vector2f v3(data), v3[0] == data[0] && v3[1] == data[1]);
TEST_OP_VERIFY("Constructor Vector2f(float, float)", Eigen::Vector2f v3(1.0f, 2.0f), v3(0) == 1.0f && v3(1) == 2.0f);
TEST_OP_VERIFY("Vector2f = Vector2f", v = v1, v.isApprox(v1));
VERIFY_OP("Vector2f + Vector2f", v = v + v1, v.isApprox(v1 + v1));
VERIFY_OP("Vector2f - Vector2f", v = v - v1, v.isApprox(v1));
VERIFY_OP("Vector2f += Vector2f", v += v1, v.isApprox(v1 + v1));
VERIFY_OP("Vector2f -= Vector2f", v -= v1, v.isApprox(v1));
TEST_OP_VERIFY("Vector2f dot Vector2f", v.dot(v1), fabs(v.dot(v1) - 5.0f) <= FLT_EPSILON);
//TEST_OP("Vector2f cross Vector2f", v1.cross(v2)); //cross product for 2d array?
}
{
Eigen::Vector3f v;
Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
Eigen::Vector3f v2(1.0f, -1.0f, 2.0f);
float data[3] = {1.0f, 2.0f, 3.0f};
TEST_OP("Constructor Vector3f()", Eigen::Vector3f v3);
TEST_OP("Constructor Vector3f(Vector3f)", Eigen::Vector3f v3(v1));
TEST_OP("Constructor Vector3f(float[])", Eigen::Vector3f v3(data));
TEST_OP("Constructor Vector3f(float, float, float)", Eigen::Vector3f v3(1.0f, 2.0f, 3.0f));
TEST_OP("Vector3f = Vector3f", v = v1);
TEST_OP("Vector3f + Vector3f", v + v1);
TEST_OP("Vector3f - Vector3f", v - v1);
TEST_OP("Vector3f += Vector3f", v += v1);
TEST_OP("Vector3f -= Vector3f", v -= v1);
TEST_OP("Vector3f * float", v1 * 2.0f);
TEST_OP("Vector3f / float", v1 / 2.0f);
TEST_OP("Vector3f *= float", v1 *= 2.0f);
TEST_OP("Vector3f /= float", v1 /= 2.0f);
TEST_OP("Vector3f dot Vector3f", v.dot(v1));
TEST_OP("Vector3f cross Vector3f", v1.cross(v2));
TEST_OP("Vector3f length", v1.norm());
TEST_OP("Vector3f length squared", v1.squaredNorm());
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
// Need pragma here intead of moving variable out of TEST_OP and just reference because
// TEST_OP measures performance of vector operations.
TEST_OP("Vector<3> element read", volatile float a = v1(0));
TEST_OP("Vector<3> element read direct", volatile float a = v1.data()[0]);
#pragma GCC diagnostic pop
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
TEST_OP("Vector<3> element write direct", v1.data()[0] = 1.0f);
}
{
Eigen::Vector4f v(0.0f, 0.0f, 0.0f, 0.0f);
Eigen::Vector4f v1(1.0f, 2.0f, 0.0f, -1.0f);
Eigen::Vector4f v2(1.0f, -1.0f, 2.0f, 0.0f);
Eigen::Vector4f vres;
float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
TEST_OP("Constructor Vector<4>()", Eigen::Vector4f v3);
TEST_OP("Constructor Vector<4>(Vector<4>)", Eigen::Vector4f v3(v1));
TEST_OP("Constructor Vector<4>(float[])", Eigen::Vector4f v3(data));
TEST_OP("Constructor Vector<4>(float, float, float, float)", Eigen::Vector4f v3(1.0f, 2.0f, 3.0f, 4.0f));
TEST_OP("Vector<4> = Vector<4>", v = v1);
TEST_OP("Vector<4> + Vector<4>", v + v1);
TEST_OP("Vector<4> - Vector<4>", v - v1);
TEST_OP("Vector<4> += Vector<4>", v += v1);
TEST_OP("Vector<4> -= Vector<4>", v -= v1);
TEST_OP("Vector<4> dot Vector<4>", v.dot(v1));
}
{
Eigen::Vector10f v1;
v1.Zero();
float data[10];
TEST_OP("Constructor Vector<10>()", Eigen::Vector10f v3);
TEST_OP("Constructor Vector<10>(Vector<10>)", Eigen::Vector10f v3(v1));
TEST_OP("Constructor Vector<10>(float[])", Eigen::Vector10f v3(data));
}
{
Eigen::Matrix3f m1;
m1.setIdentity();
Eigen::Matrix3f m2;
m2.setIdentity();
Eigen::Vector3f v1(1.0f, 2.0f, 0.0f);
TEST_OP("Matrix3f * Vector3f", m1 * v1);
TEST_OP("Matrix3f + Matrix3f", m1 + m2);
TEST_OP("Matrix3f * Matrix3f", m1 * m2);
}
{
Eigen::Matrix<float, 10, 10> m1;
m1.setIdentity();
Eigen::Matrix<float, 10, 10> m2;
m2.setIdentity();
Eigen::Vector10f v1;
v1.Zero();
TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
}
{
warnx("Nonsymmetric matrix operations test");
// test nonsymmetric +, -, +=, -=
const Eigen::Matrix<float, 2, 3> m1_orig =
(Eigen::Matrix<float, 2, 3>() << 1, 2, 3,
4, 5, 6).finished();
Eigen::Matrix<float, 2, 3> m1(m1_orig);
Eigen::Matrix<float, 2, 3> m2;
m2 << 2, 4, 6,
8, 10, 12;
Eigen::Matrix<float, 2, 3> m3;
m3 << 3, 6, 9,
12, 15, 18;
if (m1 + m2 != m3) {
warnx("Matrix<2, 3> + Matrix<2, 3> failed!");
printEigen(m1 + m2);
printf("!=\n");
printEigen(m3);
rc = 1;
}
if (m3 - m2 != m1) {
warnx("Matrix<2, 3> - Matrix<2, 3> failed!");
printEigen(m3 - m2);
printf("!=\n");
printEigen(m1);
rc = 1;
}
m1 += m2;
if (m1 != m3) {
warnx("Matrix<2, 3> += Matrix<2, 3> failed!");
printEigen(m1);
printf("!=\n");
printEigen(m3);
rc = 1;
}
m1 -= m2;
if (m1 != m1_orig) {
warnx("Matrix<2, 3> -= Matrix<2, 3> failed!");
printEigen(m1);
printf("!=\n");
printEigen(m1_orig);
rc = 1;
}
}
/* QUATERNION TESTS CURRENTLY FAILING
{
// test conversion rotation matrix to quaternion and back
Eigen::Matrix3f R_orig;
Eigen::Matrix3f R;
Eigen::Quaternionf q;
float diff = 0.1f;
float tol = 0.00001f;
warnx("Quaternion transformation methods test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R_orig.eulerAngles(roll, pitch, yaw);
q = R_orig; //from_dcm
R = q.toRotationMatrix();
for (size_t i = 0; i < 3; i++) {
for (size_t j = 0; j < 3; j++) {
if (fabsf(R_orig(i, j) - R(i, j)) > tol) {
warnx("Quaternion method 'from_dcm' or 'toRotationMatrix' outside tolerance!");
rc = 1;
}
}
}
}
}
}
// test against some known values
tol = 0.0001f;
Eigen::Quaternionf q_true = {1.0f, 0.0f, 0.0f, 0.0f};
R_orig.setIdentity();
q = R_orig;
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'from_dcm()' outside tolerance!");
rc = 1;
}
}
q_true = quatFromEuler(0.3f, 0.2f, 0.1f);
q = {0.9833f, 0.1436f, 0.1060f, 0.0343f};
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'eulerAngles()' outside tolerance!");
rc = 1;
}
}
q_true = quatFromEuler(-1.5f, -0.2f, 0.5f);
q = {0.7222f, -0.6391f, -0.2386f, 0.1142f};
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'eulerAngles()' outside tolerance!");
rc = 1;
}
}
q_true = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3);
q = {0.6830f, 0.1830f, -0.6830f, 0.1830f};
for (size_t i = 0; i < 4; i++) {
if (!q.isApprox(q_true, tol)) {
warnx("Quaternion method 'eulerAngles()' outside tolerance!");
rc = 1;
}
}
}
{
// test quaternion method "rotate" (rotate vector by quaternion)
Eigen::Vector3f vector = {1.0f, 1.0f, 1.0f};
Eigen::Vector3f vector_q;
Eigen::Vector3f vector_r;
Eigen::Quaternionf q;
Eigen::Matrix3f R;
float diff = 0.1f;
float tol = 0.00001f;
warnx("Quaternion vector rotation method test.");
for (float roll = -M_PI_F; roll <= M_PI_F; roll += diff) {
for (float pitch = -M_PI_2_F; pitch <= M_PI_2_F; pitch += diff) {
for (float yaw = -M_PI_F; yaw <= M_PI_F; yaw += diff) {
R.eulerAngles(roll, pitch, yaw);
q = quatFromEuler(roll, pitch, yaw);
vector_r = R * vector;
vector_q = q._transformVector(vector);
for (int i = 0; i < 3; i++) {
if (fabsf(vector_r(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
}
}
// test some values calculated with matlab
tol = 0.0001f;
q = quatFromEuler(M_PI_2_F, 0.0f, 0.0f);
vector_q = q._transformVector(vector);
Eigen::Vector3f vector_true = {1.00f, -1.00f, 1.00f};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q = quatFromEuler(0.3f, 0.2f, 0.1f);
vector_q = q._transformVector(vector);
vector_true = {1.1566, 0.7792, 1.0273};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q = quatFromEuler(-1.5f, -0.2f, 0.5f);
vector_q = q._transformVector(vector);
vector_true = {0.5095, 1.4956, -0.7096};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
q = quatFromEuler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
vector_q = q._transformVector(vector);
vector_true = { -1.3660, 0.3660, 1.0000};
for (size_t i = 0; i < 3; i++) {
if (fabsf(vector_true(i) - vector_q(i)) > tol) {
warnx("Quaternion method 'rotate' outside tolerance");
rc = 1;
}
}
}
*/
return rc;
}
double RobustViconTracker::angle_between(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2)
{
return acos(v1.dot(v2) / (v1.norm() * v2.norm()));
}
CloudPtr
get ()
{
//lock while we swap our cloud and reset it.
boost::mutex::scoped_lock lock (mtx_);
CloudPtr temp_cloud (new Cloud);
CloudPtr temp_cloud2 (new Cloud);
CloudPtr temp_cloud3 (new Cloud);
CloudPtr temp_cloud4 (new Cloud);
CloudPtr temp_cloud5 (new Cloud);
CloudConstPtr empty_cloud;
cout << "===============================\n"
"======Start of frame===========\n"
"===============================\n";
//cerr << "cloud size orig: " << cloud_->size() << endl;
voxel_grid.setInputCloud (cloud_);
voxel_grid.filter (*temp_cloud); // filter cloud for z depth
//cerr << "cloud size postzfilter: " << temp_cloud->size() << endl;
pcl::ModelCoefficients::Ptr planecoefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices ());
std::vector<pcl::ModelCoefficients> linecoefficients1;
pcl::ModelCoefficients model;
model.values.resize (6);
pcl::PointIndices::Ptr line_inliers (new pcl::PointIndices ());
std::vector<Eigen::Vector3f> corners;
if(temp_cloud->size() > MIN_CLOUD_POINTS) {
plane_seg.setInputCloud (temp_cloud);
plane_seg.segment (*plane_inliers, *planecoefficients); // find plane
}
//cerr << "plane inliers size: " << plane_inliers->indices.size() << endl;
cout << "planecoeffs: "
<< planecoefficients->values[0] << " "
<< planecoefficients->values[1] << " "
<< planecoefficients->values[2] << " "
<< planecoefficients->values[3] << " "
<< endl;
Eigen::Vector3f pn = Eigen::Vector3f(
planecoefficients->values[0],
planecoefficients->values[1],
planecoefficients->values[2]);
float planedeg = pcl::rad2deg(acos(pn.dot(Eigen::Vector3f::UnitZ())));
cout << "angle of camera to floor normal: " << planedeg << " degrees" << endl;
cout << "distance of camera to floor: " << planecoefficients->values[3]
<< " meters" << endl;
plane_extract.setNegative (true);
plane_extract.setInputCloud (temp_cloud);
plane_extract.setIndices (plane_inliers);
plane_extract.filter (*temp_cloud2); // remove plane
plane_extract.setNegative (false);
plane_extract.filter (*temp_cloud5); // only plane
for(size_t i = 0; i < temp_cloud5->size (); ++i)
{
temp_cloud5->points[i].r = 0;
temp_cloud5->points[i].g = 255;
temp_cloud5->points[i].b = 0;
// tint found plane green for ground
}
for(size_t j = 0 ; j < MAX_LINES && temp_cloud2->size() > MIN_CLOUD_POINTS; j++)
// look for x lines until cloud gets too small
{
// cerr << "cloud size: " << temp_cloud2->size() << endl;
line_seg.setInputCloud (temp_cloud2);
line_seg.segment (*line_inliers, model); // find line
// cerr << "line inliears size: " << line_inliers->indices.size() << endl;
if(line_inliers->indices.size() < MIN_CLOUD_POINTS)
break;
linecoefficients1.push_back (model); // store line coeffs
line_extract.setNegative (true);
line_extract.setInputCloud (temp_cloud2);
line_extract.setIndices (line_inliers);
line_extract.filter (*temp_cloud3); // remove plane
line_extract.setNegative (false);
line_extract.filter (*temp_cloud4); // only plane
for(size_t i = 0; i < temp_cloud4->size (); ++i) {
temp_cloud4->points[i].g = 0;
if(j%2) {
temp_cloud4->points[i].r = 255-j*int(255/MAX_LINES);
temp_cloud4->points[i].b = 0+j*int(255/MAX_LINES);
} else {
temp_cloud4->points[i].b = 255-j*int(255/MAX_LINES);
temp_cloud4->points[i].r = 0+j*int(255/MAX_LINES);
}
}
*temp_cloud5 += *temp_cloud4; // add line to ground
temp_cloud2.swap ( temp_cloud3); // remove line
}
cout << "found " << linecoefficients1.size() << " lines." << endl;
for(size_t i = 0; i < linecoefficients1.size(); i++)
{
//cerr << "linecoeffs: " << i << " "
// << linecoefficients1[i].values[0] << " "
// << linecoefficients1[i].values[1] << " "
// << linecoefficients1[i].values[2] << " "
// << linecoefficients1[i].values[3] << " "
// << linecoefficients1[i].values[4] << " "
// << linecoefficients1[i].values[5] << " "
// << endl;
Eigen::Vector3f lv = Eigen::Vector3f(
linecoefficients1[i].values[3],
linecoefficients1[i].values[4],
linecoefficients1[i].values[5]);
Eigen::Vector3f lp = Eigen::Vector3f(
linecoefficients1[i].values[0],
linecoefficients1[i].values[1],
linecoefficients1[i].values[2]);
float r = pn.dot(lv);
float deg = pcl::rad2deg(acos(r));
cout << "angle of line to floor normal: " << deg << " degrees" << endl;
if(abs(deg-30) < 5 || abs(deg-150) < 5)
{
cout << "found corner line" << endl;
float t = -(lp.dot(pn) + planecoefficients->values[3])/ r;
Eigen::Vector3f intersect = lp + lv*t;
cout << "corner intersects floor at: " << endl << intersect << endl;
cout << "straight line distance from camera to corner: " <<
intersect.norm() << " meters" << endl;
corners.push_back(intersect);
Eigen::Vector3f floor_distance = intersect + pn; // should be - ???
cout << "distance along floor to corner: " <<
floor_distance.norm() << " meters" << endl;
}
else if(abs(deg-90) < 5)
{
cout << "found horizontal line" << endl;
}
}
switch(corners.size())
{
case 2:
cout << "distance between corners " << (corners[0] - corners[1]).norm() << endl;
cout << "angle of pyramid to camera " <<
pcl::rad2deg(acos(((corners[0] - corners[1]).normalized()).dot(Eigen::Vector3f::UnitX())))
<< endl;
break;
case 3:
cout << "distance between corners " << (corners[0] - corners[1]).norm() << endl;
cout << "distance between corners " << (corners[0] - corners[2]).norm() << endl;
cout << "distance between corners " << (corners[1] - corners[2]).norm() << endl;
cout << "angle of corner on floor (should be 90) " <<
pcl::rad2deg(acos((corners[0] - corners[1]).dot(corners[0] - corners [2])))
<< endl;
cout << "angle of pyramid to camera " <<
pcl::rad2deg(acos(((corners[0] - corners[1]).normalized()).dot(Eigen::Vector3f::UnitX())))
<< endl;
break;
}
if (saveCloud)
{
std::stringstream stream, stream1;
std::string filename;
stream << "inputCloud" << filesSaved << ".pcd";
filename = stream.str();
if (pcl::io::savePCDFile(filename, *cloud_, true) == 0)
{
filesSaved++;
cout << "Saved " << filename << "." << endl;
}
else PCL_ERROR("Problem saving %s.\n", filename.c_str());
stream1 << "inputCloud" << filesSaved << ".pcd";
filename = stream1.str();
if (pcl::io::savePCDFile(filename, *temp_cloud5, true) == 0)
{
filesSaved++;
cout << "Saved " << filename << "." << endl;
}
else PCL_ERROR("Problem saving %s.\n", filename.c_str());
saveCloud = false;
}
empty_cloud.swap(cloud_); // set cloud_ to null
if(toggleView == 1)
return (temp_cloud); // return orig cloud
else
return (temp_cloud5); // return colored cloud
}
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
// The RF is formed as this x_axis | y_axis | normal
Eigen::Map<Eigen::Vector3f> x_axis (rf);
Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
Eigen::Map<Eigen::Vector3f> normal (rf + 6);
// Find every point within specified search_radius_
std::vector<int> nn_indices;
std::vector<float> nn_dists;
const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
if (neighb_cnt == 0)
{
for (size_t i = 0; i < desc.size (); ++i)
desc[i] = std::numeric_limits<float>::quiet_NaN ();
memset (rf, 0, sizeof (rf[0]) * 9);
return (false);
}
float minDist = std::numeric_limits<float>::max ();
int minIndex = -1;
for (size_t i = 0; i < nn_indices.size (); i++)
{
if (nn_dists[i] < minDist)
{
minDist = nn_dists[i];
minIndex = nn_indices[i];
}
}
// Get origin point
Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
// Get origin normal
// Use pre-computed normals
normal = normals[minIndex].getNormalVector3fMap ();
// Compute and store the RF direction
x_axis[0] = static_cast<float> (rnd ());
x_axis[1] = static_cast<float> (rnd ());
x_axis[2] = static_cast<float> (rnd ());
if (!pcl::utils::equal (normal[2], 0.0f))
x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
else if (!pcl::utils::equal (normal[1], 0.0f))
x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
else if (!pcl::utils::equal (normal[0], 0.0f))
x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
x_axis.normalize ();
// Check if the computed x axis is orthogonal to the normal
assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
// Store the 3rd frame vector
y_axis = normal.cross (x_axis);
// For each point within radius
for (size_t ne = 0; ne < neighb_cnt; ne++)
{
if (pcl::utils::equal (nn_dists[ne], 0.0f))
continue;
// Get neighbours coordinates
Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
/// ----- Compute current neighbour polar coordinates -----
/// Get distance between the neighbour and the origin
float r = sqrt (nn_dists[ne]);
/// Project point into the tangent plane
Eigen::Vector3f proj;
pcl::geometry::project (neighbour, origin, normal, proj);
proj -= origin;
/// Normalize to compute the dot product
proj.normalize ();
/// Compute the angle between the projection and the x axis in the interval [0,360]
Eigen::Vector3f cross = x_axis.cross (proj);
float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
/// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
Eigen::Vector3f no = neighbour - origin;
no.normalize ();
float theta = normal.dot (no);
theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));
// Bin (j, k, l)
size_t j = 0;
size_t k = 0;
size_t l = 0;
// Compute the Bin(j, k, l) coordinates of current neighbour
for (size_t rad = 1; rad < radius_bins_+1; rad++)
{
if (r <= radii_interval_[rad])
{
j = rad-1;
break;
}
}
for (size_t ang = 1; ang < elevation_bins_+1; ang++)
{
if (theta <= theta_divisions_[ang])
{
k = ang-1;
break;
}
}
for (size_t ang = 1; ang < azimuth_bins_+1; ang++)
{
if (phi <= phi_divisions_[ang])
{
l = ang-1;
break;
}
}
// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
std::vector<int> neighbour_indices;
std::vector<float> neighbour_distances;
int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
// point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
if (point_density == 0)
continue;
float w = (1.0f / static_cast<float> (point_density)) *
volume_lut_[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j];
assert (w >= 0.0);
if (w == std::numeric_limits<float>::infinity ())
PCL_ERROR ("Shape Context Error INF!\n");
if (w != w)
PCL_ERROR ("Shape Context Error IND!\n");
/// Accumulate w into correspondant Bin(j,k,l)
desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
} // end for each neighbour
// 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
memset (rf, 0, sizeof (rf[0]) * 9);
return (true);
}
void rawCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloud)
{
//double timeScanCur = laserCloud->header.stamp.toSec();
ros::Time timestamp = laserCloud->header.stamp;
//bool returnBool;
pcl::fromROSMsg(*laserCloud, *laserCloudIn);
if (visualizationFlag)
{
viewer->removeAllPointClouds(0);
viewer->removeAllShapes(0);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudLabeled(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudRaw(new pcl::PointCloud<pcl::PointXYZI>);
// std::vector<std::string> labelsC1;
// std::vector<std::string> labelsC2;
// pcl::PointCloud<SAFEFeatures>::Ptr featureCloudC1Acc(new pcl::PointCloud<SAFEFeatures>);
// pcl::PointCloud<SAFEFeatures>::Ptr featureCloudC2Acc(new pcl::PointCloud<SAFEFeatures>);
std::vector<std::string> labelsC1;
std::vector<std::string> labelsC2;
std::vector<std::string> labelsC3;
std::vector<std::string> labels;
pcl::PointCloud<AllFeatures>::Ptr featureCloudAcc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC1Acc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC2Acc(new pcl::PointCloud<AllFeatures>);
pcl::PointCloud<AllFeatures>::Ptr featureCloudC3Acc(new pcl::PointCloud<AllFeatures>);
std::vector<Eigen::Matrix3f> confMats;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr classificationCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<DatasetContainer> dataset;
std::string folder = "/home/mikkel/catkin_ws/src/trainingSet/";
pcl::copyPointCloud(*laserCloudIn, *cloudRGB);
pcl::copyPointCloud(*laserCloudIn, *cloudRaw);
// Single colored
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color1(cloudRGB, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZRGB>(cloudRGB,color1,"cloudRGB",viewP(RawCloud));
}
//viewer->addText ("RawCloud", 10, 10, "vPP text", viewP(RawCloud));
// Ground modeling
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudStat(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::PointXYZ>::Ptr planeDistCloud(new pcl::PointCloud<pcl::PointXYZ>);
// pcl::copyPointCloud(*cloudRGB,*cloudStat);
// pcl::copyPointCloud(*cloudRGB,*planeDistCloud);
// Remove everything outside the two lines
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
//pcl::copyPointCloud(*cloudRaw, *cloud_filtered);
// Rotation
float thetaZ = theta*PI/180; // The angle of rotation in radians
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.rotate (Eigen::AngleAxisf (thetaZ, Eigen::Vector3f::UnitZ()));
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::transformPointCloud (*cloudRaw, *cloud_filtered, transform_2);
// Passthrough
pcl::PassThrough<pcl::PointXYZI> passX;
passX.setInputCloud (cloud_filtered);
passX.setFilterFieldName ("x");
passX.setFilterLimits (-0.5, 100.0);
//pass.setFilterLimitsNegative (true);
passX.filter (*cloud_filtered);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filteredRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
if (receivedHough)
{
pcl::PassThrough<pcl::PointXYZI> pass;
pass.setInputCloud (cloud_filtered);
pass.setFilterFieldName ("y");
pass.setFilterLimits (r1+WALL_CLEARANCE, r2-WALL_CLEARANCE);
//pass.setFilterLimitsNegative (true);
pass.filter (*cloud_filtered);
}
pcl::copyPointCloud(*cloud_filtered, *cloud_filteredRGB);
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> color9(cloudRGB, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZRGB>(cloud_filteredRGB,color9,"LinesFiltered",viewP(LinesFiltered));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "LinesFiltered");
viewer->addText ("LinesFiltered", 10, 10, fontsize, 1, 1, 1, "LinesFiltered text", viewP(LinesFiltered));
}
// Grid Minimum
// pcl::PointCloud<pcl::PointXYZI>::Ptr gridCloud(new pcl::PointCloud<pcl::PointXYZI>);
// pcl::GridMinimum<pcl::PointXYZI> gridm(1.0); // Set grid resolution
// gridm.setInputCloud(cloud_filtered);
// gridm.filter(*gridCloud);
//*** Transform point cloud to adjust for a Ground Plane ***//
pcl::ModelCoefficients ground_coefficients;
pcl::PointIndices ground_indices;
pcl::SACSegmentation<pcl::PointXYZI> ground_finder;
ground_finder.setOptimizeCoefficients(true);
ground_finder.setModelType(pcl::SACMODEL_PLANE);
ground_finder.setMethodType(pcl::SAC_RANSAC);
ground_finder.setDistanceThreshold(0.15);
ground_finder.setInputCloud(cloud_filtered);
ground_finder.segment(ground_indices, ground_coefficients);
// Convert plane normal vector from type ModelCoefficients to Vector4f
Eigen::Vector3f np = Eigen::Vector3f(ground_coefficients.values[0],ground_coefficients.values[1],ground_coefficients.values[2]);
// Find rotation vector, u, and angle, theta
Eigen::Vector3f u = np.cross(Eigen::Vector3f(0,0,1));
u.normalize();
float theta = acos(np.dot(Eigen::Vector3f(0,0,1)));
// Construct transformation matrix (rotation matrix from axis and angle)
Eigen::Affine3f tf = Eigen::Affine3f::Identity();
float d = ground_coefficients.values[3];
tf.translation() << d*np[0], d*np[1], d*np[2];
tf.rotate (Eigen::AngleAxisf (theta, u));
// Execute the transformation
pcl::PointCloud<pcl::PointXYZI>::Ptr transformedCloud (new pcl::PointCloud<pcl::PointXYZI> ());
pcl::transformPointCloud(*cloud_filtered, *transformedCloud, tf);
// Compute statistical moments for least significant direction in neighborhood
#if (OLD_METHOD)
pcl::NeighborhoodFeatures<pcl::PointXYZI, AllFeatures> features;
pcl::PointCloud<AllFeatures>::Ptr featureCloud(new pcl::PointCloud<AllFeatures>);
features.setInputCloud(transformedCloud);
pcl::search::KdTree<pcl::PointXYZI>::Ptr search_tree5( new pcl::search::KdTree<pcl::PointXYZI>());
features.setSearchMethod(search_tree5);
//principalComponentsAnalysis.setRadiusSearch(0.6);
//features.setKSearch(30);
features.setRadiusSearch(0.01); // This value does not do anything! Look inside "NeighborgoodFeatures.h" to see adaptive radius calculation.
features.compute(*featureCloud);
// ros::Time tFeatureCalculation2 = ros::Time::now();
// ros::Duration tFeatureCalculation = tFeatureCalculation2-tPreprocessing2;
// if (executionTimes==true)
// ROS_INFO("Feature calculation time = %i",tFeatureCalculation.nsec);
visualizeFeature(*cloudRGB, *featureCloud, 0, viewP(GPDistMean), "GPDistMean");
visualizeFeature(*cloudRGB, *featureCloud, 1, viewP(GPDistMin), "GPDistMin");
visualizeFeature(*cloudRGB, *featureCloud, 2, viewP(GPDistPoint), "GPDistPoint");
visualizeFeature(*cloudRGB, *featureCloud, 3, viewP(GPDistVar), "GPDistVar");
visualizeFeature(*cloudRGB, *featureCloud, 4, viewP(PCA1), "PCA1"); // 4 = PCA1
visualizeFeature(*cloudRGB, *featureCloud, 5, viewP(PCA2), "PCA2"); // 3 = GPVar
visualizeFeature(*cloudRGB, *featureCloud, 6, viewP(PCA3), "PCA3"); // 0 = GPMean
visualizeFeature(*cloudRGB, *featureCloud, 7, viewP(PCANX), "PCANX");
visualizeFeature(*cloudRGB, *featureCloud, 8, viewP(PCANY), "PCANY");
visualizeFeature(*cloudRGB, *featureCloud, 9, viewP(PCANZ), "PCANZ"); // 9 = PCANZ
visualizeFeature(*cloudRGB, *featureCloud, 10, viewP(PointDist), "PointDist");
visualizeFeature(*cloudRGB, *featureCloud, 11, viewP(RSS), "RSS");
visualizeFeature(*cloudRGB, *featureCloud, 12, viewP(Reflectance), "Reflectance");
ROS_INFO("Computed all neighborhood features");
std::vector<float>* centroid = new std::vector<float>();
std::vector<float>* stds = new std::vector<float>();
//*** Testing ***//
if (training == false)
{
Eigen::Matrix3f confMat = Eigen::Matrix3f::Zero();
if (testdata==true)
{
*featureCloud = *featureCloudAcc;
}
pcl::copyPointCloud(*cloudRGB,*classificationCloud);
// Load feature normalization parameters
std::ifstream input_file((folder + "centroid").c_str());
std::istream_iterator<float> start(input_file), end;
std::vector<float> numbers(start, end);
std::copy(numbers.begin(), numbers.end(), std::back_inserter(*centroid));
std::ifstream input_file2((folder + "stds").c_str());
std::istream_iterator<float> start2(input_file2), end2;
std::vector<float> numbers2(start2, end2);
std::copy(numbers2.begin(), numbers2.end(), std::back_inserter(*stds));
// Normalize features
// if (pcl::io::savePCDFile ("testFeaturesNonNormalized.pcd", *featureCloud) != 0)
// return (false);
ros::Time tNormalization1 = ros::Time::now();
normalizeFeatures(*featureCloud,*featureCloud, ¢roid, &stds);
ros::Time tNormalization2 = ros::Time::now();
ros::Duration tNormalization = tNormalization2-tNormalization1;
if (executionTimes==true)
ROS_INFO("Normalization time = %f",(float)(tNormalization.nsec)/1000000);
pcl::PointIndices::Ptr unclassifiedIndices (new pcl::PointIndices ());
// if (pcl::io::savePCDFile ("testFeatures.pcd", *featureCloud) != 0)
// return (false);
CvSVM SVM;
//int dim = sizeof(featureCloud->points[0])/sizeof(float);
int dim = useFeatures.size();//sizeof(useFeatures)/sizeof(int);
SVM.load((folder + "svm").c_str());
//SVM.load((folder + "svm2014-11-06-11-22-59").c_str());
//SVM.load((folder + "svm2014-11-07-14-00-31").c_str());
//SVM.load((folder + "svm2014-11-07-13-16-28").c_str());
ros::Time tClassification1 = ros::Time::now();
//int nMissingLabels = 0;
for (size_t i=0;i<classificationCloud->points.size();i++)
{
float dataSVM[dim];
for (int d=0;d<dim;d++)
{
//dataSVM[d] = featureCloud->points[i].data[d];
dataSVM[d] = featureCloud->points[i].data[useFeatures[d]];
}
Mat labelsMat(1, dim, CV_32FC1, dataSVM);
float response = SVM.predict(labelsMat);
if (response == 1.0)
classificationCloud->points[i].b = 255;
else if (response == 2.0)
classificationCloud->points[i].g = 255;
else if (response == 3.0)
classificationCloud->points[i].r = 255;
else
ROS_INFO("Another class label = %f",response);
if (testdata==true)
{
int groundTruth;
if (labels[i].compare("ground") == 0)
groundTruth = 1;
else if (labels[i].compare("vegetation") == 0)
groundTruth = 2;
else if (labels[i].compare("object") == 0)
groundTruth = 3;
confMat(groundTruth-1,(int)response-1)++;
}
}
ros::Time tClassification2 = ros::Time::now();
ros::Duration tClassification = tClassification2-tClassification1;
if (executionTimes==true)
ROS_INFO("Classification time = %f",(float)(tClassification.nsec)/100000);
//ROS_INFO("Number of missing class labels = %i", nMissingLabels);
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC(classificationCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(classificationCloud,colorC,"Classification",viewP(Classification));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Classification");
viewer->addText ("Classification", 10, 10, fontsize, 1, 1, 1, "Classification text", viewP(Classification));
// Test set - calculate performance statistics
if (testdata == true)
{
std::cout << confMat << std::endl;
confMats.push_back(confMat);
if (true)//k==files.size()-1)
{
ofstream myfile;
string resultsFolder = "/home/mikkel/catkin_ws/src/Results/";
myfile.open ((resultsFolder + "run_number_here.txt").c_str());
// Distance threshold
myfile << "DistanceThreshold:" << distThr << ";\n";
// Neighborhood parameters
myfile << "Neighborhood:" << rmin << ";" << rmindist << ";" << rmax << ";" << rmaxdist << ";\n";
// Features
myfile << "Features:";
for (int d=0;d<dim;d++)
myfile << useFeatures[d] << ";";
myfile << "\n";
myfile << "ConfusionMatrix:";
for (size_t i=0;i<confMats.size();i++)
{
for (int r=0;r<confMats[i].rows();r++)
for (int c=0;c<confMats[i].cols();c++)
myfile << confMats[i](r,c) << ";";
myfile << "\n";
}
myfile << "Accuracy:" << confMat.diagonal().sum()/confMat.sum() << ";\n";
myfile.close();
}
}
featureCloudAcc->clear();
labels.clear();
#else
for (size_t i=0;i<transformedCloud->size();i++)
{
pcl::PointXYZI pTmp2 = transformedCloud->points[i];
pcl::PointXYZRGB pTmp;
pTmp.x = pTmp2.x;
pTmp.y = pTmp2.y;
pTmp.z = pTmp2.z;
pTmp.r = 255;
pTmp.g = 255;
if (pTmp.z > 0.1) // Object
{
classificationCloud->push_back(pTmp);
}
}
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC(classificationCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(classificationCloud,colorC,"Classification",viewP(Classification));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Classification");
viewer->addText ("Classification", 10, 10, fontsize, 1, 1, 1, "Classification text", viewP(Classification));
}
#endif
// REGION GROWING
//ROS_INFO("region growing 5");
pcl::PointCloud <pcl::PointXYZRGB>::Ptr objectCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
//pcl::PointCloud <pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZRGB>);
for (size_t i=0;i<classificationCloud->size();i++)
{
pcl::PointXYZRGB pTmp = classificationCloud->points[i];
if ((pTmp.r == 255) || (pTmp.g == 255)) // Object
{
objectCloud->push_back(pTmp);
}
}
pcl::RegionGrowing<pcl::PointXYZRGB, pcl::Normal> reg;
reg.setInputCloud (objectCloud);
//reg.setIndices (indices);
pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> > (new pcl::search::KdTree<pcl::PointXYZRGB>);
reg.setSearchMethod (tree);
//reg.setDistanceThreshold (0.0001f);
//reg.setResidualThreshold(0.01f);
//ROS_INFO("res = %f",reg.getSmoothnessThreshold());
//reg.setPointColorThreshold (6);
//reg.setRegionColorThreshold (5);
reg.setMinClusterSize (1);
reg.setResidualThreshold(min_cluster_distance);
reg.setCurvatureTestFlag(false);
// reg.setResidualTestFlag(true);
// reg.setNormalTestFlag(false);
// reg.setSmoothModeFlag(false);
std::vector <pcl::PointIndices> clusters;
reg.extract (clusters);
//ROS_INFO("clusters = %d", clusters.size());
//std::cout << "testefter" << std::endl;
pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
pcl::PointCloud <pcl::PointXYZRGB>::Ptr clustersFilteredCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector <pcl::PointIndices> clustersFiltered;
//pcl::PointXYZRGB min_pt, max_pt;
Eigen::Vector4f min_pt, max_pt;
obstacle_detection::boundingbox bbox;
obstacle_detection::boundingboxes bboxes;
bboxes.header.stamp = timestamp;
bboxes.header.frame_id = "/velodyne";
if (visualizationFlag)
viewer->removeAllShapes(viewP(SegmentsFiltered));
for (size_t i=0;i<clusters.size();i++)
{
//ROS_INFO("cluster size = %d",clusters[i].indices.size());
if (clusters[i].indices.size() > 100)
{
clustersFiltered.push_back(clusters[i]);
for (size_t pp=0;pp<clusters[i].indices.size();pp++)
{
clustersFilteredCloud->push_back(colored_cloud->points[clusters[i].indices[pp]]);
}
// Calculate bounding box
pcl::getMinMax3D(*colored_cloud, clusters[i], min_pt, max_pt);
bbox.start.x = min_pt[0];
bbox.start.y = min_pt[1];
bbox.start.z = min_pt[2];
bbox.width.x = max_pt[0]-min_pt[0];
bbox.width.y = max_pt[1]-min_pt[1];
bbox.width.z = max_pt[2]-min_pt[2];
bbox.header.stamp = timestamp;
bbox.header.frame_id = "/velodyne";
bboxes.boundingboxes.push_back(bbox);
if (visualizationFlag)
viewer->addCube (min_pt[0], max_pt[0], min_pt[1], max_pt[1], min_pt[2], max_pt[2], 1.0f, 1.0f, 1.0f, (boost::format("%04d") % i).str().c_str(), viewP(SegmentsFiltered));
}
}
pubBBoxesPointer->publish(bboxes);
//
// //pcl::visualization::CloudViewer viewer ("Cluster viewer");
// //viewer.showCloud (colored_cloud);
//
if (visualizationFlag)
{
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC2(colored_cloud);
viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud,colorC2,"Segments",viewP(Segments));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Segments");
viewer->addText ("Segments", 10, 10, fontsize, 1, 1, 1, "Segments text", viewP(Segments));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorC3(clustersFilteredCloud);
viewer->addPointCloud<pcl::PointXYZRGB>(clustersFilteredCloud,colorC3,"SegmentsFiltered",viewP(SegmentsFiltered));
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "SegmentsFiltered");
viewer->addText ("Segments", 10, 10, fontsize, 1, 1, 1, "SegmentsFiltered text", viewP(SegmentsFiltered));
}
//
//
// // BOUNDING BOXES
// viewer->addCube (min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z);
// std_msgs::Float64 testF;
// //testF.data = 10;
// testF.data = 10;
//
// Compute principal directions
// Eigen::Vector4f pcaCentroid;
// pcl::compute3DCentroid(*clustersFilteredCloud, pcaCentroid);
// Eigen::Matrix3f covariance;
// computeCovarianceMatrixNormalized(*clustersFilteredCloud, pcaCentroid, covariance);
// Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
// Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();
// eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1));
//
// // Transform the original cloud to the origin where the principal components correspond to the axes.
// Eigen::Matrix4f projectionTransform(Eigen::Matrix4f::Identity());
// projectionTransform.block<3,3>(0,0) = eigenVectorsPCA.transpose();
// projectionTransform.block<3,1>(0,3) = -1.f * (projectionTransform.block<3,3>(0,0) * pcaCentroid.head<3>());
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudPointsProjected (new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::transformPointCloud(*clustersFilteredCloud, *cloudPointsProjected, projectionTransform);
// // Get the minimum and maximum points of the transformed cloud.
// pcl::PointXYZRGB minPoint, maxPoint;
// pcl::getMinMax3D(*cloudPointsProjected, minPoint, maxPoint);
// const Eigen::Vector3f meanDiagonal = 0.5f*(maxPoint.getVector3fMap() + minPoint.getVector3fMap());
//
// // Final transform
// const Eigen::Quaternionf bboxQuaternion(eigenVectorsPCA); //Quaternions are a way to do rotations https://www.youtube.com/watch?v=mHVwd8gYLnI
// const Eigen::Vector3f bboxTransform = eigenVectorsPCA * meanDiagonal + pcaCentroid.head<3>();
// viewer->addCube(bboxTransform, bboxQuaternion, maxPoint.x - minPoint.x, maxPoint.y - minPoint.y, maxPoint.z - minPoint.z, "bbox");
//pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> colorTTF1(featureCloudTest, 0, 255, 0);
// pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> colorTTF1(featureCloudTest);
// PCL_INFO("featureCloudTest size = %i",featureCloudTest->points.size());
// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> colorTTF2(featureCloudTrain, 255, 0, 0);
// viewer->addPointCloud<pcl::PointXYZRGB>(featureCloudTest,colorTTF1,"TestTrainFeatures1",viewP(TestTrainFeatures));
// viewer->addPointCloud<pcl::PointXYZRGB>(featureCloudTrain,colorTTF2,"TestTrainFeatures2",viewP(TestTrainFeatures));
// viewer->addText ("TestTrainFeatures", 10, 10, "TestTrainFeatures text", viewP(TestTrainFeatures));
// sensor_msgs::PointCloud2 processedCloud;
// pcl::toROSMsg(*classificationCloud, processedCloud);
// processedCloud.header.stamp = ros::Time::now();//processedCloud->header.stamp;
// processedCloud.header.frame_id = "/velodyne";
// pubProcessedPointCloudPointer->publish(processedCloud);
//
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud2D(new pcl::PointCloud<pcl::PointXYZRGB>);
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr objectCloud2D(new pcl::PointCloud<pcl::PointXYZRGB>);
//
// for (size_t i=0;i<classificationCloud->size();i++)
// {
// pcl::PointXYZRGB pTmp = classificationCloud->points[i];
// pTmp.z = 0; // 3D -> 2D
// if ((pTmp.r == 255) || (pTmp.g == 255)) // Object
// {
// objectCloud2D->push_back(pTmp);
// }
// else if (pTmp.b == 255) // Object
// {
// groundCloud2D->push_back(pTmp);
// }
// }
//
// //std::vector<int> indexVector;
// unsigned int leafNodeCounter = 0;
// unsigned int voxelDensity = 0;
// unsigned int totalPoints = 0;
// //int occupancyW = OCCUPANCY_WIDTH/OCCUPANCY_GRID_RESOLUTION;
// //int occupancyH = OCCUPANCY_DEPTH/OCCUPANCY_GRID_RESOLUTION;
// std::vector<unsigned int> ocGroundGrid(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH, 0);
// std::vector<unsigned int> ocObjectGrid(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH, 0);
// std::vector<signed char> ocGridFinal(OCCUPANCY_WIDTH*OCCUPANCY_DEPTH,-1);
//
// // Ground grid
// pcl::octree::OctreePointCloudDensity< pcl::PointXYZRGB > octreeGround (OCCUPANCY_GRID_RESOLUTION);
// octreeGround.setInputCloud (groundCloud2D);
// pcl::PointXYZ bot(-OCCUPANCY_DEPTH_M/2,-OCCUPANCY_WIDTH_M/2,-0.5);
// pcl::PointXYZ top(OCCUPANCY_DEPTH_M/2,OCCUPANCY_WIDTH_M/2,0.5);
// octreeGround.defineBoundingBox(bot.x, bot.y, bot.z, top.x, top.y, top.z); // I have already calculated the BBox of my point cloud
// octreeGround.addPointsFromInputCloud ();
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it1;
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it1_end = octreeGround.leaf_end();
// int minx = 1000;
// int miny = 1000;
// int maxx = -1000;
// int maxy = -1000;
// for (it1 = octreeGround.leaf_begin(); it1 != it1_end; ++it1)
// {
// pcl::octree::OctreePointCloudDensityContainer& container = it1.getLeafContainer();
// voxelDensity = container.getPointCounter();
// Eigen::Vector3f min_pt, max_pt;
// octreeGround.getVoxelBounds (it1, min_pt, max_pt);
// //ROS_INFO("x = %f, y = %f, z = %f, x2 = %f, y2 = %f, z2 = %f",min_pt[0],min_pt[1],min_pt[2],max_pt[0],max_pt[1],max_pt[2]);
// totalPoints += voxelDensity;
// if (min_pt[0] < minx)
// minx = min_pt[0];
// if (min_pt[0] > maxx)
// maxx = min_pt[0];
// if (min_pt[1] < miny)
// miny = min_pt[1];
// if (min_pt[1] > maxy)
// maxy = min_pt[1];
//
// int r = OCCUPANCY_DEPTH-(min_pt[0]+OCCUPANCY_DEPTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// int c = OCCUPANCY_WIDTH-(min_pt[1]+OCCUPANCY_WIDTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
//
// // if ((r == 51) || (c==51))
// // ROS_INFO("r = %i,c = %i",r,c);
//
// //if (((int)min_pt[0] == -1) || ((int)min_pt[1]==-1))
// //ROS_INFO("min_pt[0] = %f, min_pt[1]=%f",min_pt[0],min_pt[1]);
// if (!((r < 0) || (c < 0) || (r > OCCUPANCY_DEPTH-1) || (c > OCCUPANCY_WIDTH-1)))
// ocGroundGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("density = %i, r = %i, c=%i",voxelDensity, r, c);
// //ROS_INFO("x (%f) -> r (%i), y (%f) -> c (%i)",min_pt[0],r,min_pt[1],c);
// leafNodeCounter++;
// }
//
//
// // Object grid
// pcl::octree::OctreePointCloudDensity< pcl::PointXYZRGB > octreeObject (OCCUPANCY_GRID_RESOLUTION);
// octreeObject.setInputCloud (objectCloud2D);
// octreeObject.defineBoundingBox(bot.x, bot.y, bot.z, top.x, top.y, top.z); // I have already calculated the BBox of my point cloud
// octreeObject.addPointsFromInputCloud ();
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it2;
// pcl::octree::OctreePointCloudDensity<pcl::PointXYZRGB>::LeafNodeIterator it2_end = octreeObject.leaf_end();
// minx = 1000;
// miny = 1000;
// maxx = -1000;
// maxy = -1000;
// leafNodeCounter = 0;
// voxelDensity = 0;
// totalPoints = 0;
// int t1 = 0;
// int t2 = 0;
// int t3 = 0;
// for (it2 = octreeObject.leaf_begin(); it2 != it2_end; ++it2)
// {
// pcl::octree::OctreePointCloudDensityContainer& container = it2.getLeafContainer();
// voxelDensity = container.getPointCounter();
// Eigen::Vector3f min_pt, max_pt;
// octreeObject.getVoxelBounds (it2, min_pt, max_pt);
// //ROS_INFO("x = %f, y = %f, z = %f, x2 = %f, y2 = %f, z2 = %f",min_pt[0],min_pt[1],min_pt[2],max_pt[0],max_pt[1],max_pt[2]);
// totalPoints += voxelDensity;
// if (min_pt[0] < minx)
// minx = min_pt[0];
// if (min_pt[0] > maxx)
// maxx = min_pt[0];
// if (min_pt[1] < miny)
// miny = min_pt[1];
// if (min_pt[1] > maxy)
// maxy = min_pt[1];
//
// int r = OCCUPANCY_DEPTH-(min_pt[0]+OCCUPANCY_DEPTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// int c = OCCUPANCY_WIDTH-(min_pt[1]+OCCUPANCY_WIDTH_M/2)/OCCUPANCY_GRID_RESOLUTION;
// if (!((r < 0) || (c < 0) || (r > OCCUPANCY_DEPTH-1) || (c > OCCUPANCY_WIDTH-1)))
// ocObjectGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("object points = %i -> %i: x (%f) -> r (%i), y (%f) -> c (%i)",voxelDensity,ocGridFinal[r+OCCUPANCY_DEPTH*c],min_pt[0],r,min_pt[1],c);
// //ocGrid[r+OCCUPANCY_DEPTH*c] = voxelDensity;
// //ROS_INFO("density = %i, r = %i, c=%i",voxelDensity, r, c);
//
// leafNodeCounter++;
// }
//
// for (int r=0;r<OCCUPANCY_DEPTH;r++)
// {
// for (int c=0;c<OCCUPANCY_WIDTH;c++)
// {
// if ((ocGroundGrid[r+OCCUPANCY_DEPTH*c] == 0) && (ocObjectGrid[r+OCCUPANCY_DEPTH*c] <= 1))
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = -1.0; // 0.5 default likelihood
// t1++;
// }
// else if ((ocObjectGrid[r+OCCUPANCY_DEPTH*c] == 0))// && (ocGroundGrid[r+OCCUPANCY_DEPTH*c] > 0))
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = OCCUPANCY_MIN_PROB;
// t2++;
// }
// else
// {
// ocGridFinal[r+OCCUPANCY_DEPTH*c] = 50+(OCCUPANCY_MAX_PROB-50)*ocObjectGrid[r+OCCUPANCY_DEPTH*c]/(ocObjectGrid[r+OCCUPANCY_DEPTH*c]+ocGroundGrid[r+OCCUPANCY_DEPTH*c]);
// t3++;
// }
// }
// }
//
// nav_msgs::OccupancyGrid occupancyGrid;
// occupancyGrid.info.resolution = OCCUPANCY_GRID_RESOLUTION;
// occupancyGrid.header.stamp = timestamp;//ros::Time::now();
// occupancyGrid.header.frame_id = "/velodyne";
// occupancyGrid.info.width = OCCUPANCY_WIDTH;
// occupancyGrid.info.height = OCCUPANCY_DEPTH;
// //geometry_msgs::Pose lidarPose;
// geometry_msgs::Point lidarPoint;
// lidarPoint.x = 0;
// lidarPoint.y = 0;
// lidarPoint.z = 0;
// geometry_msgs::Quaternion lidarQuaternion;
// lidarQuaternion.w = 1;
// lidarQuaternion.x = 0;
// lidarQuaternion.y = 0;
// lidarQuaternion.z = 0;
// occupancyGrid.info.origin.position = lidarPoint;
// occupancyGrid.info.origin.orientation = lidarQuaternion;
// occupancyGrid.data = ocGridFinal;
//
// //ROS_INFO("total points = %i, total leaves = %i",totalPoints,leafNodeCounter);
// //ROS_INFO("minx = %i, maxx = %i, miny = %i, maxy = %i",minx,maxx,miny,maxy);
// //ROS_INFO("t1 = %i, t2 = %i, t3 = %i",t1,t2,t3);
//
//
// pubOccupancyGridPointer->publish(occupancyGrid);
// int l = 0;
// while (*++itLeafs)
// {
// //Iteratively explore only the leaf nodes..
// std::vector<PointXYZ> points;
// itLeafs->getData(points);
// l++;
// }
//
// ROS_INFO("l = %i",l);
//pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (OCCUPANCY_GRID_RESOLUTION);
// sensor_msgs::PointCloud2::Ptr cloud_filtered (new sensor_msgs::PointCloud2 ());
// pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
// sor.setInputCloud (processedCloud);
// sor.setLeafSize (0.01f, 0.01f, 0.01f);
// sor.filter (*cloud_filtered);
if (n==0)
{
//viewer->setCameraPosition(-20,0,10,1,0,2,1,0,2,v1);
//viewer->setCameraPosition(-20,0,10,1,0,2,1,0,2,v2);
//viewer->loadCameraParameters("pcl_video.cam");
}
#if (OLD_METHOD)
}
#endif
if (visualizationFlag)
viewer->spinOnce();
// Save screenshot
// std::stringstream tmp;
// tmp << n;
// viewer->saveScreenshot("/home/mikkel/images/" + (boost::format("%04d") % n).str() + ".png");
// n++;
}
template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
{
const unsigned int number_of_triangles = static_cast <unsigned int> (local_triangles.size ());
std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices (number_of_triangles);
std::vector <float> triangle_area (number_of_triangles);
std::vector <float> distance_weight (number_of_triangles);
float total_area = 0.0f;
const float coeff = 1.0f / 12.0f;
const float coeff_1_div_3 = 1.0f / 3.0f;
Eigen::Vector3f feature_point (point.x, point.y, point.z);
unsigned int i_triangle = 0;
for (auto it = local_triangles.cbegin (); it != local_triangles.cend (); it++, i_triangle++)
{
Eigen::Vector3f pt[3];
for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
{
const unsigned int index = triangles_[*it].vertices[i_vertex];
pt[i_vertex] (0) = surface_->points[index].x;
pt[i_vertex] (1) = surface_->points[index].y;
pt[i_vertex] (2) = surface_->points[index].z;
}
const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
triangle_area[i_triangle] = curr_area;
total_area += curr_area;
distance_weight[i_triangle] = std::pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f);
Eigen::Matrix3f curr_scatter_matrix;
curr_scatter_matrix.setZero ();
for (const auto &i_pt : pt)
{
Eigen::Vector3f vec = i_pt - feature_point;
curr_scatter_matrix += vec * (vec.transpose ());
for (const auto &j_pt : pt)
curr_scatter_matrix += vec * ((j_pt - feature_point).transpose ());
}
scatter_matrices[i_triangle] = coeff * curr_scatter_matrix;
}
if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
total_area = 1.0f / total_area;
else
total_area = 1.0f;
Eigen::Matrix3f overall_scatter_matrix;
overall_scatter_matrix.setZero ();
std::vector<float> total_weight (number_of_triangles);
const float denominator = 1.0f / 6.0f;
for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
{
float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
overall_scatter_matrix += factor * scatter_matrices[i_triangle];
total_weight[i_triangle] = factor * denominator;
}
Eigen::Vector3f v1, v2, v3;
computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
float h1 = 0.0f;
float h3 = 0.0f;
i_triangle = 0;
for (auto it = local_triangles.cbegin (); it != local_triangles.cend (); it++, i_triangle++)
{
Eigen::Vector3f pt[3];
for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
{
const unsigned int index = triangles_[*it].vertices[i_vertex];
pt[i_vertex] (0) = surface_->points[index].x;
pt[i_vertex] (1) = surface_->points[index].y;
pt[i_vertex] (2) = surface_->points[index].z;
}
float factor1 = 0.0f;
float factor3 = 0.0f;
for (const auto &i_pt : pt)
{
Eigen::Vector3f vec = i_pt - feature_point;
factor1 += vec.dot (v1);
factor3 += vec.dot (v3);
}
h1 += total_weight[i_triangle] * factor1;
h3 += total_weight[i_triangle] * factor3;
}
if (h1 < 0.0f) v1 = -v1;
if (h3 < 0.0f) v3 = -v3;
v2 = v3.cross (v1);
lrf_matrix.row (0) = v1;
lrf_matrix.row (1) = v2;
lrf_matrix.row (2) = v3;
}
// Calculate the distance between the point and the plane
static double GetDistanceToPlane(const pcl::PointXYZ& point, const Eigen::Vector3f& normalVector, double d)
{
Eigen::Vector3f t;
t << point.x, point.y, point.z;
return fabs(t.dot(normalVector) + d);
}
bool SimpleRayCaster::intersectRayTriangle(const Eigen::Vector3f ray,
const Eigen::Vector3f a, const Eigen::Vector3f b, const Eigen::Vector3f c,
Eigen::Vector3f& isec)
{
/* As discribed by:
* http://geomalgorithms.com/a06-_intersect-2.html#intersect_RayTriangle%28%29 */
const Eigen::Vector3f p(0,0,0);
const Eigen::Vector3f u = b - a;
const Eigen::Vector3f v = c - a;
const Eigen::Vector3f n = u.cross(v);
const float n_dot_ray = n.dot(ray);
if (std::fabs(n_dot_ray) < 1e-9) {
return false;
}
const float r = n.dot(a-p) / n_dot_ray;
if (r < 0) {
return false;
}
// the ray intersection point
isec = ray * r;
const Eigen::Vector3f w = p + r * ray - a;
const float denominator = u.dot(v) * u.dot(v) - u.dot(u) * v.dot(v);
const float s_numerator = u.dot(v) * w.dot(v) - v.dot(v) * w.dot(u);
const float s = s_numerator / denominator;
if (s < 0 || s > 1) {
return false;
}
const float t_numerator = u.dot(v) * w.dot(u) - u.dot(u) * w.dot(v);
const float t = t_numerator / denominator;
if (t < 0 || s+t > 1) {
return false;
}
return true;
}
bool is_inlier(const Eigen::Vector3f& point, const Eigen::Vector4f plane, double threshold)
{
return fabs(point.dot(plane.segment<3>(0)) + plane(3)) < threshold;
}
// METHOD THAT RECEIVES POINT CLOUDS (OPEN MP)
std::vector<cluster> poseEstimationSV::poseEstimationCore_openmp(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud)
{
Tic();
std::vector <std::vector < pose > > bestPosesAux;
bestPosesAux.resize(omp_get_num_procs());
//int bestPoseAlpha;
//int bestPosePoint;
//int bestPoseVotes;
Eigen::Vector3f scenePoint;
Eigen::Vector3f sceneNormal;
pcl::PointIndices normals_nan_indices;
pcl::ExtractIndices<pcl::PointNormal> nan_extract;
float alpha;
unsigned int alphaBin,index;
// Iterators
//unsigned int sr; // scene reference point
pcl::PointCloud<pcl::PointNormal>::iterator si; // scene paired point
std::vector<pointPairSV>::iterator sameFeatureIt; // same key on hash table
std::vector<boost::shared_ptr<pose> >::iterator bestPosesIt;
Eigen::Vector4f feature;
Eigen::Vector3f _pointTwoTransformed;
std::cout<< "\tCloud size: " << cloud->size() << endl;
//////////////////////////////////////////////
// Downsample point cloud using a voxelgrid //
//////////////////////////////////////////////
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudDownsampled(new pcl::PointCloud<pcl::PointXYZ> ());
// Create the filtering object
pcl::VoxelGrid<pcl::PointXYZ> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (model->distanceStep,model->distanceStep,model->distanceStep);
sor.filter (*cloudDownsampled);
std::cout<< "\tCloud size after downsampling: " << cloudDownsampled->size() << endl;
// Compute point cloud normals (using cloud before downsampling information)
std::cout<< "\tCompute normals... ";
cloudNormals=model->computeSceneNormals(cloudDownsampled);
std::cout<< "Done" << endl;
/*boost::shared_ptr<pcl_visualization::PCLVisualizer> viewer2 = objectModel::viewportsVis(cloudFilteredNormals);
while (!viewer2->wasStopped ())
{
viewer2->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}*/
/*boost::shared_ptr<pcl_visualization::PCLVisualizer> viewer2 = objectModel::viewportsVis(model->modelCloud);
while (!viewer2->wasStopped ())
{
viewer2->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}*/
//////////////////////////////////////////////////////////////////////////////
// Filter again to remove spurious normals nans (and it's associated point) //
////////////////////////////////////////////////fa//////////////////////////////
for (unsigned int i = 0; i < cloudNormals->points.size(); ++i)
{
if (isnan(cloudNormals->points[i].normal[0]) || isnan(cloudNormals->points[i].normal[1]) || isnan(cloudNormals->points[i].normal[2]))
{
normals_nan_indices.indices.push_back(i);
}
}
nan_extract.setInputCloud(cloudNormals);
nan_extract.setIndices(boost::make_shared<pcl::PointIndices> (normals_nan_indices));
nan_extract.setNegative(true);
nan_extract.filter(*cloudWithNormalsDownSampled);
std::cout<< "\tCloud size after removing NaN normals: " << cloudWithNormalsDownSampled->size() << endl;
/////////////////////////////////////////////
// Extract reference points from the scene //
/////////////////////////////////////////////
//pcl::RandomSample< pcl::PointCloud<pcl::PointNormal> > randomSampler;
//randomSampler.setInputCloud(cloudWithNormalsDownSampled);
// Create the filtering object
int numberOfPoints=(int) (cloudWithNormalsDownSampled->size () )*referencePointsPercentage;
int totalPoints=(int) (cloudWithNormalsDownSampled->size ());
std::cout << "\tUniform sample a set of " << numberOfPoints << "(" << referencePointsPercentage*100 << "%)... ";
referencePointsIndices->indices.clear();
extractReferencePointsUniform(referencePointsPercentage,totalPoints);
std::cout << "Done" << std::endl;
//std::cout << referencePointsIndices->indices.size() << std::endl;
//////////////
// Votation //
//////////////
std::cout<< "\tVotation... ";
omp_set_num_threads(omp_get_num_procs());
//omp_set_num_threads(1);
//int iteration=0;
bestPoses.clear();
#pragma omp parallel for private(alpha,alphaBin,alphaScene,sameFeatureIt,index,feature,si,_pointTwoTransformed) //reduction(+:iteration) //nowait
for(unsigned int sr=0; sr < referencePointsIndices->indices.size(); ++sr)
{
//++iteration;
//std::cout << "iteration: " << iteration << " thread:" << omp_get_thread_num() << std::endl;
//printf("Hello from thread %d, nthreads %d\n", omp_get_thread_num(), omp_get_num_threads());
scenePoint=cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].getVector3fMap();
sceneNormal=cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].getNormalVector3fMap();
// Get transformation from scene frame to global frame
Eigen::Vector3f cross=sceneNormal.cross (Eigen::Vector3f::UnitX ()). normalized();
Eigen::Affine3f rotationSceneToGlobal;
if(isnan(cross[0]))
{
rotationSceneToGlobal=Eigen::AngleAxisf(0.0,Eigen::Vector3f::UnitX ());
}
else
rotationSceneToGlobal=Eigen::AngleAxisf(acosf (sceneNormal.dot (Eigen::Vector3f::UnitX ())),cross);
Eigen::Affine3f transformSceneToGlobal = Eigen::Translation3f ( rotationSceneToGlobal* ((-1)*scenePoint)) * rotationSceneToGlobal;
//////////////////////
// Choose best pose //
//////////////////////
// Reset pose accumulator
for(std::vector<std::vector<int> >::iterator accumulatorIt=accumulatorParallelAux[omp_get_thread_num()].begin();accumulatorIt < accumulatorParallelAux[omp_get_thread_num()].end(); ++accumulatorIt)
{
std::fill(accumulatorIt->begin(),accumulatorIt->end(),0);
}
//std::cout << std::endl;
for(si=cloudWithNormalsDownSampled->begin(); si < cloudWithNormalsDownSampled->end();++si)
{
// if same point, skip point pair
if( (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].x==si->x) && (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].y==si->y) && (cloudWithNormalsDownSampled->points[referencePointsIndices->indices[sr]].z==si->z))
{
//std::cout << si->x << " " << si->y << " " << si->z << std::endl;
continue;
}
// Compute PPF
pointPairSV PPF=pointPairSV(cloudWithNormalsDownSampled->points[sr],*si, transformSceneToGlobal);
// Compute index
index=PPF.getHash(*si,model->distanceStepInverted);
// If distance between point pairs is bigger than the maximum for this model, skip point pair
if(index>pointPairSV::maxHash)
{
//std::cout << "DEBUG" << std::endl;
continue;
}
// If there is no similar point pair features in the model, skip point pair and avoid computing the alpha
if(model->hashTable[index].size()==0)
continue;
for(sameFeatureIt=model->hashTable[index].begin(); sameFeatureIt<model->hashTable[index].end(); ++sameFeatureIt)
{
// Vote on the reference point and angle (and object)
alpha=sameFeatureIt->alpha-PPF.alpha; // alpha values between [-360,360]
// alpha values should be between [-180,180] ANGLE_MAX = 2*PI
if(alpha<(-PI))
alpha=ANGLE_MAX+alpha;
else if(alpha>(PI))
alpha=alpha-ANGLE_MAX;
//std::cout << "alpha after: " << alpha*RAD_TO_DEG << std::endl;
//std::cout << "alpha after2: " << (alpha+PI)*RAD_TO_DEG << std::endl;
alphaBin=static_cast<unsigned int> ( round((alpha+PI)*pointPair::angleStepInverted) ); // division is slower than multiplication
//std::cout << "angle1: " << alphaBin << std::endl;
/*alphaBin = static_cast<unsigned int> (floor (alpha) + floor (PI *poseAngleStepInverted));
std::cout << "angle2: " << alphaBin << std::endl;*/
//alphaBin=static_cast<unsigned int> ( floor(alpha*poseAngleStepInverted) + floor(PI*poseAngleStepInverted) );
if(alphaBin>=pointPair::angleBins)
{
alphaBin=0;
//ROS_INFO("naoooo");
//exit(1);
}
//#pragma omp critical
//{std::cout << index <<" "<<sameFeatureIt->id << " " << alphaBin << " " << omp_get_thread_num() << " " << accumulatorParallelAux[omp_get_thread_num()][sameFeatureIt->id][alphaBin] << std::endl;}
accumulatorParallelAux[omp_get_thread_num()][sameFeatureIt->id][alphaBin]+=sameFeatureIt->weight;
}
}
//ROS_INFO("DISTANCE:%f DISTANCE SQUARED:%f", model->maxModelDist, model->maxModel
// Choose best pose (highest peak on the accumulator[peak with more votes])
int bestPoseAlpha=0;
int bestPosePoint=0;
int bestPoseVotes=0;
for(size_t p=0; p < model->modelCloud->size(); ++p)
{
for(unsigned int a=0; a < pointPair::angleBins; ++a)
{
if(accumulatorParallelAux[omp_get_thread_num()][p][a]>bestPoseVotes)
{
bestPoseVotes=accumulatorParallelAux[omp_get_thread_num()][p][a];
bestPosePoint=p;
bestPoseAlpha=a;
}
}
}
// A candidate pose was found
if(bestPoseVotes!=0)
{
// Compute and store transformation from model to scene
//boost::shared_ptr<pose> bestPose(new pose( bestPoseVotes,model->modelToScene(model->modelCloud->points[bestPosePoint],transformSceneToGlobal,static_cast<float>(bestPoseAlpha)*pointPair::angleStep-PI) ));
bestPosesAux[omp_get_thread_num()].push_back(pose( bestPoseVotes,model->modelToScene(bestPosePoint,transformSceneToGlobal,static_cast<float>(bestPoseAlpha)*pointPair::angleStep-PI) ));
//bestPoses.push_back(bestPose);
//std::cout << bestPosesAux[omp_get_thread_num()].size() <<" " <<omp_get_thread_num()<< std::endl;
}
else
{
continue;
}
// Choose poses whose votes are a percentage above a given threshold of the best pose
accumulatorParallelAux[omp_get_thread_num()][bestPosePoint][bestPoseAlpha]=0; // This is more efficient than having an if condition to verify if we are considering the best pose again
for(size_t p=0; p < model->modelCloud->size(); ++p)
{
for(unsigned int a=0; a < pointPair::angleBins; ++a)
{
if(accumulatorParallelAux[omp_get_thread_num()][p][a]>=accumulatorPeakThreshold*bestPoseVotes)
{
// Compute and store transformation from model to scene
//boost::shared_ptr<pose> bestPose(new pose( accumulatorParallelAux[omp_get_thread_num()][p][a],model->modelToScene(model->modelCloud->points[p],transformSceneToGlobal,static_cast<float>(a)*pointPair::angleStep-PI ) ));
//bestPoses.push_back(bestPose);
bestPosesAux[omp_get_thread_num()].push_back(pose( bestPoseVotes,model->modelToScene(bestPosePoint,transformSceneToGlobal,static_cast<float>(bestPoseAlpha)*pointPair::angleStep-PI) ));
//std::cout << bestPosesAux[omp_get_thread_num()].size() <<" " <<omp_get_thread_num()<< std::endl;
}
}
}
}
std::cout << "Done" << std::endl;
for(int i=0; i<omp_get_num_procs(); ++i)
{
for(unsigned int j=0; j<bestPosesAux[i].size(); ++j)
bestPoses.push_back(bestPosesAux[i][j]);
}
std::cout << "\thypothesis number: " << bestPoses.size() << std::endl << std::endl;
if(bestPoses.size()==0)
{
clusters.clear();
return clusters;
}
//////////////////////
// Compute clusters //
//////////////////////
Tac();
std::cout << "\tCompute clusters... ";
Tic();
clusters=poseClustering(bestPoses);
Tac();
std::cout << "Done" << std::endl;
return clusters;
}
int main(int argc, char *argv[]){
if(argc>1){
CloudPtr cloud (new Cloud);
ColorCloudPtr color_cloud (new ColorCloud);
if (pcl::io::loadPCDFile(argv[1], *cloud) == -1) //* load the file
{
PCL_ERROR ("Couldn't read file.\n");
return -1;
}
for(int i = 0; i<cloud->size(); i++){
Point p1 = cloud->points[i];
ColorPoint p2;
p2.x = p1.x;
p2.y = p1.y;
p2.z = p1.z;
p2.r = 0;
p2.g = 0;
p2.b = 0;
color_cloud->push_back(p2);
}
float centroid[3];
calculateCentroid(cloud,centroid);
pcl::PolygonMesh pm;
pcl::ConvexHull<ColorPoint> chull;
chull.setInputCloud(color_cloud);
chull.reconstruct(pm);
pcl::fromROSMsg(pm.cloud,*color_cloud);
vector<float> distances(color_cloud->size(),999999);
for(int i = 0; i<pm.polygons.size(); i++){
int i0 = pm.polygons[i].vertices[0];
int i1 = pm.polygons[i].vertices[1];
int i2 = pm.polygons[i].vertices[2];
ColorPoint p0 = color_cloud->points[i0];
ColorPoint p1 = color_cloud->points[i1];
ColorPoint p2 = color_cloud->points[i2];
Eigen::Vector3f v0;
Eigen::Vector3f v1;
Eigen::Vector3f v2;
v0 << p0.x,p0.y,p0.z;
v1 << p1.x,p1.y,p1.z;
v2 << p2.x,p2.y,p2.z;
Eigen::Vector3f normal = (v1-v0).cross(v2-v0).normalized();
Eigen::Vector3f p;
p << centroid[0], centroid[1], centroid[2];
Eigen::Vector3f to_p = p-v0;
Eigen::Vector3f projected_p = p-(normal* normal.dot(to_p));
float dist0 = (v1-v0).dot(projected_p-v0)/(v1-v0).norm();
float dist1 = (v2-v0).dot(projected_p-v0)/(v2-v0).norm();
distances[i0] = min(distances[i0],(dist0+dist1));
distances[i1] = min(distances[i1],(dist0+dist1));
distances[i2] = min(distances[i2],(dist0+dist1));
}
float max_dist = *max_element(distances.begin(),distances.end());
float thresh = 500;
for(int i = 0; i<color_cloud->size(); i++){
ColorPoint* p1 = &color_cloud->points[i];
float color = 255 - distances[i]*(255/max_dist);
if(distances[i]>thresh)
color = 0;
p1->r = color;
p1->g = 0;
p1->b = 0;
}
chull.setInputCloud(color_cloud);
chull.reconstruct(pm);
pcl::io::saveVTKFile("hull.vtk",pm);
}
}
void SaveClustersWorker::doWork(const QString &filename)
{
bool is_success(false);
QByteArray ba = filename.toLocal8Bit();
string* strfilename = new string(ba.data());
dataLibrary::Status = STATUS_SAVECLUSTERS;
dataLibrary::start = clock();
//begin of processing
//compute centor point and normal
float nx_all, ny_all, nz_all;
float curvature_all;
Eigen::Matrix3f convariance_matrix_all;
Eigen::Vector4f xyz_centroid_all, plane_parameters_all;
pcl::compute3DCentroid(*dataLibrary::cloudxyz, xyz_centroid_all);
pcl::computeCovarianceMatrix(*dataLibrary::cloudxyz, xyz_centroid_all, convariance_matrix_all);
pcl::solvePlaneParameters(convariance_matrix_all, nx_all, ny_all, nz_all, curvature_all);
Eigen::Vector3f centroid_all;
dataLibrary::plane_normal_all(0)=nx_all;
dataLibrary::plane_normal_all(1)=ny_all;
dataLibrary::plane_normal_all(2)=nz_all;
centroid_all(0)=xyz_centroid_all(0);
centroid_all(1)=xyz_centroid_all(1);
centroid_all(2)=xyz_centroid_all(2);
//calculate total surface roughness of outcrop
float total_distance=0.0;
for(int i=0; i<dataLibrary::cloudxyz->size(); i++)
{
Eigen::Vector3f Q;
Q(0)=dataLibrary::cloudxyz->at(i).x;
Q(1)=dataLibrary::cloudxyz->at(i).y;
Q(2)=dataLibrary::cloudxyz->at(i).z;
total_distance+=std::abs((Q-centroid_all).dot(dataLibrary::plane_normal_all)/std::sqrt((dataLibrary::plane_normal_all.dot(dataLibrary::plane_normal_all))));
}
float roughness=total_distance/dataLibrary::cloudxyz->size();
//project all points
pcl::ModelCoefficients::Ptr coefficients_all (new pcl::ModelCoefficients());
coefficients_all->values.resize(4);
coefficients_all->values[0] = nx_all;
coefficients_all->values[1] = ny_all;
coefficients_all->values[2] = nz_all;
coefficients_all->values[3] = - (nx_all*xyz_centroid_all[0] + ny_all*xyz_centroid_all[1] + nz_all*xyz_centroid_all[2]);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected_all (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj_all;
proj_all.setModelType(pcl::SACMODEL_PLANE);
proj_all.setInputCloud(dataLibrary::cloudxyz);
proj_all.setModelCoefficients(coefficients_all);
proj_all.filter(*cloud_projected_all);
//compute convex hull
pcl::ConvexHull<pcl::PointXYZ> chull_all;
chull_all.setInputCloud(cloud_projected_all);
chull_all.reconstruct(*dataLibrary::cloud_hull_all);
/*//compute concave hull
pcl::ConcaveHull<pcl::PointXYZ> chull_all;
chull_all.setInputCloud(cloud_projected_all);
chull_all.setAlpha(0.1);
chull_all.reconstruct(*dataLibrary::cloud_hull_all);*/
//compute area
float area_all = 0.0f;
int num_points_all = dataLibrary::cloud_hull_all->size();
int j = 0;
Eigen::Vector3f va_all, vb_all, res_all;
res_all(0) = res_all(1) = res_all(2) = 0.0f;
for(int i = 0; i < num_points_all; i++)
{
j = (i+1) % num_points_all;
va_all = dataLibrary::cloud_hull_all->at(i).getVector3fMap();
vb_all = dataLibrary::cloud_hull_all->at(j).getVector3fMap();
res_all += va_all.cross(vb_all);
}
area_all = fabs(res_all.dot(dataLibrary::plane_normal_all) * 0.5);
//initial total length of fracture traces
float total_length=0.0;
//initial over estimate length of fracture traces
float error_length=0.0;
//initial total displacement
float total_displacement=0.0;
//initial mean dip2plane angle
float total_dip2plane=0.0;
int inside_num=0;
string textfilename = strfilename->substr(0, strfilename->size()-4) += "_table.txt";
string dip_dipdir_file = strfilename->substr(0, strfilename->size()-4) += "_dip_dipdir.txt";
string dipdir_dip_file = strfilename->substr(0, strfilename->size()-4) += "_dipdir_dip.txt";
string fracture_intensity = strfilename->substr(0, strfilename->size()-4) += "_fracture_intensity.txt";
ofstream fout(textfilename.c_str());
ofstream dip_dipdir_out(dip_dipdir_file.c_str());
ofstream dipdir_dip_out(dipdir_dip_file.c_str());
ofstream fracture_intensity_out(fracture_intensity.c_str());
fout<<"Flag"<<"\t"<<"Number"<<"\t"<<"Points"<<"\t"<<"Direc"<<"\t"<<"Dip"<<"\t"<<"Area"<<"\t"<<"Length"<<"\t"<<"Roughness"<<"\n";
int num_of_clusters = dataLibrary::clusters.size();
ofstream fbinaryout(strfilename->c_str(), std::ios::out|std::ios::binary|std::ios::app);
fbinaryout.write(reinterpret_cast<const char*>(&num_of_clusters), sizeof(num_of_clusters));
for(int cluster_index = 0; cluster_index < dataLibrary::clusters.size(); cluster_index++)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud (new pcl::PointCloud<pcl::PointXYZ>);
int num_of_points = dataLibrary::clusters[cluster_index].indices.size();
fbinaryout.write(reinterpret_cast<const char*>(&num_of_points), sizeof(num_of_points));
float rgb = dataLibrary::cloudxyzrgb_clusters->at(dataLibrary::clusters[cluster_index].indices[0]).rgb;
fbinaryout.write(reinterpret_cast<const char*>(&rgb), sizeof(rgb));
float x, y, z;
for(int j = 0; j < dataLibrary::clusters[cluster_index].indices.size(); j++)
{
plane_cloud->push_back(dataLibrary::cloudxyz->at(dataLibrary::clusters[cluster_index].indices[j]));
x = dataLibrary::cloudxyz->at(dataLibrary::clusters[cluster_index].indices[j]).x;
y = dataLibrary::cloudxyz->at(dataLibrary::clusters[cluster_index].indices[j]).y;
z = dataLibrary::cloudxyz->at(dataLibrary::clusters[cluster_index].indices[j]).z;
fbinaryout.write(reinterpret_cast<const char*>(&x), sizeof(x));
fbinaryout.write(reinterpret_cast<const char*>(&y), sizeof(y));
fbinaryout.write(reinterpret_cast<const char*>(&z), sizeof(z));
}
//prepare for projecting data onto plane
float nx, ny, nz;
float curvature;
Eigen::Matrix3f convariance_matrix;
Eigen::Vector4f xyz_centroid, plane_parameters;
pcl::compute3DCentroid(*plane_cloud, xyz_centroid);
pcl::computeCovarianceMatrix(*plane_cloud, xyz_centroid, convariance_matrix);
pcl::solvePlaneParameters(convariance_matrix, nx, ny, nz, curvature);
Eigen::Vector3f centroid;
centroid(0)=xyz_centroid(0);
centroid(1)=xyz_centroid(1);
centroid(2)=xyz_centroid(2);
//project data onto plane
//set plane parameter
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients());
coefficients->values.resize(4);
coefficients->values[0] = nx;
coefficients->values[1] = ny;
coefficients->values[2] = nz;
coefficients->values[3] = - (nx*xyz_centroid[0] + ny*xyz_centroid[1] + nz*xyz_centroid[2]);
//projecting
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(plane_cloud);
proj.setModelCoefficients(coefficients);
proj.filter(*cloud_projected);
//generate a concave or convex
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConvexHull<pcl::PointXYZ> chull;
chull.setInputCloud(cloud_projected);
chull.reconstruct(*cloud_hull);
//calculate polygon area
Eigen::Vector3f normal;
normal(0) = nx;
normal(1) = ny;
normal(2) = nz;
float area = 0.0f;
int num_points = cloud_hull->size();
int j = 0;
Eigen::Vector3f va, vb, res;
res(0) = res(1) = res(2) = 0.0f;
for(int i = 0; i < num_points; i++)
{
j = (i+1) % num_points;
va = cloud_hull->at(i).getVector3fMap();
vb = cloud_hull->at(j).getVector3fMap();
res += va.cross(vb);
}
area = fabs(res.dot(normal) * 0.5);
float dip_direction, dip;
if(nz < 0.0)
{
nx = -nx;
ny = -ny;
nz = -nz;
}
//Dip Direction
if(nx == 0.0)
{
if((ny > 0.0)||(ny == 0.0))
dip_direction = 0.0;
else
dip_direction = 180.0;
}
else if(nx > 0.0)
{
dip_direction = 90.0 - atan(ny/nx)*180/M_PI;
}
else
{
dip_direction = 270.0 - atan(ny/nx)*180/M_PI;
}
//dip
if((nx*nx + ny*ny) == 0.0)
{
dip = 0.0;
}
else
{
dip = 90.0 - atan(fabs(nz)/sqrt((nx*nx + ny*ny)))*180/M_PI;
}
//calculate fracture surface roughness
float fracture_total_distance=0.0;
for(int j = 0; j < plane_cloud->size(); j++)
{
Eigen::Vector3f Q;
Q(0)=plane_cloud->at(j).x;
Q(1)=plane_cloud->at(j).y;
Q(2)=plane_cloud->at(j).z;
fracture_total_distance+=std::abs((Q-centroid).dot(normal)/std::sqrt((normal.dot(normal))));
}
float fracture_roughness=fracture_total_distance/plane_cloud->size();
float length;
bool flag = dataLibrary::CheckClusters(dataLibrary::plane_normal_all, centroid_all, dataLibrary::cloud_hull_all, normal, centroid, cloud_projected, cluster_index, length, false);
fout<<flag<<"\t"<<cluster_index+1<<"\t"<<dataLibrary::clusters[cluster_index].indices.size()<<"\t"<<dip_direction<<"\t"<<dip<<"\t"<<area<<"\t"<<length<<"\t"<<fracture_roughness<<"\n";
//calculate displacement
Eigen::Vector3f line_direction = normal.cross(dataLibrary::plane_normal_all.cross(normal));
if((line_direction(0) == 0)&&(line_direction(1) == 0)&&(line_direction(2) == 0))
{
total_displacement+=0.0;
}
else
{
if(flag)
{
float max_value, min_value;
int max_index, min_index;
Eigen::Vector3f point;
point(0) = cloud_projected->at(0).x;
point(1) = cloud_projected->at(0).y;
point(2) = cloud_projected->at(0).z;
float mod_line_direction = std::sqrt(line_direction.dot(line_direction));
max_value = min_value = line_direction.dot(point - centroid)/mod_line_direction;
max_index = min_index = 0;
for(int i=1; i<cloud_projected->size(); i++)
{
Eigen::Vector3f point;
point(0) = cloud_projected->at(i).x;
point(1) = cloud_projected->at(i).y;
point(2) = cloud_projected->at(i).z;
float value = line_direction.dot(point - centroid)/mod_line_direction;
if(max_value<value)
{
max_value = value;
max_index = i;
}
if(min_value>value)
{
min_value = value;
min_index = i;
}
}
float alpha = std::acos(std::abs(dataLibrary::plane_normal_all.dot(normal)/(std::sqrt(dataLibrary::plane_normal_all.dot(dataLibrary::plane_normal_all))*std::sqrt(normal.dot(normal)))));
total_dip2plane+=alpha*360.0/TWOPI;
inside_num+=1;
float tangent_alpha = std::tan(alpha);
float height_max = std::abs(dataLibrary::plane_normal_all.dot(cloud_projected->at(max_index).getVector3fMap()-centroid_all)/std::sqrt(dataLibrary::plane_normal_all.dot(dataLibrary::plane_normal_all)));
float height_min = std::abs(dataLibrary::plane_normal_all.dot(cloud_projected->at(min_index).getVector3fMap()-centroid_all)/std::sqrt(dataLibrary::plane_normal_all.dot(dataLibrary::plane_normal_all)));
float displacement_max = height_max/tangent_alpha;
float displacement_min = height_min/tangent_alpha;
total_displacement += (displacement_max+displacement_min)/2.0;
}
}
if(flag)
{
total_length += length;
dip_dipdir_out<<dip<<"\t"<<dip_direction<<"\n";
dipdir_dip_out<<dip_direction<<"\t"<<dip<<"\n";
dataLibrary::out_dips.push_back(dip);
dataLibrary::out_dip_directions.push_back(dip_direction);
dataLibrary::selectedPatches.push_back(cluster_index);
}
else
{
error_length += length;
}
fbinaryout.write(reinterpret_cast<const char*>(&dip), sizeof(dip));
fbinaryout.write(reinterpret_cast<const char*>(&dip_direction), sizeof(dip_direction));
fbinaryout.write(reinterpret_cast<const char*>(&area), sizeof(area));
}
fracture_intensity_out<<"Outcrop surface roughness:"<<"\t"<<roughness<<"\n";
fracture_intensity_out<<"Total area:"<<"\t"<<area_all<<"\n";
fracture_intensity_out<<"Percentage of displacement errors:"<<"\t"<<total_displacement/total_length*100<<" \%\n";
fracture_intensity_out<<"Percentage of over estimated errors:"<<"\t"<<error_length/total_length*100<<" \%\n";
fracture_intensity_out<<"Mean dip to plane angle:"<<"\t"<<total_dip2plane/inside_num<<"\n";
fracture_intensity_out<<"Fracture Density:"<<"\t"<<total_length/area_all;
fout<<flush;
fout.close();
dip_dipdir_out<<flush;
dip_dipdir_out.close();
dipdir_dip_out<<flush;
dipdir_dip_out.close();
fracture_intensity_out<<flush;
fracture_intensity_out.close();
fbinaryout.close();
//save outcrop convex hull and fracture traces
Eigen::Vector3f V_x = dataLibrary::cloud_hull_all->at(1).getVector3fMap() - dataLibrary::cloud_hull_all->at(0).getVector3fMap();
Eigen::Vector3f V_y = dataLibrary::plane_normal_all.cross(V_x);
std::vector<Eigen::Vector2f> convex_hull_all_2d;
dataLibrary::projection322(V_x, V_y, dataLibrary::cloud_hull_all, convex_hull_all_2d);
string hull_traces = strfilename->substr(0, strfilename->size()-4) += "_convex_hull&fracture_traces.txt";
ofstream hull_traces_out(hull_traces.c_str());
hull_traces_out<<"hull\t"<<dataLibrary::cloud_hull_all->points.size()<<"\t"<<dataLibrary::plane_normal_all(0)<<"\t"<<dataLibrary::plane_normal_all(1)<<"\t"<<dataLibrary::plane_normal_all(2)<<"\n";
for(int i=0; i<dataLibrary::cloud_hull_all->points.size(); i++)
{
hull_traces_out<<dataLibrary::cloud_hull_all->points[i].x<<"\t"<<dataLibrary::cloud_hull_all->points[i].y<<"\t"<<dataLibrary::cloud_hull_all->points[i].z<<"\t"<<convex_hull_all_2d[i](0)<<"\t"<<convex_hull_all_2d[i](1)<<"\n";
}
hull_traces_out<<"traces\n";
Eigen::Vector3f point_in_begin, point_in_end;
Eigen::Vector2f point_out_begin, point_out_end;
for(int i=0; i<dataLibrary::Lines.size(); i++)
{
point_in_begin(0)=dataLibrary::Lines[i].begin.x;
point_in_begin(1)=dataLibrary::Lines[i].begin.y;
point_in_begin(2)=dataLibrary::Lines[i].begin.z;
point_in_end(0)=dataLibrary::Lines[i].end.x;
point_in_end(1)=dataLibrary::Lines[i].end.y;
point_in_end(2)=dataLibrary::Lines[i].end.z;
dataLibrary::projection322(V_x, V_y, point_in_begin, point_out_begin);
dataLibrary::projection322(V_x, V_y, point_in_end, point_out_end);
hull_traces_out<<dataLibrary::Lines[i].begin.x<<"\t"<<dataLibrary::Lines[i].begin.y<<"\t"<<dataLibrary::Lines[i].begin.z<<"\t"<<dataLibrary::Lines[i].end.x<<"\t"<<dataLibrary::Lines[i].end.y<<"\t"<<dataLibrary::Lines[i].end.z<<"\t"<<point_out_begin(0)<<"\t"<<point_out_begin(1)<<"\t"<<point_out_end(0)<<"\t"<<point_out_end(1)<<"\n";
}
hull_traces_out<<flush;
hull_traces_out.close();
is_success = true;
//end of processing
dataLibrary::finish = clock();
if(this->getWriteLogMpde()&&is_success)
{
std::string log_text = "\tSaving Clusters costs: ";
std::ostringstream strs;
strs << (double)(dataLibrary::finish-dataLibrary::start)/CLOCKS_PER_SEC;
log_text += (strs.str() +" seconds.");
dataLibrary::write_text_to_log_file(log_text);
}
if(!this->getMuteMode()&&is_success)
{
emit SaveClustersReady(filename);
}
dataLibrary::Status = STATUS_READY;
emit showReadyStatus();
delete strfilename;
if(this->getWorkFlowMode()&&is_success)
{
this->Sleep(1000);
emit GoWorkFlow();
}
}
bool operator() (const Item& item) {
dirs_vox.index(0) = item.pos[0];
dirs_vox.index(1) = item.pos[1];
dirs_vox.index(2) = item.pos[2];
if (check_input (item)) {
for (auto l = Loop(3) (dirs_vox); l; ++l)
dirs_vox.value() = NAN;
return true;
}
std::vector<Direction> all_peaks;
for (size_t i = 0; i < size_t(dirs.rows()); i++) {
Direction p (dirs (i,0), dirs (i,1));
p.a = Math::SH::get_peak (item.data, lmax, p.v);
if (std::isfinite (p.a)) {
for (size_t j = 0; j < all_peaks.size(); j++) {
if (std::abs (p.v.dot (all_peaks[j].v)) > DOT_THRESHOLD) {
p.a = NAN;
break;
}
}
}
if (std::isfinite (p.a) && p.a >= threshold)
all_peaks.push_back (p);
}
if (ipeaks_vox) {
ipeaks_vox->index(0) = item.pos[0];
ipeaks_vox->index(1) = item.pos[1];
ipeaks_vox->index(2) = item.pos[2];
for (int i = 0; i < npeaks; i++) {
Eigen::Vector3f p;
ipeaks_vox->index(3) = 3*i;
for (int n = 0; n < 3; n++) {
p[n] = ipeaks_vox->value();
ipeaks_vox->index(3)++;
}
p.normalize();
value_type mdot = 0.0;
for (size_t n = 0; n < all_peaks.size(); n++) {
value_type f = std::abs (p.dot (all_peaks[n].v));
if (f > mdot) {
mdot = f;
peaks_out[i] = all_peaks[n];
}
}
}
}
else if (true_peaks.size()) {
for (int i = 0; i < npeaks; i++) {
value_type mdot = 0.0;
for (size_t n = 0; n < all_peaks.size(); n++) {
value_type f = std::abs (all_peaks[n].v.dot (true_peaks[i].v));
if (f > mdot) {
mdot = f;
peaks_out[i] = all_peaks[n];
}
}
}
}
else std::partial_sort_copy (all_peaks.begin(), all_peaks.end(), peaks_out.begin(), peaks_out.end());
int actual_npeaks = std::min (npeaks, (int) all_peaks.size());
dirs_vox.index(3) = 0;
for (int n = 0; n < actual_npeaks; n++) {
dirs_vox.value() = peaks_out[n].a*peaks_out[n].v[0];
dirs_vox.index(3)++;
dirs_vox.value() = peaks_out[n].a*peaks_out[n].v[1];
dirs_vox.index(3)++;
dirs_vox.value() = peaks_out[n].a*peaks_out[n].v[2];
dirs_vox.index(3)++;
}
for (; dirs_vox.index(3) < 3*npeaks; dirs_vox.index(3)++) dirs_vox.value() = NAN;
return true;
}
void MainController::run()
{
while(!pangolin::ShouldQuit() && !((!logReader->hasMore()) && quiet) && !(eFusion->getTick() == end && quiet))
{
if(!gui->pause->Get() || pangolin::Pushed(*gui->step))
{
if((logReader->hasMore() || rewind) && eFusion->getTick() < end)
{
TICK("LogRead");
if(rewind)
{
if(!logReader->hasMore())
{
logReader->getBack();
}
else
{
logReader->getNext();
}
if(logReader->rewound())
{
logReader->currentFrame = 0;
}
}
else
{
logReader->getNext();
}
TOCK("LogRead");
if(eFusion->getTick() < start)
{
eFusion->setTick(start);
logReader->fastForward(start);
}
float weightMultiplier = framesToSkip + 1;
if(framesToSkip > 0)
{
eFusion->setTick(eFusion->getTick() + framesToSkip);
logReader->fastForward(logReader->currentFrame + framesToSkip);
framesToSkip = 0;
}
Eigen::Matrix4f * currentPose = 0;
if(groundTruthOdometry)
{
currentPose = new Eigen::Matrix4f;
currentPose->setIdentity();
*currentPose = groundTruthOdometry->getIncrementalTransformation(logReader->timestamp);
}
eFusion->processFrame(logReader->rgb, logReader->depth, logReader->timestamp, currentPose, weightMultiplier);
if(currentPose)
{
delete currentPose;
}
if(frameskip && Stopwatch::getInstance().getTimings().at("Run") > 1000.f / 30.f)
{
framesToSkip = int(Stopwatch::getInstance().getTimings().at("Run") / (1000.f / 30.f));
}
}
}
else
{
eFusion->predict();
}
TICK("GUI");
if(gui->followPose->Get())
{
pangolin::OpenGlMatrix mv;
Eigen::Matrix4f currPose = eFusion->getCurrPose();
Eigen::Matrix3f currRot = currPose.topLeftCorner(3, 3);
Eigen::Quaternionf currQuat(currRot);
Eigen::Vector3f forwardVector(0, 0, 1);
Eigen::Vector3f upVector(0, iclnuim ? 1 : -1, 0);
Eigen::Vector3f forward = (currQuat * forwardVector).normalized();
Eigen::Vector3f up = (currQuat * upVector).normalized();
Eigen::Vector3f eye(currPose(0, 3), currPose(1, 3), currPose(2, 3));
eye -= forward;
Eigen::Vector3f at = eye + forward;
Eigen::Vector3f z = (eye - at).normalized(); // Forward
Eigen::Vector3f x = up.cross(z).normalized(); // Right
Eigen::Vector3f y = z.cross(x);
Eigen::Matrix4d m;
m << x(0), x(1), x(2), -(x.dot(eye)),
y(0), y(1), y(2), -(y.dot(eye)),
z(0), z(1), z(2), -(z.dot(eye)),
0, 0, 0, 1;
memcpy(&mv.m[0], m.data(), sizeof(Eigen::Matrix4d));
gui->s_cam.SetModelViewMatrix(mv);
}
gui->preCall();
std::stringstream stri;
stri << eFusion->getModelToModel().lastICPCount;
gui->trackInliers->Ref().Set(stri.str());
std::stringstream stre;
stre << (isnan(eFusion->getModelToModel().lastICPError) ? 0 : eFusion->getModelToModel().lastICPError);
gui->trackRes->Ref().Set(stre.str());
if(!gui->pause->Get())
{
gui->resLog.Log((isnan(eFusion->getModelToModel().lastICPError) ? std::numeric_limits<float>::max() : eFusion->getModelToModel().lastICPError), icpErrThresh);
gui->inLog.Log(eFusion->getModelToModel().lastICPCount, icpCountThresh);
}
Eigen::Matrix4f pose = eFusion->getCurrPose();
if(gui->drawRawCloud->Get() || gui->drawFilteredCloud->Get())
{
eFusion->computeFeedbackBuffers();
}
if(gui->drawRawCloud->Get())
{
eFusion->getFeedbackBuffers().at(FeedbackBuffer::RAW)->render(gui->s_cam.GetProjectionModelViewMatrix(), pose, gui->drawNormals->Get(), gui->drawColors->Get());
}
if(gui->drawFilteredCloud->Get())
{
eFusion->getFeedbackBuffers().at(FeedbackBuffer::FILTERED)->render(gui->s_cam.GetProjectionModelViewMatrix(), pose, gui->drawNormals->Get(), gui->drawColors->Get());
}
if(gui->drawGlobalModel->Get())
{
glFinish();
TICK("Global");
if(gui->drawFxaa->Get())
{
gui->drawFXAA(gui->s_cam.GetProjectionModelViewMatrix(),
gui->s_cam.GetModelViewMatrix(),
eFusion->getGlobalModel().model(),
eFusion->getConfidenceThreshold(),
eFusion->getTick(),
eFusion->getTimeDelta(),
iclnuim);
}
else
{
eFusion->getGlobalModel().renderPointCloud(gui->s_cam.GetProjectionModelViewMatrix(),
eFusion->getConfidenceThreshold(),
gui->drawUnstable->Get(),
gui->drawNormals->Get(),
gui->drawColors->Get(),
gui->drawPoints->Get(),
gui->drawWindow->Get(),
gui->drawTimes->Get(),
eFusion->getTick(),
eFusion->getTimeDelta());
}
glFinish();
TOCK("Global");
}
if(eFusion->getLost())
{
glColor3f(1, 1, 0);
}
else
{
glColor3f(1, 0, 1);
}
gui->drawFrustum(pose);
glColor3f(1, 1, 1);
if(gui->drawFerns->Get())
{
glColor3f(0, 0, 0);
for(size_t i = 0; i < eFusion->getFerns().frames.size(); i++)
{
if((int)i == eFusion->getFerns().lastClosest)
continue;
gui->drawFrustum(eFusion->getFerns().frames.at(i)->pose);
}
glColor3f(1, 1, 1);
}
if(gui->drawDefGraph->Get())
{
const std::vector<GraphNode*> & graph = eFusion->getLocalDeformation().getGraph();
for(size_t i = 0; i < graph.size(); i++)
{
pangolin::glDrawCross(graph.at(i)->position(0),
graph.at(i)->position(1),
graph.at(i)->position(2),
0.1);
for(size_t j = 0; j < graph.at(i)->neighbours.size(); j++)
{
pangolin::glDrawLine(graph.at(i)->position(0),
graph.at(i)->position(1),
graph.at(i)->position(2),
graph.at(graph.at(i)->neighbours.at(j))->position(0),
graph.at(graph.at(i)->neighbours.at(j))->position(1),
graph.at(graph.at(i)->neighbours.at(j))->position(2));
}
}
}
if(eFusion->getFerns().lastClosest != -1)
{
glColor3f(1, 0, 0);
gui->drawFrustum(eFusion->getFerns().frames.at(eFusion->getFerns().lastClosest)->pose);
glColor3f(1, 1, 1);
}
const std::vector<PoseMatch> & poseMatches = eFusion->getPoseMatches();
int maxDiff = 0;
for(size_t i = 0; i < poseMatches.size(); i++)
{
if(poseMatches.at(i).secondId - poseMatches.at(i).firstId > maxDiff)
{
maxDiff = poseMatches.at(i).secondId - poseMatches.at(i).firstId;
}
}
for(size_t i = 0; i < poseMatches.size(); i++)
{
if(gui->drawDeforms->Get())
{
if(poseMatches.at(i).fern)
{
glColor3f(1, 0, 0);
}
else
{
glColor3f(0, 1, 0);
}
for(size_t j = 0; j < poseMatches.at(i).constraints.size(); j++)
{
pangolin::glDrawLine(poseMatches.at(i).constraints.at(j).sourcePoint(0), poseMatches.at(i).constraints.at(j).sourcePoint(1), poseMatches.at(i).constraints.at(j).sourcePoint(2),
poseMatches.at(i).constraints.at(j).targetPoint(0), poseMatches.at(i).constraints.at(j).targetPoint(1), poseMatches.at(i).constraints.at(j).targetPoint(2));
}
}
}
glColor3f(1, 1, 1);
eFusion->normaliseDepth(0.3f, gui->depthCutoff->Get());
for(std::map<std::string, GPUTexture*>::const_iterator it = eFusion->getTextures().begin(); it != eFusion->getTextures().end(); ++it)
{
if(it->second->draw)
{
gui->displayImg(it->first, it->second);
}
}
eFusion->getIndexMap().renderDepth(gui->depthCutoff->Get());
gui->displayImg("ModelImg", eFusion->getIndexMap().imageTex());
gui->displayImg("Model", eFusion->getIndexMap().drawTex());
std::stringstream strs;
strs << eFusion->getGlobalModel().lastCount();
gui->totalPoints->operator=(strs.str());
std::stringstream strs2;
strs2 << eFusion->getLocalDeformation().getGraph().size();
gui->totalNodes->operator=(strs2.str());
std::stringstream strs3;
strs3 << eFusion->getFerns().frames.size();
gui->totalFerns->operator=(strs3.str());
std::stringstream strs4;
strs4 << eFusion->getDeforms();
gui->totalDefs->operator=(strs4.str());
std::stringstream strs5;
strs5 << eFusion->getTick() << "/" << logReader->getNumFrames();
gui->logProgress->operator=(strs5.str());
std::stringstream strs6;
strs6 << eFusion->getFernDeforms();
gui->totalFernDefs->operator=(strs6.str());
gui->postCall();
logReader->flipColors = gui->flipColors->Get();
eFusion->setRgbOnly(gui->rgbOnly->Get());
eFusion->setPyramid(gui->pyramid->Get());
eFusion->setFastOdom(gui->fastOdom->Get());
eFusion->setConfidenceThreshold(gui->confidenceThreshold->Get());
eFusion->setDepthCutoff(gui->depthCutoff->Get());
eFusion->setIcpWeight(gui->icpWeight->Get());
eFusion->setSo3(gui->so3->Get());
eFusion->setFrameToFrameRGB(gui->frameToFrameRGB->Get());
resetButton = pangolin::Pushed(*gui->reset);
if(gui->autoSettings)
{
static bool last = gui->autoSettings->Get();
if(gui->autoSettings->Get() != last)
{
last = gui->autoSettings->Get();
static_cast<LiveLogReader *>(logReader)->setAuto(last);
}
}
Stopwatch::getInstance().sendAll();
if(resetButton)
{
break;
}
if(pangolin::Pushed(*gui->save))
{
eFusion->savePly();
}
TOCK("GUI");
}
}
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> float
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::segmentToSegmentDist (
const std::vector <int> &base_indices,
float (&ratio)[2])
{
// get point vectors
Eigen::Vector3f u = target_->points[base_indices[1]].getVector3fMap () - target_->points[base_indices[0]].getVector3fMap ();
Eigen::Vector3f v = target_->points[base_indices[3]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
Eigen::Vector3f w = target_->points[base_indices[0]].getVector3fMap () - target_->points[base_indices[2]].getVector3fMap ();
// calculate segment distances
float a = u.dot (u);
float b = u.dot (v);
float c = v.dot (v);
float d = u.dot (w);
float e = v.dot (w);
float D = a * c - b * b;
float sN = 0.f, sD = D;
float tN = 0.f, tD = D;
// check segments
if (D < small_error_)
{
sN = 0.f;
sD = 1.f;
tN = e;
tD = c;
}
else
{
sN = (b * e - c * d);
tN = (a * e - b * d);
if (sN < 0.f)
{
sN = 0.f;
tN = e;
tD = c;
}
else if (sN > sD)
{
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.f)
{
tN = 0.f;
if (-d < 0.f)
sN = 0.f;
else if (-d > a)
sN = sD;
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD)
{
tN = tD;
if ((-d + b) < 0.f)
sN = 0.f;
else if ((-d + b) > a)
sN = sD;
else
{
sN = (-d + b);
sD = a;
}
}
// set intersection ratios
ratio[0] = (std::abs (sN) < small_error_) ? 0.f : sN / sD;
ratio[1] = (std::abs (tN) < small_error_) ? 0.f : tN / tD;
Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
return (x.norm ());
}
BlockFitter::Result BlockFitter::
go() {
Result result;
result.mSuccess = false;
if (mCloud->size() < 100) return result;
// voxelize
LabeledCloud::Ptr cloud(new LabeledCloud());
pcl::VoxelGrid<pcl::PointXYZL> voxelGrid;
voxelGrid.setInputCloud(mCloud);
voxelGrid.setLeafSize(mDownsampleResolution, mDownsampleResolution,
mDownsampleResolution);
voxelGrid.filter(*cloud);
for (int i = 0; i < (int)cloud->size(); ++i) cloud->points[i].label = i;
if (mDebug) {
std::cout << "Original cloud size " << mCloud->size() << std::endl;
std::cout << "Voxelized cloud size " << cloud->size() << std::endl;
pcl::io::savePCDFileBinary("cloud_full.pcd", *cloud);
}
if (cloud->size() < 100) return result;
// pose
cloud->sensor_origin_.head<3>() = mOrigin;
cloud->sensor_origin_[3] = 1;
Eigen::Vector3f rz = mLookDir;
Eigen::Vector3f rx = rz.cross(Eigen::Vector3f::UnitZ());
Eigen::Vector3f ry = rz.cross(rx);
Eigen::Matrix3f rotation;
rotation.col(0) = rx.normalized();
rotation.col(1) = ry.normalized();
rotation.col(2) = rz.normalized();
Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
pose.linear() = rotation;
pose.translation() = mOrigin;
// ground removal
if (mRemoveGround) {
Eigen::Vector4f groundPlane;
// filter points
float minZ = mMinGroundZ;
float maxZ = mMaxGroundZ;
if ((minZ > 10000) && (maxZ > 10000)) {
std::vector<float> zVals(cloud->size());
for (int i = 0; i < (int)cloud->size(); ++i) {
zVals[i] = cloud->points[i].z;
}
std::sort(zVals.begin(), zVals.end());
minZ = zVals[0]-0.1;
maxZ = minZ + 0.5;
}
LabeledCloud::Ptr tempCloud(new LabeledCloud());
for (int i = 0; i < (int)cloud->size(); ++i) {
const Eigen::Vector3f& p = cloud->points[i].getVector3fMap();
if ((p[2] < minZ) || (p[2] > maxZ)) continue;
tempCloud->push_back(cloud->points[i]);
}
// downsample
voxelGrid.setInputCloud(tempCloud);
voxelGrid.setLeafSize(0.1, 0.1, 0.1);
voxelGrid.filter(*tempCloud);
if (tempCloud->size() < 100) return result;
// find ground plane
std::vector<Eigen::Vector3f> pts(tempCloud->size());
for (int i = 0; i < (int)tempCloud->size(); ++i) {
pts[i] = tempCloud->points[i].getVector3fMap();
}
const float kGroundPlaneDistanceThresh = 0.01; // TODO: param
PlaneFitter planeFitter;
planeFitter.setMaxDistance(kGroundPlaneDistanceThresh);
planeFitter.setRefineUsingInliers(true);
auto res = planeFitter.go(pts);
groundPlane = res.mPlane;
if (groundPlane[2] < 0) groundPlane = -groundPlane;
if (mDebug) {
std::cout << "dominant plane: " << groundPlane.transpose() << std::endl;
std::cout << " inliers: " << res.mInliers.size() << std::endl;
}
if (std::acos(groundPlane[2]) > 30*M_PI/180) {
std::cout << "error: ground plane not found!" << std::endl;
std::cout << "proceeding with full segmentation (may take a while)..." <<
std::endl;
}
else {
// compute convex hull
result.mGroundPlane = groundPlane;
{
tempCloud.reset(new LabeledCloud());
for (int i = 0; i < (int)cloud->size(); ++i) {
Eigen::Vector3f p = cloud->points[i].getVector3fMap();
float dist = groundPlane.head<3>().dot(p) + groundPlane[3];
if (std::abs(dist) > kGroundPlaneDistanceThresh) continue;
p -= (groundPlane.head<3>()*dist);
pcl::PointXYZL cloudPt;
cloudPt.getVector3fMap() = p;
tempCloud->push_back(cloudPt);
}
pcl::ConvexHull<pcl::PointXYZL> chull;
pcl::PointCloud<pcl::PointXYZL> hull;
chull.setInputCloud(tempCloud);
chull.reconstruct(hull);
result.mGroundPolygon.resize(hull.size());
for (int i = 0; i < (int)hull.size(); ++i) {
result.mGroundPolygon[i] = hull[i].getVector3fMap();
}
}
// remove points below or near ground
tempCloud.reset(new LabeledCloud());
for (int i = 0; i < (int)cloud->size(); ++i) {
Eigen::Vector3f p = cloud->points[i].getVector3fMap();
float dist = p.dot(groundPlane.head<3>()) + groundPlane[3];
if ((dist < mMinHeightAboveGround) ||
(dist > mMaxHeightAboveGround)) continue;
float range = (p-mOrigin).norm();
if (range > mMaxRange) continue;
tempCloud->push_back(cloud->points[i]);
}
std::swap(tempCloud, cloud);
if (mDebug) {
std::cout << "Filtered cloud size " << cloud->size() << std::endl;
}
}
}
// normal estimation
auto t0 = std::chrono::high_resolution_clock::now();
if (mDebug) {
std::cout << "computing normals..." << std::flush;
}
RobustNormalEstimator normalEstimator;
normalEstimator.setMaxEstimationError(0.01);
normalEstimator.setRadius(0.1);
normalEstimator.setMaxCenterError(0.02);
normalEstimator.setMaxIterations(100);
NormalCloud::Ptr normals(new NormalCloud());
normalEstimator.go(cloud, *normals);
if (mDebug) {
auto t1 = std::chrono::high_resolution_clock::now();
auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0);
std::cout << "finished in " << dt.count()/1e3 << " sec" << std::endl;
}
// filter non-horizontal points
const float maxNormalAngle = mMaxAngleFromHorizontal*M_PI/180;
LabeledCloud::Ptr tempCloud(new LabeledCloud());
NormalCloud::Ptr tempNormals(new NormalCloud());
for (int i = 0; i < (int)normals->size(); ++i) {
const auto& norm = normals->points[i];
Eigen::Vector3f normal(norm.normal_x, norm.normal_y, norm.normal_z);
float angle = std::acos(normal[2]);
if (angle > maxNormalAngle) continue;
tempCloud->push_back(cloud->points[i]);
tempNormals->push_back(normals->points[i]);
}
std::swap(tempCloud, cloud);
std::swap(tempNormals, normals);
if (mDebug) {
std::cout << "Horizontal points remaining " << cloud->size() << std::endl;
pcl::io::savePCDFileBinary("cloud.pcd", *cloud);
pcl::io::savePCDFileBinary("robust_normals.pcd", *normals);
}
// plane segmentation
t0 = std::chrono::high_resolution_clock::now();
if (mDebug) {
std::cout << "segmenting planes..." << std::flush;
}
PlaneSegmenter segmenter;
segmenter.setData(cloud, normals);
segmenter.setMaxError(0.05);
segmenter.setMaxAngle(5);
segmenter.setMinPoints(100);
PlaneSegmenter::Result segmenterResult = segmenter.go();
if (mDebug) {
auto t1 = std::chrono::high_resolution_clock::now();
auto dt = std::chrono::duration_cast<std::chrono::milliseconds>(t1-t0);
std::cout << "finished in " << dt.count()/1e3 << " sec" << std::endl;
std::ofstream ofs("labels.txt");
for (const int lab : segmenterResult.mLabels) {
ofs << lab << std::endl;
}
ofs.close();
ofs.open("planes.txt");
for (auto it : segmenterResult.mPlanes) {
auto& plane = it.second;
ofs << it.first << " " << plane.transpose() << std::endl;
}
ofs.close();
}
// create point clouds
std::unordered_map<int,std::vector<Eigen::Vector3f>> cloudMap;
for (int i = 0; i < (int)segmenterResult.mLabels.size(); ++i) {
int label = segmenterResult.mLabels[i];
if (label <= 0) continue;
cloudMap[label].push_back(cloud->points[i].getVector3fMap());
}
struct Plane {
MatrixX3f mPoints;
Eigen::Vector4f mPlane;
};
std::vector<Plane> planes;
planes.reserve(cloudMap.size());
for (auto it : cloudMap) {
int n = it.second.size();
Plane plane;
plane.mPoints.resize(n,3);
for (int i = 0; i < n; ++i) plane.mPoints.row(i) = it.second[i];
plane.mPlane = segmenterResult.mPlanes[it.first];
planes.push_back(plane);
}
std::vector<RectangleFitter::Result> results;
for (auto& plane : planes) {
RectangleFitter fitter;
fitter.setDimensions(mBlockDimensions.head<2>());
fitter.setAlgorithm((RectangleFitter::Algorithm)mRectangleFitAlgorithm);
fitter.setData(plane.mPoints, plane.mPlane);
auto result = fitter.go();
results.push_back(result);
}
if (mDebug) {
std::ofstream ofs("boxes.txt");
for (int i = 0; i < (int)results.size(); ++i) {
auto& res = results[i];
for (auto& p : res.mPolygon) {
ofs << i << " " << p.transpose() << std::endl;
}
}
ofs.close();
ofs.open("hulls.txt");
for (int i = 0; i < (int)results.size(); ++i) {
auto& res = results[i];
for (auto& p : res.mConvexHull) {
ofs << i << " " << p.transpose() << std::endl;
}
}
ofs.close();
ofs.open("poses.txt");
for (int i = 0; i < (int)results.size(); ++i) {
auto& res = results[i];
auto transform = res.mPose;
ofs << transform.matrix() << std::endl;
}
ofs.close();
}
for (int i = 0; i < (int)results.size(); ++i) {
const auto& res = results[i];
if (mBlockDimensions.head<2>().norm() > 1e-5) {
float areaRatio = mBlockDimensions.head<2>().prod()/res.mConvexArea;
if ((areaRatio < mAreaThreshMin) ||
(areaRatio > mAreaThreshMax)) continue;
}
Block block;
block.mSize << res.mSize[0], res.mSize[1], mBlockDimensions[2];
block.mPose = res.mPose;
block.mPose.translation() -=
block.mPose.rotation().col(2)*mBlockDimensions[2]/2;
block.mHull = res.mConvexHull;
result.mBlocks.push_back(block);
}
if (mDebug) {
std::cout << "Surviving blocks: " << result.mBlocks.size() << std::endl;
}
result.mSuccess = true;
return result;
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
std::string base_frame("base_link");
geometry_msgs::PointStamped pout;
geometry_msgs::PointStamped pin;
pin.header.frame_id = msg->header.frame_id;
pin.point.x = 0; pin.point.y = 0; pin.point.z = 0;
geometry_msgs::Vector3Stamped vout;
geometry_msgs::Vector3Stamped vin;
vin.header.frame_id = base_frame;
vin.vector.x = 0; vin.vector.y = 0; vin.vector.z = 1;
float height;
Eigen::Vector3f normal;
try {
listener->transformPoint(base_frame, ros::Time(0), pin, msg->header.frame_id, pout);
height = pout.point.z;
listener->transformVector(msg->header.frame_id, ros::Time(0), vin, base_frame, vout);
normal = Eigen::Vector3f(vout.vector.x, vout.vector.y, vout.vector.z);
}
catch (tf::TransformException ex) {
ROS_INFO("%s",ex.what());
return;
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::fromROSMsg(*msg, *cloud);
Eigen::Vector3f p = -height*normal; // a point in the floor plane
float d = -p.dot(normal); // = height, d in the plane equation
obstacle_cloud->points.clear();
obstacle_cloud->points.reserve(cloud->size());
floor_cloud->points.clear();
Eigen::Vector3f temp;
float floor_dist;
pcl::PointXYZ point;
for (size_t i = 0; i < cloud->size(); ++i) {
/*temp = cloud->points[i].getVector3fMap(); // DEBUG!
if (i%640 < 300 && i%640 > 200 && i < 640*200 && i > 640*100) {
temp -= 0.06*normal;
}*/
// check signed distance to floor
floor_dist = normal.dot(cloud->points[i].getVector3fMap()) + d;
//floor_dist = normal.dot(temp) + d; // DEBUG
// if enough below, consider a stair point
if (floor_dist < below_threshold) {
temp = cloud->points[i].getVector3fMap(); // RELEASE
point.getVector3fMap() = -(d/normal.dot(temp))*temp + normal*0.11;
floor_cloud->points.push_back(point);
}
else { // add as a normal obstacle or clearing point
obstacle_cloud->points.push_back(cloud->points[i]);
}
}
sensor_msgs::PointCloud2 floor_msg;
pcl::toROSMsg(*floor_cloud, floor_msg);
floor_msg.header = msg->header;
floor_pub.publish(floor_msg);
sensor_msgs::PointCloud2 obstacle_msg;
pcl::toROSMsg(*obstacle_cloud, obstacle_msg);
obstacle_msg.header = msg->header;
obstacle_pub.publish(obstacle_msg);
}
void RegionGrowingMultiplePlaneSegmentation::segment(
const sensor_msgs::PointCloud2::ConstPtr& msg,
const sensor_msgs::PointCloud2::ConstPtr& normal_msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!done_initialization_) {
return;
}
vital_checker_->poke();
{
jsk_recognition_utils::ScopedWallDurationReporter r
= timer_.reporter(pub_latest_time_, pub_average_time_);
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
pcl::fromROSMsg(*msg, *cloud);
pcl::fromROSMsg(*normal_msg, *normal);
// concatenate fields
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
all_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloud, *normal, *all_cloud);
pcl::PointIndices::Ptr indices (new pcl::PointIndices);
for (size_t i = 0; i < all_cloud->points.size(); i++) {
if (!isnan(all_cloud->points[i].x) &&
!isnan(all_cloud->points[i].y) &&
!isnan(all_cloud->points[i].z) &&
!isnan(all_cloud->points[i].normal_x) &&
!isnan(all_cloud->points[i].normal_y) &&
!isnan(all_cloud->points[i].normal_z) &&
!isnan(all_cloud->points[i].curvature)) {
if (all_cloud->points[i].curvature < max_curvature_) {
indices->indices.push_back(i);
}
}
}
pcl::ConditionalEuclideanClustering<pcl::PointXYZRGBNormal> cec (true);
// vector of pcl::PointIndices
pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters);
cec.setInputCloud (all_cloud);
cec.setIndices(indices);
cec.setClusterTolerance(cluster_tolerance_);
cec.setMinClusterSize(min_size_);
//cec.setMaxClusterSize(max_size_);
{
boost::mutex::scoped_lock lock2(global_custom_condigion_function_mutex);
setCondifionFunctionParameter(angular_threshold_, distance_threshold_);
cec.setConditionFunction(
&RegionGrowingMultiplePlaneSegmentation::regionGrowingFunction);
//ros::Time before = ros::Time::now();
cec.segment (*clusters);
// ros::Time end = ros::Time::now();
// ROS_INFO("segment took %f sec", (before - end).toSec());
}
// Publish raw cluster information
// pcl::IndicesCluster is typdefed as std::vector<pcl::PointIndices>
jsk_recognition_msgs::ClusterPointIndices ros_clustering_result;
ros_clustering_result.cluster_indices
= pcl_conversions::convertToROSPointIndices(*clusters, msg->header);
ros_clustering_result.header = msg->header;
pub_clustering_result_.publish(ros_clustering_result);
// estimate planes
std::vector<pcl::PointIndices::Ptr> all_inliers;
std::vector<pcl::ModelCoefficients::Ptr> all_coefficients;
jsk_recognition_msgs::PolygonArray ros_polygon;
ros_polygon.header = msg->header;
for (size_t i = 0; i < clusters->size(); i++) {
pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]);
ransacEstimation(cloud, cluster,
*plane_inliers, *plane_coefficients);
if (plane_inliers->indices.size() > 0) {
jsk_recognition_utils::ConvexPolygon::Ptr convex
= jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZRGB>(
cloud, plane_inliers, plane_coefficients);
if (convex) {
if (min_area_ > convex->area() || convex->area() > max_area_) {
continue;
}
{
// check direction consistency of coefficients and convex
Eigen::Vector3f coefficient_normal(plane_coefficients->values[0],
plane_coefficients->values[1],
plane_coefficients->values[2]);
if (convex->getNormalFromVertices().dot(coefficient_normal) < 0) {
convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
}
}
// Normal should direct to origin
{
Eigen::Vector3f p = convex->getPointOnPlane();
Eigen::Vector3f n = convex->getNormal();
if (p.dot(n) > 0) {
convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
Eigen::Vector3f new_normal = convex->getNormal();
plane_coefficients->values[0] = new_normal[0];
plane_coefficients->values[1] = new_normal[1];
plane_coefficients->values[2] = new_normal[2];
plane_coefficients->values[3] = convex->getD();
}
}
geometry_msgs::PolygonStamped polygon;
polygon.polygon = convex->toROSMsg();
polygon.header = msg->header;
ros_polygon.polygons.push_back(polygon);
all_inliers.push_back(plane_inliers);
all_coefficients.push_back(plane_coefficients);
}
}
}
jsk_recognition_msgs::ClusterPointIndices ros_indices;
ros_indices.cluster_indices =
pcl_conversions::convertToROSPointIndices(all_inliers, msg->header);
ros_indices.header = msg->header;
pub_inliers_.publish(ros_indices);
jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
ros_coefficients.header = msg->header;
ros_coefficients.coefficients =
pcl_conversions::convertToROSModelCoefficients(
all_coefficients, msg->header);
pub_coefficients_.publish(ros_coefficients);
pub_polygons_.publish(ros_polygon);
}
}
void run ()
{
auto in_data_image = Fixel::open_fixel_data_file<float> (argument[0]);
if (in_data_image.size(2) != 1)
throw Exception ("Only a single scalar value for each fixel can be output as a track scalar file, "
"therefore the input fixel data file must have dimension Nx1x1");
Header in_index_header = Fixel::find_index_header (Fixel::get_fixel_directory (argument[0]));
auto in_index_image = in_index_header.get_image<uint32_t>();
auto in_directions_image = Fixel::find_directions_header (Fixel::get_fixel_directory (argument[0])).get_image<float>().with_direct_io();
DWI::Tractography::Properties properties;
DWI::Tractography::Reader<float> reader (argument[1], properties);
properties.comments.push_back ("Created using fixel2tsf");
properties.comments.push_back ("Source fixel image: " + Path::basename (argument[0]));
properties.comments.push_back ("Source track file: " + Path::basename (argument[1]));
DWI::Tractography::ScalarWriter<float> tsf_writer (argument[2], properties);
float angular_threshold = get_option_value ("angle", DEFAULT_ANGULAR_THRESHOLD);
const float angular_threshold_dp = cos (angular_threshold * (Math::pi / 180.0));
const size_t num_tracks = properties["count"].empty() ? 0 : to<int> (properties["count"]);
DWI::Tractography::Mapping::TrackMapperBase mapper (in_index_image);
mapper.set_use_precise_mapping (true);
ProgressBar progress ("mapping fixel values to streamline points", num_tracks);
DWI::Tractography::Streamline<float> tck;
Transform transform (in_index_image);
Eigen::Vector3 voxel_pos;
while (reader (tck)) {
SetVoxelDir dixels;
mapper (tck, dixels);
vector<float> scalars (tck.size(), 0.0);
for (size_t p = 0; p < tck.size(); ++p) {
voxel_pos = transform.scanner2voxel * tck[p].cast<default_type> ();
for (SetVoxelDir::const_iterator d = dixels.begin(); d != dixels.end(); ++d) {
if ((int)round(voxel_pos[0]) == (*d)[0] && (int)round(voxel_pos[1]) == (*d)[1] && (int)round(voxel_pos[2]) == (*d)[2]) {
assign_pos_of (*d).to (in_index_image);
Eigen::Vector3f dir = d->get_dir().cast<float>();
dir.normalize();
float largest_dp = 0.0;
int32_t closest_fixel_index = -1;
in_index_image.index(3) = 0;
uint32_t num_fixels_in_voxel = in_index_image.value();
in_index_image.index(3) = 1;
uint32_t offset = in_index_image.value();
for (size_t fixel = 0; fixel < num_fixels_in_voxel; ++fixel) {
in_directions_image.index(0) = offset + fixel;
float dp = std::abs (dir.dot (Eigen::Vector3f (in_directions_image.row(1))));
if (dp > largest_dp) {
largest_dp = dp;
closest_fixel_index = fixel;
}
}
if (largest_dp > angular_threshold_dp) {
in_data_image.index(0) = offset + closest_fixel_index;
scalars[p] = in_data_image.value();
} else {
scalars[p] = 0.0;
}
break;
}
}
}
tsf_writer (scalars);
progress++;
}
}
template <typename PointInT, typename PointOutT, typename PointRFT> void
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc)
{
pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0],
frames_->points[index].x_axis[1],
frames_->points[index].x_axis[2]);
//const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap ();
const Eigen::Vector3f normal (frames_->points[index].z_axis[0],
frames_->points[index].z_axis[1],
frames_->points[index].z_axis[2]);
// Find every point within specified search_radius_
std::vector<int> nn_indices;
std::vector<float> nn_dists;
const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
// For each point within radius
for (size_t ne = 0; ne < neighb_cnt; ne++)
{
if (pcl::utils::equal(nn_dists[ne], 0.0f))
continue;
// Get neighbours coordinates
Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
// ----- Compute current neighbour polar coordinates -----
// Get distance between the neighbour and the origin
float r = std::sqrt (nn_dists[ne]);
// Project point into the tangent plane
Eigen::Vector3f proj;
pcl::geometry::project (neighbour, origin, normal, proj);
proj -= origin;
// Normalize to compute the dot product
proj.normalize ();
// Compute the angle between the projection and the x axis in the interval [0,360]
Eigen::Vector3f cross = x_axis.cross (proj);
float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
/// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
Eigen::Vector3f no = neighbour - origin;
no.normalize ();
float theta = normal.dot (no);
theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));
/// Bin (j, k, l)
size_t j = 0;
size_t k = 0;
size_t l = 0;
/// Compute the Bin(j, k, l) coordinates of current neighbour
for (size_t rad = 1; rad < radius_bins_ + 1; rad++)
{
if (r <= radii_interval_[rad])
{
j = rad - 1;
break;
}
}
for (size_t ang = 1; ang < elevation_bins_ + 1; ang++)
{
if (theta <= theta_divisions_[ang])
{
k = ang - 1;
break;
}
}
for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
{
if (phi <= phi_divisions_[ang])
{
l = ang - 1;
break;
}
}
/// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
std::vector<int> neighbour_indices;
std::vector<float> neighbour_didtances;
float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
/// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
(k*radius_bins_) +
j];
assert (w >= 0.0);
if (w == std::numeric_limits<float>::infinity ())
PCL_ERROR ("Shape Context Error INF!\n");
if (w != w)
PCL_ERROR ("Shape Context Error IND!\n");
/// Accumulate w into correspondant Bin(j,k,l)
desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
} // end for each neighbour
}
template<typename PointInT, typename PointNT, typename PointOutT> bool
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
PointInTPtr & grid, pcl::PointIndices & indices)
{
Eigen::Vector3f plane_normal;
plane_normal[0] = -centroid[0];
plane_normal[1] = -centroid[1];
plane_normal[2] = -centroid[2];
Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
plane_normal.normalize ();
Eigen::Vector3f axis = plane_normal.cross (z_vector);
double rotation = -asin (axis.norm ());
axis.normalize ();
Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));
grid->points.resize (processed->points.size ());
for (size_t k = 0; k < processed->points.size (); k++)
grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap ();
pcl::transformPointCloud (*grid, *grid, transformPC);
Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);
centroid4f = transformPC * centroid4f;
normal_centroid4f = transformPC * normal_centroid4f;
Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);
Eigen::Vector4f farthest_away;
pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
farthest_away[3] = 0;
float max_dist = (farthest_away - centroid4f).norm ();
pcl::demeanPointCloud (*grid, centroid4f, *grid);
Eigen::Matrix4f center_mat;
center_mat.setIdentity (4, 4);
center_mat (0, 3) = -centroid4f[0];
center_mat (1, 3) = -centroid4f[1];
center_mat (2, 3) = -centroid4f[2];
Eigen::Matrix3f scatter;
scatter.setZero ();
float sum_w = 0.f;
//for (int k = 0; k < static_cast<intgrid->points[k].getVector3fMap ();> (grid->points.size ()); k++)
for (int k = 0; k < static_cast<int> (indices.indices.size ()); k++)
{
Eigen::Vector3f pvector = grid->points[indices.indices[k]].getVector3fMap ();
float d_k = (pvector).norm ();
float w = (max_dist - d_k);
Eigen::Vector3f diff = (pvector);
Eigen::Matrix3f mat = diff * diff.transpose ();
scatter = scatter + mat * w;
sum_w += w;
}
scatter /= sum_w;
Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
Eigen::Vector3f evx = svd.matrixV ().col (0);
Eigen::Vector3f evy = svd.matrixV ().col (1);
Eigen::Vector3f evz = svd.matrixV ().col (2);
Eigen::Vector3f evxminus = evx * -1;
Eigen::Vector3f evyminus = evy * -1;
Eigen::Vector3f evzminus = evz * -1;
float s_xplus, s_xminus, s_yplus, s_yminus;
s_xplus = s_xminus = s_yplus = s_yminus = 0.f;
//disambiguate rf using all points
for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
{
Eigen::Vector3f pvector = grid->points[k].getVector3fMap ();
float dist_x, dist_y;
dist_x = std::abs (evx.dot (pvector));
dist_y = std::abs (evy.dot (pvector));
if ((pvector).dot (evx) >= 0)
s_xplus += dist_x;
else
s_xminus += dist_x;
if ((pvector).dot (evy) >= 0)
s_yplus += dist_y;
else
s_yminus += dist_y;
}
if (s_xplus < s_xminus)
evx = evxminus;
if (s_yplus < s_yminus)
evy = evyminus;
//select the axis that could be disambiguated more easily
float fx, fy;
float max_x = static_cast<float> (std::max (s_xplus, s_xminus));
float min_x = static_cast<float> (std::min (s_xplus, s_xminus));
float max_y = static_cast<float> (std::max (s_yplus, s_yminus));
float min_y = static_cast<float> (std::min (s_yplus, s_yminus));
fx = (min_x / max_x);
fy = (min_y / max_y);
Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
if (normal3f.dot (evz) < 0)
evz = evzminus;
//if fx/y close to 1, it was hard to disambiguate
//what if both are equally easy or difficult to disambiguate, namely fy == fx or very close
float max_axis = std::max (fx, fy);
float min_axis = std::min (fx, fy);
if ((min_axis / max_axis) > axis_ratio_)
{
PCL_WARN("Both axis are equally easy/difficult to disambiguate\n");
Eigen::Vector3f evy_copy = evy;
Eigen::Vector3f evxminus = evx * -1;
Eigen::Vector3f evyminus = evy * -1;
if (min_axis > min_axis_value_)
{
//combination of all possibilities
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evxminus;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evy_copy;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
evx = evyminus;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
else
{
//1-st case (evx selected)
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
//2-nd case (evy selected)
evx = evy_copy;
evy = evx.cross (evz);
trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
}
else
{
if (fy < fx)
{
evx = evy;
fx = fy;
}
evy = evx.cross (evz);
Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
transformations.push_back (trans);
}
return true;
}
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
const PointCloudIn &input,
const std::vector<int> &nn_indices,
std::vector<float> &nn_sqr_dists,
PointCloudOut &projected_points,
NormalCloud &projected_points_normals)
{
// Compute the plane coefficients
//pcl::computePointNormal<PointInT> (*input_, nn_indices, model_coefficients, curvature);
EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
Eigen::Vector4f xyz_centroid;
// Estimate the XYZ centroid
pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
//pcl::compute3DCentroid (input, nn_indices, xyz_centroid);
pcl::computeCovarianceMatrix (input, nn_indices, xyz_centroid, covariance_matrix);
// Compute the 3x3 covariance matrix
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
Eigen::Vector4f model_coefficients;
pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector);
//model_coefficients.head<3> () = eigen_vector;
model_coefficients[0] = eigen_vector[0];
model_coefficients[1] = eigen_vector[1];
model_coefficients[2] = eigen_vector[2];
model_coefficients[3] = 0;
model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
// Projected query point
Eigen::Vector3f point = input[(*indices_)[index]].getVector3fMap ();
float distance = point.dot (model_coefficients.head<3> ()) + model_coefficients[3];
point -= distance * model_coefficients.head<3> ();
float curvature = covariance_matrix.trace ();
// Compute the curvature surface change
if (curvature != 0)
curvature = fabsf (eigen_value / curvature);
// Get a copy of the plane normal easy access
Eigen::Vector3d plane_normal = model_coefficients.head<3> ().cast<double> ();
// Vector in which the polynomial coefficients will be put
Eigen::VectorXd c_vec;
// Local coordinate system (Darboux frame)
Eigen::Vector3d v (0.0f, 0.0f, 0.0f), u (0.0f, 0.0f, 0.0f);
// Perform polynomial fit to update point and normal
////////////////////////////////////////////////////
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_)
{
// Update neighborhood, since point was projected, and computing relative
// positions. Note updating only distances for the weights for speed
std::vector<Eigen::Vector3d> de_meaned (nn_indices.size ());
for (size_t ni = 0; ni < nn_indices.size (); ++ni)
{
de_meaned[ni][0] = input_->points[nn_indices[ni]].x - point[0];
de_meaned[ni][1] = input_->points[nn_indices[ni]].y - point[1];
de_meaned[ni][2] = input_->points[nn_indices[ni]].z - point[2];
nn_sqr_dists[ni] = static_cast<float> (de_meaned[ni].dot (de_meaned[ni]));
}
// Allocate matrices and vectors to hold the data used for the polynomial fit
Eigen::VectorXd weight_vec (nn_indices.size ());
Eigen::MatrixXd P (nr_coeff_, nn_indices.size ());
Eigen::VectorXd f_vec (nn_indices.size ());
Eigen::MatrixXd P_weight; // size will be (nr_coeff_, nn_indices.size ());
Eigen::MatrixXd P_weight_Pt (nr_coeff_, nr_coeff_);
// Get local coordinate system (Darboux frame)
v = plane_normal.unitOrthogonal ();
u = plane_normal.cross (v);
// Go through neighbors, transform them in the local coordinate system,
// save height and the evaluation of the polynome's terms
double u_coord, v_coord, u_pow, v_pow;
for (size_t ni = 0; ni < nn_indices.size (); ++ni)
{
// (re-)compute weights
weight_vec (ni) = exp (-nn_sqr_dists[ni] / sqr_gauss_param_);
// transforming coordinates
u_coord = de_meaned[ni].dot (u);
v_coord = de_meaned[ni].dot (v);
f_vec (ni) = de_meaned[ni].dot (plane_normal);
// compute the polynomial's terms at the current point
int j = 0;
u_pow = 1;
for (int ui = 0; ui <= order_; ++ui)
{
v_pow = 1;
for (int vi = 0; vi <= order_ - ui; ++vi)
{
P (j++, ni) = u_pow * v_pow;
v_pow *= v_coord;
}
u_pow *= u_coord;
}
}
// Computing coefficients
P_weight = P * weight_vec.asDiagonal ();
P_weight_Pt = P_weight * P.transpose ();
c_vec = P_weight * f_vec;
P_weight_Pt.llt ().solveInPlace (c_vec);
}
switch (upsample_method_)
{
case (NONE):
{
Eigen::Vector3d normal = plane_normal;
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
{
point += (c_vec[0] * plane_normal).cast<float> ();
// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normals_)
normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
}
PointOutT aux;
aux.x = point[0];
aux.y = point[1];
aux.z = point[2];
projected_points.push_back (aux);
if (compute_normals_)
{
pcl::Normal aux_normal;
aux_normal.normal_x = static_cast<float> (normal[0]);
aux_normal.normal_y = static_cast<float> (normal[1]);
aux_normal.normal_z = static_cast<float> (normal[2]);
aux_normal.curvature = curvature;
projected_points_normals.push_back (aux_normal);
}
break;
}
case (SAMPLE_LOCAL_PLANE):
{
// Uniformly sample a circle around the query point using the radius and step parameters
for (float u_disp = -static_cast<float> (upsampling_radius_); u_disp <= upsampling_radius_; u_disp += static_cast<float> (upsampling_step_))
for (float v_disp = -static_cast<float> (upsampling_radius_); v_disp <= upsampling_radius_; v_disp += static_cast<float> (upsampling_step_))
if (u_disp*u_disp + v_disp*v_disp < upsampling_radius_*upsampling_radius_)
{
PointOutT projected_point;
pcl::Normal projected_normal;
projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec,
static_cast<int> (nn_indices.size ()),
projected_point, projected_normal);
projected_points.push_back (projected_point);
if (compute_normals_)
projected_points_normals.push_back (projected_normal);
}
break;
}
case (RANDOM_UNIFORM_DENSITY):
{
// Compute the local point density and add more samples if necessary
int num_points_to_add = static_cast<int> (floor (desired_num_points_in_radius_ / 2.0 / static_cast<double> (nn_indices.size ())));
// Just add the query point, because the density is good
if (num_points_to_add <= 0)
{
// Just add the current point
Eigen::Vector3d normal = plane_normal;
if (polynomial_fit_ && static_cast<int> (nn_indices.size ()) >= nr_coeff_ && pcl_isfinite (c_vec[0]))
{
// Projection onto MLS surface along Darboux normal to the height at (0,0)
point += (c_vec[0] * plane_normal).cast<float> ();
// Compute tangent vectors using the partial derivates evaluated at (0,0) which is c_vec[order_+1] and c_vec[1]
if (compute_normals_)
normal = plane_normal - c_vec[order_ + 1] * u - c_vec[1] * v;
}
PointOutT aux;
aux.x = point[0];
aux.y = point[1];
aux.z = point[2];
projected_points.push_back (aux);
if (compute_normals_)
{
pcl::Normal aux_normal;
aux_normal.normal_x = static_cast<float> (normal[0]);
aux_normal.normal_y = static_cast<float> (normal[1]);
aux_normal.normal_z = static_cast<float> (normal[2]);
aux_normal.curvature = curvature;
projected_points_normals.push_back (aux_normal);
}
}
else
{
// Sample the local plane
for (int num_added = 0; num_added < num_points_to_add;)
{
float u_disp = (*rng_uniform_distribution_) (),
v_disp = (*rng_uniform_distribution_) ();
// Check if inside circle; if not, try another coin flip
if (u_disp * u_disp + v_disp * v_disp > search_radius_ * search_radius_/4)
continue;
PointOutT projected_point;
pcl::Normal projected_normal;
projectPointToMLSSurface (u_disp, v_disp, u, v, plane_normal, curvature, point, c_vec,
static_cast<int> (nn_indices.size ()),
projected_point, projected_normal);
projected_points.push_back (projected_point);
if (compute_normals_)
projected_points_normals.push_back (projected_normal);
num_added ++;
}
}
break;
}
case (VOXEL_GRID_DILATION):
{
// Take all point pairs and sample space between them in a grid-fashion
// \note consider only point pairs with increasing indices
MLSResult result (plane_normal, u, v, c_vec, static_cast<int> (nn_indices.size ()), curvature);
mls_results_[index] = result;
break;
}
}
}
Eigen::Vector3f linePlaneIntersection( const Eigen::Vector3f &linePoint, const Eigen::Vector3f &lineDir, const Eigen::Vector3f &planePoint, const Eigen::Vector3f &planeNormal )
{
//source: http://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection
float d=(planePoint-linePoint).dot(planeNormal)/lineDir.dot(planeNormal);
return linePoint+d*lineDir;
}
void PointWithNormalStatistcsGenerator::computeNormalsAndSVD(PointWithNormalVector& points, PointWithNormalSVDVector& svds, const Eigen::MatrixXi& indices,
const Eigen::Matrix3f& cameraMatrix, const Eigen::Isometry3f& transform){
_integralImage.compute(indices,points);
int q=0;
int outerStep = _numThreads * _step;
PixelMapper pixelMapper;
pixelMapper.setCameraMatrix(cameraMatrix);
pixelMapper.setTransform(transform);
Eigen::Isometry3f inverseTransform = transform.inverse();
#pragma omp parallel
{
#ifdef _PWN_USE_OPENMP_
int threadNum = omp_get_thread_num();
#else // _PWN_USE_OPENMP_
int threadNum = 0;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
#endif // _PWN_USE_OPENMP_
for (int c=threadNum; c<indices.cols(); c+=outerStep) {
#ifdef _PWN_USE_OPENMP_
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver;
#endif // _PWN_USE_OPENMP_
for (int r=0; r<indices.rows(); r+=_step, q++){
int index = indices(r,c);
//cerr << "index(" << r <<"," << c << ")=" << index << endl;
if (index<0)
continue;
// determine the region
PointWithNormal& point = points[index];
PointWithNormalSVD& originalSVD = svds[index];
PointWithNormalSVD svd;
Eigen::Vector3f normal = point.normal();
Eigen::Vector3f coord = pixelMapper.projectPoint(point.point()+Eigen::Vector3f(_worldRadius, _worldRadius, 0));
svd._z=point(2);
coord.head<2>()*=(1./coord(2));
int dx = abs(c - coord[0]);
int dy = abs(r - coord[1]);
if (dx>_imageRadius)
dx = _imageRadius;
if (dy>_imageRadius)
dy = _imageRadius;
PointAccumulator acc = _integralImage.getRegion(c-dx, c+dx, r-dy, r+dy);
svd._mean=point.point();
if (acc.n()>_minPoints){
Eigen::Vector3f mean = acc.mean();
svd._mean = mean;
svd._n = acc.n();
Eigen::Matrix3f cov = acc.covariance();
eigenSolver.compute(cov);
svd._U=eigenSolver.eigenvectors();
svd._singularValues=eigenSolver.eigenvalues();
if (svd._singularValues(0) <0)
svd._singularValues(0) = 0;
/*
cerr << "\t svd.singularValues():" << svd.singularValues() << endl;
cerr << "\t svd.U():" << endl << svd.U() << endl;
//cerr << "\t svd.curvature():" << svd.curvature() << endl;
*/
normal = eigenSolver.eigenvectors().col(0).normalized();
if (normal.dot(inverseTransform * mean) > 0.0f)
normal =-normal;
svd.updateCurvature();
//cerr << "n(" << index << ") c:" << svd.curvature() << endl << point.tail<3>() << endl;
if (svd.curvature()>_maxCurvature){
//cerr << "region: " << c-dx << " " << c+dx << " " << r-dx << " " << r+dx << " points: " << acc.n() << endl;
normal.setZero();
}
} else {
normal.setZero();
svd = PointWithNormalSVD();
}
if (svd.n() > originalSVD.n()){
originalSVD = svd;
point.setNormal(normal);
}
}
}
}
}
bool SnapIt::extractPointsInsidePlanePole(geometry_msgs::PolygonStamped target_plane,
pcl::PointIndices::Ptr inliers,
EigenVector3fVector& points,
Eigen::Vector3f &n,
Eigen::Vector3f &p)
{
for (size_t i = 0; i < target_plane.polygon.points.size(); i++) {
geometry_msgs::PointStamped point_stamped, transformed_point_stamped;
point_stamped.header = target_plane.header;
point_stamped.point.x = target_plane.polygon.points[i].x;
point_stamped.point.y = target_plane.polygon.points[i].y;
point_stamped.point.z = target_plane.polygon.points[i].z;
tf_listener_->transformPoint(input_frame_id_, point_stamped, transformed_point_stamped);
Eigen::Vector3f eigen_points;
eigen_points[0] = transformed_point_stamped.point.x;
eigen_points[1] = transformed_point_stamped.point.y;
eigen_points[2] = transformed_point_stamped.point.z;
points.push_back(eigen_points);
}
// create model
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
// extract independent 3 points
Eigen::Vector3f O = points[0];
Eigen::Vector3f A = points[1];
for (size_t i = 2; i < points.size(); i++) {
Eigen::Vector3f B = points[i];
// check O, A, B on the sampe line or not...
// OA x OB
Eigen::Vector3f result = (A - O).cross(B - O);
if (result.norm() < 1.0e-08) {
// too small
continue;
}
else {
result.normalize();
// big enough
coefficients->values.resize (4);
// A(x - ox) + B(y - oy) + C(z - oz) = 0
// result = (A, B, C)
coefficients->values[0] = result[0];
coefficients->values[1] = result[1];
coefficients->values[2] = result[2];
coefficients->values[3] = O.dot(result);
n = result;
p = O;
}
}
if (coefficients->values.size() != 4) {
NODELET_ERROR("all the points of target_plane are on a line");
return false;
}
// project input_ to the plane
pcl::PointCloud<pcl::PointXYZ> cloud_projected;
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType (pcl::SACMODEL_PLANE);
proj.setInputCloud (input_);
proj.setModelCoefficients (coefficients);
proj.filter (cloud_projected);
// next, check the points inside of the plane or not...
for (size_t i = 0; i < cloud_projected.points.size(); i++) {
pcl::PointXYZ point = cloud_projected.points[i];
Eigen::Vector3f point_vector = point.getVector3fMap();
if (checkPointInsidePlane(points, n, point_vector)) {
inliers->indices.push_back(i);
}
}
//ROS_INFO("%lu points inside the plane", inliers->indices.size());
return true;
}
template<typename PointT> bool
pcl::CropHull<PointT>::rayTriangleIntersect (const PointT& point,
const Eigen::Vector3f& ray,
const Vertices& verts,
const PointCloud& cloud)
{
// Algorithm here is adapted from:
// http://softsurfer.com/Archive/algorithm_0105/algorithm_0105.htm#intersect_RayTriangle()
//
// Original copyright notice:
// Copyright 2001, softSurfer (www.softsurfer.com)
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
//
assert (verts.vertices.size () == 3);
const Eigen::Vector3f p = point.getVector3fMap ();
const Eigen::Vector3f a = cloud[verts.vertices[0]].getVector3fMap ();
const Eigen::Vector3f b = cloud[verts.vertices[1]].getVector3fMap ();
const Eigen::Vector3f c = cloud[verts.vertices[2]].getVector3fMap ();
const Eigen::Vector3f u = b - a;
const Eigen::Vector3f v = c - a;
const Eigen::Vector3f n = u.cross (v);
const float n_dot_ray = n.dot (ray);
if (std::fabs (n_dot_ray) < 1e-9)
return (false);
const float r = n.dot (a - p) / n_dot_ray;
if (r < 0)
return (false);
const Eigen::Vector3f w = p + r * ray - a;
const float denominator = u.dot (v) * u.dot (v) - u.dot (u) * v.dot (v);
const float s_numerator = u.dot (v) * w.dot (v) - v.dot (v) * w.dot (u);
const float s = s_numerator / denominator;
if (s < 0 || s > 1)
return (false);
const float t_numerator = u.dot (v) * w.dot (u) - u.dot (u) * w.dot (v);
const float t = t_numerator / denominator;
if (t < 0 || s+t > 1)
return (false);
return (true);
}
float mrpt::pbmap::dist3D_Segment_to_Segment2(Segment S1, Segment S2)
{
Eigen::Vector3f u = diffPoints(S1.P1, S1.P0);
Eigen::Vector3f v = diffPoints(S2.P1, S2.P0);
Eigen::Vector3f w = diffPoints(S1.P0, S2.P0);
float a = u.dot(u); // always >= 0
float b = u.dot(v);
float c = v.dot(v); // always >= 0
float d = u.dot(w);
float e = v.dot(w);
float D = a * c - b * b; // always >= 0
float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < SMALL_NUM)
{ // the lines are almost parallel
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else
{ // get the closest points on the infinite lines
sN = (b * e - c * d);
tN = (a * e - b * d);
if (sN < 0.0)
{ // sc < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD)
{ // sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0)
{ // tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0)
sN = 0.0;
else if (-d > a)
sN = sD;
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD)
{ // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else
{
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (fabs(sN) < SMALL_NUM ? 0.0 : sN / sD);
tc = (fabs(tN) < SMALL_NUM ? 0.0 : tN / tD);
// get the difference of the two closest points
Eigen::Vector3f dP = w + (sc * u) - (tc * v); // = S1(sc) - S2(tc)
return dP.squaredNorm(); // return the closest distance
}
// Returns squared mahalanobis distance
float Reco::mahalanobis_distance (Eigen::Matrix3f cov, Eigen::Vector3f mean, pcl::PointXYZ pt)
{
Eigen::Vector3f diff (pt.x - mean[0], pt.y - mean[1], pt.z - mean[2]);
//return diff.dot(cov.inverse() * diff);
return sqrt(diff.dot(cov.inverse() * diff));
}
/** Estimate monocular visual odometry.
* @param std::vector<Match> vector with matches
* @param Eigen::Matrix3f& (output) estimated rotation matrix
* @param Eigen::Vector3f& (output) estimated translation vector
* @param bool show optical flow (true), don't show otherwise
* @param std::vector<Match> output vector with all inlier matches
* @param std::vector<Eigen::Vector3f> output vector with 3D points, triangulated from all inlier matches
* @return bool true is motion successfully estimated, false otherwise */
bool MonoOdometer8::estimateMotion(std::vector<Match> matches, Eigen::Matrix3f &R, Eigen::Vector3f &t, bool showOpticalFlow, std::vector<Match> &inlierMatches, std::vector<Eigen::Vector3f> &points3D)
{
// check number of correspondences
int N = matches.size();
if(N < param_odometerMinNumberMatches_)
{
// too few matches to compute F
R = Eigen::Matrix3f::Identity();
t << 0.0, 0.0, 0.0;
return false;
}
// normalize 2D features
Eigen::Matrix3f NormTPrev, NormTCurr;
std::vector<Match> matchesNorm = normalize2DPoints(matches, NormTPrev, NormTCurr);
Eigen::Matrix3f F, E;
std::vector<int> inlierIndices;
// RANSAC loop
for(int j=0; j<param_odometerRansacIters_; j++)
{
// get random sample
std::vector<int> chosenIndices = getRandomSample(matchesNorm.size(), 8);
// compute fundamental matrix
F = getF(matchesNorm, chosenIndices);
// get inliers
std::vector<int> inlierIndicesCurr = getInliers(matchesNorm, F);
if(inlierIndicesCurr.size() > inlierIndices.size())
{
inlierIndices = inlierIndicesCurr;
}
}
// check number of inliers
if(inlierIndices.size() < param_odometerMinNumberMatches_)
{
R = Eigen::Matrix3f::Identity();
t << 0.0, 0.0, 0.0;
return false;
}
// compute fundamental matrix out of all inliers
F = getF(matchesNorm, inlierIndices);
// save inlier and outlier matches
std::vector<Match> outlierMatches;
for(int i=0; i<matches.size(); i++)
{
if(elemInVec(inlierIndices, i))
{
inlierMatches.push_back(matches[i]);
}
else
{
outlierMatches.push_back(matches[i]);
}
}
// plot optical flow and print #inliers (for debugging)
if(showOpticalFlow)
{
cv::Mat image(1024, 768, CV_8UC1, cv::Scalar(0));
cv::Mat of1 = highlightOpticalFlow(image, inlierMatches, cv::Scalar(0, 255, 0));
cv::Mat of2 = highlightOpticalFlow(of1, outlierMatches, cv::Scalar(0, 0, 255));
cv::namedWindow("Optical flow", CV_WINDOW_AUTOSIZE);
cv::imshow("Optical flow", of2);
cv::waitKey(10);
}
// denormalize F
F = NormTCurr.transpose() * F * NormTPrev;
// compute essential matrix E
E = F2E(F);
// get rotation and translation and triangulate points
Eigen::Matrix<float, 4, Eigen::Dynamic> points3DMat;
E2Rt(E, inlierMatches, R, t, points3DMat);
// normalize 3D points (force last coordinate to 0)
for(int j=0; j<points3DMat.cols(); j++)
{
Eigen::Vector3f pt = points3DMat.block<3, 1>(0, j);
double lastCoord = points3DMat(3, j);
pt = pt / lastCoord;
if(pt(2) > 0)
{
points3D.push_back(pt);
}
else
{
// remove match if not a valid point
inlierMatches.erase(inlierMatches.begin() + j);
}
}
// check number of valid points
if(points3D.size() < param_odometerMinNumberMatches_)
{
R = Eigen::Matrix3f::Identity();
t << 0.0, 0.0, 0.0;
return false;
}
// inforce translation norm to 1
t = t / sqrt(t.dot(t));
return true;
}