void overlayPoints(pcl::PointCloud<pcl::PointXYZ> detector_points, tf::Transform &transform, cv_bridge::CvImagePtr& image)
{
// Overlay calibration points on the image
pcl::PointCloud<pcl::PointXYZ> transformed_detector_points;
pcl_ros::transformPointCloud(detector_points, transformed_detector_points, transform);
int font_face = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
double font_scale = 1;
int thickness = 2;
int radius = 5;
for (unsigned int i=0; i < transformed_detector_points.size(); i++)
{
pcl::PointXYZ pt = transformed_detector_points[i];
cv::Point3d pt_cv(pt.x, pt.y, pt.z);
cv::Point2d uv;
uv = cam_model_.project3dToPixel(pt_cv);
cv::circle(image->image, uv, radius, CV_RGB(255,0,0), -1);
cv::Size text_size;
int baseline = 0;
std::stringstream out;
out << i+1;
text_size = cv::getTextSize(out.str(), font_face, font_scale, thickness, &baseline);
cv::Point origin = cvPoint(uv.x - text_size.width / 2,
uv.y - radius - baseline/* - thickness*/);
cv::putText(image->image, out.str(), origin, font_face, font_scale, CV_RGB(255,0,0), thickness);
}
}
void FindObjectOnPlane::generateStartPoints(
const cv::Point2f& point_2d,
const image_geometry::PinholeCameraModel& model,
const pcl::ModelCoefficients::Ptr& coefficients,
std::vector<cv::Point3f>& search_points_3d,
std::vector<cv::Point2f>& search_points_2d)
{
NODELET_INFO("generateStartPoints");
jsk_recognition_utils::Plane::Ptr plane
(new jsk_recognition_utils::Plane(coefficients->values));
cv::Point3d ray = model.projectPixelTo3dRay(point_2d);
Eigen::Vector3f projected_point = rayPlaneInteersect(ray, plane);
const double resolution_step = 0.01;
const int resolution = 5;
search_points_3d.clear();
search_points_2d.clear();
for (int i = - resolution; i < resolution; ++i) {
for (int j = - resolution; j < resolution; ++j) {
double x_diff = resolution_step * i;
double y_diff = resolution_step * j;
Eigen::Vector3f moved_point = projected_point + Eigen::Vector3f(x_diff, y_diff, 0);
Eigen::Vector3f projected_moved_point;
plane->project(moved_point, projected_moved_point);
cv::Point3f projected_moved_point_cv(projected_moved_point[0],
projected_moved_point[1],
projected_moved_point[2]);
search_points_3d.push_back(cv::Point3f(projected_moved_point_cv));
cv::Point2d p2d = model.project3dToPixel(projected_moved_point_cv);
search_points_2d.push_back(p2d);
}
}
}
void projectPoints(const image_geometry::PinholeCameraModel &cam_model,
const cv::Point3d &points3D,
cv::Point2d *points2D)
{
*points2D = cam_model.project3dToPixel(points3D);
// *points2D = cam_model.rectifyPoint(*points2D);
}
bool objectSrv(jsk_smart_gui::point2screenpoint::Request &req,
jsk_smart_gui::point2screenpoint::Response &res)
{
ROS_INFO("3dtopixel request:x=%lf,y=%lf,z=%lf",req.point.point.x,req.point.point.y,req.point.point.z);
geometry_msgs::PointStamped point_transformed;
tf_listener_.transformPoint(camera_topic, req.point, point_transformed);
cv::Point3d xyz;
cv::Point2d uv_rect;
xyz.x = point_transformed.point.x;
xyz.y = point_transformed.point.y;
xyz.z = point_transformed.point.z;
uv_rect = cam_model_.project3dToPixel(xyz);
res.x=uv_rect.x;
res.y=uv_rect.y;
return true;
}
cv::Point2d FindObjectOnPlane::getUyEnd(
const cv::Point2d& ux_start,
const cv::Point2d& ux_end,
const image_geometry::PinholeCameraModel& model,
const jsk_recognition_utils::Plane::Ptr& plane)
{
cv::Point3d start_ray = model.projectPixelTo3dRay(ux_start);
cv::Point3d end_ray = model.projectPixelTo3dRay(ux_end);
Eigen::Vector3f ux_start_3d = rayPlaneInteersect(start_ray, plane);
Eigen::Vector3f ux_end_3d = rayPlaneInteersect(end_ray, plane);
Eigen::Vector3f ux_3d = ux_end_3d - ux_start_3d;
Eigen::Vector3f normal = plane->getNormal();
Eigen::Vector3f uy_3d = normal.cross(ux_3d).normalized();
Eigen::Vector3f uy_end_3d = ux_start_3d + uy_3d;
cv::Point2d uy_end = model.project3dToPixel(cv::Point3d(uy_end_3d[0],
uy_end_3d[1],
uy_end_3d[2]));
return uy_end;
}
void doOverlay(const sensor_msgs::ImageConstPtr& img_msg,
const sensor_msgs::CameraInfoConstPtr& info_msg) {
// convert camera image into opencv
cam_model.fromCameraInfo(info_msg);
cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::RGB8);
double alpha_mult;
ros::param::param<double>("~alpha_mult", alpha_mult, 0.5);
uint8_t r, g, b;
if(aligned_pc) {
if(!tf_list->waitForTransform(img_msg->header.frame_id, "/base_link",
img_msg->header.stamp, ros::Duration(3)))
return;
tf::StampedTransform transform;
tf_list->lookupTransform(img_msg->header.frame_id, "/base_link",
img_msg->header.stamp, transform);
PCRGB::Ptr tf_pc(new PCRGB());
pcl_ros::transformPointCloud<PRGB>(*aligned_pc, *tf_pc, transform);
for(uint32_t i=0;i<tf_pc->size();i++) {
cv::Point3d proj_pt_cv(tf_pc->points[i].x, tf_pc->points[i].y,
tf_pc->points[i].z);
cv::Point pt2d = cam_model.project3dToPixel(proj_pt_cv);
extractRGB(tf_pc->points[i].rgb, r, g, b);
if(pt2d.x >= 0 && pt2d.y >= 0 &&
pt2d.x < cv_img->image.rows && pt2d.y < cv_img->image.cols) {
double old_r = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[0];
double old_g = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[1];
double old_b = cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[2];
cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[0] =
(uint8_t) min(alpha_mult*old_r+r, 255.0);
cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[1] =
(uint8_t) min(alpha_mult*old_g+g, 255.0);
cv_img->image.at<cv::Vec3b>(pt2d.y, pt2d.x)[2] =
(uint8_t) min(alpha_mult*old_b+b, 255.0);
}
}
}
overlay_pub.publish(cv_img->toImageMsg());
}
inline void
filter_depth_projection( const image_geometry::PinholeCameraModel &camera_model,
const pcl::PointCloud<PointT> &in,
pcl::PointCloud<PointT> &out,
int rows,
int cols,
int k_neighbors = 2,
float threshold = 0.3)
{
std::vector<std::vector<bool> > hit( cols, std::vector<bool>(rows));
std::vector<int> points2d_indices;
pcl::PointCloud<pcl::PointXY> points2d;
pcl::PointCloud<PointT> z_filtred;
#ifdef DEBUG
std::cout << "in points: "<< in.size() << " width: " << cols << " height: " << rows << "\n";
#endif
project2d::Points2d<PointT> point_map;
point_map.init(camera_model, in, rows, cols);
point_map.get_points(z_filtred, 25);
// Project points into image space
for(unsigned int i=0; i < z_filtred.size(); ++i){
const PointT* pt = &z_filtred.points.at(i);
//if( pt->z > 1)
{ // min distance from camera 1m
cv::Point2i point_image = camera_model.project3dToPixel(cv::Point3d(pt->x, pt->y, pt->z));
if( between<int>(0, point_image.x, cols )
&& between<int>( 0, point_image.y, rows )
)
{
// Point allready at this position?
if(!hit[point_image.x][point_image.y]){
hit[point_image.x][point_image.y] = true;
pcl::PointXY p_image;
p_image.x = point_image.x;
p_image.y = point_image.y;
points2d.push_back(p_image);
points2d_indices.push_back(i);
}
else{
#ifdef DEBUG
std::cout << "[" << point_image.x << "][" << point_image.y << "] already inserted " << pt << "\n";
#endif
}
}
}
}
#ifdef DEBUG
std::cout << "Z_filtred: " << z_filtred.size() << " projected 2d points: "<< points2d.size() << " indices: " << points2d_indices.size() << "\n";
#endif
pcl::search::KdTree<pcl::PointXY> tree_n;
tree_n.setInputCloud(points2d.makeShared());
tree_n.setEpsilon(0.5);
for(unsigned int i=0; i < points2d.size(); ++i){
std::vector<int> k_indices;
std::vector<float> square_distance;
//tree_n.nearestKSearch(points2d.at(i), k_neighbors, k_indices, square_distance);
tree_n.radiusSearch(points2d.at(i), k_neighbors, k_indices, square_distance);
look_up_indices(points2d_indices, k_indices);
float distance_value;
if(is_edge_threshold(z_filtred, points2d_indices.at(i), k_indices, square_distance, distance_value, threshold)){
out.push_back(z_filtred.points.at(points2d_indices.at(i)));
out.at(out.size()-1).intensity = sqrt(distance_value);
}
}
#ifdef DEBUG
std::cout << "out 2d points: "<< out.size() << "\n";
#endif
}
inline void
hit_same_point( const image_geometry::PinholeCameraModel &camera_model,
const pcl::PointCloud<PointT> &in,
pcl::PointCloud<PointT> &out,
int rows,
int cols,
float z_threshold = 0.3)
{
std::vector<std::vector <std::vector<PointT> > > hit( cols, std::vector< std::vector<PointT> >(rows, std::vector<PointT>()));
// Project points into image space
for(unsigned int i=0; i < in.points.size(); ++i){
const PointT* pt = &in.points.at(i);
if( pt->z > 1) { // min distance from camera 1m
cv::Point2i point_image = camera_model.project3dToPixel(cv::Point3d(pt->x, pt->y, pt->z));
if( between<int>(0, point_image.x, cols )
&& between<int>( 0, point_image.y, rows )
)
{
// Sort all points into image
{
hit[point_image.x][point_image.y].push_back(in.points.at(i));
}
}
}
}
assert(out.empty());
for(int x = 0; x < hit.size(); ++x ){
for(int y = 0; y < hit[0].size(); ++y){
if(hit[x][y].size()>1){
PointT min_z = hit[x][y][0];
float max_z = min_z.z;
for(int p = 1; p < hit[x][y].size(); ++p){
// find min and max z
max_z = MAX(max_z, hit[x][y][p].z);
#ifdef DEBUG
std::cout << hit[x][y].size() << "\t";
#endif
if(hit[x][y][p].z < min_z.z)
{
min_z = hit[x][y][p];
}
}
#ifdef DEBUG
std::cout << min_z.z << "\t" << max_z << "\t";
#endif
if(max_z - min_z.z > z_threshold){
#ifdef DEBUG
std::cout << min_z << std::endl;
#endif
out.push_back(min_z);
}
}
}
}
#ifdef DEBUG
std::cout << "hit_same_point in: "<< in.size() << "\t out: " << out.size() << "\n";
#endif
}
inline void
remove_cluster_2d( const image_geometry::PinholeCameraModel &camera_model,
const pcl::PointCloud<PointT> &in,
pcl::PointCloud<PointT> &out,
int rows,
int cols,
int k_neighbors = 4,
int border = 25)
{
std::vector<int> points2d_indices;
pcl::PointCloud<pcl::PointXY> points2d;
#ifdef DEBUG
std::cout << "in points: "<< in.size() << "\n";
#endif
// Project points into image space
for(unsigned int i=0; i < in.points.size(); ++i){
const PointT* pt = &in.points.at(i);
if( pt->z > 1) { // min distance from camera 1m
cv::Point2i point_image = camera_model.project3dToPixel(cv::Point3d(pt->x, pt->y, pt->z));
if( between<int>(0+border, point_image.x, cols-border )
&& between<int>( 0+border, point_image.y, rows-border )
)
{
pcl::PointXY p_image;
p_image.x = point_image.x;
p_image.y = point_image.y;
points2d.push_back(p_image);
points2d_indices.push_back(i);
}
}
}
#ifdef DEBUG
std::cout << "projected 2d points: "<< points2d.size() << " indices: " << points2d_indices.size() << "\n";
#endif
pcl::search::KdTree<pcl::PointXY> tree_n;
tree_n.setInputCloud(points2d.makeShared());
tree_n.setEpsilon(0.1);
for(unsigned int i=0; i < points2d.size(); ++i){
std::vector<int> k_indices;
std::vector<float> square_distance;
//tree_n.nearestKSearch(points2d.at(i), k_neighbors, k_indices, square_distance);
tree_n.radiusSearch(points2d.at(i), k_neighbors, k_indices, square_distance);
if(k_indices.empty()) continue; // Dont add points without neighbors
look_up_indices(points2d_indices, k_indices);
float edginess = 0;
if(is_edge_z(in, points2d_indices.at(i), k_indices, square_distance, edginess, 0.1)){
out.push_back(in.points.at(points2d_indices.at(i)));
out.at(out.size()-1).intensity = edginess;
}
}
#ifdef DEBUG
std::cout << "out 2d points: "<< out.size() << "\n";
#endif
}