void FeatureVisualization::overlayAndPublishDisparity(const stereo_msgs::DisparityImage& disparity_img, cv::Mat imageWithFeatures)
{
//TODO:: find a better way to convert disparity img to cv.
const float d_max = disparity_img.max_disparity - disparity_img.min_disparity;
sensor_msgs::ImagePtr img_ptr(new sensor_msgs::Image(disparity_img.image));
cv::Mat disparity = convertSensorMsgToCV(img_ptr);
cv::Mat output_img;
overlayImages(imageWithFeatures, disparity, d_max, output_img);
dispaity_image_publisher_.publish(convertCVToSensorMsg(output_img));
}
bool find_grip_point(image_processor::ProcessBridge::Request &req,
image_processor::ProcessBridge::Response &res )
{
sensor_msgs::Image image = req.image;
sensor_msgs::ImagePtr img_ptr(new sensor_msgs::Image(image));
sensor_msgs::CameraInfo cam_info = req.info;
IplImage *cv_image = NULL;
try
{
cv_image = bridge_.imgMsgToCv(img_ptr, "bgr8");
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("error");
return false;
}
//Crop image
cvSetImageROI(cv_image,cvRect(CROP_X,CROP_Y,CROP_WIDTH,CROP_HEIGHT));
IplImage* cropped_image = cvCloneImage(cv_image);
CvPoint2D64f temp_pts[128];
double temp_params[128];
IplImage *output_cv_image;
int num_pts = 0;
int num_params = 0;
ROS_INFO("Ready to call find_grip_point...");
output_cv_image = find_grip_point_process(cropped_image,temp_pts,temp_params,num_pts,num_params);
for( int i = 0; i < num_pts; i++){
res.pts_x.push_back(temp_pts[i].x+CROP_X);
res.pts_y.push_back(temp_pts[i].y+CROP_Y);
}
for ( int i = 0; i < num_params; i++){
res.params.push_back(temp_params[i]);
}
sensor_msgs::ImagePtr output_img_ptr;
sensor_msgs::Image output_img;
try
{
output_img_ptr = bridge_.cvToImgMsg(output_cv_image, "bgr8");
output_img = *output_img_ptr;
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("error");
return false;
}
res.image_annotated = output_img;
return true;
}
bool add_sock_to_match(image_processor::ProcessBridge::Request &req,
image_processor::ProcessBridge::Response &res )
{
sensor_msgs::Image image = req.image;
sensor_msgs::Image image2 = req.image2;
sensor_msgs::ImagePtr img_ptr(new sensor_msgs::Image(image));
IplImage *cv_image;
IplImage *cv_image2;
try
{
cv_image = bridge_.imgMsgToCv(img_ptr, "bgr8");
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("error");
return false;
}
CvPoint2D64f temp_pts[128];
double temp_params[128];
//Crop image
cvSetImageROI(cv_image,cvRect(CROP_X,CROP_Y,CROP_WIDTH,CROP_HEIGHT));
IplImage* cropped_image = cvCloneImage(cv_image);
cvSaveImage("/home/stephen/cropped_image1.png",cropped_image);
sensor_msgs::ImagePtr img2_ptr(new sensor_msgs::Image(image2));
try{
cv_image2 = bridge_.imgMsgToCv(img2_ptr, "bgr8");
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("error");
return false;
}
cvSetImageROI(cv_image,cvRect(CROP_X,CROP_Y,CROP_WIDTH,CROP_HEIGHT));
IplImage* cropped_image2 = cvCloneImage(cv_image2);
cvSaveImage("/home/stephen/cropped_image2.png",cropped_image2);
cvReleaseImage(&cropped_image);
cvReleaseImage(&cropped_image2);
IplImage* new_cropped_image = cvLoadImage("/home/stephen/cropped_image1.png");
IplImage* new_cropped_image2 = cvLoadImage("/home/stephen/cropped_image2.png");
match_detector->addImageToList(new_cropped_image,new_cropped_image2);
res.image_annotated = req.image;
return true;
}
LONG img_addr(WORD n)
{
return LLGET(img_ptr(n));
}
bool is_thick(image_processor::ProcessBridge::Request &req,
image_processor::ProcessBridge::Response &res )
{
sensor_msgs::Image image = req.image;
sensor_msgs::Image image2 = req.image2;
sensor_msgs::ImagePtr img_ptr(new sensor_msgs::Image(image));
sensor_msgs::CameraInfo cam_info = req.info;
IplImage *cv_image;
IplImage *cv_image2;
try
{
cv_image = bridge_.imgMsgToCv(img_ptr, "bgr8");
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("error");
return false;
}
CvPoint2D64f temp_pts[128];
double temp_params[128];
//Crop image
cvSetImageROI(cv_image,cvRect(CROP_X,CROP_Y,CROP_WIDTH,CROP_HEIGHT));
IplImage* cropped_image = cvCloneImage(cv_image);
cvSaveImage("/home/stephen/cropped_image1.png",cropped_image);
sensor_msgs::ImagePtr img2_ptr(new sensor_msgs::Image(image2));
try{
cv_image2 = bridge_.imgMsgToCv(img2_ptr, "bgr8");
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("error");
return false;
}
cvSetImageROI(cv_image,cvRect(CROP_X,CROP_Y,CROP_WIDTH,CROP_HEIGHT));
IplImage* cropped_image2 = cvCloneImage(cv_image2);
cvSaveImage("/home/stephen/cropped_image2.png",cropped_image2);
cvReleaseImage(&cropped_image);
cvReleaseImage(&cropped_image2);
IplImage* new_cropped_image = cvLoadImage("/home/stephen/cropped_image1.png");
IplImage* new_cropped_image2 = cvLoadImage("/home/stephen/cropped_image2.png");
IplImage *output_cv_image;
int num_pts = 0;
int num_params = 0;
output_cv_image = is_thick_process(new_cropped_image,temp_pts,temp_params,num_pts,num_params,new_cropped_image2);
for( int i = 0; i < num_pts; i++){
res.pts_x.push_back(temp_pts[i].x);
res.pts_y.push_back(temp_pts[i].y);
}
for ( int i = 0; i < num_params; i++){
res.params.push_back(temp_params[i]);
}
sensor_msgs::ImagePtr output_img_ptr;
sensor_msgs::Image output_img;
try
{
output_img_ptr = bridge_.cvToImgMsg(output_cv_image, "bgr8");
output_img = *output_img_ptr;
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR("error");
return false;
}
res.image_annotated = output_img;
return true;
}
