int main(int argc, char** argv)
{
std::cout << "INITIALIZING COLOR_RECOGNIZER..." << std::endl;
ros::init(argc, argv, "color_recog");
ros::NodeHandle n;
ros::Rate loop(30);
std::cout << "ColorRecognizer.->Triying to initialize kinect sensor... " << std::endl;
cv::VideoCapture capture(CV_CAP_OPENNI);
if(!capture.isOpened())
{
std::cout << "ColorRecognizer.->Cannot open kinect :'(" << std::endl;
return 1;
}
capture.set(CV_CAP_OPENNI_DEPTH_GENERATOR_REGISTRATION, CV_CAP_OPENNI_DEPTH_GENERATOR_REGISTRATION_ON);
std::cout << "ColorRecognizer.->Kinect sensor started :D" << std::endl;
cv::Mat matDepth;
cv::Mat matColor;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pclFrame(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pclNotPlane(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pclPlane(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::visualization::CloudViewer viewer("Original");
while(ros::ok() && cv::waitKey(10) != 27 && !viewer.wasStopped())
{
if(!capture.grab())
{
loop.sleep();
ros::spinOnce();
continue;
}
capture.retrieve(matDepth, CV_CAP_OPENNI_POINT_CLOUD_MAP);
capture.retrieve(matColor, CV_CAP_OPENNI_BGR_IMAGE);
cvMat2Pcl(matColor, matDepth, *pclFrame);
extractPlane(pclFrame, pclFrame, pclPlane, pclNotPlane);
cv::imshow("Original", matColor);
viewer.showCloud(pclFrame);
ros::spinOnce();
}
}
static void
ppmTo16ColorPcx(pixel ** const pixels,
int const cols,
int const rows,
struct pcxCmapEntry const pcxcmap[],
int const colors,
colorhash_table const cht,
bool const packbits,
unsigned int const planesRequested,
unsigned int const xPos,
unsigned int const yPos) {
int Planes, BytesPerLine, BitsPerPixel;
unsigned char *indexRow; /* malloc'ed */
/* indexRow[x] is the palette index of the pixel at column x of
the row currently being processed
*/
unsigned char *planesrow; /* malloc'ed */
/* This is the input for a single row to the compressor */
int row;
if (packbits) {
Planes = 1;
if (colors > 4) BitsPerPixel = 4;
else if (colors > 2) BitsPerPixel = 2;
else BitsPerPixel = 1;
} else {
BitsPerPixel = 1;
if (planesRequested)
Planes = planesRequested;
else {
if (colors > 8) Planes = 4;
else if (colors > 4) Planes = 3;
else if (colors > 2) Planes = 2;
else Planes = 1;
}
}
BytesPerLine = ((cols * BitsPerPixel) + 7) / 8;
MALLOCARRAY_NOFAIL(indexRow, cols);
MALLOCARRAY_NOFAIL(planesrow, BytesPerLine);
write_header(stdout, cols, rows, BitsPerPixel, Planes, pcxcmap,
xPos, yPos);
for (row = 0; row < rows; ++row) {
int col;
for (col = 0; col < cols; ++col)
indexRow[col] = indexOfColor(cht, pixels[row][col]);
if (packbits) {
PackBits(indexRow, cols, planesrow, BitsPerPixel);
PCXEncode(stdout, planesrow, BytesPerLine);
} else {
unsigned int plane;
for (plane = 0; plane < Planes; ++plane) {
extractPlane(indexRow, cols, planesrow, plane);
PCXEncode(stdout, planesrow, BytesPerLine);
}
}
}
free(planesrow);
free(indexRow);
}
