bool CameraLidarTransformer::transformServiceCallback(
navigator_msgs::CameraToLidarTransform::Request &req,
navigator_msgs::CameraToLidarTransform::Response &res) {
visualization_msgs::MarkerArray markers;
sensor_msgs::PointCloud2ConstPtr scloud =
lidarCache.getElemAfterTime(req.header.stamp);
if (!scloud) {
res.success = false;
res.error = navigator_msgs::CameraToLidarTransform::Response::CLOUD_NOT_FOUND;
return true;
}
geometry_msgs::TransformStamped transform =
tfBuffer.lookupTransform(req.header.frame_id, "velodyne", req.header.stamp);
sensor_msgs::PointCloud2 cloud_transformed;
tf2::doTransform(*scloud, cloud_transformed, transform);
#ifdef DO_ROS_DEBUG
cv::Mat debug_image(camera_info.height, camera_info.width, CV_8UC3,
cv::Scalar(0));
#endif
Eigen::Matrix3d matrixFindPlaneA;
matrixFindPlaneA << 0, 0, 0, 0, 0, 0, 0, 0, 0;
Eigen::Vector3d matrixFindPlaneB(0, 0, 0);
cv::circle(debug_image, cv::Point(req.point.x, req.point.y), 8,
cv::Scalar(255, 0, 0), -1);
//Tracks the closest lidar point to the requested camera point
double minDistance = std::numeric_limits<double>::max();
for (auto ii = 0, jj = 0; ii < cloud_transformed.width;
++ii, jj += cloud_transformed.point_step) {
floatConverter x, y, z, i;
for (int kk = 0; kk < 4; ++kk) {
x.data[kk] = cloud_transformed.data[jj + kk];
y.data[kk] = cloud_transformed.data[jj + 4 + kk];
z.data[kk] = cloud_transformed.data[jj + 8 + kk];
i.data[kk] = cloud_transformed.data[jj + 16 + kk];
}
cam_model.fromCameraInfo(camera_info);
if (int(z.f) < 0 || int(z.f) > 30) continue;
cv::Point2d point = cam_model.project3dToPixel(cv::Point3d(x.f, y.f, z.f));
if (int(point.x) > 0 && int(point.x) < camera_info.width &&
int(point.y) > 0 && int(point.y) < camera_info.height) {
#ifdef DO_ROS_DEBUG
visualization_msgs::Marker marker_point;
marker_point.header = req.header;
marker_point.header.seq = 0;
marker_point.header.frame_id = req.header.frame_id;
marker_point.id = (int)ii;
marker_point.type = visualization_msgs::Marker::CUBE;
marker_point.pose.position.x = x.f;
marker_point.pose.position.y = y.f;
marker_point.pose.position.z = z.f;
marker_point.scale.x = 1;
marker_point.scale.y = 0.1;
marker_point.scale.z = 0.1;
marker_point.color.a = 1.0;
marker_point.color.r = 0.0;
marker_point.color.g = 1.0;
marker_point.color.b = 0.0;
//marker_point.lifetime = ros::Duration(0.5);
markers.markers.push_back(marker_point);
// Draw
if (int(point.x - 20) > 0 && int(point.x + 20) < camera_info.width &&
int(point.y - 20) > 0 && int(point.y + 20) < camera_info.height) {
cv::circle(debug_image, point, 3,
cv::Scalar(0, 0, 255 * std::min(1.0, z.f / 20.0)), -1);
}
#endif
double distance = sqrt(pow(point.x - req.point.x ,2) + pow(point.y - req.point.y,2) ); //Distance (2D) from request point to projected lidar point
if (distance < req.tolerance) {
#ifdef DO_ROS_DEBUG
if (int(point.x - 20) > 0 && int(point.x + 20) < camera_info.width &&
int(point.y - 20) > 0 && int(point.y + 20) < camera_info.height) {
cv::circle(debug_image, point, 3, cv::Scalar(0, 255, 0), -1);
}
#endif
matrixFindPlaneA(0, 0) += x.f * x.f;
matrixFindPlaneA(1, 0) += y.f * x.f;
matrixFindPlaneA(2, 0) += x.f;
matrixFindPlaneA(0, 1) += x.f * y.f;
matrixFindPlaneA(1, 1) += y.f * y.f;
matrixFindPlaneA(2, 1) += y.f;
matrixFindPlaneA(0, 2) += x.f;
matrixFindPlaneA(1, 2) += y.f;
matrixFindPlaneA(2, 2) += 1;
matrixFindPlaneB(0, 0) -= x.f * z.f;
matrixFindPlaneB(1, 0) -= y.f * z.f;
matrixFindPlaneB(2, 0) -= z.f;
geometry_msgs::Point geo_point;
geo_point.x = x.f;
geo_point.y = y.f;
geo_point.z = z.f;
if (distance < minDistance)
{
res.closest = geo_point;
minDistance = distance;
}
res.transformed.push_back(geo_point);
}
}
}
if (res.transformed.size() < 1) {
res.success = false;
res.error = navigator_msgs::CameraToLidarTransform::Response::NO_POINTS_FOUND;
return true;
}
//Filter out lidar points behind the plane (I don't think this will work, no good way to do this)
// ~double min_z = (*std::min_element(res.transformed.begin(),res.transformed.end(), [=] (const geometry_msgs::Point& a,const geometry_msgs::Point& b) {
// ~return a.z < b.z && a.z > MIN_Z;
// ~})).z;
// ~res.transformed.erase(std::remove_if(res.transformed.begin(),res.transformed.end(),[min_z] (const geometry_msgs::Point& p) {
// ~return (p.z - min_z) < MAX_Z_ERROR;
// ~}),res.transformed.end());
res.distance = res.closest.z;
Eigen::Vector3d normalvec = matrixFindPlaneA.colPivHouseholderQr()
.solve(matrixFindPlaneB)
.normalized();
geometry_msgs::Vector3 normalvec_ros;
normalvec_ros.x = normalvec(0, 0);
normalvec_ros.y = normalvec(1, 0);
normalvec_ros.z = normalvec(2, 0);
res.normal = normalvec_ros;
std::cout << "Plane solution: " << normalvec << std::endl;
#ifdef DO_ROS_DEBUG
//Create marker for normal
visualization_msgs::Marker marker_normal;
marker_normal.header = req.header;
marker_normal.header.seq = 0;
marker_normal.header.frame_id = req.header.frame_id;
marker_normal.id = 3000;
marker_normal.type = visualization_msgs::Marker::ARROW;
geometry_msgs::Point sdp_normalvec_ros;
sdp_normalvec_ros.x = normalvec_ros.x + res.transformed[0].x;
sdp_normalvec_ros.y = normalvec_ros.y + res.transformed[0].y;
sdp_normalvec_ros.z = normalvec_ros.z + res.transformed[0].z;
marker_normal.points.push_back(res.closest);
marker_normal.points.push_back(sdp_normalvec_ros);
marker_normal.scale.x = 0.1;
marker_normal.scale.y = 0.5;
marker_normal.scale.z = 0.5;
marker_normal.color.a = 1.0;
marker_normal.color.r = 1.0;
marker_normal.color.g = 0.0;
marker_normal.color.b = 0.0;
markers.markers.push_back(marker_normal);
//Publish 3D debug market
pubMarkers.publish(markers);
//Publish debug image
cv_bridge::CvImage ros_debug_image;
ros_debug_image.encoding = "bgr8";
ros_debug_image.image = debug_image.clone();
points_debug_publisher.publish(ros_debug_image.toImageMsg());
#endif
res.success = true;
return true;
}
void patchArrayCallback(const samplereturn_msgs::PatchArrayConstPtr& msg)
{
samplereturn_msgs::NamedPointArray points_out;
points_out.header = msg->header;
bool enable_debug = (pub_debug_image.getNumSubscribers() != 0);
cv::Mat debug_image;
if (enable_debug) {
debug_image = cv::Mat::zeros(msg->cam_info.height,
msg->cam_info.width, CV_8UC3);
}
for (size_t i = 0; i < msg->patch_array.size(); i++) {
cv_bridge::CvImageConstPtr cv_ptr_mask, cv_ptr_img;
sensor_msgs::ImageConstPtr msg_mask(&(msg->patch_array[i].mask), boost::serialization::null_deleter());
sensor_msgs::ImageConstPtr msg_img(&(msg->patch_array[i].image), boost::serialization::null_deleter());
try {
cv_ptr_mask = cv_bridge::toCvShare(msg_mask, "mono8");
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge mask exception: %s", e.what());
}
try {
cv_ptr_img = cv_bridge::toCvShare(msg_img, "rgb8");
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge image exception: %s", e.what());
}
if (enable_debug) {
cv_ptr_img->image.copyTo(debug_image(cv::Rect(msg->patch_array[i].image_roi.x_offset,
msg->patch_array[i].image_roi.y_offset,
msg->patch_array[i].image_roi.width,
msg->patch_array[i].image_roi.height)));
}
// check foreground/background distance
samplereturn::ColorModel cm(cv_ptr_img->image, cv_ptr_mask->image);
samplereturn::HueHistogram hh_inner = cm.getInnerHueHistogram(config_.min_color_saturation, config_.low_saturation_limit, config_.high_saturation_limit);
samplereturn::HueHistogram hh_outer = cm.getOuterHueHistogram(config_.min_color_saturation, config_.low_saturation_limit, config_.high_saturation_limit);
double background_distance = hh_inner.distance(hh_outer);
// compare background to fence color
std::vector<std::tuple<double, double>> edges;
edges.push_back(std::make_tuple(0, config_.max_fence_hue));
samplereturn::HueHistogram hh_fence_measured = cm.getOuterHueHistogram(config_.min_fence_color_saturation,0.0, config_.fence_high_saturation_limit);
samplereturn::HueHistogramExemplar hh_fence_exemplar = samplereturn::ColorModel::getColoredSampleModel(edges, 0.0, config_.fence_high_saturation_limit);
double fence_distance = hh_fence_exemplar.distance(hh_fence_measured);
// Check inner hist against targets, either well-saturated in the specified
// hue range, or unsaturated with a high value (metal and pre-cached)
edges.clear();
edges.push_back(std::make_tuple(0, config_.max_target_hue));
edges.push_back(std::make_tuple(config_.min_target_hue, 180));
samplereturn::HueHistogramExemplar hh_colored_sample = samplereturn::ColorModel::getColoredSampleModel(edges, config_.low_saturation_limit, config_.high_saturation_limit);
samplereturn::HueHistogramExemplar hh_value_sample = samplereturn::ColorModel::getValuedSampleModel(config_.low_saturation_limit, config_.high_saturation_limit);
double hue_exemplar_distance = hh_colored_sample.distance(hh_inner);
double value_exemplar_distance = hh_value_sample.distance(hh_inner);
if (enable_debug) {
int x,y,h;
x = msg->patch_array[i].image_roi.x_offset;
y = msg->patch_array[i].image_roi.y_offset;
//w = msg->patch_array[i].image_roi.width;
h = msg->patch_array[i].image_roi.height;
hh_inner.draw_histogram(debug_image, x, y, config_.debug_font_scale);
hh_outer.draw_histogram(debug_image, x, y + h + 25*config_.debug_font_scale, config_.debug_font_scale);
char edist[100];
snprintf(edist, 100, "b:%3.2f h: %3.2f v:%3.2f f:%3.2f", background_distance, hue_exemplar_distance, value_exemplar_distance, fence_distance);
cv::putText(debug_image,edist,cv::Point2d(x+70, y + h + 50*config_.debug_font_scale),
cv::FONT_HERSHEY_SIMPLEX, config_.debug_font_scale, cv::Scalar(255,0,0),4,cv::LINE_8);
}
// is a sample if high enough bg/fg distance, high enough fence distance,
// and close to one of the exemplars
bool is_sample = ((background_distance>config_.min_inner_outer_distance) &&
(fence_distance>config_.min_fence_distance) &&
((hue_exemplar_distance<config_.max_exemplar_distance) ||
(value_exemplar_distance<config_.max_exemplar_distance)));
if (!is_sample)
{
if(enable_debug)
{
// Nix region
int x,y,w,h;
x = msg->patch_array[i].image_roi.x_offset;
y = msg->patch_array[i].image_roi.y_offset;
w = msg->patch_array[i].image_roi.width;
h = msg->patch_array[i].image_roi.height;
cv::line(debug_image,
cv::Point2f(x, y),
cv::Point2f(x + w, y + h), cv::Scalar(255,0,0), 20);
cv::line(debug_image,
cv::Point2f(x + w, y),
cv::Point2f(x, y + h), cv::Scalar(255,0,0), 20);
}
continue;
}
samplereturn_msgs::NamedPoint np_msg;
np_msg.header.stamp = msg->header.stamp;
np_msg.header.frame_id = msg->patch_array[i].world_point.header.frame_id;
np_msg.point = msg->patch_array[i].world_point.point;
hh_inner.to_msg(&np_msg.model.hue);
np_msg.sensor_frame = msg->header.frame_id;
np_msg.name = (hue_exemplar_distance < value_exemplar_distance) ?
std::string("Hue object") : std::string("Value object");
if(is_sample && config_.compute_grip_angle)
{
cv::RotatedRect griprect;
if(samplereturn::computeGripAngle(cv_ptr_mask->image, &griprect, &np_msg.grip_angle) &&
enable_debug)
{
griprect.center += cv::Point2f(
msg->patch_array[i].image_roi.x_offset,
msg->patch_array[i].image_roi.y_offset);
samplereturn::drawGripRect(debug_image, griprect);
}
}
points_out.points.push_back(np_msg);
}
pub_named_points.publish(points_out);
if (enable_debug) {
sensor_msgs::ImagePtr debug_image_msg =
cv_bridge::CvImage(msg->header,"rgb8",debug_image).toImageMsg();
pub_debug_image.publish(debug_image_msg);
}
}
void patchArrayCallback(const samplereturn_msgs::PatchArrayConstPtr& msg)
{
samplereturn_msgs::NamedPointArray points_out;
points_out.header = msg->header;
bool enable_debug = (pub_debug_image.getNumSubscribers() != 0);
cv::Mat debug_image;
if (enable_debug) {
debug_image = cv::Mat::zeros(msg->cam_info.height,
msg->cam_info.width, CV_8UC3);
}
for (size_t i = 0; i < msg->patch_array.size(); i++) {
cv_bridge::CvImageConstPtr cv_ptr_mask, cv_ptr_img;
sensor_msgs::ImageConstPtr msg_mask(&(msg->patch_array[i].mask), boost::serialization::null_deleter());
sensor_msgs::ImageConstPtr msg_img(&(msg->patch_array[i].image), boost::serialization::null_deleter());
try {
cv_ptr_mask = cv_bridge::toCvShare(msg_mask, "mono8");
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge mask exception: %s", e.what());
}
try {
cv_ptr_img = cv_bridge::toCvShare(msg_img, "rgb8");
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge image exception: %s", e.what());
}
if (enable_debug) {
cv_ptr_img->image.copyTo(debug_image(cv::Rect(msg->patch_array[i].image_roi.x_offset,
msg->patch_array[i].image_roi.y_offset,
msg->patch_array[i].image_roi.width,
msg->patch_array[i].image_roi.height)));
}
cv::Mat Lab;
cv::cvtColor(cv_ptr_img->image, Lab, cv::COLOR_RGB2Lab);
cv::Mat point(1,1,CV_32FC3);
point = cv::mean(Lab, cv_ptr_mask->image);
cv::Mat scaled_point = (point.reshape(1) - mean_)/std_;
cv::Mat prediction;
svm->predict(scaled_point, prediction, cv::ml::StatModel::RAW_OUTPUT);
float distance = prediction.at<float>(0);
if (enable_debug) {
int x,y,h;
x = msg->patch_array[i].image_roi.x_offset;
y = msg->patch_array[i].image_roi.y_offset;
//w = msg->patch_array[i].image_roi.width;
h = msg->patch_array[i].image_roi.height;
char edist[100];
snprintf(edist, 100, "d:%3.2f", distance);
cv::putText(debug_image,edist,cv::Point2d(x+70, y + h + 50*config_.debug_font_scale),
cv::FONT_HERSHEY_SIMPLEX, config_.debug_font_scale, cv::Scalar(255,0,0),4,cv::LINE_8);
}
bool is_sample = distance<0;
if(!is_sample)
{
if(enable_debug)
{
// Nix region
int x,y,w,h;
x = msg->patch_array[i].image_roi.x_offset;
y = msg->patch_array[i].image_roi.y_offset;
w = msg->patch_array[i].image_roi.width;
h = msg->patch_array[i].image_roi.height;
cv::line(debug_image,
cv::Point2f(x, y),
cv::Point2f(x + w, y + h), cv::Scalar(255,0,0), 20);
cv::line(debug_image,
cv::Point2f(x + w, y),
cv::Point2f(x, y + h), cv::Scalar(255,0,0), 20);
}
continue;
}
samplereturn_msgs::NamedPoint np_msg;
if(config_.compute_grip_angle)
{
cv::RotatedRect griprect;
if(samplereturn::computeGripAngle(cv_ptr_mask->image, &griprect, &np_msg.grip_angle) &&
enable_debug)
{
griprect.center += cv::Point2f(
msg->patch_array[i].image_roi.x_offset,
msg->patch_array[i].image_roi.y_offset);
samplereturn::drawGripRect(debug_image, griprect);
}
}
np_msg.header.stamp = msg->header.stamp;
np_msg.header.frame_id = msg->patch_array[i].world_point.header.frame_id;
np_msg.point = msg->patch_array[i].world_point.point;
np_msg.sensor_frame = msg->header.frame_id;
np_msg.name = "SVM";
points_out.points.push_back(np_msg);
}
pub_named_points.publish(points_out);
if (enable_debug) {
sensor_msgs::ImagePtr debug_image_msg =
cv_bridge::CvImage(msg->header,"rgb8",debug_image).toImageMsg();
pub_debug_image.publish(debug_image_msg);
}
}
