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 openniTracking::computeContourAnalysis(){
for (unsigned int i = 0; i < contourFinder.nBlobs; i++){
int length_of_contour = contourFinder.blobs[i].pts.size();
// ellipse
fitEllipse(contourFinder.blobs[i].pts, box[i]);
// angles
blobAngle[i] = getOrientation(contourFinder.blobs[i].pts);
// assign smoothed and normalized blobs geometry vectors vars
if(computeContourGeometry){
findLines(contourFinder.blobs[i].pts,geomLines[i],30,40,30);
_s_blobGeom[i].clear();
_s_blobGeom[i].assign(geomLines[i].size(), ofVec4f());
_osc_blobGeom[i].clear();
_osc_blobGeom[i].assign(geomLines[i].size(), ofVec4f());
}
// smoothed, simple and convexHull contours computing
contourReg[i].clear();
contourReg[i].assign(length_of_contour, ofVec2f());
contourSmooth[i].clear();
contourSmooth[i].assign(length_of_contour, ofVec2f());
_s_contourSmooth[i].clear();
_s_contourSmooth[i].assign(length_of_contour, ofVec2f());
_osc_contourSmooth[i].clear();
_osc_contourSmooth[i].assign(length_of_contour, ofVec2f());
for(int j = 0; j < length_of_contour; j++){
contourReg[i].at(j) = contourFinder.blobs[i].pts[j];
}
contourS.smooth(contourReg[i], contourSmooth[i], smoothPct);
contourSimple[i].clear();
contourS.simplify(contourSmooth[i], contourSimple[i], tolerance);
_s_contourSimple[i].clear();
_s_contourSimple[i].assign(contourSimple[i].size(), ofVec2f());
_osc_contourSimple[i].clear();
_osc_contourSimple[i].assign(contourSimple[i].size(), ofVec2f());
}
}
AREXPORT std::map<int, ArLineFinderSegment *> *ArLineFinder::getLines(void)
{
// fill the laser readings into myPoints
fillPointsFromLaser();
// make lines out of myPoints into myLines
findLines();
// put the lines from myLines into combined lines
// this will recurse itself until there are no more
combineLines();
// now filter out the short lines
filterLines();
// combines the lines again
combineLines();
return myLines;
}
void GripPipeline::Process(cv::Mat & source0)
{
cv::Mat cvResizeSrc = source0;
cv::Size cvResizeDsize(0, 0);
double cvResizeFx = 0.75; // default Double
double cvResizeFy = 0.75; // default Double
int cvResizeInterpolation = cv::INTER_LINEAR;
cvResize(cvResizeSrc, cvResizeDsize, cvResizeFx, cvResizeFy, cvResizeInterpolation, this->cvResizeOutput);
cv::Mat hsvThresholdInput = cvResizeOutput;
double hsvThresholdHue[] = {69,180};
double hsvThresholdSaturation[] = {172,255};
double hsvThresholdValue[] = {112,255};
hsvThreshold(hsvThresholdInput, hsvThresholdHue, hsvThresholdSaturation, hsvThresholdValue, this->hsvThresholdOutput);
cv::Mat findContoursInput = hsvThresholdOutput;
source0= hsvThresholdOutput;
std::vector<std::vector<cv::Point> > filterContoursContours = findContoursOutput;
cv::Mat cvErodeSrc = hsvThresholdOutput;
cv::Mat cvErodeKernel;
cv::Point cvErodeAnchor(-1, -1);
double cvErodeIterations = 1;
int cvErodeBordertype = cv::BORDER_CONSTANT;
cv::Scalar cvErodeBordervalue(-1);
cvErode(cvErodeSrc, cvErodeKernel, cvErodeAnchor, cvErodeIterations, cvErodeBordertype, cvErodeBordervalue, this->cvErodeOutput);
cv::Mat findLinesInput = cvErodeOutput;
findLines(findLinesInput, this->findLinesOutput);
// print the lines
// GripPipeline::printLines(source0, findLinesOutput);
// find center
GripPipeline::findCenter(source0, findLinesOutput);
// find distance
GripPipeline::findDistance(source0, findLinesOutput, difference);
}
int main(int argc, char** argv)
{
list<line_T> theLines;
findLines(argv[1], theLines, 0);
}
void CHoughTransform::Process( const Mat& edges )
{
fillHoughMatrix( edges );
findLines();
}
