TEST (PCL, compute3DCentroidCloudIterator) { pcl::PointIndices pindices; std::vector<int> indices; PointXYZ point; PointCloud<PointXYZ> cloud; Eigen::Vector4f centroid_f; for (point.x = -1; point.x < 2; point.x += 2) { for (point.y = -1; point.y < 2; point.y += 2) { for (point.z = -1; point.z < 2; point.z += 2) { cloud.push_back (point); } } } cloud.is_dense = true; indices.resize (4); // only positive y values indices [0] = 2; indices [1] = 3; indices [2] = 6; indices [3] = 7; // Test finite data { ConstCloudIterator<PointXYZ> it (cloud, indices); EXPECT_EQ (compute3DCentroid (it, centroid_f), 4); EXPECT_EQ (centroid_f[0], 0.0f); EXPECT_EQ (centroid_f[1], 1.0f); EXPECT_EQ (centroid_f[2], 0.0f); EXPECT_EQ (centroid_f[3], 1.0f); Eigen::Vector4d centroid_d; it.reset (); EXPECT_EQ (compute3DCentroid (it, centroid_d), 4); EXPECT_EQ (centroid_d[0], 0.0); EXPECT_EQ (centroid_d[1], 1.0); EXPECT_EQ (centroid_d[2], 0.0); EXPECT_EQ (centroid_d[3], 1.0); } // Test for non-finite data { point.getVector3fMap() << std::numeric_limits<float>::quiet_NaN (), std::numeric_limits<float>::quiet_NaN (), std::numeric_limits<float>::quiet_NaN (); cloud.push_back (point); cloud.is_dense = false; ConstCloudIterator<PointXYZ> it (cloud); EXPECT_EQ (8, compute3DCentroid (it, centroid_f)); EXPECT_EQ_VECTORS (Eigen::Vector4f (0.f, 0.f, 0.f, 1.f), centroid_f); } }
double BearingAngleImage::getAngle (const PointXYZ &point1, const PointXYZ &point2) { double a, b, c; double theta; const Eigen::Vector3d& p1 = point1.getVector3dMap (); const Eigen::Vector3d& p2 = point2.getVector3dMap (); a = p1.squaredNorm (); b = (p1 - p2).squaredNorm (); c = p2.squaredNorm (); if (a != 0 && b != 0) { theta = acos ((a + b - c) / (2 * sqrt (a) * sqrt (b))) * 180 / M_PI; } else { theta = 0.0; } return theta; }
TEST (PCL, Matrix4Affine3Transform) { float rot_x = 2.8827f; float rot_y = -0.31190f; float rot_z = -0.93058f; Eigen::Affine3f affine; pcl::getTransformation (0, 0, 0, rot_x, rot_y, rot_z, affine); EXPECT_NEAR (affine (0, 0), 0.56854731f, 1e-4); EXPECT_NEAR (affine (0, 1), -0.82217032f, 1e-4); EXPECT_NEAR (affine (0, 2), -0.028107658f, 1e-4); EXPECT_NEAR (affine (1, 0), -0.76327348f, 1e-4); EXPECT_NEAR (affine (1, 1), -0.51445758f, 1e-4); EXPECT_NEAR (affine (1, 2), -0.39082864f, 1e-4); EXPECT_NEAR (affine (2, 0), 0.30686751f, 1e-4); EXPECT_NEAR (affine (2, 1), 0.24365838f, 1e-4); EXPECT_NEAR (affine (2, 2), -0.920034f, 1e-4); // Approximative!!! Uses SVD internally! See http://eigen.tuxfamily.org/dox/Transform_8h_source.html Eigen::Matrix3f rotation = affine.rotation (); EXPECT_NEAR (rotation (0, 0), 0.56854731f, 1e-4); EXPECT_NEAR (rotation (0, 1), -0.82217032f, 1e-4); EXPECT_NEAR (rotation (0, 2), -0.028107658f, 1e-4); EXPECT_NEAR (rotation (1, 0), -0.76327348f, 1e-4); EXPECT_NEAR (rotation (1, 1), -0.51445758f, 1e-4); EXPECT_NEAR (rotation (1, 2), -0.39082864f, 1e-4); EXPECT_NEAR (rotation (2, 0), 0.30686751f, 1e-4); EXPECT_NEAR (rotation (2, 1), 0.24365838f, 1e-4); EXPECT_NEAR (rotation (2, 2), -0.920034f, 1e-4); float trans_x, trans_y, trans_z; pcl::getTransformation (0.1f, 0.2f, 0.3f, rot_x, rot_y, rot_z, affine); pcl::getTranslationAndEulerAngles (affine, trans_x, trans_y, trans_z, rot_x, rot_y, rot_z); EXPECT_FLOAT_EQ (trans_x, 0.1f); EXPECT_FLOAT_EQ (trans_y, 0.2f); EXPECT_FLOAT_EQ (trans_z, 0.3f); EXPECT_FLOAT_EQ (rot_x, 2.8827f); EXPECT_FLOAT_EQ (rot_y, -0.31190f); EXPECT_FLOAT_EQ (rot_z, -0.93058f); Eigen::Matrix4f transformation (Eigen::Matrix4f::Identity ()); transformation.block<3, 3> (0, 0) = affine.rotation (); transformation.block<3, 1> (0, 3) = affine.translation (); PointXYZ p (1.f, 2.f, 3.f); Eigen::Vector3f v3 = p.getVector3fMap (); Eigen::Vector4f v4 = p.getVector4fMap (); Eigen::Vector3f v3t (affine * v3); Eigen::Vector4f v4t (transformation * v4); PointXYZ pt = pcl::transformPoint (p, affine); EXPECT_NEAR (pt.x, v3t.x (), 1e-4); EXPECT_NEAR (pt.x, v4t.x (), 1e-4); EXPECT_NEAR (pt.y, v3t.y (), 1e-4); EXPECT_NEAR (pt.y, v4t.y (), 1e-4); EXPECT_NEAR (pt.z, v3t.z (), 1e-4); EXPECT_NEAR (pt.z, v4t.z (), 1e-4); PointNormal pn; pn.getVector3fMap () = p.getVector3fMap (); pn.getNormalVector3fMap () = Eigen::Vector3f (0.60f, 0.48f, 0.64f); Eigen::Vector3f n3 = pn.getNormalVector3fMap (); Eigen::Vector4f n4 = pn.getNormalVector4fMap (); Eigen::Vector3f n3t (affine.rotation() * n3); Eigen::Vector4f n4t (transformation * n4); PointNormal pnt = pcl::transformPointWithNormal (pn, affine); EXPECT_NEAR (pnt.x, v3t.x (), 1e-4); EXPECT_NEAR (pnt.x, v4t.x (), 1e-4); EXPECT_NEAR (pnt.y, v3t.y (), 1e-4); EXPECT_NEAR (pnt.y, v4t.y (), 1e-4); EXPECT_NEAR (pnt.z, v3t.z (), 1e-4); EXPECT_NEAR (pnt.z, v4t.z (), 1e-4); EXPECT_NEAR (pnt.normal_x, n3t.x (), 1e-4); EXPECT_NEAR (pnt.normal_x, n4t.x (), 1e-4); EXPECT_NEAR (pnt.normal_y, n3t.y (), 1e-4); EXPECT_NEAR (pnt.normal_y, n4t.y (), 1e-4); EXPECT_NEAR (pnt.normal_z, n3t.z (), 1e-4); EXPECT_NEAR (pnt.normal_z, n4t.z (), 1e-4); PointCloud<PointXYZ> c, ct; c.push_back (p); pcl::transformPointCloud (c, ct, affine); EXPECT_FLOAT_EQ (pt.x, ct[0].x); EXPECT_FLOAT_EQ (pt.y, ct[0].y); EXPECT_FLOAT_EQ (pt.z, ct[0].z); pcl::transformPointCloud (c, ct, transformation); EXPECT_FLOAT_EQ (pt.x, ct[0].x); EXPECT_FLOAT_EQ (pt.y, ct[0].y); EXPECT_FLOAT_EQ (pt.z, ct[0].z); affine = transformation; std::vector<int> indices (1); indices[0] = 0; pcl::transformPointCloud (c, indices, ct, affine); EXPECT_FLOAT_EQ (pt.x, ct[0].x); EXPECT_FLOAT_EQ (pt.y, ct[0].y); EXPECT_FLOAT_EQ (pt.z, ct[0].z); pcl::transformPointCloud (c, indices, ct, transformation); EXPECT_FLOAT_EQ (pt.x, ct[0].x); EXPECT_FLOAT_EQ (pt.y, ct[0].y); EXPECT_FLOAT_EQ (pt.z, ct[0].z); }
#include <iostream> #include <Eigen/Dense> #include <typeinfo> #include <regex> // My includes #include "pointxyz.h" #include "pointcloud.h" #include "filereader.h" #include "filewriter.h" #include "pointbasedregistration.h" // Test PointXYZ template TEST_CASE("Successful manipulation of the PointXYZ template", "[PointXYZ]") { PointXYZ<> a; PointXYZ<float> b; PointXYZ<float> c; PointXYZ<double> d; a.x(1.0); a.y(2.0); a.z(3.0); b[0] = 4.0; b[1] = 7.0; b[2] = 12.0; SECTION("Retrieve and set point coordinates") { REQUIRE(a.x() == 1.0); REQUIRE(a.y() == 2.0);