void FeatureCalculate::pca(CloudPtr cloud,PlanSegment &plane)
{
Eigen::MatrixXf X(3,cloud->size());
double avr_x=0.0,avr_y=0.0,avr_z=0.0;
for (int i = 0; i <cloud->size(); i++) {
avr_x = avr_x * double(i) / (i + 1) + cloud->points[i].x / double(i + 1);
avr_y = avr_y * double(i) / (i + 1) + cloud->points[i].y / double(i + 1);
avr_z = avr_z * double(i) / (i + 1) + cloud->points[i].z / double(i + 1);
}
for (int i=0;i<cloud->size();i++)
{
X(0,i)=cloud->points[i].x-avr_x;
X(1,i)=cloud->points[i].y-avr_y;
X(2,i)=cloud->points[i].z-avr_z;
}
Eigen::MatrixXf XT(cloud->size(),3);
XT=X.transpose();
Eigen::Matrix3f XXT;
XXT=X*XT;
EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
pcl::eigen33 (XXT, eigen_value, eigen_vector);
plane.normal.normal_x = eigen_vector [0];
plane.normal.normal_y = eigen_vector [1];
plane.normal.normal_z = eigen_vector [2];
plane.distance = - (plane.normal.normal_x * avr_x + plane.normal.normal_y * avr_y + plane.normal.normal_z * avr_z);
float eig_sum = XXT.coeff (0) + XXT.coeff (4) + XXT.coeff (8);
plane.min_value=eigen_value;
if (eig_sum != 0)
plane.normal.curvature = fabsf (eigen_value / eig_sum);
else
plane.normal.curvature = 0.0;
}
int
main (int argc, char **argv)
{
double dist = 0.1;
pcl::console::parse_argument (argc, argv, "-d", dist);
double rans = 0.1;
pcl::console::parse_argument (argc, argv, "-r", rans);
int iter = 100;
pcl::console::parse_argument (argc, argv, "-i", iter);
pcl::registration::ELCH<PointType> elch;
pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
icp->setMaximumIterations (iter);
icp->setMaxCorrespondenceDistance (dist);
icp->setRANSACOutlierRejectionThreshold (rans);
elch.setReg (icp);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
elch.addPointCloud (clouds[i].second);
}
int first = 0, last = 0;
for (size_t i = 0; i < clouds.size (); i++)
{
if (loopDetection (int (i), clouds, 3.0, first, last))
{
std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
elch.setLoopStart (first);
elch.setLoopEnd (last);
elch.compute ();
}
}
for (const auto &cloud : clouds)
{
std::string result_filename (cloud.first);
result_filename = result_filename.substr (result_filename.rfind ('/') + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(cloud.second));
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
void detection::GraspPointDetector::detect(const CloudPtr &input_object)
{
clock_t beginCallback = clock();
// If no cloud or no new images since last time, do nothing.
if (input_object->size() == 0) return;
world_obj_ = input_object->makeShared();
pub_cluster_.publish(world_obj_);
// Check if computation succeeded
if (doProcessing())
ROS_INFO("[%g ms] Detection Success", durationMillis(beginCallback));
else
ROS_ERROR("[%g ms] Detection Failed", durationMillis(beginCallback));
}
int
main (int argc, char **argv)
{
pcl::registration::ELCH<PointType> elch;
pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
icp->setMaximumIterations (100);
icp->setMaxCorrespondenceDistance (0.1);
icp->setRANSACOutlierRejectionThreshold (0.1);
elch.setReg (icp);
CloudVector clouds;
for (int i = 1; i < argc; i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[i], *pc);
clouds.push_back (CloudPair (argv[i], pc));
std::cout << "loading file: " << argv[i] << " size: " << pc->size () << std::endl;
elch.addPointCloud (clouds[i-1].second);
}
int first = 0, last = 0;
for (size_t i = 0; i < clouds.size (); i++)
{
if (loopDetection (int (i), clouds, 3.0, first, last))
{
std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
elch.setLoopStart (first);
elch.setLoopEnd (last);
elch.compute ();
}
}
for (size_t i = 0; i < clouds.size (); i++)
{
std::string result_filename (clouds[i].first);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
void FeatureCalculate::rpca_plane(CloudPtr cloud, PlanSegment &plane_rpca, float Pr, float epi,float reliability)
{
CloudPtr cloud_h(new Cloud);
CloudPtr cloud_final(new Cloud);
int iteration_number;
int h_free;
int point_num=cloud->size();
vector<PlanePoint> plane_points;
plane_points.resize(point_num);
h_free = int(reliability * point_num);
iteration_number = compute_iteration_number(Pr, epi, h_free);
vector<PlanSegment> planes;
for (int i = 0; i < iteration_number; i++) {
CloudPtr points(new Cloud);
int num[3];
for (int n = 0; n < 3; n++) {
num[n] = rand() % point_num;
points->push_back(cloud->points[num[n]]);
}
if (num[0] == num[1] || num[0] == num[2] || num[1] == num[2])
continue;
PlanSegment a_plane;
pca(points,a_plane);
for (int n = 0; n < point_num; n++) {
plane_points[n].dis =compute_distance_from_point_to_plane(cloud->points[n],a_plane);
plane_points[n].point=cloud->points[n];
}
std::sort(plane_points.begin(), plane_points.end(), CmpDis);
for (int n = 0; n < h_free; n++) {
CloudItem temp_point=plane_points[n].point;
cloud_h->points.push_back(temp_point);
}
pca(cloud_h,a_plane);
cloud_h->points.clear();
planes.push_back(a_plane);
}
sort(planes.begin(), planes.end(), Cmpmin_value);
PlanSegment final_plane;
final_plane = planes[0];
planes.clear();
final_plane.points_id=plane_rpca.points_id;
plane_rpca=final_plane;
}
int main (int argc, char **argv)
{
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
pcl::PointCloud<pcl::PointXYZRGB>::Ptr sum_clouds (new pcl::PointCloud<pcl::PointXYZRGB>);
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
*sum_clouds += *pc;
}
std::string result_filename ("merged");
result_filename += getTimeAsString();
result_filename += ".pcd";
pcl::io::savePCDFileASCII (result_filename.c_str (), *sum_clouds);
std::cout << "saving result to " << result_filename << std::endl;
return 0;
}
int
main (int argc, char **argv)
{
double dist = 2.5;
pcl::console::parse_argument (argc, argv, "-d", dist);
int iter = 10;
pcl::console::parse_argument (argc, argv, "-i", iter);
int lumIter = 1;
pcl::console::parse_argument (argc, argv, "-l", lumIter);
double loopDist = 5.0;
pcl::console::parse_argument (argc, argv, "-D", loopDist);
int loopCount = 20;
pcl::console::parse_argument (argc, argv, "-c", loopCount);
pcl::registration::LUM<PointType> lum;
lum.setMaxIterations (lumIter);
lum.setConvergenceThreshold (0.001f);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
lum.addPointCloud (clouds[i].second);
}
for (int i = 0; i < iter; i++)
{
for (size_t i = 1; i < clouds.size (); i++)
for (size_t j = 0; j < i; j++)
{
Eigen::Vector4f ci, cj;
pcl::compute3DCentroid (*(clouds[i].second), ci);
pcl::compute3DCentroid (*(clouds[j].second), cj);
Eigen::Vector4f diff = ci - cj;
//std::cout << i << " " << j << " " << diff.norm () << std::endl;
if(diff.norm () < loopDist && (i - j == 1 || i - j > loopCount))
{
if(i - j > loopCount)
std::cout << "add connection between " << i << " (" << clouds[i].first << ") and " << j << " (" << clouds[j].first << ")" << std::endl;
pcl::registration::CorrespondenceEstimation<PointType, PointType> ce;
ce.setInputTarget (clouds[i].second);
ce.setInputSource (clouds[j].second);
pcl::CorrespondencesPtr corr (new pcl::Correspondences);
ce.determineCorrespondences (*corr, dist);
if (corr->size () > 2)
lum.setCorrespondences (j, i, corr);
}
}
lum.compute ();
for(size_t i = 0; i < lum.getNumVertices (); i++)
{
//std::cout << i << ": " << lum.getTransformation (i) (0, 3) << " " << lum.getTransformation (i) (1, 3) << " " << lum.getTransformation (i) (2, 3) << std::endl;
clouds[i].second = lum.getTransformedCloud (i);
}
}
for(size_t i = 0; i < lum.getNumVertices (); i++)
{
std::string result_filename (clouds[i].first);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
//std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
void FeatureCalculate::rpca(CloudPtr cloud,NormalPtr normals, int num_of_neighbors, float Pr, float epi,float reliability)
{
pcl::KdTreeFLANN<CloudItem> kdtree;
kdtree.setInputCloud(cloud);
normals->resize(cloud->size());
#pragma omp parallel for
for (int j = 0; j < cloud->size(); j++) {
CloudPtr cloud_h(new Cloud);
CloudPtr cloud_final(new Cloud);
vector<int> point_id_search;
vector<float> point_distance;
int point_num = kdtree.nearestKSearch(j,num_of_neighbors,point_id_search,point_distance);
point_distance.clear();
vector<PlanePoint> plane_points;
plane_points.resize(point_num);
if (point_num > 3) {
int iteration_number;
int h_free;
h_free = int(reliability * point_num);
iteration_number = compute_iteration_number(Pr, epi, h_free);
vector<PlanSegment> planes;
for (int i = 0; i < iteration_number; i++) {
CloudPtr points(new Cloud);
int num[3];
for (int n = 0; n < 3; n++) {
num[n] = rand() % point_num;
points->push_back(cloud->points[point_id_search[num[n]]]);
}
if (num[0] == num[1] || num[0] == num[2] || num[1] == num[2])
continue;
PlanSegment a_plane;
pca(points,a_plane);
for (int n = 0; n < point_num; n++) {
plane_points[n].dis =compute_distance_from_point_to_plane(cloud->points[point_id_search[n]],a_plane);
plane_points[n].point=cloud->points[point_id_search[n]];
}
std::sort(plane_points.begin(), plane_points.end(), CmpDis);
for (int n = 0; n < h_free; n++) {
CloudItem temp_point=plane_points[n].point;
cloud_h->points.push_back(temp_point);
}
pca(cloud_h,a_plane);
cloud_h->points.clear();
planes.push_back(a_plane);
}
sort(planes.begin(), planes.end(), Cmpmin_value);
PlanSegment final_plane;
final_plane = planes[0];
planes.clear();
vector<float> distance;
vector<float> distance_sort;
for (int n = 0; n < point_num; n++) {
float dis_from_point_plane;
dis_from_point_plane =compute_distance_from_point_to_plane(cloud->points[point_id_search[n]],final_plane);
distance.push_back(dis_from_point_plane);
distance_sort.push_back(dis_from_point_plane);
}
sort(distance_sort.begin(), distance_sort.end());
float distance_median;
distance_median = distance_sort[point_num / 2];
distance_sort.clear();
float MAD;
vector<float> temp_MAD;
for (int n = 0; n < point_num; n++) {
temp_MAD.push_back(abs(distance[n] - distance_median));
}
sort(temp_MAD.begin(), temp_MAD.end());
MAD = 1.4826 * temp_MAD[point_num / 2];
if (MAD == 0) {
for (int n = 0; n < point_num; n++) {
CloudItem temp_point=cloud->points[point_id_search[n]];
cloud_final->points.push_back(temp_point);
}
} else {
for (int n = 0; n < point_num; n++) {
float Rz;
Rz = (abs(distance[n] - distance_median)) / MAD;
if (Rz < 2.5) {
CloudItem temp_point=cloud->points[point_id_search[n]];
cloud_final->points.push_back(temp_point);
}
}
}
point_id_search.clear();
if (cloud_final->points.size() > 3)
{
pca(cloud_final,final_plane);
normals->points[j]= final_plane.normal;
} else {
normals->points[j].normal_x = 0.0;
normals->points[j].normal_y = 0.0;
normals->points[j].normal_z = 0.0;
normals->points[j].curvature = 1.0;
}
cloud_final->clear();
} else {
normals->points[j].normal_x = 0.0;
normals->points[j].normal_y = 0.0;
normals->points[j].normal_z = 0.0;
normals->points[j].curvature = 1.0;
}
}
}
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);
}
}
float get_rot_icp_internal(CloudPtr cloud_src, CloudPtr cloud_temp, Matrix4f& mat_rot){
int verbose = 0;
bool do_scale = false;
bool do_affine = false;
bool bulkmode = false;
TriMesh::set_verbose(verbose);
TriMesh* mesh1 = new TriMesh();
TriMesh* mesh2 = new TriMesh();
mesh1->vertices.resize(cloud_src->size());
mesh2->vertices.resize(cloud_temp->size());
for (int i = 0; i < cloud_src->size(); i++) {
mesh1->vertices[i] = point(cloud_src->points[i].x, cloud_src->points[i].y,
cloud_src->points[i].z);
}
for (int i = 0; i < cloud_temp->size(); i++) {
mesh2->vertices[i] = point(cloud_temp->points[i].x, cloud_temp->points[i].y,
cloud_temp->points[i].z);
}
xform xf1;
xform xf2;
KDtree* kd1 = new KDtree(mesh1->vertices);
KDtree* kd2 = new KDtree(mesh2->vertices);
vector< float> weights1, weights2;
if (bulkmode) {
float area1 = mesh1->stat(TriMesh::STAT_TOTAL, TriMesh::STAT_FACEAREA);
float area2 = mesh2->stat(TriMesh::STAT_TOTAL, TriMesh::STAT_FACEAREA);
float overlap_area, overlap_dist;
find_overlap(mesh1, mesh2, xf1, xf2, kd1, kd2, overlap_area, overlap_dist);
float frac_overlap = overlap_area / min(area1, area2);
if (frac_overlap < 0.1f) {
TriMesh::eprintf("Insufficient overlap\n");
exit(1);
} else {
TriMesh::dprintf("%.1f%% overlap\n", frac_overlap * 100.0);
}
}
float err = ICP(mesh1, mesh2, xf1, xf2, kd1, kd2, weights1, weights2, 0, verbose,
do_scale, do_affine);
if (err >= 0.0f)
err = ICP(mesh1, mesh2, xf1, xf2, kd1, kd2, weights1, weights2,0,
verbose, do_scale, do_affine);
if (err < 0.0f) {
TriMesh::eprintf("ICP failed\n");
//exit(1);
}
//TriMesh::eprintf("ICP succeeded - distance = %f\n", err);
delete kd1;
delete kd2;
delete mesh1;
delete mesh2;
if (bulkmode) {
xform xf12 = inv(xf2) * xf1;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
mat_rot(i, j) = xf12[i + 4 * j];
}
}
} else {
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
mat_rot(i, j) = xf2[i + 4 * j];
}
}
}
return err;
}
