void callback(const sensor_msgs::ImageConstPtr &imgPtr){
cvImageFromROS =cv_bridge::toCvCopy(imgPtr);
diff =cv_bridge::toCvCopy(imgPtr);
cvImageFromROS->image.copyTo(image);
diff->image.copyTo(DiffImage);
out_msg.header = imgPtr->header;
out_msg.encoding = "mono16";
int cols=image.cols;
int rows=image.rows;
ushort v;
cv::Point p;
for(int i=0;i<cols;i++){
for(int j=0;j<rows;j++){
p.x=i;
p.y=j;
v=((float)image.at<ushort>(p));
v*=multiplier->cell(p.y,p.x,v);
image.at<ushort>(p)=(ushort)v;
}
}
out_msg.image = image;
pub.publish(out_msg.toImageMsg());
out_msg.image = image-DiffImage;
pub2.publish(out_msg.toImageMsg());
//std::cout<<"global diff: "<< cv::sum(out_msg.image)/(out_msg.image.rows*out_msg.image.cols)<<std::endl;
}
bool capture(int camNumber)
{
if (camNumber > numWebcams_ || camNumber < 0)
{
cout << "invalid camera number requested for capture in webcams, camera number = " << camNumber << endl;
return false;
}
char filename[80];
sprintf(filename, "webcam%d", camNumber);
Mat image;
if (camera_[camNumber].capture(&image))
{
std::string camType = "webcam";
std_msgs::Header imgHeader;
imgHeader.seq = webcamTilt_;
imgHeader.frame_id = camType;
imgHeader.stamp = ros::Time::now();
sensor_msgs::ImagePtr msg = cv_bridge::CvImage(imgHeader, "bgr8", image).toImageMsg();
//sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();
if (cap_home_)
{
home_image_pub_.publish(msg); // home target image
ROS_INFO("webcams published a home image");
return true;
}
else
{
image_pub_.publish(msg); // publish the image
ROS_INFO("webcams published a target image");
return true;
}
}
return false;
}
/// \brief Main callback which takes care of republishing.
///
/// This callback is called by the Synchronizer filter when
/// messages for both left/right images and cameras information
/// have been collected.
///
/// The callback the left image timestamps in all the other messages.
///
/// \param pub_l left image publisher (binded)
/// \param pub_cam_l left camera information publisher (binded)
/// \param pub_r right image publisher (binded)
/// \param pub_cam_r right camera information publisher (binded)
/// \param left left image
/// \param left_camera left camera information
/// \param right right image
/// \param right_camera right camera information
void imageCallback(image_transport::Publisher& pub_l,
ros::Publisher& pub_cam_l,
image_transport::Publisher& pub_r,
ros::Publisher& pub_cam_r,
const sensor_msgs::ImageConstPtr& left,
const sensor_msgs::CameraInfoConstPtr& left_camera,
const sensor_msgs::ImageConstPtr& right,
const sensor_msgs::CameraInfoConstPtr& right_camera
)
{
sensor_msgs::Image left_(*left);
sensor_msgs::Image right_(*right);
sensor_msgs::CameraInfo left_camera_(*left_camera);
sensor_msgs::CameraInfo right_camera_(*right_camera);
// Fix timestamps.
// FIXME: a warning should be issued if the differences between
// timestamps are too important.
left_camera_.header.stamp = left_.header.stamp;
right_camera_.header.stamp = left_.header.stamp;
right_.header.stamp = left_.header.stamp;
pub_l.publish(left_);
pub_cam_l.publish(left_camera_);
pub_r.publish(right_);
pub_cam_r.publish(right_camera_);
}
void callback(const sensor_msgs::ImageConstPtr& msg) {
// First extract the image to a cv:Mat structure, from opencv2
cv::Mat img(cv_bridge::toCvShare(msg,"mono8")->image);
try{
// Then receive the transformation between the robot body and
// the world
tf::StampedTransform transform;
// Use the listener to find where we are. Check the
// tutorials... (note: the arguments of the 2 functions below
// need to be completed
listener_.waitForTransform("","",ros::Time::now(),ros::Duration(1.0));
listener_.lookupTransform("","", ros::Time::now(), transform);
double proj_x = transform.getOrigin().x();
double proj_y = transform.getOrigin().y();
double proj_theta = -tf::getYaw(transform.getRotation());
printf("We were at %.2f %.2f theta %.2f\n",proj_x,proj_y,proj_theta*180./M_PI);
// Once the transformation is know, you can use it to find the
// affine transform mapping the local floor to the global floor
// and use cv::warpAffine to fill p_floor
cv::Mat_<uint8_t> p_floor(floor_size_pix,floor_size_pix,0xFF);
cv::Mat affine = (cv::Mat_<float>(2,3)
<< 1, 0, 0,
0, 1, 0);
cv::warpAffine(img,p_floor,affine, p_floor.size(),
cv::INTER_NEAREST,cv::BORDER_CONSTANT,0xFF);
// This published the projected floor on the p_floor topic
cv_bridge::CvImage pbr(msg->header,"mono8",p_floor);
ptpub_.publish(pbr.toImageMsg());
// Now that p_floor and floor have the same size, you can use
// the following for loop to go through all the pixels and fuse
// the current observation with the previous one.
for (unsigned int j=0;j<(unsigned)floor_.rows;j++) {
for (unsigned int i=0;i<(unsigned)floor_.cols;i++) {
uint8_t p = p_floor.at<uint8_t>(i,j);
uint8_t f = floor_.at<uint8_t>(i,j);
// Compute the new value of pixel i,j here
floor_.at<uint8_t>(i,j) = f;
}
}
// Finally publish the floor estimation.
cv_bridge::CvImage br(msg->header,"mono8",floor_);
tpub_.publish(br.toImageMsg());
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}
//callback method for operations that require a depth image
void depthCallback(const sensor_msgs::ImageConstPtr &msg)
{
//Load image into OpenCV
bridge_image_depth = cv_bridge::toCvCopy(msg, msg->encoding);
std::cout << "bridge_image created" << std::endl;
cvImage_depth = bridge_image_depth->image;
std::cout << "cvImage created" << std::endl;
//set cv_bridge image matrix to output and publish
getDepthThresholdImage(cvImage_depth, depthThresholdOut);
std::cout << "got depthThresholdImage" << std::endl;
bridge_image_depth->image = depthThresholdOut;
std::cout << "bridge_image->image = depthThresholdOut" << std::endl;
depthThresholdPub.publish(bridge_image_depth->toImageMsg());
//printImagePixels(depthThresholdOut, "output depth threshold");
//std::cout << depthThresholdOut << "\n*****\n" << std::endl;
//cv::Mat temp;
//depthThresholdOut.copyTo(temp);
//printImageData(depthThresholdOut, "depthThesholdOut");
if (lastImage.rows > 0) {
getImageDifference(cvImage_depth, lastImage);
bridge_image_depth->image = lastImage;
motionDetectPub.publish(bridge_image_depth->toImageMsg());
} else {
std::cout << "lastImage hasn't been created yet" << std::endl;
}
lastImage = cvImage_depth;
/*
cv::Mat depthThresholdBWOut;
//getDepthThresholdBWImage(depthThresholdOut, depthThresholdBWOut);
//printImageData(depthThresholdBWOut,"depthBW");
//depthThresholdBWOut = depthThresholdOut;
bridge_image->image = depthThresholdBWOut;
depthThresholdBWPub.publish(bridge_image->toImageMsg());
*/
//delete &depthThresholdOut;
//delete &cvImage;
/*cvReleaseMat(&cvImage);
cvReleaseMat(&depthThresholdOut);
*/
}
void faceDetect(){
//Detect faces
cv::cvtColor( frame, frame_gray, CV_BGR2GRAY );
cv::equalizeHist( frame_gray, frame_gray );
face_cascade.detectMultiScale( frame_gray, faces, 1.1, 2, 0|CV_HAAR_SCALE_IMAGE, cv::Size(30, 30) );
//for each face draws an ellipse arround and look for the red color at a distance from
for( i = 0; i < faces.size(); i++ )
{
cv::Point center( faces[i].x + faces[i].width*0.5, faces[i].y + faces[i].height*0.5 );
cv::ellipse( frame, center, cv::Size( faces[i].width*0.5, faces[i].height*0.5), 0, 0, 360, cv::Scalar( 0, 255, 0 ), 2, 8, 0 );
faceX = (float) faces[i].x;
faceY = (float) faces[i].y;
if( ((faceX + faceColorThresh) > (posX ) | (faceX - faceColorThresh) < (posX )) ) {
face = true;
//publishing camera image
out_msg.image = frame; //frame
out_msg.encoding = sensor_msgs::image_encodings::BGR8;
msg = out_msg.toImageMsg();
pub.publish(msg);
ROS_FATAL("PERSON DETECTED");
break;
}
}
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
//int c, key, total_max;
//Creates matrices available for filling later
cv::Mat image;
//CALL FACE REC HERE, PASS FORWARD
//ESSENTIALLY, THIS IS MAIN
// Draw an example circle on the video stream
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
// Update GUI Window
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
}
void stitcherCallback(const sensor_msgs::ImageConstPtr& original_image) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
return;
}
if (vImg.size() == stitcher_num_frames - 1) {
vImg.push_back(cv_ptr->image);
cv::Stitcher stitcher = cv::Stitcher::createDefault();
stitcher.stitch(vImg, cv_ptr->image);
cv::imwrite("images/Panorama.jpg", cv_ptr->image);
cv::imshow(WINDOW, cv_ptr->image);
cv::waitKey(3);
pub.publish(cv_ptr->toImageMsg());
sleep(2);
vImg.clear();
} else {
vImg.push_back(cv_ptr->image);
}
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr, gray_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
gray_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
{
cvtColor(cv_ptr->image, gray_ptr->image, CV_BGR2GRAY);
}
cv::imshow(WINDOW, gray_ptr->image);
cv::waitKey(3);
image_pub_.publish(cv_ptr->toImageMsg());
}
void SobelDepthImage::sobelCallback( const ImageConstPtr& image ) {
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(image, sensor_msgs::image_encodings::TYPE_32FC1);
using namespace cv;
Mat gauss, gradX, gradY, grad, thres;
GaussianBlur(cv_ptr->image, gauss, Size(5,5), 0, 0, BORDER_DEFAULT);
Sobel(gauss, gradX, CV_16S, 1, 0, 3, 1, 0, BORDER_DEFAULT);
Sobel(gauss, gradY, CV_16S, 0, 1, 3, 1, 0, BORDER_DEFAULT);
gradX = abs(gradX);
gradY = abs(gradY);
bitwise_or(gradX, gradY, grad);
grad.convertTo(thres, CV_32FC1);
double dynThres = 0.25*mean(grad).val[0];
std::cout << dynThres << std::endl;
threshold(thres, cv_ptr->image, dynThres , 1.0 , 0);
//~ imshow(OPENCV_WINDOW, cv_ptr->image);
//~ waitKey(3);
out.publish(cv_ptr->toImageMsg());
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
static int count = 0;
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat img = cv_ptr->image;
int pos[2];
search(img, pos);
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60 && pos[0] != -1 && pos[1] != -1)
cv::circle(cv_ptr->image, cv::Point(pos[0], pos[1]), 10, CV_RGB(0, 0, 255));
else {
printf("not found %d\n", count);
count++;
}
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
image_pub_.publish(cv_ptr->toImageMsg());
}
// TEMP code to show output of frames
void IirImage::pubImage(const ros::TimerEvent& e)
{
if (!dirty_)
return;
cv::Mat out_frame;
for (size_t i = 0; i < in_frames_.size() && i < b_coeffs_.size(); ++i)
{
const double bn = b_coeffs_[i];
if (i == 0)
out_frame = in_frames_[i] * bn;
else if ((out_frame.size() == in_frames_[i].size()) &&
(out_frame.type() == in_frames_[i].type()))
{
if (bn > 0)
out_frame += in_frames_[i] * bn;
else
out_frame -= in_frames_[i] * -bn;
}
}
{
// TODO(lucasw) this may be argument for keeping original Image messages around
cv_bridge::CvImage cv_image;
cv_image.header.stamp = ros::Time::now(); // or reception time of original message?
cv_image.image = out_frame;
cv_image.encoding = "rgb8";
image_pub_.publish(cv_image.toImageMsg());
}
dirty_ = false;
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
//imshow("pashmak",cv_ptr->image);
cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(), 1, 1);
//imshow("input", cv_ptr->image);
cv::Mat rotated,rotated1,rotated2;
rotate(cv_ptr->image, 180, rotated);
rotate(cv_ptr->image, 90, rotated1);
rotate(cv_ptr->image, 270, rotated2);
imshow("rotate",rotated);
// Update GUI Window
//cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat img = cv_ptr->image;
int pos[2];
search(img, pos);
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60 && pos[0] != -1 && pos[1] != -1) {
cv::circle(cv_ptr->image, cv::Point(pos[0], pos[1]), 10, CV_RGB(0, 0, 255), 3);
printf("found\n");
difx = img.cols/2 - pos[0];
dify = img.rows/2 - pos[1];
} else printf("not found\n");
cv::imshow("image1", img);
cv::waitKey(3);
image_pub_.publish(cv_ptr->toImageMsg());
}
void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception& e) {
JSK_ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat cv_img = cv_ptr->image;
cv::Mat cv_og_img;
cv::Mat cv_out_img;
// calcOrientedGradient() will write oriented gradient to
// 2nd arg image as HSV format,
// H is orientation and V is intensity
calcOrientedGradient(cv_img, cv_og_img);
cv::cvtColor(cv_og_img, cv_out_img, CV_HSV2BGR);
// cv::imshow("OrinetedGradient", cv_out_img);
// cv::waitKey(1);
sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(
msg->header,
sensor_msgs::image_encodings::BGR8,
cv_out_img).toImageMsg();
image_pub_.publish(out_img);
}
void imageCallback(const sensor_msgs::ImageConstPtr& original_image)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
return;
}
for(int i=0; i<cv_ptr->image.rows; i++)
{
for(int j=0; j<cv_ptr->image.cols; j++)
{
for(int k=0; k<cv_ptr->image.channels(); k++)
{
cv_ptr->image.data[i*cv_ptr->image.rows*4+j*3 + k] = \
255-cv_ptr->image.data[i*cv_ptr->image.rows*4+j*3 + k];
}
}
}
cv::imshow(WINDOW, cv_ptr->image);
cv::waitKey(3);
pub.publish(cv_ptr->toImageMsg());
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
if (g_trigger) {
ROS_INFO("Saving image as snapshot_image.png");
g_trigger=false;
cv::Mat &mat = cv_ptr->image; // does this help?
imwrite("snapshot_image.png",mat);
}
// Draw an example circle on the video stream
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
// Update GUI Window
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
}
// NOTE: Not used in SPENCER! We use the version with ground plane below!
void imageCallback(const Image::ConstPtr &msg)
{
// ROS_INFO("Entered img callback");
std::vector<cudaHOG::Detection> detHog;
// unsigned char image
QImage image_rgb(&msg->data[0], msg->width, msg->height, QImage::Format_RGB888);
int returnPrepare = hog->prepare_image(image_rgb.convertToFormat(QImage::Format_ARGB32).bits(), (short unsigned int) msg->width, (short unsigned int) msg->height);
if(returnPrepare)
{
ROS_ERROR("groundHOG: Error while preparing the image");
return;
}
hog->test_image(detHog);
hog->release_image();
int w = 64, h = 128;
GroundHOGDetections detections;
detections.header = msg->header;
for(unsigned int i=0;i<detHog.size();i++)
{
float score = detHog[i].score;
float scale = detHog[i].scale;
float width = (w - 32.0f)*scale;
float height = (h - 32.0f)*scale;
float x = (detHog[i].x + 16.0f*scale);
float y = (detHog[i].y + 16.0f*scale);
detections.scale.push_back(scale);
detections.score.push_back(score);
detections.pos_x.push_back(x);
detections.pos_y.push_back(y);
detections.width.push_back(width);
detections.height.push_back(height);
}
if(pub_result_image.getNumSubscribers()) {
ROS_DEBUG("Publishing image");
render_bbox_2D(detections, image_rgb, 255, 0, 0, 2);
Image sensor_image;
sensor_image.header = msg->header;
sensor_image.height = image_rgb.height();
sensor_image.width = image_rgb.width();
sensor_image.step = msg->step;
vector<unsigned char> image_bits(image_rgb.bits(), image_rgb.bits()+sensor_image.height*sensor_image.width*3);
sensor_image.data = image_bits;
sensor_image.encoding = msg->encoding;
pub_result_image.publish(sensor_image);
}
pub_message.publish(detections);
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, enc::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
//bool f = imwrite("/home/sid/ros_workspace/project1/src/sample1.bmp",cv_ptr->image);
// call main2
main2(cv_ptr->image);
// if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
// cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
/* get the image data
int height = cv_ptr->image.rows;
int width = cv_ptr->image.cols;
int step = cv_ptr->image.step;
uchar* data = (uchar *)cv_ptr->image.data;
for(int i=0;i<height;i++) for(int j=0;j<width;j++)
data[i*step+j]=255-data[i*step+j];
*/
cv::imshow(WINDOW, image2);
cv::waitKey(3);
image_pub_.publish(cv_ptr->toImageMsg());
}
inline void publish(Mat img, imgColorType t, ros::Time now =
ros::Time::now())
{
hasListener=(img_pub.getNumSubscribers() > 0);
if (!enable || !hasListener)
return;
Mat res;
switch (t)
{
case gray:
msg.encoding = sensor_msgs::image_encodings::MONO8;
res = img;
break;
case hsv:
msg.encoding = sensor_msgs::image_encodings::BGR8;
cvtColor(img, res, CV_HSV2BGR);
break;
case bgr:
default:
msg.encoding = sensor_msgs::image_encodings::BGR8;
res = img;
break;
}
msg.header.stamp = now;
msg.image = res;
img_pub.publish(msg.toImageMsg());
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
//ROS_INFO("Image received");
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::RGB8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception:%s",e.what());
return;
}
//Draw an example circle on the video stream
if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
cv::circle(cv_ptr->image, cv::Point(50,50), 10, CV_RGB(255,0,0));
// Update GUI Window
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
}
void alpha_image_cb(const sensor_msgs::ImageConstPtr& msg_ptr){
calc_and_publish_BWMask(msg_ptr->header.stamp, msg_ptr->header.frame_id);
IplImage* cam_image = bridge.imgMsgToCv(msg_ptr);
IplImage* cam_alpha_image = cvCreateImage(cvGetSize(cam_image), IPL_DEPTH_8U, 4);
//b
cvSetImageCOI(cam_alpha_image, 1);
cvSetImageCOI(cam_image, 1);
cvCopy(cam_image, cam_alpha_image);
//g
cvSetImageCOI(cam_alpha_image, 2);
cvSetImageCOI(cam_image, 2);
cvCopy(cam_image, cam_alpha_image);
//r
cvSetImageCOI(cam_alpha_image, 3);
cvSetImageCOI(cam_image, 3);
cvCopy(cam_image, cam_alpha_image);
//alpha
cvSetImageCOI(cam_alpha_image, 4);
cvCopy(ipl_maskBW, cam_alpha_image);
cvSetImageCOI(cam_alpha_image, 0);
sensor_msgs::ImagePtr img_msg = sensor_msgs::CvBridge::cvToImgMsg(cam_alpha_image);
img_msg->header = msg_ptr->header;
image_publisher.publish(img_msg);
cvReleaseImage(&cam_alpha_image);
}
void imageCallback(const sensor_msgs::ImageConstPtr& original_image)
{
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("ros_opencv::edge.cpp::cv_bridge exception: %s", e.what());
return;
}
int const max_lowThreshold = 10000;
int ratio = 3;
cvtColor( cv_ptr->image, cv_ptr->image, CV_BGR2GRAY );
int kernel_size = 7;
createTrackbar("blur size", WINDOW1, &blursize,10 );
blur( cv_ptr->image, cv_ptr->image, Size(blursize*2+1,blursize*2+1) );
createTrackbar("thrshold", WINDOW1, &lowThreshold, max_lowThreshold);
/// Canny detector
Canny( cv_ptr->image, cv_ptr->image, lowThreshold, lowThreshold*ratio, kernel_size );
//show image
cv::imshow (WINDOW1,cv_ptr->image);
cv::waitKey(3);
cvtColor( cv_ptr->image, cv_ptr->image, CV_GRAY2BGR );
pub.publish(cv_ptr->toImageMsg());
//image_old = cv_ptr->image;
}
void imageCb(const sensor_msgs::ImageConstPtr& msg) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
// look for red pixels; turn all other pixels black, and turn red pixels white
int npix = 0;
int isum = 0;
int jsum = 0;
int redval,blueval,greenval,testval;
cv::Vec3b rgbpix;
//image.at<uchar>(j,i)= 255;
/**/
for (int i = 0; i < cv_ptr->image.cols; i++)
for (int j = 0; j < cv_ptr->image.rows; j++) {
rgbpix = cv_ptr->image.at<cv::Vec3b>(j,i); //[j][i];
redval = rgbpix[2];
blueval = rgbpix[0]+1;
greenval = rgbpix[1]+1;
testval = redval/(blueval+greenval);
//redval = (int) cv_ptr->image.at<cv::Vec3b>(j, i)[0];
//cout<<"image("<<j<<","<<i<<")[0] = "<<redval<<endl;
if (testval > g_redthresh) {
cv_ptr->image.at<cv::Vec3b>(j, i)[0] = 255;
cv_ptr->image.at<cv::Vec3b>(j, i)[1] = 0;
cv_ptr->image.at<cv::Vec3b>(j, i)[2] = 0;
npix++;
isum += i;
jsum += j;
}
else {
/*
for (int j = 0; j < cv_ptr->image.rows; j++) {
cv_ptr->image.at<cv::Vec3b>(j, i)[0] = 128;
cv_ptr->image.at<cv::Vec3b>(j, i)[1] = 0;
cv_ptr->image.at<cv::Vec3b>(j, i)[2] = 0;
}
* */
}
}
/* */
cout << "npix: " << npix << endl;
if (npix>0) {
cout << "i_avg: " << isum / npix << endl;
cout << "j_avg: " << jsum / npix << endl;
}
g_du_left= (((double) (isum))/((double) npix))-width_in_pixels/2;
g_dv_left= (((double) (jsum))/((double) npix))-height_in_pixels/2;
// Update GUI Window
cv::imshow(OPENCV_WINDOW, cv_ptr->image);
cv::waitKey(3);
// Output modified video stream
image_pub_.publish(cv_ptr->toImageMsg());
}
void buildPanorama(const vector<sensor_msgs::ImageConstPtr>& images, const vector<sensor_msgs::CameraInfoConstPtr>& cameraInfos)
{
double averageTimestampSecs = 0.0;
static cv::Mat panorama;
static cv::Mat rotatedImages[MAX_CAMERAS];
std::string encoding = images[0]->encoding;
if(encoding == sensor_msgs::image_encodings::YUV422) encoding = sensor_msgs::image_encodings::BGR8;
//else encoding = sensor_msgs::image_encodings::MONO8; // debug hack for visualization
// Rotate individual images
for(size_t i = 0; i < images.size(); i++) {
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(images[i], encoding);
rotateImage(imagePtr->image, g_inplaneRotations[i], rotatedImages[i]);
}
// Determine full panorama width from rotated camera images
unsigned int panoramaHeight = 0, panoramaWidth = 0;
for(size_t i = 0; i < images.size(); i++) {
if(vertical) {
panoramaHeight += rotatedImages[i].rows;
panoramaWidth = max(panoramaWidth, (unsigned)rotatedImages[i].cols);
}
else {
panoramaWidth += rotatedImages[i].cols;
panoramaHeight = max(panoramaHeight, (unsigned)rotatedImages[i].rows);
}
}
panorama.create(panoramaHeight, panoramaWidth, rotatedImages[0].type()); // only reallocates if size has changed
// Merge rotated images into panorama
int currentPos = 0;
for(size_t i = 0; i < images.size(); i++)
{
if(vertical) {
rotatedImages[i].copyTo(panorama(cv::Rect(0, currentPos, rotatedImages[i].cols, rotatedImages[i].rows)));
currentPos += rotatedImages[i].rows;
}
else {
rotatedImages[i].copyTo(panorama(cv::Rect(currentPos, 0, rotatedImages[i].cols, rotatedImages[i].rows)));
currentPos += rotatedImages[i].cols;
}
averageTimestampSecs += images[i]->header.stamp.toSec();
}
averageTimestampSecs /= images.size();
// Create output image message
cv_bridge::CvImagePtr outputImagePtr(new cv_bridge::CvImage());
outputImagePtr->image = panorama;
outputImagePtr->header.seq = images[0]->header.seq;
// Timestamp must be in the timeframe of the original images, in case we are operating on a bag file!
outputImagePtr->header.stamp = ros::Time(averageTimestampSecs);
outputImagePtr->encoding = encoding;
g_panoramaPublisher.publish(outputImagePtr->toImageMsg());
}
void imageCallback (const sensor_msgs::ImageConstPtr & msg_ptr)
{
// Convert ROS Imput Image Message to IplImage
try
{
cv_input_ = bridge_.imgMsgToCv (msg_ptr, "bgr8");
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR ("CvBridge Input Error");
}
// Convert IplImage to cv::Mat
img_in_ = cv::Mat (cv_input_).clone ();
// Convert Input image from BGR to HSV
cv::cvtColor (img_in_, img_hsv_, CV_BGR2HSV);
// Display HSV Image in HighGUI window
cv::imshow ("hsv", img_hsv_);
// Needed to keep the HighGUI window open
cv::waitKey (3);
// Convert cv::Mat to IplImage
cv_output_ = img_hsv_;
// Convert IplImage to ROS Output Image Message and Publish
try
{
image_pub_.publish (bridge_.cvToImgMsg (&cv_output_, "bgr8"));
}
catch (sensor_msgs::CvBridgeException error)
{
ROS_ERROR ("CvBridge Output error");
}
}
//This function is called everytime a new image is published
void imageCallback(const sensor_msgs::ImageConstPtr& original_image)
{
//Convert from the ROS image message to a CvImage suitable for working with OpenCV for processing
cv_bridge::CvImagePtr cv_ptr;
try
{
//Always copy, returning a mutable CvImage
//OpenCV expects color images to use BGR channel order.
cv_ptr = cv_bridge::toCvCopy(original_image, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
//if there is an error during conversion, display it
ROS_ERROR("videofeed::main.cpp::cv_bridge exception: %s", e.what());
return;
};
//Display the image using OpenCV
cv::imshow(WINDOW, cv_ptr->image);
//Add some delay in miliseconds. The function only works if there is at least one HighGUI window created and the window is active. If there are several HighGUI windows, any of them can be active.
cv::waitKey(3);
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor in main().
*/
//Convert the CvImage to a ROS image message and publish it on the "camera/image_processed" topic.
pub.publish(cv_ptr->toImageMsg());
}
// Callback function
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
sensor_msgs::Image pub_image, classified_image;
pub_image = *msg;
ROS_INFO("passed1");
// Adjusting the data of the image according to the parameters
classified_image.height = pub_image.height;
classified_image.width = pub_image.width;
classified_image.step = pub_image.width;
size_t st0 = (pub_image.height * pub_image.width);
classified_image.data.resize(st0);
classified_image.encoding = "mono8";
m_colorTable.ClassifyImage(&pub_image, &classified_image, false);
ROS_INFO("passed2");
//*************************************test**********************************
ROS_INFO("width=%d height=%d", classified_image.width, classified_image.height);
/* for(int i = 0; i < classified_image.height; i++)
{
for(int j = 0; j < classified_image.width; j++)
printf("%d ",classified_image.data[i*classified_image.width+j]);
printf("\n");
}*/
//*************************************test********************************
//Publishing the adjusted image
pub.publish(classified_image);
}
void DemoNode::imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
sensor_msgs::CvBridge bridge;
cv::Mat image;
cv::Mat output;
try
{
image = cv::Mat(bridge.imgMsgToCv(msg, "bgr8"));
}
catch (sensor_msgs::CvBridgeException& e)
{
ROS_ERROR("Could not convert from '%s' to 'bgr8'. E was %s", msg->encoding.c_str(), e.what());
}
try {
//normalizeColors(image, output);
//blobfind(image, output);
findLines(image, output);
//cv::imshow("view", output);
IplImage temp = output;
image_pub_.publish(bridge.cvToImgMsg(&temp, "bgr8"));
}
catch (sensor_msgs::CvBridgeException& e) {
ROS_ERROR("Could not convert to 'bgr8'. Ex was %s", e.what());
}
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
cv_bridge::CvImagePtr cv_ptr;
cv_bridge::CvImagePtr cv_ptr_dest;
cv_bridge::CvImagePtr cv_ptr_dest2;
cv_bridge::CvImagePtr cv_ptr_dest3;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
cv_ptr_dest = cv_bridge::toCvCopy(msg, enc::BGR8);
cv_ptr_dest2 = cv_bridge::toCvCopy(msg, enc::BGR8);
cv_ptr_dest3 = cv_bridge::toCvCopy(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::cvtColor(cv_ptr->image, cv_ptr_dest->image, 6, 0);
//if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
// cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
cv::Canny(cv_ptr_dest->image, cv_ptr_dest2->image, 128, 100, 3, false);
//std::vector<cv::Vec3f> circles;
//cv::HoughCircles(cv_ptr_dest->image, circles, 3, 1, 10,100, 100, 10, 1000 );
cv::cvtColor(cv_ptr_dest2->image, cv_ptr_dest3->image, 8, 0);
ROS_INFO("Publish image");
image_pub_.publish(cv_ptr_dest3->toImageMsg());
}
