pcl::ihs::ICP::CloudNormalConstPtr pcl::ihs::ICP::selectModelPoints (const CloudModelConstPtr& cloud_model, const Transformation& T_init_inv) const { const CloudNormalPtr cloud_model_out (new CloudNormal ()); cloud_model_out->reserve (cloud_model->size ()); CloudModel::const_iterator it_in = cloud_model->begin (); for (; it_in!=cloud_model->end (); ++it_in) { // Don't consider points that are facing away from the cameara. if ((T_init_inv * it_in->getNormalVector4fMap ()).z () < 0.f) { PointNormal pt; pt.getVector4fMap () = it_in->getVector4fMap (); pt.getNormalVector4fMap () = it_in->getNormalVector4fMap (); // NOTE: Not the transformed points!! cloud_model_out->push_back (pt); } } // Shrink to fit ("Scott Meyers swap trick") CloudNormal (*cloud_model_out).swap (*cloud_model_out); return (cloud_model_out); }
pcl::ihs::ICP::CloudNormalConstPtr pcl::ihs::ICP::selectDataPoints (const CloudProcessedConstPtr& cloud_data) const { const CloudNormalPtr cloud_data_out (new CloudNormal ()); cloud_data_out->reserve (cloud_data->size ()); CloudProcessed::const_iterator it_in = cloud_data->begin (); for (; it_in!=cloud_data->end (); ++it_in) { if (pcl::isFinite (*it_in)) { PointNormal pt; pt.getVector4fMap () = it_in->getVector4fMap (); pt.getNormalVector4fMap () = it_in->getNormalVector4fMap (); cloud_data_out->push_back (pt); } } // Shrink to fit ("Scott Meyers swap trick") CloudNormal (*cloud_data_out).swap (*cloud_data_out); return (cloud_data_out); }
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); }
void NormalHoughProposer::houghVote(Entry &query, Entry &target, bin_t& bins) { // Compute the reference point for the R-table Eigen::Vector4f centroid4; compute3DCentroid(*(target.cloud), centroid4); Eigen::Vector3f centroid(centroid4[0], centroid4[1], centroid4[2]); assert(query.keypoints->size() == query.features->size()); assert(target.keypoints->size() == target.features->size()); // Figure out bin dimension Eigen::Vector4f query_min4, query_max4; getMinMax3D(*query.cloud, query_min4, query_max4); Eigen::Vector3f query_min(query_min4[0], query_min4[1], query_min4[2]); Eigen::Vector3f query_max(query_max4[0], query_max4[1], query_max4[2]); Eigen::Affine3f t; getTransformation(0, 0, 0, M_PI, 0.5, 1.5, t); int correctly_matched = 0; for (unsigned int i = 0; i < query.keypoints->size(); i++) { std::vector<int> feature_indices; std::vector<float> sqr_distances; int num_correspondences = 2; if (!pcl_isfinite (query.features->points.row(i)(0))) continue; int num_found = target.tree->nearestKSearch(*query.features, i, num_correspondences, feature_indices, sqr_distances); for (int j = 0; j < num_found; j++) { // For each one of the feature correspondences int feature_index = feature_indices[j]; Eigen::Vector3f query_keypoint = query.keypoints->at(i).getVector3fMap(); Eigen::Vector3f target_keypoint = target.keypoints->at(feature_index).getVector3fMap(); target_keypoint = t * target_keypoint; if ((query_keypoint - target_keypoint).norm() < 0.05) { correctly_matched++; } // Get corresponding target keypoint, and calculate its r to its centroid PointNormal correspondence = target.keypoints->at(feature_index); // Since features correspond to the keypoints Eigen::Vector3f r = correspondence.getVector3fMap() - centroid; // Calculate the rotation transformation from the target normal to the query normal Eigen::Vector3f target_normal = correspondence.getNormalVector3fMap(); target_normal.normalize(); Eigen::Vector3f query_normal = query.keypoints->at(i).getNormalVector3fMap(); query_normal.normalize(); double angle = acos( target_normal.dot(query_normal) / (target_normal.norm() * query_normal.norm()) ); Eigen::Vector3f axis = target_normal.normalized().cross(query_normal.normalized()); axis.normalize(); Eigen::Affine3f rot_transform; rot_transform = Eigen::AngleAxisf (angle, axis); // Check that the rotation matrix is correct Eigen::Vector3f projected = rot_transform * target_normal; projected.normalize(); // Transform r based on the difference between the normals Eigen::Vector3f transformed_r = rot_transform * r; for (int k = 0; k < num_angles_; k++) { float query_angle = (float(k) / num_angles_) * 2.0f * float (M_PI); Eigen::Affine3f query_rot; query_rot = Eigen::AngleAxisf(query_angle, query_normal.normalized()); Eigen::Vector3f guess_r = query_rot * transformed_r; Eigen::Vector3f centroid_est = query.keypoints->at(i).getVector3fMap() - guess_r; Eigen::Vector3f region = query_max - query_min; Eigen::Vector3f bin_size = region / float (bins_); Eigen::Vector3f diff = (centroid_est - query_min); Eigen::Vector3f indices = diff.cwiseQuotient(bin_size); castVotes(indices, bins); } } } }