DrawPointCloud::DrawPointCloud(std::vector<cv::Point3f> points3d, cv::Mat inputRGB, std::vector<cv::Mat> all_r, std::vector<cv::Mat> all_t) { std::vector<cv::Point2f> allpoints; //for (int row = 0; row < inputRGB.rows; row++) //{ // for (int col = 0; col < inputRGB.cols; col += inputRGB.channels()) // { // } //} cloud.width = points3d.size(); cloud.height = 1; cloud.is_dense = false; cloud.points.resize(cloud.width*cloud.height); for (size_t i = 0; i < cloud.points.size(); i++) { float temp = 255.0; cloud.points[i].x = points3d[i].x ; cloud.points[i].y = points3d[i].y ; cloud.points[i].z = points3d[i].z ; } //pcl::visualization::CloudViewer viewer("point cloud viewer"); pcl::visualization::PCLVisualizer viewer("point cloud viewer");; pcl::PointCloud<pcl::PointXYZ>::Ptr cloudptr(new pcl::PointCloud<pcl::PointXYZ>); *cloudptr = cloud; cv::Mat R0; cv::Rodrigues(all_r[1],R0); viewer.addPointCloud(cloudptr); viewer.addCube(5, 5, 5, 0, 0, 0, 1.0, 0, 0, "cube", 0); viewer.addCoordinateSystem(1, "axis", 0); pcl::io::savePCDFileASCII("pointcloud.pcd", cloud); cv::waitKey(); }
bool FilterFindfloor::getHeightPitchRoll(double height_pitch_roll[]){ // Take the floor part of the PCD: pcl::PointCloud<pcl::PointXYZ> cloudfloor; cloudfloor.width = incloud->points.size(); cloudfloor.height = 1; cloudfloor.is_dense = false; cloudfloor.points.resize (cloudfloor.width * cloudfloor.height); // TODO // OLD: seem to need a duplicate PC as XYZRGBA cannot be used as ransac input // NEW: dec2011: i think this can not be fixed and is not required. mfallon pcl::PointCloud<pcl::PointXYZRGB> cloudfloorRGB; cloudfloorRGB.width = incloud->points.size(); cloudfloorRGB.height = 1; cloudfloorRGB.is_dense = false; cloudfloorRGB.points.resize (cloudfloorRGB.width * cloudfloorRGB.height); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudfloorRGBptr (new pcl::PointCloud<pcl::PointXYZRGB> (cloudfloorRGB)); if(verbose_text>0){ std::cout << "No of points into floor filter: " << incloud->points.size() << std::endl; } int Nransac=0; for(int j3=0; j3< incloud->points.size(); j3++) { if ((incloud->points[j3].z < -0.75 )&& (incloud->points[j3].x < 5 )) { // RANSAC head filter // -0.75m for 2011_03_16_cart_stata // -1.25m for 2011_03_14_brookshire // either -1.10 -1.00m for me cloudfloor.points[Nransac].x =incloud->points[j3].x; cloudfloor.points[Nransac].y =incloud->points[j3].y; cloudfloor.points[Nransac].z =incloud->points[j3].z; //cloudfloor.points[j].rgba =cloud.points[j].rgba; cloudfloorRGB.points[Nransac].x =incloud->points[j3].x; cloudfloorRGB.points[Nransac].y =incloud->points[j3].y; cloudfloorRGB.points[Nransac].z =incloud->points[j3].z; // cloudfloorRGB.points[Nransac].rgb =incloud->points[j3].rgb; //cloudfloorRGB.points[Nransac].rgba =incloud->points[j3].rgba; Nransac++; } } cloudfloor.points.resize (Nransac); cloudfloor.width = (Nransac); cloudfloorRGB.points.resize (Nransac); cloudfloorRGB.width = (Nransac); if(verbose_text>0){ std::cout << "No of points into ransac: " << Nransac << std::endl; } pcl::ModelCoefficients coeff; pcl::PointIndices inliers; // Create the segmentation object pcl::SACSegmentation<pcl::PointXYZ> seg; // Optional seg.setOptimizeCoefficients (true); // Mandatory seg.setModelType (pcl::SACMODEL_PLANE); seg.setMethodType (pcl::SAC_RANSAC); seg.setDistanceThreshold (0.01); // was 0.01m pcl::PointCloud<pcl::PointXYZ>::Ptr cloudptr (new pcl::PointCloud<pcl::PointXYZ> (cloudfloor)); seg.setInputCloud (cloudptr); seg.segment (inliers, coeff); if (inliers.indices.size () == 0) { printf ("[FilterFindfloor::getHeightPitchRoll] Could not estimate a planar model for the given dataset.\n"); return -1; } double pitch = atan(coeff.values[0]/coeff.values[2]); double roll =- atan(coeff.values[1]/coeff.values[2]); double coeff_norm = sqrt(pow(coeff.values[0],2) + pow(coeff.values[1],2) + pow(coeff.values[2],2)); double height = (coeff.values[2]*coeff.values[3]) / coeff_norm; // the point directly under the kinect, having corrected the rotation: // double sub_pt[3]; // sub_pt[1]= -coeff.values[0]*coeff.values[3]/pow(coeff_norm,2) // sub_pt[2]= -coeff.values[1]*coeff.values[3]/pow(coeff_norm,2) // sub_pt[3]= -coeff.values[2]*coeff.values[3]/pow(coeff_norm,2) if (verbose_text>0){ cout << "\nRANSAC Floor Coefficients: " << coeff.values[0] << " " << coeff.values[1] << " " << coeff.values[2] << " " << coeff.values[3] << endl; cout << "Pitch: " << pitch << " (" << (pitch*180/M_PI) << "d). positive nose down\n"; cout << "Roll : " << roll << " (" << (roll*180/M_PI) << "d). positive right side down\n"; cout << "Height : " << height << " of device off ground [m]\n"; cout << "Total points: " << Nransac << ". inliers: " << inliers.indices.size () << endl << endl; //std::cout << std::setprecision(20) << "corresponding obj_coll " << state->bot_id // << " and kinect " << state->msg->timestamp << "\n"; } int floor_to_file =0; if(floor_to_file==1){ //pcl::io::savePCDFile ("RANSAC_floorcloud.pcd", cloudfloor, false); //std::cout << "about to publish to RANSAC_floorcloud.pcd - " << cloudfloor.points.size () << "pts" << std::endl; } // Filter Ridiculous Values from the Ransac Floor estimator: int skip_floor_estimate =0; if ((height > 3) || (height < 0.8)){ skip_floor_estimate =1; } if ((pitch > 10 ) || (height < -10)){ skip_floor_estimate =1; } if ((roll > 10 ) || (roll < -10)){ skip_floor_estimate =1; } if (skip_floor_estimate){ std::cout << "Ransac has produced ridiculous results, ignore\n"; height= last_height_pitch_roll[0]; pitch= last_height_pitch_roll[1]; roll= last_height_pitch_roll[2]; } // use a fixed prior for the DOFs estimated by the kinect: (for testing) /* int use_fixed_prior=1; if (use_fixed_prior){ roll = -0.5*M_PI/180;// 5*M_PI/180 ; pitch = 9.5*M_PI/180;// 5*M_PI/180 ; height = 1.2; } */ if(verbose_lcm>0){ // important // turn inlier points red: for (size_t i = 0; i < inliers.indices.size (); ++i){ size_t this_index = inliers.indices[i]; unsigned char red[]={255,0,0,0}; // rgba unsigned char* rgba_ptr = (unsigned char*)&cloudfloorRGB.points[this_index].rgba; (*rgba_ptr) = red[0]; (*(rgba_ptr+1)) = red[1]; (*(rgba_ptr+2)) = red[2]; (*(rgba_ptr+3)) = red[3]; } int floor_obj_collection; int64_t floor_obj_element_id; floor_obj_collection= 4; floor_obj_element_id = pose_element_id;// bot_timestamp_now(); /*Ptcoll_cfg floorcoll_cfg; floorcoll_cfg.id = 1; floorcoll_cfg.name ="Floor RANSAC"; floorcoll_cfg.reset=true; floorcoll_cfg.type =1; floorcoll_cfg.point_lists_id = pose_element_id ; floorcoll_cfg.collection = floor_obj_collection; floorcoll_cfg.element_id = floor_obj_element_id; floorcoll_cfg.npoints = Nransac; float colorm_temp0[] ={-1.0,-1.0,-1.0,-1.0}; floorcoll_cfg.rgba.assign(colorm_temp0,colorm_temp0+4*sizeof(float)); // floorcoll_cfg.rgba = {-1,-1,-1,-1}; pcdXYZRGB_to_lcm(publish_lcm,floorcoll_cfg, cloudfloorRGB); */ if (verbose_lcm > 2){ // unimportant // 0 - position (fixed) at 000 with corrected pitch and roll vs_obj_collection_t objs; objs.id = floor_obj_collection; objs.name = (char*) "Floor Pose"; // "Trajectory"; objs.type = 1; // a pose objs.reset = 0; // true will delete them from the viewer objs.nobjs = 1; vs_obj_t poses[objs.nobjs]; poses[0].id = floor_obj_element_id; poses[0].x = 0;// state->bot_pos[0] ; poses[0].y = 0;// state->bot_pos[1] ; poses[0].z = height; // state->bot_pos[2] ; poses[0].yaw = 0;// state->bot_rpy[2] ; poses[0].pitch = -pitch; //state->bot_rpy[1]; poses[0].roll = -roll; //state->bot_rpy[0]; objs.objs = poses; vs_obj_collection_t_publish(publish_lcm, "OBJ_COLLECTION", &objs); bot_core_pose_t testdata; testdata.utime = pose_element_id; testdata.pos[0]=0; testdata.pos[1]=0; testdata.pos[2]=height; Eigen::Quaterniond quat =euler_to_quat(roll,pitch,0); // rpy testdata.orientation[0] = quat.w(); testdata.orientation[1] = quat.x(); testdata.orientation[2] = quat.y(); testdata.orientation[3] = quat.z(); // double temp_rpy[] ={ roll, pitch, 0}; // no yaw estimate // bot_roll_pitch_yaw_to_quat(temp_rpy, testdata.orientation) ; testdata.vel[0] =0; testdata.vel[1] =0; testdata.vel[2] =0; testdata.rotation_rate[0] =0; testdata.rotation_rate[1] =0; testdata.rotation_rate[2] =0; testdata.accel[0] =0; testdata.accel[1] =0; testdata.accel[2] =0; bot_core_pose_t_publish(publish_lcm,"POSE_FINDGROUND", &testdata); } } height_pitch_roll[0] = height; height_pitch_roll[1] = pitch; height_pitch_roll[2] = roll; return 1; }