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();
}
