QDebug operator << (QDebug dbg, const Histogram &histogram) { dbg.nospace() << "["; for (int i = histogram.min(); i <= histogram.max(); ++i) { if (histogram.at(i) > 0) { dbg.nospace() << i << "=" << histogram.at(i); if (i <= histogram.max() - 1) dbg.nospace() << " "; } } dbg.nospace() << "]"; return dbg.space(); }
pcl::PointCloud<pcl::PointXYZRGB> KinectFilter::remove_error(Histogram noise) { // Only remove Z error for now.... std::vector<float> avg_distance(noise.size(), 0.0), sum_distance(noise.size(), 0.0), number_distance(noise.size(), 0.0); pcl::PointCloud<pcl::PointXYZRGB> p_out_cloud_ret; for (size_t i = 0; i < p_in_cloud->size(); i++) { Eigen::Vector3f xyz = p_in_cloud->points.at(i).getVector3fMap(); Eigen::Vector3i rgb; rgb = p_in_cloud->points.at(i).getRGBVector3i(); for (size_t j = 0; j < noise.size(); j++) { if (xyz(2) >= noise.at(j).r_min && xyz(2) < noise.at(j).r_max) { xyz(2) -= noise.at(j).err; sum_distance.at(j) += xyz(2); number_distance.at(j)++; pcl::PointXYZRGB filtered_p(rgb(0), rgb(1), rgb(2)); filtered_p.getVector3fMap() = xyz; p_out_cloud_ret.points.push_back(filtered_p); break; } } } for (size_t j = 0; j < noise.size(); j++) { avg_distance.at(j) = sum_distance.at(j)/number_distance.at(j); std::cout << "distance " << j << ": " << avg_distance.at(j) << std::endl; } return p_out_cloud_ret; }
int main(int argc, char **argv) { ros::init(argc, argv, "kinect_filter"); ros::NodeHandle nh; Histogram hist; KinectFilter kf(nh); ros::Publisher pub_out_cloud = nh.advertise<sensor_msgs::PointCloud2>("camera/depth_registered/filtered/points", 1); pcl::PointCloud<pcl::PointXYZRGB> out_cloud; std::fstream myfile("/home/luc/indigo/histogram.dat", std::ios_base::in); float a; int i = 0; int j; while (myfile >> a) { j = std::floor(i/4); Bin b; if(i%4 == 0) { b.quantity = a; } else if(i%4 == 1) { b.err = a; } else if(i%4 == 2) { b.r_min = a; } else if(i%4 == 3) { b.r_max = a; hist.push_back(b); } i++; } sensor_msgs::PointCloud2 ros_view_cloud; ROS_INFO("got histogram..."); for (size_t i = 0; i < hist.size(); i++) { std::cout << "Bin " << i << ": quantity: " << hist.at(i).quantity << "| err: " << hist.at(i).err << " (" << hist.at(i).r_min << ", " << hist.at(i).r_max << ")" << std::endl; } ROS_INFO("about to filter..."); while(ros::ok) { out_cloud = kf.remove_error(hist); pcl::toROSMsg(out_cloud, ros_view_cloud); ros_view_cloud.header.stamp = ros::Time::now(); ros_view_cloud.header.frame_id = kf.getPCFrame(); pub_out_cloud.publish(ros_view_cloud); ros::spinOnce(); } }
int main(int argc, char **argv) { ros::init(argc, argv, "character"); ros::NodeHandle nh; if (!nh.getParam("/characterize_noise/pointcloud_topic", g_pcl_topic)) g_pcl_topic = "/camera/depth_registered/points"; Characterizer c (nh); Histogram hist; std::vector<float> errors; std::vector<Bin> error_vector; std::vector<pcl::PointXYZ> offsets; float far_z, close_z; float mean_error; pcl::PointCloud<pcl::PointXYZ>::Ptr p_cloud; uint number_of_sample_points = 16; uint sample_number = 0; while (ros::ok() && number_of_sample_points > sample_number) { // Check to see if we got a point cloud... if (g_got_pcl) { p_cloud = c.getCloud(); sample_number++; // Begin processing! g_processing = true; // Transform the points into a frame that is based at the // lowest plane of the points. TODO(jonathankoss): Does this make sense // could also transform the points to the fixed frame of the flat surface // that we are viewing // Going to assume that the setup is like this: // /--> [--------wall--------] // [o kinect o] ---> [--------wall--------] // \--> [--------wall--------] // OK, so we now have a pcl_Kinect_clr_ptr full of points to deal with. // Furthest X point away will be where we transform the points to. // Everything is in relation to the kinect_frame // ! Instead of doing a literal transform we could just subtract that X from all // of the points to find the 'error'. //http://www.gnu.org/software/gsl/manual/html_node/Histograms.html far_z = c.getFurthest(p_cloud); close_z = c.getClosest(p_cloud); std::cout << "far_z: " << far_z << " | close_z: " << close_z << std::endl; float avg_z = (far_z + close_z)/2; offsets = c.getOffsetVec(avg_z, p_cloud); //errors = c.getErrorVec(offsets); mean_error = c.getMeanError(offsets); std::cout << "mean error: " << mean_error << std::endl; Bin b; b.r_min = close_z; b.r_max = far_z; b.err = mean_error; error_vector.push_back(b); ros::Duration(0.5).sleep(); if(sample_number%4 == 0) { ROS_INFO("Move it, soldier!!!"); ros::Duration(5.0).sleep(); } g_processing = false; // give a histogram of distances and mean errors } ros::spinOnce(); } //c.addToHistogram(errors, close_z, far_z); c.createHistogram(error_vector); std::ofstream output_file; output_file.open("histogram.dat"); hist = c.getHistogram(); for (size_t i = 0; i < hist.size(); i++) { std::cout << "Bin " << i << ": quantity: " << hist.at(i).quantity << "| err: " << hist.at(i).err << " (" << hist.at(i).r_min << ", " << hist.at(i).r_max << ")" << std::endl; output_file << hist.at(i).quantity << " " << hist.at(i).err << " " << hist.at(i).r_min << " " << hist.at(i).r_max << std::endl; } output_file.close(); }