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); }