void pointCloudCallback(const sensor_msgs::PointCloud2& traversability_msg) { //pcl::PointCloud<pcl::PointXYZI> traversability_pcl; pcl::fromROSMsg(traversability_msg, traversability_pcl); ROS_INFO("path planner input set"); if (traversability_pcl.size() > 0 && wall_flag){ tf::StampedTransform robot_pose; getRobotPose(robot_pose); pcl::PointXYZI robot; robot.x = robot_pose.getOrigin().x(); robot.y = robot_pose.getOrigin().y(); robot.z = robot_pose.getOrigin().z(); //pcl::KdTreeFLANN<pcl::PointXYZI> traversability_kdtree; traversability_kdtree.setInputCloud(traversability_pcl.makeShared()); std::vector<int> pointIdxNKNSearch(1); std::vector<float> pointNKNSquaredDistance(1); traversability_kdtree.nearestKSearch(robot, 1, pointIdxNKNSearch, pointNKNSquaredDistance); robot_idx = pointIdxNKNSearch[0]; //---------------------------------------------------------------------------------------------------------------- //ho commentato questa parte perchè vogliamo disaccoppiare il planning dall'acquisizione della Point Cloud //---------------------------------------------------------------------------------------------------------------- // planner->set_input(traversability_pcl, wall_pcl, wall_kdTree, traversability_kdtree, pointIdxNKNSearch[0]); // wall_flag = false; // if (goal_selection){ // nav_msgs::Path path; // ROS_INFO("compute path"); // if(goal_selection){ // if(planner->planning(path)){ // path_pub.publish(path); // } // else{ // ROS_INFO("no path exist for desired goal, please choose another goal"); // goal_selection = true; // } // ROS_INFO("path_computed"); // } // } } }
//if a point in a plane bool isInPlane(Point p, pcl::KdTreeFLANN<Point> tree){ // Neighbors containers vector<int> k_indices; vector<float> k_distances; tree.radiusSearch (p, 0.005, k_indices, k_distances); if(k_indices.size()>0){ return true; } return false; }
std::vector<Quadric> HandSearch::findQuadrics(const PointCloud::Ptr cloud, const Eigen::VectorXi& pts_cam_source, const pcl::KdTreeFLANN<pcl::PointXYZ>& kdtree, const std::vector<int>& indices) { double t1 = omp_get_wtime(); std::vector<int> nn_indices; std::vector<float> nn_dists; std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > T_cams; T_cams.push_back(cam_tf_left_); T_cams.push_back(cam_tf_right_); std::vector<Quadric> quadric_list(indices.size()); #ifdef _OPENMP // parallelization using OpenMP #pragma omp parallel for private(nn_indices, nn_dists) num_threads(num_threads_) #endif for (int i = 0; i < indices.size(); i++) { const pcl::PointXYZ& sample = cloud->points[indices[i]]; // std::cout << "i: " << i << ", index: " << indices[i] << ", sample: " << sample << std::endl; if (kdtree.radiusSearch(sample, nn_radius_taubin_, nn_indices, nn_dists) > 0) { Eigen::VectorXi nn_cam_source(nn_indices.size()); // std::cout << " Found " << nn_indices.size() << " neighbors.\n"; for (int j = 0; j < nn_cam_source.size(); j++) { nn_cam_source(j) = pts_cam_source(nn_indices[j]); } Eigen::Vector3d sample_eigen = sample.getVector3fMap().cast<double>(); Quadric q(T_cams, cloud, sample_eigen, uses_determinstic_normal_estimation_); q.fitQuadric(nn_indices); // std::cout << " Fitted quadric\n"; q.findTaubinNormalAxis(nn_indices, nn_cam_source); // std::cout << " Found local axes\n"; quadric_list[i] = q; cloud_normals_.col(indices[i]) = q.getNormal(); } } double t2 = omp_get_wtime(); //std::cout << "Fitted " << quadric_list.size() << " quadrics in " << t2 - t1 << " sec.\n"; // quadric_list[0].print(); // debugging // plot_.plotLocalAxes(quadric_list, cloud); return quadric_list; }
Extractor() { tree.reset(new pcl::KdTreeFLANN<Point > ()); cloud.reset(new pcl::PointCloud<Point>); cloud_filtered.reset(new pcl::PointCloud<Point>); cloud_normals.reset(new pcl::PointCloud<pcl::Normal>); coefficients_plane_.reset(new pcl::ModelCoefficients); coefficients_cylinder_.reset(new pcl::ModelCoefficients); inliers_plane_.reset(new pcl::PointIndices); inliers_cylinder_.reset(new pcl::PointIndices); // Filter Pass pass.setFilterFieldName("z"); pass.setFilterLimits(-100, 100); // VoxelGrid for Downsampling LEAFSIZE = 0.01f; vg.setLeafSize(LEAFSIZE, LEAFSIZE, LEAFSIZE); // Any object < CUT_THRESHOLD will be abandoned. //CUT_THRESHOLD = (int) (LEAFSIZE * LEAFSIZE * 7000000); // 700 CUT_THRESHOLD = 700; //for nonfiltering // Clustering cluster_.setClusterTolerance(0.06 * UNIT); cluster_.setMinClusterSize(50.0); cluster_.setSearchMethod(clusters_tree_); // Normals ne.setSearchMethod(tree); ne.setKSearch(50); // 50 by default // plane SAC seg_plane.setOptimizeCoefficients(true); seg_plane.setModelType(pcl::SACMODEL_NORMAL_PLANE); seg_plane.setNormalDistanceWeight(0.1); // 0.1 seg_plane.setMethodType(pcl::SAC_RANSAC); seg_plane.setMaxIterations(1000); // 10000 seg_plane.setDistanceThreshold(0.05); // 0.03 // cylinder SAC seg_cylinder.setOptimizeCoefficients(true); seg_cylinder.setModelType(pcl::SACMODEL_CYLINDER); seg_cylinder.setMethodType(pcl::SAC_RANSAC); seg_cylinder.setNormalDistanceWeight(0.1); seg_cylinder.setMaxIterations(10000); seg_cylinder.setDistanceThreshold(0.02); // 0.05 seg_cylinder.setRadiusLimits(0.02, 0.07); // [0, 0.1] }
float averageKDistance( pcl::KdTreeFLANN<PointT> & kdtree, std::vector<int> & pointIdxNKNSearch, std::vector<float> & pointNKNSquaredDistance, PointT point, int K ) { if (kdtree.nearestKSearch(point, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) { return average(pointNKNSquaredDistance); } else { return -1; } }
float GetPointDelatZ(pcl::PointXYZ &new_Point, pcl::PointCloud<pcl::PointXY>::Ptr old2d_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr old_cloud, pcl::KdTreeFLANN<pcl::PointXY> &kdtree, int &oldpointindex){ //pcl::PointCloud<pcl::PointXY>::Ptr old2dlayercloud_cloud(new pcl::PointCloud<pcl::PointXY>); std::vector<int> pointId_vector; //Vector with 1 lenght for save index of nearest point in old_cloud std::vector<float> pointRadius_vector; pcl::PointXY *searchPoint = new pcl::PointXY(); searchPoint->x = new_Point.x; searchPoint->y = new_Point.y; float deltaZ = 0; if (kdtree.nearestKSearch(*searchPoint, 1, pointId_vector, pointRadius_vector) > 0) { pcl::PointXYZ founded_old_point = old_cloud->points[pointId_vector[0]]; oldpointindex = pointId_vector[0]; deltaZ = founded_old_point.z - new_Point.z; //Keep that axiz oZ is inverted (goes from up to down) } return deltaZ; }
int main(int argc, char** argv) { ros::init(argc, argv, "laserMapping"); ros::NodeHandle nh; ros::Subscriber subLaserCloudLast2 = nh.subscribe<sensor_msgs::PointCloud2> ("/laser_cloud_last_2", 2, laserCloudLastHandler); ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry> ("/cam_to_init", 5, laserOdometryHandler); ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2> ("/laser_cloud_surround", 1); //ros::Publisher pub1 = nh.advertise<sensor_msgs::PointCloud2> ("/pc1", 1); //ros::Publisher pub2 = nh.advertise<sensor_msgs::PointCloud2> ("/pc2", 1); //ros::Publisher pub3 = nh.advertise<sensor_msgs::PointCloud2> ("/pc3", 1); //ros::Publisher pub4 = nh.advertise<sensor_msgs::PointCloud2> ("/pc4", 1); ros::Publisher pubOdomBefMapped = nh.advertise<nav_msgs::Odometry> ("/bef_mapped_to_init_2", 5); nav_msgs::Odometry odomBefMapped; odomBefMapped.header.frame_id = "/camera_init_2"; odomBefMapped.child_frame_id = "/bef_mapped"; ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init_2", 5); nav_msgs::Odometry odomAftMapped; odomAftMapped.header.frame_id = "/camera_init_2"; odomAftMapped.child_frame_id = "/aft_mapped"; std::vector<int> pointSearchInd; std::vector<float> pointSearchSqDis; pcl::PointXYZHSV pointOri, pointSel, pointProj, coeff; pcl::PointXYZI pointSurround; cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0)); cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1)); cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0)); cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0)); cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0)); cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0)); for (int i = 0; i < laserCloudNum; i++) { laserCloudArray[i].reset(new pcl::PointCloud<pcl::PointXYZHSV>()); } bool status = ros::ok(); while (status) { ros::spinOnce(); if (newLaserCloudLast && newLaserOdometry && fabs(timeLaserCloudLast - timeLaserOdometry) < 0.005) { newLaserCloudLast = false; newLaserOdometry = false; transformAssociateToMap(); pcl::PointXYZHSV pointOnZAxis; pointOnZAxis.x = 0.0; pointOnZAxis.y = 0.0; pointOnZAxis.z = 10.0; pointAssociateToMap(&pointOnZAxis, &pointOnZAxis); int centerCubeI = int((transformTobeMapped[3] + 10.0) / 20.0) + laserCloudCenWidth; int centerCubeJ = int((transformTobeMapped[4] + 10.0) / 20.0) + laserCloudCenHeight; int centerCubeK = int((transformTobeMapped[5] + 10.0) / 20.0) + laserCloudCenDepth; if (transformTobeMapped[3] + 10.0 < 0) centerCubeI--; if (transformTobeMapped[4] + 10.0 < 0) centerCubeJ--; if (transformTobeMapped[5] + 10.0 < 0) centerCubeK--; if (centerCubeI < 0 || centerCubeI >= laserCloudWidth || centerCubeJ < 0 || centerCubeJ >= laserCloudHeight || centerCubeK < 0 || centerCubeK >= laserCloudDepth) { transformUpdate(); continue; } int laserCloudValidNum = 0; int laserCloudSurroundNum = 0; for (int i = centerCubeI - 1; i <= centerCubeI + 1; i++) { for (int j = centerCubeJ - 1; j <= centerCubeJ + 1; j++) { for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++) { if (i >= 0 && i < laserCloudWidth && j >= 0 && j < laserCloudHeight && k >= 0 && k < laserCloudDepth) { float centerX = 20.0 * (i - laserCloudCenWidth); float centerY = 20.0 * (j - laserCloudCenHeight); float centerZ = 20.0 * (k - laserCloudCenDepth); bool isInLaserFOV = false; for (int ii = -1; ii <= 1; ii += 2) { for (int jj = -1; jj <= 1; jj += 2) { for (int kk = -1; kk <= 1; kk += 2) { float cornerX = centerX + 10.0 * ii; float cornerY = centerY + 10.0 * jj; float cornerZ = centerZ + 10.0 * kk; float check = 100.0 + (transformTobeMapped[3] - cornerX) * (transformTobeMapped[3] - cornerX) + (transformTobeMapped[4] - cornerY) * (transformTobeMapped[4] - cornerY) + (transformTobeMapped[5] - cornerZ) * (transformTobeMapped[5] - cornerZ) - (pointOnZAxis.x - cornerX) * (pointOnZAxis.x - cornerX) - (pointOnZAxis.y - cornerY) * (pointOnZAxis.y - cornerY) - (pointOnZAxis.z - cornerZ) * (pointOnZAxis.z - cornerZ); if (check > 0) { isInLaserFOV = true; } } } } if (isInLaserFOV) { laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; laserCloudValidNum++; } laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k; laserCloudSurroundNum++; } } } } laserCloudFromMap->clear(); for (int i = 0; i < laserCloudValidNum; i++) { *laserCloudFromMap += *laserCloudArray[laserCloudValidInd[i]]; } int laserCloudFromMapNum = laserCloudFromMap->points.size(); laserCloudCornerFromMap->clear(); laserCloudSurfFromMap->clear(); for (int i = 0; i < laserCloudFromMapNum; i++) { if (fabs(laserCloudFromMap->points[i].v - 2) < 0.005 || fabs(laserCloudFromMap->points[i].v - 1) < 0.005) { laserCloudCornerFromMap->push_back(laserCloudFromMap->points[i]); } else { laserCloudSurfFromMap->push_back(laserCloudFromMap->points[i]); } } laserCloudCorner->clear(); laserCloudSurf->clear(); int laserCloudLastNum = laserCloudLast->points.size(); for (int i = 0; i < laserCloudLastNum; i++) { if (fabs(laserCloudLast->points[i].v - 2) < 0.005 || fabs(laserCloudLast->points[i].v - 1) < 0.005) { laserCloudCorner->push_back(laserCloudLast->points[i]); } else { laserCloudSurf->push_back(laserCloudLast->points[i]); } } laserCloudCorner2->clear(); pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter; downSizeFilter.setInputCloud(laserCloudCorner); downSizeFilter.setLeafSize(0.05, 0.05, 0.05); downSizeFilter.filter(*laserCloudCorner2); laserCloudSurf2->clear(); downSizeFilter.setInputCloud(laserCloudSurf); downSizeFilter.setLeafSize(0.1, 0.1, 0.1); downSizeFilter.filter(*laserCloudSurf2); laserCloudLast->clear(); *laserCloudLast = *laserCloudCorner2 + *laserCloudSurf2; laserCloudLastNum = laserCloudLast->points.size(); if (laserCloudSurfFromMap->points.size() > 500) { kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap); kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap); for (int iterCount = 0; iterCount < 10; iterCount++) { laserCloudOri->clear(); //laserCloudSel->clear(); //laserCloudCorr->clear(); //laserCloudProj->clear(); coeffSel->clear(); for (int i = 0; i < laserCloudLastNum; i++) { if (fabs(laserCloudLast->points[i].x > 0.3) || fabs(laserCloudLast->points[i].y > 0.3) || fabs(laserCloudLast->points[i].z > 0.3)) { pointOri = laserCloudLast->points[i]; pointAssociateToMap(&pointOri, &pointSel); if (fabs(pointOri.v) < 0.05 || fabs(pointOri.v + 1) < 0.05) { kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); if (pointSearchSqDis[4] < 1.0) { for (int j = 0; j < 5; j++) { matA0.at<float>(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x; matA0.at<float>(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y; matA0.at<float>(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z; } cv::solve(matA0, matB0, matX0, cv::DECOMP_QR); float pa = matX0.at<float>(0, 0); float pb = matX0.at<float>(1, 0); float pc = matX0.at<float>(2, 0); float pd = 1; float ps = sqrt(pa * pa + pb * pb + pc * pc); pa /= ps; pb /= ps; pc /= ps; pd /= ps; bool planeValid = true; for (int j = 0; j < 5; j++) { if (fabs(pa * laserCloudSurfFromMap->points[pointSearchInd[j]].x + pb * laserCloudSurfFromMap->points[pointSearchInd[j]].y + pc * laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.05) { planeValid = false; break; } } if (planeValid) { float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd; pointProj = pointSel; pointProj.x -= pa * pd2; pointProj.y -= pb * pd2; pointProj.z -= pc * pd2; float s = 1; if (iterCount >= 6) { s = 1 - 8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x + pointSel.y * pointSel.y + pointSel.z * pointSel.z)); } coeff.x = s * pa; coeff.y = s * pb; coeff.z = s * pc; coeff.h = s * pd2; if (s > 0.2) { laserCloudOri->push_back(pointOri); //laserCloudSel->push_back(pointSel); //laserCloudProj->push_back(pointProj); //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[0]]); //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[1]]); //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[2]]); //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[3]]); //laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[4]]); coeffSel->push_back(coeff); } } } } else { kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); if (pointSearchSqDis[4] < 1.0) { float cx = 0; float cy = 0; float cz = 0; for (int j = 0; j < 5; j++) { cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x; cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y; cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z; } cx /= 5; cy /= 5; cz /= 5; float a11 = 0; float a12 = 0; float a13 = 0; float a22 = 0; float a23 = 0; float a33 = 0; for (int j = 0; j < 5; j++) { float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx; float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy; float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz; a11 += ax * ax; a12 += ax * ay; a13 += ax * az; a22 += ay * ay; a23 += ay * az; a33 += az * az; } a11 /= 5; a12 /= 5; a13 /= 5; a22 /= 5; a23 /= 5; a33 /= 5; matA1.at<float>(0, 0) = a11; matA1.at<float>(0, 1) = a12; matA1.at<float>(0, 2) = a13; matA1.at<float>(1, 0) = a12; matA1.at<float>(1, 1) = a22; matA1.at<float>(1, 2) = a23; matA1.at<float>(2, 0) = a13; matA1.at<float>(2, 1) = a23; matA1.at<float>(2, 2) = a33; cv::eigen(matA1, matD1, matV1); if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) { float x0 = pointSel.x; float y0 = pointSel.y; float z0 = pointSel.z; float x1 = cx + 0.1 * matV1.at<float>(0, 0); float y1 = cy + 0.1 * matV1.at<float>(0, 1); float z1 = cz + 0.1 * matV1.at<float>(0, 2); float x2 = cx - 0.1 * matV1.at<float>(0, 0); float y2 = cy - 0.1 * matV1.at<float>(0, 1); float z2 = cz - 0.1 * matV1.at<float>(0, 2); float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))); float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2)); float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12; float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12; float ld2 = a012 / l12; pointProj = pointSel; pointProj.x -= la * ld2; pointProj.y -= lb * ld2; pointProj.z -= lc * ld2; float s = 2 * (1 - 8 * fabs(ld2)); coeff.x = s * la; coeff.y = s * lb; coeff.z = s * lc; coeff.h = s * ld2; if (s > 0.4) { laserCloudOri->push_back(pointOri); //laserCloudSel->push_back(pointSel); //laserCloudProj->push_back(pointProj); //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[0]]); //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[1]]); //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[2]]); //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[3]]); //laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[4]]); coeffSel->push_back(coeff); } } } } } } int laserCloudSelNum = laserCloudOri->points.size(); float srx = sin(transformTobeMapped[0]); float crx = cos(transformTobeMapped[0]); float sry = sin(transformTobeMapped[1]); float cry = cos(transformTobeMapped[1]); float srz = sin(transformTobeMapped[2]); float crz = cos(transformTobeMapped[2]); if (laserCloudSelNum < 50) { continue; } cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0)); cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0)); cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0)); cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0)); for (int i = 0; i < laserCloudSelNum; i++) { pointOri = laserCloudOri->points[i]; coeff = coeffSel->points[i]; float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z; float ary = ((cry*srx*srz - crz*sry)*pointOri.x + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x + ((-cry*crz - srx*sry*srz)*pointOri.x + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z; float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz - srx*sry*srz)*pointOri.y)*coeff.x + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry - cry*srx*srz)*pointOri.y)*coeff.z; matA.at<float>(i, 0) = arx; matA.at<float>(i, 1) = ary; matA.at<float>(i, 2) = arz; matA.at<float>(i, 3) = coeff.x; matA.at<float>(i, 4) = coeff.y; matA.at<float>(i, 5) = coeff.z; matB.at<float>(i, 0) = -coeff.h; } cv::transpose(matA, matAt); matAtA = matAt * matA; matAtB = matAt * matB; cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR); if (fabs(matX.at<float>(0, 0)) < 0.5 && fabs(matX.at<float>(1, 0)) < 0.5 && fabs(matX.at<float>(2, 0)) < 0.5 && fabs(matX.at<float>(3, 0)) < 1 && fabs(matX.at<float>(4, 0)) < 1 && fabs(matX.at<float>(5, 0)) < 1) { transformTobeMapped[0] += matX.at<float>(0, 0); transformTobeMapped[1] += matX.at<float>(1, 0); transformTobeMapped[2] += matX.at<float>(2, 0); transformTobeMapped[3] += matX.at<float>(3, 0); transformTobeMapped[4] += matX.at<float>(4, 0); transformTobeMapped[5] += matX.at<float>(5, 0); } else { //ROS_INFO ("Mapping update out of bound"); } } } transformUpdate(); for (int i = 0; i < laserCloudLastNum; i++) { if (fabs(laserCloudLast->points[i].x) > 0.3 || fabs(laserCloudLast->points[i].y) > 0.3 || fabs(laserCloudLast->points[i].z) > 0.3) { pointAssociateToMap(&laserCloudLast->points[i], &pointSel); int cubeI = int((pointSel.x + 10.0) / 20.0) + laserCloudCenWidth; int cubeJ = int((pointSel.y + 10.0) / 20.0) + laserCloudCenHeight; int cubeK = int((pointSel.z + 10.0) / 20.0) + laserCloudCenDepth; if (pointSel.x + 10.0 < 0) cubeI--; if (pointSel.y + 10.0 < 0) cubeJ--; if (pointSel.z + 10.0 < 0) cubeK--; int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK; laserCloudArray[cubeInd]->push_back(pointSel); } } for (int i = 0; i < laserCloudValidNum; i++) { laserCloudCorner->clear(); laserCloudSurf->clear(); pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCubePointer = laserCloudArray[laserCloudValidInd[i]]; int laserCloudCubeNum = laserCloudCubePointer->points.size(); for (int j = 0; j < laserCloudCubeNum; j++) { if (fabs(laserCloudCubePointer->points[j].v - 2) < 0.005 || fabs(laserCloudCubePointer->points[j].v - 1) < 0.005) { laserCloudCorner->push_back(laserCloudCubePointer->points[j]); } else { laserCloudSurf->push_back(laserCloudCubePointer->points[j]); } } laserCloudCorner2->clear(); pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter; downSizeFilter.setInputCloud(laserCloudCorner); downSizeFilter.setLeafSize(0.05, 0.05, 0.05); downSizeFilter.filter(*laserCloudCorner2); laserCloudSurf2->clear(); downSizeFilter.setInputCloud(laserCloudSurf); downSizeFilter.setLeafSize(0.1, 0.1, 0.1); downSizeFilter.filter(*laserCloudSurf2); laserCloudCubePointer->clear(); *laserCloudCubePointer = *laserCloudCorner2 + *laserCloudSurf2; } laserCloudSurround->clear(); for (int i = 0; i < laserCloudSurroundNum; i++) { pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCubePointer = laserCloudArray[laserCloudSurroundInd[i]]; int laserCloudCubeNum = laserCloudCubePointer->points.size(); for (int j = 0; j < laserCloudCubeNum; j++) { pointSurround.x = laserCloudCubePointer->points[j].x; pointSurround.y = laserCloudCubePointer->points[j].y; pointSurround.z = laserCloudCubePointer->points[j].z; pointSurround.intensity = laserCloudCubePointer->points[j].h; laserCloudSurround->push_back(pointSurround); } } sensor_msgs::PointCloud2 laserCloudSurround2; pcl::toROSMsg(*laserCloudSurround, laserCloudSurround2); laserCloudSurround2.header.stamp = ros::Time().fromSec(timeLaserCloudLast); laserCloudSurround2.header.frame_id = "/camera_init_2"; pubLaserCloudSurround.publish(laserCloudSurround2); geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw (transformBefMapped[2], -transformBefMapped[0], -transformBefMapped[1]); odomBefMapped.header.stamp = ros::Time().fromSec(timeLaserCloudLast); odomBefMapped.pose.pose.orientation.x = -geoQuat.y; odomBefMapped.pose.pose.orientation.y = -geoQuat.z; odomBefMapped.pose.pose.orientation.z = geoQuat.x; odomBefMapped.pose.pose.orientation.w = geoQuat.w; odomBefMapped.pose.pose.position.x = transformBefMapped[3]; odomBefMapped.pose.pose.position.y = transformBefMapped[4]; odomBefMapped.pose.pose.position.z = transformBefMapped[5]; pubOdomBefMapped.publish(odomBefMapped); geoQuat = tf::createQuaternionMsgFromRollPitchYaw (transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]); odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserCloudLast); odomAftMapped.pose.pose.orientation.x = -geoQuat.y; odomAftMapped.pose.pose.orientation.y = -geoQuat.z; odomAftMapped.pose.pose.orientation.z = geoQuat.x; odomAftMapped.pose.pose.orientation.w = geoQuat.w; odomAftMapped.pose.pose.position.x = transformAftMapped[3]; odomAftMapped.pose.pose.position.y = transformAftMapped[4]; odomAftMapped.pose.pose.position.z = transformAftMapped[5]; pubOdomAftMapped.publish(odomAftMapped); /*sensor_msgs::PointCloud2 pc12; pcl::toROSMsg(*laserCloudCornerFromMap, pc12); pc12.header.stamp = ros::Time().fromSec(timeLaserCloudLast); pc12.header.frame_id = "/camera_init_2"; pub1.publish(pc12); sensor_msgs::PointCloud2 pc22; pcl::toROSMsg(*laserCloudSel, pc22); pc22.header.stamp = ros::Time().fromSec(timeLaserCloudLast); pc22.header.frame_id = "/camera_init_2"; pub2.publish(pc22); sensor_msgs::PointCloud2 pc32; pcl::toROSMsg(*laserCloudCorr, pc32); pc32.header.stamp = ros::Time().fromSec(timeLaserCloudLast); pc32.header.frame_id = "/camera_init_2"; pub3.publish(pc32); sensor_msgs::PointCloud2 pc42; pcl::toROSMsg(*laserCloudProj, pc42); pc42.header.stamp = ros::Time().fromSec(timeLaserCloudLast); pc42.header.frame_id = "/camera_init_2"; pub4.publish(pc42);*/ } status = ros::ok(); cv::waitKey(10); } return 0; }
std::vector<GraspHypothesis> HandSearch::findHands(const PointCloud::Ptr cloud, const Eigen::VectorXi& pts_cam_source, const std::vector<Quadric>& quadric_list, const Eigen::VectorXi& hands_cam_source, const pcl::KdTreeFLANN<pcl::PointXYZ>& kdtree) { double t1 = omp_get_wtime(); std::vector<int> nn_indices; std::vector<float> nn_dists; Eigen::Matrix3Xd nn_normals(3, nn_indices.size()); Eigen::VectorXi nn_cam_source(nn_indices.size()); Eigen::Matrix3Xd centered_neighborhood(3, nn_indices.size()); std::vector<RotatingHand> hand_list(quadric_list.size()); // std::vector<RotatingHand> hand_list; double time_eval_hand = 0.0; double time_iter = 0.0; double time_nn = 0.0; double time_tf = 0.0; std::vector< std::vector<GraspHypothesis> > grasp_lists(quadric_list.size(), std::vector<GraspHypothesis>(0)); #ifdef _OPENMP // parallelization using OpenMP #pragma omp parallel for private(nn_indices, nn_dists, nn_normals, nn_cam_source, centered_neighborhood) num_threads(num_threads_) #endif for (std::size_t i = 0; i < quadric_list.size(); i++) { double timei = omp_get_wtime(); pcl::PointXYZ sample; sample.x = quadric_list[i].getSample()(0); sample.y = quadric_list[i].getSample()(1); sample.z = quadric_list[i].getSample()(2); // std::cout << "i: " << i << ", sample: " << sample << std::endl; if (kdtree.radiusSearch(sample, nn_radius_hands_, nn_indices, nn_dists) > 0) { time_nn += omp_get_wtime() - timei; nn_normals.setZero(3, nn_indices.size()); nn_cam_source.setZero(nn_indices.size()); centered_neighborhood.setZero(3, nn_indices.size()); for (int j = 0; j < nn_indices.size(); j++) { nn_cam_source(j) = pts_cam_source(nn_indices[j]); centered_neighborhood.col(j) = (cloud->points[nn_indices[j]].getVector3fMap() - sample.getVector3fMap()).cast<double>(); nn_normals.col(j) = cloud_normals_.col(nn_indices[j]); } FingerHand finger_hand(finger_width_, hand_outer_diameter_, hand_depth_); Eigen::Vector3d sample_eig = sample.getVector3fMap().cast<double>(); RotatingHand rotating_hand(cam_tf_left_.block<3, 1>(0, 3) - sample_eig, cam_tf_right_.block<3, 1>(0, 3) - sample_eig, finger_hand, tolerant_antipodal_, hands_cam_source(i)); const Quadric& q = quadric_list[i]; double time_tf1 = omp_get_wtime(); rotating_hand.transformPoints(centered_neighborhood, q.getNormal(), q.getCurvatureAxis(), nn_normals, nn_cam_source, hand_height_); time_tf += omp_get_wtime() - time_tf1; double time_eval1 = omp_get_wtime(); std::vector<GraspHypothesis> grasps = rotating_hand.evaluateHand(init_bite_, sample_eig, true); time_eval_hand += omp_get_wtime() - time_eval1; if (grasps.size() > 0) { // grasp_list.insert(grasp_list.end(), grasps.begin(), grasps.end()); grasp_lists[i] = grasps; } } time_iter += omp_get_wtime() - timei; } time_eval_hand /= quadric_list.size(); time_nn /= quadric_list.size(); time_iter /= quadric_list.size(); time_tf /= quadric_list.size(); //std::cout << " avg time for transforming point neighborhood: " << time_tf << " sec.\n"; //std::cout << " avg time for NN search: " << time_nn << " sec.\n"; //std::cout << " avg time for rotating_hand.evaluate(): " << time_eval_hand << " sec.\n"; //std::cout << " avg time per iteration: " << time_iter << " sec.\n"; std::vector<GraspHypothesis> grasp_list; for (std::size_t i = 0; i < grasp_lists.size(); i++) { // std::cout << i << " " << grasp_lists[i].size() << "\n"; if (grasp_lists[i].size() > 0) grasp_list.insert(grasp_list.end(), grasp_lists[i].begin(), grasp_lists[i].end()); } double t2 = omp_get_wtime(); //std::cout << " Found " << grasp_list.size() << " robot hand poses in " << t2 - t1 << " sec.\n"; return grasp_list; }
void wallCallback(const sensor_msgs::PointCloud2& wall_msg){ pcl::fromROSMsg(wall_msg, wall_pcl); wall_kdTree.setInputCloud(wall_pcl.makeShared()); wall_flag = true; }