TEST (CorrespondenceEstimation, CorrespondenceEstimationSetSearchMethod)
{
// Generating 3 random clouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
for ( size_t i = 0; i < 50; i++ )
{
cloud1->points.emplace_back(float (rand()), float (rand()), float (rand()));
cloud2->points.emplace_back(float (rand()), float (rand()), float (rand()));
}
// Build a KdTree for each
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree1 (new pcl::search::KdTree<pcl::PointXYZ> ());
tree1->setInputCloud (cloud1);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree2 (new pcl::search::KdTree<pcl::PointXYZ> ());
tree2->setInputCloud (cloud2);
// Compute correspondences
pcl::registration::CorrespondenceEstimation<pcl::PointXYZ, pcl::PointXYZ, double> ce;
ce.setInputSource (cloud1);
ce.setInputTarget (cloud2);
pcl::Correspondences corr_orig;
ce.determineCorrespondences(corr_orig);
// Now set the kd trees
ce.setSearchMethodSource (tree1, true);
ce.setSearchMethodTarget (tree2, true);
pcl::Correspondences corr_cached;
ce.determineCorrespondences (corr_cached);
// Ensure they're the same
EXPECT_EQ(corr_orig.size(), corr_cached.size());
for(size_t i = 0; i < corr_orig.size(); i++)
{
EXPECT_EQ(corr_orig[i].index_query, corr_cached[i].index_query);
EXPECT_EQ(corr_orig[i].index_match, corr_cached[i].index_match);
}
}
void main()
{
try {
// ビューアーを作成する
pcl::visualization::PCLVisualizer viewer( "Point Cloud Viewer" );
// 点群
pcl::PointCloud<PointType>::Ptr cloud1( new pcl::PointCloud<PointType> );
pcl::PointCloud<PointType>::Ptr cloud2( new pcl::PointCloud<PointType> );
// PLYファイルを読み込む
pcl::PLYReader reader;
reader.read( "model1.ply", *cloud1 );
reader.read( "model2.ply", *cloud2 );
// ビューアーに追加して更新(Qキーで次の処理へ)
viewer.addPointCloud( cloud1, "cloud1" );
viewer.addPointCloud( cloud2, "cloud2" );
viewer.spin();
// VoxelGrid Filter
voxelGridFilter( cloud1 );
voxelGridFilter( cloud2 );
// 位置合わせ
iterativeClosestPoint( cloud1, cloud2 );
// ビューアーを更新する(Qキーで次の処理へ)
viewer.updatePointCloud( cloud1, "cloud1" );
viewer.updatePointCloud( cloud2, "cloud2" );
viewer.spin();
// 点群をマージして重複を削除する
*cloud1 += *cloud2;
voxelGridFilter( cloud1 );
// PLYファイルとして書き出す
pcl::PLYWriter writer;
writer.write( "output.ply", *cloud1 );
// ビューアーを更新する(Qキーで次の処理へ)
viewer.updatePointCloud( cloud1, "cloud1" );
viewer.removePointCloud( "cloud2" );
viewer.spin();
// メッシュ化してファイルに出力
auto triangles = createMesh( cloud1 );
pcl::io::savePLYFile( "output-mesh.ply", triangles );
// 表示を更新(Qキーで次の処理へ)
viewer.removeAllPointClouds();
viewer.addPolygonMesh( triangles );
viewer.spin();
}
catch ( std::exception& ex ){
std::cout << ex.what() << std::endl;
}
}
int main(int argc, char* argv[])
{
std::cout<<"Merging maps...\n";
// *** Check input
if(argc < 4)
{
std::cout<<"Usage: ./mergeMaps <map1.pcd> <map2.pcd> <mergedMap.pcd> [transform-2-to-1.txt]";
return -1;
}
// *** Read data
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZRGB>());
if (pcl::io::loadPCDFile (argv[1], *cloud1) < 0)
{
std::cout << "Error loading point cloud " << argv[1] << std::endl;
return -1;
}
if (pcl::io::loadPCDFile (argv[2], *cloud2) < 0)
{
std::cout << "Error loading point cloud " << argv[2] << std::endl;
return -1;
}
// *** Transformation (optional)
Eigen::Matrix4f transform;
if(argc > 4)
readTransform(argv[4], transform);
else
transform.setIdentity();
pcl::transformPointCloud(*cloud1, *cloud1, transform);
// *** Merging
pcl::PointCloud<pcl::PointXYZRGB>::Ptr finalCloud(new pcl::PointCloud<pcl::PointXYZRGB>());
*finalCloud = *cloud1 + *cloud2;
// *** Save to file
if (pcl::io::savePCDFile(argv[3], *finalCloud) < 0)
{
std::cout << "Error saving the point cloud in "<<argv[4] << std::endl;
return -1;
}
std::cout << "Successfully merged maps" << std::endl;
return 0;
}
TEST (CorrespondenceEstimation, CorrespondenceEstimationNormalShooting)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ> ());
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
// Defining two parallel planes differing only by the y co-ordinate
for (float i = 0; i < 10; i += 0.2)
{
for (float z = 0; z < 5; z += 0.2)
{
cloud1->points.push_back (pcl::PointXYZ (i, 0, z));
cloud2->points.push_back (pcl::PointXYZ (i, 2, z)); // Ideally this should be the corresponding point to the point defined in the previous line
}
}
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud1);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud1_normals (new pcl::PointCloud<pcl::Normal>);
ne.setKSearch (5);
ne.compute (*cloud1_normals); // All normals are perpendicular to the plane defined
pcl::CorrespondencesPtr corr (new pcl::Correspondences);
pcl::registration::CorrespondenceEstimationNormalShooting <pcl::PointXYZ, pcl::PointXYZ, pcl::Normal> ce;
ce.setInputCloud (cloud1);
ce.setKSearch (10);
ce.setSourceNormals (cloud1_normals);
ce.setInputTarget (cloud2);
ce.determineCorrespondences (*corr);
// Based on the data defined, the correspondence indices should be 1 <-> 1 , 2 <-> 2 , 3 <-> 3 etc.
for (unsigned int i = 0; i < corr->size (); i++)
{
EXPECT_EQ ((*corr)[i].index_query, (*corr)[i].index_match);
}
}
void geonDetector::initialize()
{
totalCount = 0;
pcl::PointCloud<PointT>::Ptr cloud1 (new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_filtered1 (new pcl::PointCloud<PointT>());
pcl::PointCloud<PointT>::Ptr cloud_filtered21 (new pcl::PointCloud<PointT>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals1 (new pcl::PointCloud<pcl::Normal>());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals21 (new pcl::PointCloud<pcl::Normal>());
pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients());
pcl::PointIndices::Ptr geonIndices1 (new pcl::PointIndices());
pcl::search::KdTree<PointT>::Ptr tree2 (new pcl::search::KdTree<PointT>());
this->cloud = cloud1;
this->cloud_filtered = cloud_filtered1;
this->cloud_normals = cloud_normals1;
this->cloud_filtered2 = cloud_filtered21;
this->cloud_normals2 = cloud_normals21;
this->coefficients = coefficients1;
this->geonIndices = geonIndices1;
this->tree = tree2;
}
//extract table clouds, convex hull, and convert to msg stored in DB
bool extract_table_msg(pcl_cloud::Ptr cloud_in, bool is_whole, bool store_cloud=false, bool store_convex=false)
{
pcl_cloud::Ptr cloud1(new pcl_cloud());
pcl_cloud::Ptr cloud_out(new pcl_cloud());
//filter out range that may not be a table plane
pcl::PassThrough<Point> pass;
pass.setFilterFieldName("z");
pass.setFilterLimits(0.45,1.2);
pass.setInputCloud(cloud_in);
pass.filter(*cloud1);
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
pcl::SACSegmentation<Point> seg;
seg.setOptimizeCoefficients (true);
//plane model
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.01);
seg.setInputCloud (cloud1);
seg.segment (*inliers, *coefficients);
if(inliers->indices.size () == 0){
ROS_INFO("No plane detected!");
return false;
}
//form a new cloud from indices
pcl::ExtractIndices<Point> extract;
extract.setInputCloud (cloud1);
extract.setIndices (inliers);
extract.setNegative (false);
extract.filter (*cloud_out);
//filter out noise
pcl_cloud::Ptr cloud_nonoise(new pcl_cloud());
outlier_filter_radius(cloud_out,cloud_nonoise);
/*
//normal estimation and filter table plane
pcl::NormalEstimationOMP<Point, pcl::Normal> ne;
ne.setInputCloud (cloud_nonoise);
pcl::search::KdTree<Point>::Ptr tree (new pcl::search::KdTree<Point> ());
ne.setSearchMethod (tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch (0.03);
ne.compute (*cloud_normals);
pcl::Normal nor;
float size = 0.0;
for(int i=0;i<cloud_normals->height*cloud_normals->width;i++){
if(isnan(cloud_normals->at(i).normal_x)){
//std::cout<<cloud_normals->at(i).normal_x<<std::endl;
}
else{
nor.normal_x += cloud_normals->at(i).normal_x;
nor.normal_y += cloud_normals->at(i).normal_y;
nor.normal_z += cloud_normals->at(i).normal_z;
size++;
}
}
//normalize instead of average
float normal_length = sqrt(nor.normal_x*nor.normal_x+nor.normal_y*nor.normal_y+nor.normal_z*nor.normal_z);
nor.normal_x = nor.normal_x/normal_length;
nor.normal_y = nor.normal_y/normal_length;
nor.normal_z = nor.normal_z/normal_length;
*/
//instead of computer normal again, using it from plane model
pcl::Normal nor;
nor.normal_x = coefficients->values[0];
nor.normal_y = coefficients->values[1];
nor.normal_z = coefficients->values[2];
if(Debug){
std::cout<<nor.normal_x<<std::endl;
std::cout<<nor.normal_y<<std::endl;
std::cout<<nor.normal_z<<std::endl;
}
//the default viewpoint is (0,0,0),thus using global cloud some plane and viewpoint are in a same plane,
//which may not helpful to flip normal direction
pcl::Normal standard_normal;
standard_normal.normal_x = 0.0;
standard_normal.normal_y = 0.0;
standard_normal.normal_z = 1.0;
float cosin_angle = (standard_normal.normal_z*nor.normal_z);
float tmp_angle = acos(cosin_angle) * (180.0/PI);
//if(tmp_angle>90.0?(tmp_angle-90.0)<normal_angle:tmp_angle<normal_angle){
if(tmp_angle-90.0>0.0){
if((180.0 - tmp_angle)<normal_angle){
ROS_INFO("Normal direction meet requirement %f, angle calculated is: 180.0-%f=%f. ", normal_angle, tmp_angle, 180.0-tmp_angle);
}
else{
ROS_INFO("Normal direction exceed threshold %f, angle calculated is: 180.0-%f=%f. skip.", normal_angle, tmp_angle, 180.0-tmp_angle);
return false;
}
}
else{
if(tmp_angle<normal_angle) {
ROS_INFO("Normal direction meet requirement angle %f, angle calculated is %f", normal_angle, tmp_angle);
}
else{
ROS_INFO("Normal direction exceed threshold %f, angle calculated is: %f. skip.", normal_angle, tmp_angle);
return false;
}
}
pcl_cloud::Ptr cloud_hull(new pcl_cloud());
pcl_cloud::Ptr cloud_cave(new pcl_cloud());
extract_convex(cloud1,inliers,coefficients,cloud_hull,cloud_cave);
//reject some size convex
//store the plane that pass the table filter
if(store_cloud) {
ROS_INFO("Storing table cloud (noise & filted)...");
if (is_whole) {
mongodb_store::MessageStoreProxy table_whole_cloud_noise(*nh, "whole_table_clouds_noise");
mongodb_store::MessageStoreProxy table_whole_cloud(*nh, "whole_table_clouds");
//conversion
sensor_msgs::PointCloud2 whole_table_cloud_noise;
pcl::toROSMsg(*cloud_out, whole_table_cloud_noise);
table_whole_cloud_noise.insert(whole_table_cloud_noise);
sensor_msgs::PointCloud2 whole_table_cloud;
pcl::toROSMsg(*cloud_nonoise, whole_table_cloud);
table_whole_cloud.insert(whole_table_cloud);
}
else {
mongodb_store::MessageStoreProxy dbtable_cloud_noise(*nh, "table_clouds_noise");
mongodb_store::MessageStoreProxy dbtable_cloud(*nh, "table_clouds");
//conversion
sensor_msgs::PointCloud2 table_cloud_noise;
pcl::toROSMsg(*cloud_out, table_cloud_noise);
dbtable_cloud_noise.insert(table_cloud_noise);
sensor_msgs::PointCloud2 table_cloud;
pcl::toROSMsg(*cloud_nonoise, table_cloud);
dbtable_cloud.insert(table_cloud);
//store table centre for each scan
ROS_INFO("Storing table centre...");
Table<Point> tb(nh);
tb.table_cloud_centre(table_cloud);
}
}
ROS_INFO("Convex cloud has been extracted contains %lu points", (*cloud_hull).points.size());
if(store_convex){
ROS_INFO("Storing convex cloud...");
if(is_whole){
mongodb_store::MessageStoreProxy dbtable_whole_convex_cloud(*nh, "whole_table_convex");
mongodb_store::MessageStoreProxy dbtable_whole_concave_cloud(*nh, "whole_table_concave");
//conversion
sensor_msgs::PointCloud2 whole_table_convex;
pcl::toROSMsg(*cloud_hull, whole_table_convex);
dbtable_whole_convex_cloud.insert(whole_table_convex);
sensor_msgs::PointCloud2 whole_table_concave;
pcl::toROSMsg(*cloud_cave, whole_table_concave);
dbtable_whole_concave_cloud.insert(whole_table_concave);
}
else{
mongodb_store::MessageStoreProxy dbtable_convex_cloud(*nh, "table_convex");
mongodb_store::MessageStoreProxy dbtable_concave_cloud(*nh, "table_concave");
//conversion
sensor_msgs::PointCloud2 table_convex;
pcl::toROSMsg(*cloud_hull, table_convex);
dbtable_convex_cloud.insert(table_convex);
sensor_msgs::PointCloud2 table_concave;
pcl::toROSMsg(*cloud_cave, table_concave);
dbtable_concave_cloud.insert(table_concave);
}
}
//construct a table msg
table_detection::Table table_msg;
table_msg.pose.pose.orientation.w = 1.0;
table_msg.header.frame_id = "/map";
table_msg.header.stamp = ros::Time();
for(int i=0;i<(*cloud_hull).points.size();i++){
geometry_msgs::Point32 pp;
pp.x = (*cloud_hull).at(i).x;
pp.y = (*cloud_hull).at(i).y;
pp.z = (*cloud_hull).at(i).z;
table_msg.tabletop.points.push_back(pp);
}
//insert to mongodb
if(is_whole){
mongodb_store::MessageStoreProxy whole_table_shape(*nh, "whole_table_shapes");
whole_table_shape.insert(table_msg);
ROS_INFO("Insert whole table shape to collection.");
}
else{
mongodb_store::MessageStoreProxy table_shape(*nh, "table_shapes");
table_shape.insert(table_msg);
ROS_INFO("Insert table shape to collection.");
}
return true;
}
int main (int argc, char** argv)
{
/* DOWNSAMPLING ********************************************************************************************************************/
std::ofstream output_file("properties.txt");
std::ofstream curvature("curvature.txt");
std::ofstream normals_text("normals.txt");
/*
std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height
<< " data points (" << pcl::getFieldsList (*cloud) << ")." << std::endl;
// Create the filtering object
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud (cloud);
sor.setLeafSize (0.01f, 0.01f, 0.01f);
sor.filter (*cloud_filtered);
output_file << "Number of filtered points" << std::endl;
output_file << cloud_filtered->width * cloud_filtered->height<<std::endl;
std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height
<< " data points (" << pcl::getFieldsList (*cloud_filtered) << ")." << std::endl;
pcl::PointCloud<pcl::PointXYZ>::Ptr descriptor (new pcl::PointCloud<pcl::PointXYZ>);
descriptor->width = 5000 ;
descriptor->height = 1 ;
descriptor->points.resize (descriptor->width * descriptor->height) ;
std::cerr << descriptor->points.size() << std::endl ;
pcl::PCDWriter writer;
writer.write ("out.pcd", *cloud_filtered, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);
*/
/***********************************************************************************************************************************/
/* CALCULATING VOLUME AND SURFACE AREA ********************************************************************************************************************/
/*pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ConcaveHull<pcl::PointXYZ> chull;
chull.setInputCloud(test);
chull.reconstruct(*cloud_hull);*/
/*for (size_t i=0; i < test->points.size (); i++)
{
std::cout<< test->points[i].x <<std::endl;
std::cout<< test->points[i].y <<std::endl;
std::cout<< test->points[i].z <<std::endl;
}*/
//std::cout<<data.size()<<std::endl;
// Load input file into a PointCloud<T> with an appropriate type
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCLPointCloud2 cloud_blob;
pcl::io::loadPCDFile ("mini_soccer_ball_downsampled.pcd", cloud_blob);
pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (cloud_blob, *cloud1);
//* the data should be available in cloud
// Normal estimation*
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree->setInputCloud (cloud);
n.setInputCloud (cloud);
n.setSearchMethod (tree);
n.setKSearch (20);
n.compute (*normals);
//* normals should not contain the point normals + surface curvatures
// Concatenate the XYZ and normal fields*
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields (*cloud, *normals,*cloud_with_normals);
//* cloud_with_normals = cloud + normals
// Create search tree*
pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
tree2->setInputCloud (cloud_with_normals);
// Initialize objects
pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
pcl::PolygonMesh triangles;
// Set the maximum distance between connected points (maximum edge length)
gp3.setSearchRadius (0.025);
// Set typical values for the parameters
gp3.setMu (2.5);
gp3.setMaximumNearestNeighbors (100);
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
gp3.setMinimumAngle(M_PI/18); // 10 degrees
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
gp3.setNormalConsistency(false);
// Get result
gp3.setInputCloud (cloud_with_normals);
gp3.setSearchMethod (tree2);
gp3.reconstruct (triangles);
// Additional vertex information
std::vector<int> parts = gp3.getPartIDs();
std::vector<int> states = gp3.getPointStates();
pcl::PolygonMesh::Ptr mesh(&triangles);
pcl::PointCloud<pcl::PointXYZ>::Ptr triangle_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2(mesh->cloud, *triangle_cloud);
/* for(int i = 0; i < 2; i++){
std::cout<<triangles.polygons[i]<<std::endl;
}
*/ std::cout<<"first vertice "<<triangles.polygons[0].vertices[0] << std::endl;
//std::cout<<"Prolly not gonna work "<<triangle_cloud->points[triangles.polygons[0].vertices[0]] << std::endl;
//pcl::fromPCLPointCloud2(triangles.cloud, triangle_cloud);
std::cout << "size of points " << triangle_cloud->points.size() << std::endl ;
std::cout<<triangle_cloud->points[0]<<std::endl;
for(unsigned i = 0; i < triangle_cloud->points.size(); i++){
std::cout << triangles.polgyons[i].getVector3fMap() <<"test"<< std::endl;
}
//std::cout<<"surface: "<<calculateAreaPolygon(triangles, triangle_cloud)<<std::endl;
/******************************************************************************************************************************************/
/* CALCULATING CURVATURE AND NORMALS ********************************************************************************************************************/
// Create the normal estimation class, and pass the input dataset to it
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud (cloud);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree3 (new pcl::search::KdTree<pcl::PointXYZ> ());
ne.setSearchMethod (tree3);
// Output datasets
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Use all neighbors in a sphere of radius 3cm
ne.setRadiusSearch (0.03);
// Compute the features
ne.compute (*cloud_normals);
output_file << "size of points " << cloud->points.size() << std::endl ;
output_file << "size of the normals " << cloud_normals->points.size() << std::endl ;
//pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr (new pcl::PointCloud<pcl::PointXYZRGB>);
/************************************************************************************************************************************/
/* PARSING DATA ********************************************************************************************************************/
int k=0 ;
float dist ;
float square_of_dist ;
float x1,y1,z1,x2,y2,z2 ;
float nu[3], nv[3], pv_pu[3], pu_pv[3] ;
float highest = triangle_cloud->points[0].z;
float lowest = triangle_cloud->points[0].z;
for ( int i = 0; i < cloud_normals->points.size() ; i++)
{
output_file<<i<<": triangulated "<<triangle_cloud->points[i].x<<", "<<triangle_cloud->points[i].y<<", "<<triangle_cloud->points[i].z<<std::endl;
output_file<<i<<": normal"<<cloud1->points[i].x<<", "<<cloud1->points[i].y<<", "<<cloud1->points[i].z<<std::endl;
if(triangle_cloud->points[i].z > highest)
{
highest = triangle_cloud->points[i].z;
}
if(triangle_cloud->points[i].z < lowest)
{
lowest = triangle_cloud->points[i].z;
}
normals_text <<i+1 <<": "<<" x-normal-> "<<cloud_normals->points[i].normal_x<<" y-normal-> "<<cloud_normals->points[i].normal_y<<" z-normal-> "<<cloud_normals->points[i].normal_z<<std::endl;
curvature <<i+1 <<": curvature: "<<cloud_normals->points[i].curvature<<std::endl;
float x, y, z, dist, nx, ny, nz, ndist;
/*
if(i != cloud_normals->points.size()-1){
x = cloud->points[i+1].x - cloud->points[i].x;
y = cloud->points[i+1].y - cloud->points[i].y;
z = cloud->points[i+1].z - cloud->points[i].z;
dist = sqrt(pow(x, 2)+pow(y, 2) + pow(z, 2));
output_file << i+1 <<" -> "<< i+2 << " distance normal: " << dist <<std::endl;
nx = triangle_cloud[i+1].indices[0] - triangle_cloud.points[i].x;
ny = triangle_cloud.points[i+1].y - triangle_cloud.points[i].y;
nz = triangle_cloud.points[i+1].z - triangle_cloud.points[i].z;
ndist = sqrt(pow(nx, 2)+pow(ny, 2) + pow(nz, 2));
output_file << i+1 <<" -> "<< i+2 << " distance triangulated: " << ndist <<std::endl;
}
*/
/*
pcl::PointXYZRGB point;
point.x = cloud_filtered_converted->points[i].x;
point.y = cloud_filtered_converted->points[i].y;
point.z = cloud_filtered_converted->points[i].z;
point.r = 0;
point.g = 100;
point.b = 200;
point_cloud_ptr->points.push_back (point);
*/
}
output_file << "highest point: "<< highest<<std::endl;
output_file << "lowest point: "<< lowest<<std::endl;
//pcl::PointCloud<pcl::PointXYZ>::Ptr test (new pcl::PointCloud<pcl::PointXYZ>);
//float surface_area = calculateAreaPolygon(test);
//std::cout<< surface_area<<std::endl;
/*
descriptor->width = k ;
descriptor->height = 1 ;
descriptor->points.resize (descriptor->width * descriptor->height) ;
std::cerr << descriptor->points.size() << std::endl ;
float voxelSize = 0.01f ; // how to find appropriate voxel resolution
pcl::octree::OctreePointCloud<pcl::PointXYZ> octree (voxelSize);
octree.setInputCloud(descriptor) ;
ctree.defineBoundingBox(0.0,0.0,0.0,3.14,3.14,3.14) ; //octree.defineBoundingBox (minX, minY, minZ, maxX, maxY, maxZ)
octree.addPointsFromInputCloud (); // octree created for block
int k_ball=0 ;
float dist_ball ;
float square_of_dist_ball ;
double X,Y,Z ;
bool occupied ;
highest = cloud_ball->points[0].z;
for ( int i = 0; i < cloud_normals_ball->points.size() ; i++)
{
if(cloud->points[i].z > highest){
highest = cloud_ball->points[i].z;
}
for (int j = i+1 ; (j < cloud_normals_ball->points.size()) ; j++)
{
x1 = cloud_ball->points[i].x ;
y1 = cloud_ball->points[i].y ;
z1 = cloud_ball->points[i].z ;
nu[0] = cloud_normals_ball->points[i].normal_x ;
nu[1] = cloud_normals_ball->points[i].normal_y ;
nu[2] = cloud_normals_ball->points[i].normal_z ;
x2 = cloud_ball->points[j].x ;
y2 = cloud_ball->points[j].y ;
z2 = cloud_ball->points[j].z ;
nv[0] = cloud_normals_ball->points[j].normal_x ;
nv[1] = cloud_normals_ball->points[j].normal_y ;
nv[2] = cloud_normals_ball->points[j].normal_z ;
square_of_dist = ((x2-x1)*(x2-x1)) + ((y2-y1)*(y2-y1)) + ((z2-z1)*(z2-z1)) ;
dist = sqrt(square_of_dist) ;
//std::cerr << dist ;
pv_pu[0] = x2-x1 ;
pv_pu[1] = y2-y1 ;
pv_pu[2] = z2-z1 ;
pu_pv[0] = x1-x2 ;
pu_pv[1] = y1-y2 ;
pu_pv[2] = z1-z2 ;
if ((dist > 0.0099) && (dist < 0.0101))
{
X = angle_between_vectors (nu, nv) ;
Y = angle_between_vectors (nu, pv_pu) ;
Z = angle_between_vectors (nv, pu_pv) ;
// output_file << descriptor->points[k].x << "\t" << descriptor->points[k].y << "\t" << descriptor->points[k].z ;
// output_file << "\n";
//k_ball = k_ball + 1 ;
occupied = octree.isVoxelOccupiedAtPoint (X,Y,Z) ;
if (occupied == 1)
{
//k_ball = k_ball + 1 ;
std::cerr << "Objects Matched" << "\t" << k_ball << std::endl ;
return(0);
}
}
}
}
*/
/***********************************************************************************************************************************/
/*
points.open("secondItemPoints.txt");
myfile<<"Second point \n";
myfile<<"second highest "<<highest;
for(int i = 0; i < cloud_normals->points.size(); i++){
points<<cloud->points[i].x<<", "<<cloud->points[i].y<<", "<<cloud->points[i].z<<"\n";
if(cloud->points[i].z >= highest - (highest/100)){
myfile<<cloud->points[i].x<<", "<<cloud->points[i].y<<", "<<cloud->points[i].z<<"\n";
}
}
points.close();
myfile.close();
std::cerr << "Objects Not Matched" << "\t" << k_ball << std::endl ;
*/
//output_file <<"Volume: "<<volume <<std::endl;
//output_file <<"Surface Area: "<<surface_area <<std::endl;
return (0);
}
bool change_detection (tms_msg_ss::ods_furniture::Request &req, tms_msg_ss::ods_furniture::Response &res)
{
//***************************
// local variable declaration
//***************************
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr model (new pcl::PointCloud<pcl::PointXYZRGB>);
std::cout << mkdir("/tmp/tms_ss_ods", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
std::cout << mkdir("/tmp/tms_ss_ods/ods_change_detection", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
std::cout << mkdir("/tmp/tms_ss_ods/ods_change_detection/table", S_IRUSR | S_IWUSR | S_IXUSR) << std::endl;
//***************************
// capture kinect data
//***************************
tms_msg_ss::ods_pcd srv;
srv.request.id = 3;
if(commander_to_kinect_capture.call(srv)){
pcl::fromROSMsg (srv.response.cloud, *cloud1);
}
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/input.pcd", *cloud1);
//pcl::fromROSMsg (req.model, *model);
pcl::io::loadPCDFile("/tmp/tms_ss_ods/ods_change_detection/table/input.pcd", *cloud1);
TABLE = req.id;
if(TABLE == 1)
pcl::io::loadPCDFile ("/tmp/tms_ss_ods/ods_change_detection/table/model_table1.pcd", *model);
else if(TABLE == 2)
pcl::io::loadPCDFile ("/tmp/tms_ss_ods/ods_change_detection/table/model_table2.pcd", *model);
//***************************
// Fill in the cloud data
//***************************
// table.x = req.furniture.position.x/1000.0;
// table.y = req.furniture.position.y/1000.0;
// table.z = req.furniture.position.z/1000.0;
// table.theta = req.furniture.orientation.z;
// robot.x = req.robot.x/1000.0;
// robot.y = req.robot.y/1000.0;
// robot.theta = req.robot.theta;
// sensor.x = req.sensor.position.x;
// sensor.y = req.sensor.position.y;
// sensor.z = req.sensor.position.z;
// sensor.theta = req.sensor.orientation.y;
table.x = 1.0;
table.y = 1.5;
table.z = 0.7;
table.theta = 0.0;
robot.x = 2.6;
robot.y = 1.9;
robot.theta = 180.0;
sensor.x = 0.0;
sensor.y = 0.0;
sensor.z = 1.0;
sensor.theta = 20.0;
std::cout << robot.x << " " << robot.y << " " << robot.theta << std::endl;
//***************************
// transform input cloud
//***************************
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tfm_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
transformation(*cloud1, *model);
for(int i=0;i<model->points.size();i++){
model->points[i].r = 255;
model->points[i].g = 0;
model->points[i].b = 0;
}
*tfm_cloud = *model + *cloud1;
if(!(tfm_cloud->points.size())){
std::cout << "tfm_cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/tfm_cloud1.pcd", *tfm_cloud);
//***************************
// filtering by using model
//***************************
std::cout << "filtering" << std::endl;
filtering(*cloud1, *model);
if(!(cloud1->points.size())){
std::cout << "filtered cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/filter.pcd", *cloud1);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr dsp_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud1, *dsp_cloud);
downsampling(*dsp_cloud, 0.01);
//***************************
// registration between two input pcd data
//***************************
std::cout << "registration" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rgs_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr view_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
Eigen::Matrix4f m;
int n = 0;
while (1){
registration(*dsp_cloud, *model, *rgs_cloud, *cloud1, m);
if((double)(m(0,0)+m(1,1)+m(2,2)+m(3,3)) >= 4){
if(n > 2) break;
}
n++;
}
pcl::copyPointCloud(*cloud1, *view_cloud);
if(!(rgs_cloud->points.size())){
std::cout << "registered cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/rgs_cloud.pcd", *rgs_cloud);
//***************************
// init t_voxel
//***************************
std::cout << "init table_voxel" << std::endl;
make_tablevoxel(*model);
//***************************
// remove table
//***************************
std::cout << "remove table" << std::endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rmc_cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr rmc_cloud2 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::io::loadPCDFile("/tmp/tms_ss_ods/ods_change_detection/table/memory.pcd", *rmc_cloud2);
remove_table(*cloud1, *rmc_cloud1);
if(!(rmc_cloud1->size() != 0)){
std::cout << "removed table cloud has no point" << std::endl;
return false;
}
else
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/remove_table1.pcd", *rmc_cloud1, false);
//***************************
// segmentation
//***************************
segmentation(*rmc_cloud1, 0.015, 1);
if(rmc_cloud2->size() != 0)
segmentation(*rmc_cloud2, 0.015, 2);
if(rmc_cloud1->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/memory.pcd", *rmc_cloud1, true);
if(rmc_cloud2->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/obj_cloud2.pcd", *rmc_cloud2, true);
//***************************
// compare segments with memory
//***************************
segment_matching();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb1 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_rgb2 (new pcl::PointCloud<pcl::PointXYZRGB>);
//***************************************
// rewrite Object data on TMS database
//***************************************
tms_msg_db::tmsdb_ods_object_data srv3;
ros::Time time = ros::Time::now() + ros::Duration(9*60*60);
int c=0;
float colors[6][3] ={{255, 0, 0}, {0,255,0}, {0,0,255}, {255,255,0}, {0,255,255}, {255,0,255}};
for(int i=0;i<Object_Map.size();i++){
std::cout << i << " → " << Object_Map[i].tf << std::endl;
if((Object_Map[i].tf == 1)||(Object_Map[i].tf == 2)){
std::stringstream ss;
ss << "/tmp/tms_ss_ods/ods_change_detection/table/result_rgb" << i << ".pcd";
if(Object_Map[i].cloud_rgb.size() != 0){
pcl::io::savePCDFile(ss.str (), Object_Map[i].cloud_rgb);
for(int j=0;j<Object_Map[i].cloud_rgb.points.size();j++){
Object_Map[i].cloud_rgb.points[j].r = colors[c%6][0];
Object_Map[i].cloud_rgb.points[j].g = colors[c%6][1];
Object_Map[i].cloud_rgb.points[j].b = colors[c%6][2];
}
c++;
}
else
std::cout << "no cloud_rgb data" << std::endl;
if(Object_Map[i].tf == 1){
*tmp_rgb1 += Object_Map[i].cloud_rgb;
tms_msg_db::tmsdb_data data;
data.tMeasuredTime = time;
data.iType = 5;
data.iID = 53;
data.fX = 1000.0 * (Object_Map[i].g.x - 1.0);
data.fY = 1000.0 * (Object_Map[i].g.y - 1.5);
data.fZ = 700.0;
data.fTheta = 0.0;
data.iPlace = 14;
data.iState = 1;
srv3.request.srvTMSInfo.push_back(data);
geometry_msgs::Pose pose;
pose.position.x = Object_Map[i].g.x; pose.position.y = Object_Map[i].g.y; pose.position.z = Object_Map[i].g.z;
pose.orientation.x = 0.0; pose.orientation.y = 0.0; pose.orientation.z = 0.0; pose.orientation.w = 0.0;
res.objects.poses.push_back(pose);
std::cout << Object_Map[i].g.x << " " << Object_Map[i].g.y << " " << Object_Map[i].g.z << std::endl;
}
else if(Object_Map[i].tf == 2)
*tmp_rgb2 += Object_Map[i].cloud_rgb;
}
}
if(srv3.request.srvTMSInfo.size() != 0){
if(commander_to_ods_object_data.call(srv3))
std::cout << "Rewrite TMS database!!" << std::endl;
}
if((tmp_rgb1->size() == 0) && (tmp_rgb2->size() == 0)){
std::cout << "No change on table!!" << std::endl;
return true;
}
if(tmp_rgb1->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/result1.pcd", *tmp_rgb1, true);
if(tmp_rgb2->size() != 0)
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/result2.pcd", *tmp_rgb2, true);
*view_cloud += *tmp_rgb1;
pcl::io::savePCDFile("/tmp/tms_ss_ods/ods_change_detection/table/view_result.pcd", *view_cloud, true);
//pcl::toROSMsg (*cloud1, res.cloud);
Object_Map.clear();
return true;
}
/** classic ICP using PCL Library*/
void ICP_PCL()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (file1, *cloud1) == -1) //* load the file
{
LOGI("Couldn't read file1");
return;
}
LOGI("Loaded %d data points from file1",cloud1->width * cloud1->height);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (file2, *cloud2) == -1) //* load the file
{
LOGI("Couldn't read file2");
return;
}
LOGI("Loaded %d data points from file2",cloud2->width * cloud2->height);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
MyReg* mr=new MyReg();
PointCloud::Ptr src (new PointCloud(mr->width_ds,mr->height_ds));
PointCloud::Ptr tgt (new PointCloud(mr->width_ds,mr->height_ds));
LOGI("Start Downsampling...");
mr->DownSampling(cloud1,cloud2,src,tgt);
for (size_t i = 0; i < src->points.size (); ++i){
if(isnan(src->points[i].x)) {src->points[i].x=0;src->points[i].y=0;src->points[i].z=0;}
}
for (size_t i = 0; i < tgt->points.size (); ++i){
if(isnan(tgt->points[i].x)) {tgt->points[i].x=0;tgt->points[i].y=0;tgt->points[i].z=0;}
}
pcl::PointCloud<pcl::PointXYZ> Final;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputCloud(src);
icp.setInputTarget(tgt);
LOGI("Start Aligning...");
icp.align(Final);
string outputstr;
ostringstream out_message;
out_message << icp.getFinalTransformation();
outputstr=out_message.str();
LOGI("Final Transform: \n %s", outputstr.c_str());
Eigen::Matrix4f GlobalTransf=icp.getFinalTransformation();
pcl::PointCloud<pcl::PointXYZ>::Ptr transf (new pcl::PointCloud<pcl::PointXYZ>);
pcl::transformPointCloud (*src, *transf, GlobalTransf);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
LOGI("Time of registration: :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);
ShowPCLtoKiwi(transf,255,0,0,-1.5);
ShowPCLtoKiwi(tgt,0,255,0,-1.5);
ShowPCLtoKiwi(src,255,0,0,1.5);
ShowPCLtoKiwi(tgt,0,255,0,1.5);
}
/** registration two clouds using ICP with projective correspondence */
void ICP()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (file1, *cloud1) == -1) //* load the file
{
LOGI("Couldn't read file1");
return;
}
LOGI("Loaded %d data points from file2",cloud1->width * cloud1->height);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (file2, *cloud2) == -1) //* load the file
{
LOGI("Couldn't read file2");
return;
}
LOGI("Loaded %d data points from file2",cloud1->width * cloud1->height);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time1);
///////////////////ICP - REGISTRATION ////////////////
MyReg* mr=new MyReg();
PointCloud::Ptr src (new PointCloud(mr->width_ds,mr->height_ds));
PointCloud::Ptr tgt (new PointCloud(mr->width_ds,mr->height_ds));
mr->DownSampling(cloud1,cloud2,src,tgt);
LOGI("Start Downsampling...");
pcl::PointCloud<PointNormalT>::Ptr points_with_normals_src (new pcl::PointCloud<PointNormalT>);
pcl::PointCloud<PointNormalT>::Ptr points_with_normals_tgt (new pcl::PointCloud<PointNormalT>);
LOGI("Start Normals estimation...");
mr->Normals(points_with_normals_src,points_with_normals_tgt,src,tgt);
LOGI("Start Matrix estimation...");
Eigen::Matrix4f GlobalTransf;
mr->MatrixEstimation(points_with_normals_src, points_with_normals_tgt, GlobalTransf);
string outputstr;
ostringstream out_message;
out_message << GlobalTransf;
outputstr=out_message.str();
LOGI("%s", outputstr.c_str());
PointCloud::Ptr transf (new PointCloud);
// pcl::transformPointCloud (*cloud1, *transf, GlobalTransf);
pcl::transformPointCloud (*src, *transf, GlobalTransf);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &time2);
LOGI("Time of registration: :%d:%d",diff(time1,time2).tv_sec,diff(time1,time2).tv_nsec);
// LOGI("Transform");
// std::cout<< GlobalTransf <<endl;
// LOGI("point-size: %d", transf->points.size());
ShowPCLtoKiwi(src,255,0,0,-1.5);
ShowPCLtoKiwi(tgt,0,255,0,-1.5);
ShowPCLtoKiwi(transf,255,0,0,1.5);
ShowPCLtoKiwi(tgt,0,255,0,1.5);
}
void point_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr);
float leaf = 0.005;
sor.setLeafSize(leaf, leaf, leaf);
sor.filter(cloud_filtered);
sensor_msgs::PointCloud2 sensor_pcl;
pcl_conversions::moveFromPCL(cloud_filtered, sensor_pcl);
//publish pcl data
pub_voxel.publish(sensor_pcl);
global_counter++;
if((theta == 0.0 && y_offset == 0.0) || global_counter < 800 ){
// part for planar segmentation starts here ..
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_seg(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_rotated(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_transformed(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2 plane_sensor, plane_transf_sensor;
//convert sen
pcl::fromROSMsg(*cloud_msg, *cloud1);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.01);
seg.setInputCloud(cloud1);
seg.segment (*inliers, *coefficients);
Eigen::Matrix<float, 1, 3> surface_normal;
Eigen::Matrix<float, 1, 3> floor_normal;
surface_normal[0] = coefficients->values[0];
surface_normal[1] = coefficients->values[1];
surface_normal[2] = coefficients->values[2];
std::cout << surface_normal[0] << "\n" << surface_normal[1] << "\n" << surface_normal[2];
floor_normal[0] = 0.0;
floor_normal[1] = 1.0;
floor_normal[2] = 0.0;
theta = acos(surface_normal.dot(floor_normal));
//extract the inliers - copied from tutorials...
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud1);
extract.setIndices (inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
ROS_INFO("print cloud info %d", cloud_p->height);
pcl::toROSMsg(*cloud_p, plane_sensor);
pub_plane_simple.publish(plane_sensor);
// anti rotate the point cloud by first finding the angle of rotation
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
transform_1.translation() << 0.0, 0.0, 0.0;
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_rotated, transform_1);
double y_sum = 0;
// int num_points =
for (int i = 0; i < 20; i++){
y_sum = cloud_p_rotated->points[i].y;
}
y_offset = y_sum / 20;
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.translation() << 0.0, -y_offset, 0.0;
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_transformed, transform_2);
pcl::transformPointCloud(*cloud1, *cloud_transformed, transform_2);
//now remove the y offset because of the camera rotation
pcl::toROSMsg(*cloud_p_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud1, plane_transf_sensor);
pub_plane_transf.publish(plane_transf_sensor);
}
ras_msgs::Cam_transform cft;
cft.theta = theta;
cft.y_offset = y_offset;
pub_ctf.publish(cft);
// pub_tf.publish();
}
