Revision history [back]

I would assume your error lies here

if((int) cvImage_out.at<float>(j,i) > thresh)
{
  circle(cvImage_out, cv::Point(i,j),5,cv::Scalar(0),2,8,0);
}

I did not read the full code but a quick look at it lets me guess that you access the image outside its boundaries. It looks like you need to swap i and j

I would assume your error lies here

if((int) cvImage_out.at<float>(j,i) > thresh)
{
  circle(cvImage_out, cv::Point(i,j),5,cv::Scalar(0),2,8,0);
}

I did not read the full code but a quick look at it lets me guess that you access the image outside its boundaries. It looks like you need to swap i and j

Edit: Have a look at http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages for the right way to transform sensor_msgs::image to cvimage It looks like you just assign a const ptr from the message to a const ptr from a cvimage. This is most certainly not correct. Try something like this:

cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);