//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 equalizeHistogram( cv_bridge::CvImagePtr cvPtr )
{
cv::vector< cv::Mat > splitImages;
cv::split( cvPtr->image, splitImages );
for( int i = 0; i < splitImages.size(); i++ )
{
cv::equalizeHist( splitImages[i], splitImages[i] );
}
cv::merge( splitImages, cvPtr->image );
histPub_.publish( cvPtr->toImageMsg() );
}
void imageCallback(const sensor_msgs::ImageConstPtr& image_msg)
{
try
{
input_bridge_ = cv_bridge::toCvCopy(image_msg, "mono8");
output_bridge_ = cv_bridge::toCvCopy(image_msg, "bgr8");
}
catch (cv_bridge::Exception& ex)
{
ROS_ERROR("[calibrate] Failed to convert image");
return;
}
Eigen::Vector3f translation;
Eigen::Quaternionf orientation;
if (!pattern_detector_.detectPattern(input_bridge_->image, translation, orientation, output_bridge_->image))
{
ROS_INFO("[calibrate] Couldn't detect checkerboard, make sure it's visible in the image.");
return;
}
tf::Transform target_transform;
tf::StampedTransform base_transform;
try
{
ros::Time acquisition_time = image_msg->header.stamp;
ros::Duration timeout(1.0 / 30.0);
target_transform.setOrigin( tf::Vector3(translation.x(), translation.y(), translation.z()) );
target_transform.setRotation( tf::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w()) );
tf_broadcaster_.sendTransform(tf::StampedTransform(target_transform, image_msg->header.stamp, image_msg->header.frame_id, target_frame));
}
catch (tf::TransformException& ex)
{
ROS_WARN("[calibrate] TF exception:\n%s", ex.what());
return;
}
publishCloud(ideal_points_, target_transform, image_msg->header.frame_id);
overlayPoints(ideal_points_, target_transform, output_bridge_);
// Publish calibration image
pub_.publish(output_bridge_->toImageMsg());
pcl_ros::transformPointCloud(ideal_points_, image_points_, target_transform);
cout << "Got an image callback!" << endl;
calibrate(image_msg->header.frame_id);
ros::shutdown();
}
void ImageFeed::publishImage()
{
std::stringstream sstm;
sstm << path_ << x << "_" << y << ".ppm";
std::string next_image = sstm.str();
msg_ptr_->image = cv::imread(next_image);
cv::imshow(WINDOW, msg_ptr_->image);
cv::waitKey(30);
ROS_INFO("path: %s", next_image.c_str());
pub_.publish(msg_ptr_->toImageMsg());
}
void applyMorphology( cv_bridge::CvImagePtr cvPtr )
{
cv::Mat element = cv::getStructuringElement( 0, morphSize_, cv::Point( 1, 1 ) );
// close then open
cv::morphologyEx(
cvPtr->image, cvPtr->image,
cv::MORPH_CLOSE, element );
cv::morphologyEx(
cvPtr->image, cvPtr->image,
cv::MORPH_OPEN, element );
morphPub_.publish( cvPtr->toImageMsg() );
}
void thresholdImage( cv_bridge::CvImagePtr cvPtr, bool byColor )
{
// blur first
cv::GaussianBlur( cvPtr->image, cvPtr->image, cv::Size( 5, 5 ), 0 );
if( byColor == true )
{
cv::cvtColor( cvPtr->image, cvPtr->image, CV_BGR2HSV );
cv::Mat destImage;
cv::Scalar lowerBound( std::max( hValue_ - hRange_/2, 0 ), 50, 50 );
cv::Scalar upperBound( std::min( hValue_ + hRange_/2, 255 ), 255, 255 );
cv::inRange( cvPtr->image, lowerBound, upperBound, cvPtr->image );
}
else
{
cv::cvtColor( cvPtr->image, cvPtr->image, CV_BGR2GRAY );
cv::Point min_loc;
cv::Point max_loc;
double min_val;
double max_val;
cv::minMaxLoc( cvPtr->image,
&min_val,
&max_val,
&min_loc,
&max_loc );
cv::threshold(
cvPtr->image,
cvPtr->image,
max_val - 1,
//100,
255,
CV_THRESH_BINARY );
}
cvPtr->encoding = enc::MONO8;
thresholdPub_.publish(cvPtr->toImageMsg());
}
void imageCb(const sensor_msgs::ImageConstPtr& msg)
{
//cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
geometry_msgs::PoseStamped tmppose;
CvPoint objectNextPos;
int nbPixels;
/*
objectNextPos = binarisation(&cv_ptr->image.operator IplImage(), &nbPixels, h_CD, s_CD, v_CD, tolerance_CD, Scalar(0,255,0));
x_CD.data = objectNextPos.x;
y_CD.data = objectNextPos.y;
*/
Mat gray;
cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
// smooth it, otherwise a lot of false circles may be detected
GaussianBlur( gray, gray, Size(9, 9), 2, 2 );
vector<Vec3f> circles;
HoughCircles(gray, circles, CV_HOUGH_GRADIENT, 2, 200, 200, 100, 50 );
cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
type_obj.data = 0;
for( size_t i = 0; i < circles.size(); i++ )
{
Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
int radius = cvRound(circles[i][2]);
// draw the circle center
circle( cv_ptr->image, center, 3, Scalar(0,255,0), -1, 8, 0 );
// draw the circle outline
circle( cv_ptr->image, center, radius, Scalar(0,0,255), 3, 8, 0 );
x_CD.data = cvRound(circles[i][0]);
y_CD.data = cvRound(circles[i][1]);
type_obj.data = 1;
}
/*
if(x_CD.data < 1)
type_obj.data = 0;
else
type_obj.data = 1;
*/
objectNextPos = binarisation(&cv_ptr->image.operator IplImage(), &nbPixels, h_BAR, s_BAR, v_BAR, tolerance_BAR, Scalar(0,0,255));
x_BAR.data = objectNextPos.x;
y_BAR.data = objectNextPos.y;
if(x_BAR.data < 1)
type_obj.data = type_obj.data;
else
type_obj.data = type_obj.data + 2;
// imshow( "Camera output", cv_ptr->image );
if(mode == TAKE_AUTONOMOUSLY) {
if(type_obj.data > 0) {
switch(type_obj.data) {
case 0 :
nb_img_cnt = 0;
break;
case 1 : // Only CD is seen
if(nb_img_cnt > MAX_CNT_OBJECT) { // Object seen X times
tmppose.pose.position.x = ((288)*90.0/144.0 -((double)y_CD.data)*90.0/144.0 + (50))/1000.0 - 0.02;
tmppose.pose.position.y = (-((double)x_CD.data)*250.0/352.0+(167.0*278.0/352.0))/1000.0 + 0.015;
tmppose.pose.position.z = 0.072;
object_pub.publish(tmppose);
ROS_ERROR("Taking CD : %f : %f", tmppose.pose.position.x, tmppose.pose.position.y);
nb_img_cnt = 0;
usleep(2000000);
}
else {
nb_img_cnt++;
}
break;
case 2 : // Only BAR is seen
if(nb_img_cnt > MAX_CNT_OBJECT) { // Object seen X times
tmppose.pose.position.x = ((480)*90.0/240.0 -((double)y_BAR.data)*90.0/240.0 + (50))/1000.0;
tmppose.pose.position.y = (-((double)x_BAR.data)*278.0/640.0+(335.0*278.0/640.0))/1000.0;
tmppose.pose.position.z = 0.062;
object_pub.publish(tmppose);
ROS_ERROR("Taking BAR : %f : %f", tmppose.pose.position.x, tmppose.pose.position.y);
nb_img_cnt = 0;
}
else {
nb_img_cnt++;
}
break;
case 3 : // Two different objects are seen
if(nb_img_cnt > MAX_CNT_OBJECT) { // Object seen X times
if(y_CD.data > y_BAR.data) {
ROS_ERROR("Taking CD");
nb_img_cnt = 0;
}
else {
ROS_ERROR("Taking BAR");
nb_img_cnt = 0;
}
}
else {
nb_img_cnt++;
}
break;
}
}
else {
nb_img_cnt--;
if(nb_img_cnt < 0)
nb_img_cnt = 0;
}
}
else {
nb_img_cnt = 0;
}
/* Mat gray;
cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
// smooth it, otherwise a lot of false circles may be detected
GaussianBlur( gray, gray, Size(9, 9), 2, 2 );
vector<Vec3f> circles;
HoughCircles(gray, circles, CV_HOUGH_GRADIENT, 2, 200, 200, 100, 50 );
cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
for( size_t i = 0; i < circles.size(); i++ )
{
Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
int radius = cvRound(circles[i][2]);
// draw the circle center
circle( cv_ptr->image, center, 3, Scalar(0,255,0), -1, 8, 0 );
// draw the circle outline
circle( cv_ptr->image, center, radius, Scalar(0,0,255), 3, 8, 0 );
}*/
/*
Canny( gray, gray, 16, 33, 3 );
vector<Vec4i> lines;
HoughLinesP( gray, lines, 1, CV_PI/180, 10.0, 240.0, 15.0 );
*/
/* for( size_t i = 0; i < lines.size(); i++ )
{
if( ((lines.size() - i) > 1) && (sqrt((lines[i][0]-lines[i+1][0])*(lines[i][0]-lines[i+1][0]) + (lines[i][1]-lines[i+1][1])*(lines[i][1]-lines[i+1][1])) > 125 ) && (sqrt((lines[i][0]-lines[i+1][0])*(lines[i][0]-lines[i+1][0]) + (lines[i][1]-lines[i+1][1])*(lines[i][1]-lines[i+1][1])) < 170 )) {
line( cv_ptr->image, Point(lines[i][0], lines[i][1]),
Point(lines[i+1][2], lines[i+1][3]), Scalar(255,255,0), 3, 8 );
line( cv_ptr->image, Point(lines[i+1][0], lines[i+1][1]),
Point(lines[i][2], lines[i][3]), Scalar(255,255,0), 3, 8 );
*/
/* Test of line intersection */
// int a, b, c, d, x, y;
/* Segment equation */
/* a = (lines[i][0] - lines[i+1][2]) / (lines[i][1] - lines[i+1][3]);
b = lines[i][1] - a * lines[i][0];
c = (lines[i+1][0] - lines[i][2]) / (lines[i+1][1] - lines[i][3]);
d = lines[i+1][1] - c * lines[i+1][0];
*/ /* Intersection point */
/* x = (d - b) / (a - c);
y = a * x + b;
ROS_ERROR("intersection : %i | %i ", x, y);
// draw the circle center
Point center2(cvRound(x), cvRound(y));
circle( cv_ptr->image, center2, 3, Scalar(0,255,0), -1, 8, 0 );
i++;
break;
}
else {
line( cv_ptr->image, Point(lines[i][0], lines[i][1]),
Point(lines[i][2], lines[i][3]), Scalar(255,0,0), 3, 8 );
}
}
*/
/* Mat gray = Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC3);
Mat out;
cvtColor(cv_ptr->image, gray, CV_BGR2GRAY);
// smooth it, otherwise a lot of false circles may be detected
GaussianBlur( gray, gray, Size(9, 9), 7, 9 );
Canny( gray, out, 16, 33, 3 );
Mat gray2;
cvtColor(cv_ptr->image, gray2, CV_BGR2GRAY);
//Canny( gray2, out, 16, 33, 3 );
*/
/*
vector<Vec3f> circles;
HoughCircles(out, circles, CV_HOUGH_GRADIENT, 3, 506, 90, 56, 50 );
for( size_t i = 0; i < circles.size(); i++ )
{
Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
int radius = cvRound(circles[i][2]);
// draw the circle center
circle( cv_ptr->image, center, 3, Scalar(0,255,0), -1, 8, 0 );
// draw the circle outline
circle( cv_ptr->image, center, radius, Scalar(0,0,255), 3, 8, 0 );
}
*/
/*
vector<Vec2f> lines;
HoughLines( out, lines, 1, CV_PI/180, 100 );
for( size_t i = 0; i < lines.size(); i++ )
{
float rho = lines[i][0];
float theta = lines[i][1];
double a = cos(theta), b = sin(theta);
double x0 = a*rho, y0 = b*rho;
Point pt1(cvRound(x0 + 1000*(-b)),
cvRound(y0 + 1000*(a)));
Point pt2(cvRound(x0 - 1000*(-b)),
cvRound(y0 - 1000*(a)));
line( cv_ptr->image, pt1, pt2, Scalar(0,0,255), 3, 8 );
}
*/
/*
vector<Vec4i> lines;
HoughLinesP( out, lines, 1, CV_PI/180, 10.0, 140.0, 15.0 );
for( size_t i = 0; i < lines.size(); i++ )
{
line( cv_ptr->image, Point(lines[i][0], lines[i][1]),
Point(lines[i][2], lines[i][3]), Scalar(255,0,0), 3, 8 );
}
*/
/* cv::namedWindow( "Source", 1 );
cv::imshow( "Source", cv_ptr->image );
cv::Mat dst = cv::Mat::zeros(cv_ptr->image.rows, cv_ptr->image.cols, CV_8UC3);
cv::vector<cv::vector<cv::Point> > contours;
cv::vector<cv::Vec4i> hierarchy;
cv::findContours( cv_ptr->image, contours, hierarchy,
CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
// iterate through all the top-level contours,
// draw each connected component with its own random color
int idx = 0;
for( ; idx >= 0; idx = hierarchy[idx][0] )
{
cv::Scalar color( rand()&255, rand()&255, rand()&255 );
cv::drawContours( dst, contours, idx, color, CV_FILLED, 8, hierarchy );
}
// 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::imshow(WINDOW, cv_ptr->image);
// cv::imshow(WINDOW, dst);
*/
image_pub_.publish(cv_ptr->toImageMsg());
waitKey(3);
}
void callback(const sensor_msgs::ImageConstPtr& msg)
{
if (image_0_ == NULL)
{
// Take first image:
try
{
image_0_ = cv_bridge::toCvCopy(msg,
sensor_msgs::image_encodings::isColor(msg->encoding) ?
sensor_msgs::image_encodings::BGR8 :
sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR_STREAM("Failed to take first image: " << e.what());
return;
}
ROS_INFO("First image taken");
// Detect keypoints:
detector_->detect(image_0_->image, keypoints_0_);
ROS_INFO_STREAM(keypoints_0_.size() << " points found.");
// Extract keypoints' descriptors:
extractor_->compute(image_0_->image, keypoints_0_, descriptors_0_);
}
else
{
// Take second image:
try
{
image_1_ = cv_bridge::toCvShare(msg,
sensor_msgs::image_encodings::isColor(msg->encoding) ?
sensor_msgs::image_encodings::BGR8 :
sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR_STREAM("Failed to take image: " << e.what());
return;
}
// Detect keypoints:
std::vector<cv::KeyPoint> keypoints_1;
detector_->detect(image_1_->image, keypoints_1);
ROS_INFO_STREAM(keypoints_1.size() << " points found on the new image.");
// Extract keypoints' descriptors:
cv::Mat descriptors_1;
extractor_->compute(image_1_->image, keypoints_1, descriptors_1);
// Compute matches:
std::vector<cv::DMatch> matches;
match(descriptors_0_, descriptors_1, matches);
// Compute homography:
cv::Mat H;
homography(keypoints_0_, keypoints_1, matches, H);
// Draw matches:
const int s = std::max(image_0_->image.rows, image_0_->image.cols);
cv::Size size(s, s);
cv::Mat draw_image;
warped_image_ = boost::make_shared<cv_bridge::CvImage>(
image_0_->header, image_0_->encoding,
cv::Mat(size, image_0_->image.type()));
if (!H.empty()) // filter outliers
{
std::vector<char> matchesMask(matches.size(), 0);
const size_t N = matches.size();
std::vector<int> queryIdxs(N), trainIdxs(N);
for (size_t i = 0; i < N; ++i)
{
queryIdxs[i] = matches[i].queryIdx;
trainIdxs[i] = matches[i].trainIdx;
}
std::vector<cv::Point2f> points1, points2;
cv::KeyPoint::convert(keypoints_0_, points1, queryIdxs);
cv::KeyPoint::convert(keypoints_1, points2, trainIdxs);
cv::Mat points1t;
cv::perspectiveTransform(cv::Mat(points1), points1t, H);
double maxInlierDist = threshold_ < 0 ? 3 : threshold_;
for (size_t i1 = 0; i1 < points1.size(); ++i1)
{
if (cv::norm(points2[i1] - points1t.at<cv::Point2f>((int)i1,0)) <= maxInlierDist ) // inlier
matchesMask[i1] = 1;
}
// draw inliers
cv::drawMatches(
image_0_->image, keypoints_0_,
image_1_->image, keypoints_1, matches,
draw_image, cv::Scalar(0, 255, 0), cv::Scalar(0, 0, 255),
matchesMask,
cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
// draw outliers
for (size_t i1 = 0; i1 < matchesMask.size(); ++i1)
matchesMask[i1] = !matchesMask[i1];
cv::drawMatches(
image_0_->image, keypoints_0_,
image_1_->image, keypoints_1, matches,
draw_image, cv::Scalar(0, 0, 255), cv::Scalar(255, 0, 0),
matchesMask,
cv::DrawMatchesFlags::DRAW_OVER_OUTIMG | cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
ROS_INFO_STREAM("Number of inliers: " << cv::countNonZero(matchesMask));
// Wrap the new image using the homography,
// so we should have something similar to the original image:
warpPerspective(
image_1_->image, warped_image_->image, H, size,
cv::INTER_LINEAR | cv::WARP_INVERSE_MAP);
// Print the homography found:
ROS_INFO_STREAM("Homography = " << H);
}
else
{
cv::drawMatches(
image_0_->image, keypoints_0_,
image_1_->image, keypoints_1, matches,
draw_image);
image_1_->image.copyTo(warped_image_->image);
ROS_WARN_STREAM("No homography transformation found!");
}
// Publish warped image (using homography):
warped_image_->header = image_1_->header;
pub_.publish(warped_image_->toImageMsg());
// Show images:
cv::imshow("correspondences", draw_image);
cv::imshow("homography", warped_image_->image);
cv::waitKey(3);
}
}