void convertFalseColor(const cv::Mat& srcmat, cv::Mat& dstmat, palette::palettetypes paltype){
palette pal = GetPalette(paltype);
dstmat.create(srcmat.rows, srcmat.cols, CV_8UC3);
cv::Size sz = srcmat.size();
const uchar* src = srcmat.data;
uchar* dst = dstmat.data;
if( srcmat.isContinuous() && dstmat.isContinuous() )
{
sz.width *= sz.height;
sz.height = 1;
}
for(int i = 0;i<sz.width;++i){
for(int j = 0;j<sz.height;++j){
int idx = j*sz.width + i;
uint8_t val = src[idx];
dst[idx*dstmat.channels() + 0] = pal.colors[val].rgbBlue;
dst[idx*dstmat.channels() + 1] = pal.colors[val].rgbGreen;
dst[idx*dstmat.channels() + 2] = pal.colors[val].rgbRed;
}
}
}
void Binalize(const cv::Mat& depth_image, const cv::Mat& uv_map, cv::Mat1b& binary_image, short depth_threshold_mm) {
assert(depth_image.size() == uv_map.size());
assert(depth_image.type() == CV_16SC1);
assert(uv_map.type() == CV_32FC2);
cv::Size binary_size = binary_image.size();
//Clear all pixels of binary_image
binary_image = 0;
cv::Size loop_size = depth_image.size();
if(depth_image.isContinuous() && uv_map.isContinuous()) {
loop_size.width *= loop_size.height;
loop_size.height = 1;
}
for(int i = 0; i < loop_size.height; ++i) {
const short* depth = depth_image.ptr<short>(i);
const cv::Vec2f* uv = uv_map.ptr<cv::Vec2f>(i);
for(int j = 0; j < loop_size.width; ++j) {
if(depth[j] < depth_threshold_mm) {
int x = cvRound(uv[j][0] * binary_size.width);
int y = cvRound(uv[j][1] * binary_size.height);
if(0 <= x && x < binary_size.width && 0 <= y && y < binary_size.height) {
binary_image.at<unsigned char>(cv::Point(x, y)) = 255;
}
}
}
}
}
void fhogToCol(const cv::Mat& img, cv::Mat& cvFeatures,
int binSize, int colIdx, PRIMITIVE_TYPE cosFactor) {
const int orientations = 9;
// ensure array is continuous
const cv::Mat& image = (img.isContinuous() ? img : img.clone());
int channels = image.channels();
int computeChannels = 32;
int width = image.cols;
int height = image.rows;
int widthBin = width / binSize;
int heightBin = height / binSize;
CV_Assert(channels == 1 || channels == 3);
CV_Assert(cvFeatures.channels() == 1 && cvFeatures.isContinuous());
float* const H = (float*)wrCalloc(static_cast<size_t>(widthBin * heightBin * computeChannels), sizeof(float));
float* const I = (float*)wrCalloc(static_cast<size_t>(width * height * channels), sizeof(float));
float* const M = (float*)wrCalloc(static_cast<size_t>(width * height), sizeof(float));
float* const O = (float*)wrCalloc(static_cast<size_t>(width * height), sizeof(float));
// row major (interleaved) to col major (non-interleaved;clustered;block)
float* imageData = reinterpret_cast<float*>(image.data);
float* const redChannel = I;
float* const greenChannel = I + width * height;
float* const blueChannel = I + 2 * width * height;
int colMajorPos = 0, rowMajorPos = 0;
for (int row = 0; row < height; ++row) {
for (int col = 0; col < width; ++col) {
colMajorPos = col * height + row;
rowMajorPos = row * channels * width + col * channels;
blueChannel[colMajorPos] = imageData[rowMajorPos];
greenChannel[colMajorPos] = imageData[rowMajorPos + 1];
redChannel[colMajorPos] = imageData[rowMajorPos + 2];
}
}
// calc fhog in col major
gradMag(I, M, O, height, width, channels, true);
fhog(M, O, H, height, width, binSize, orientations, -1, 0.2f);
// the order of rows in cvFeatures does not matter
// as long as it is the same for all columns;
// zero channel is not copied as it is the last
// channel in H and cvFeatures rows doesn't include it
PRIMITIVE_TYPE* cdata = reinterpret_cast<PRIMITIVE_TYPE*>(cvFeatures.data);
int outputWidth = cvFeatures.cols;
for (int row = 0; row < cvFeatures.rows; ++row)
{ cdata[outputWidth * row + colIdx] = H[row] * cosFactor; }
wrFree(H);
wrFree(M);
wrFree(O);
wrFree(I);
}
void fhogToCvColT(const cv::Mat& img, cv::Mat& cvFeatures,
int binSize, int colIdx, PRIMITIVE_TYPE cosFactor) {
const int orientations = 9;
// ensure array is continuous
const cv::Mat& image = (img.isContinuous() ? img : img.clone());
int channels = image.channels();
int computeChannels = 32;
int width = image.cols;
int height = image.rows;
int widthBin = width / binSize;
int heightBin = height / binSize;
CV_Assert(channels == 1 || channels == 3);
CV_Assert(cvFeatures.channels() == 1 && cvFeatures.isContinuous());
float* const H = (float*)wrCalloc(static_cast<size_t>(widthBin * heightBin * computeChannels), sizeof(float));
float* const M = (float*)wrCalloc(static_cast<size_t>(width * height), sizeof(float));
float* const O = (float*)wrCalloc(static_cast<size_t>(width * height), sizeof(float));
float* I = NULL;
if (channels == 1)
{ I = reinterpret_cast<float*>(image.data); }
else {
I = (float*)wrCalloc(static_cast<size_t>(width * height * channels), sizeof(float));
float* imageData = reinterpret_cast<float*>(image.data);
float* redChannel = I;
float* greenChannel = I + width * height;
float* blueChannel = I + 2 * width * height;
for (int i = 0; i < height * width; ++i) {
blueChannel[i] = imageData[i * 3];
greenChannel[i] = imageData[i * 3 + 1];
redChannel[i] = imageData[i * 3 + 2];
}
}
// calc fhog in col major - switch width and height
gradMag(I, M, O, width, height, channels, true);
fhog(M, O, H, width, height, binSize, orientations, -1, 0.2f);
// the order of rows in cvFeatures does not matter
// as long as it is the same for all columns;
// zero channel is not copied as it is the last
// channel in H and cvFeatures rows doesn't include it
PRIMITIVE_TYPE* cdata = reinterpret_cast<PRIMITIVE_TYPE*>(cvFeatures.data);
int outputWidth = cvFeatures.cols;
for (int row = 0; row < cvFeatures.rows; ++row)
{ cdata[outputWidth * row + colIdx] = H[row] * cosFactor; }
wrFree(H);
wrFree(M);
wrFree(O);
if (channels != 1)
{ wrFree(I); }
}
void fhog(const cv::Mat& image, vector<Mat>& fhogs, int binSize,
int orientations) {
assert(image.type() == CV_32F || image.type() == CV_32FC3);
assert(image.isContinuous());
float *M = new float[image.rows * image.cols];
float *O = new float[image.rows * image.cols];
int hb = image.rows / binSize;
int wb = image.cols / binSize;
int nChannls = orientations * 3 + 5;
float *H = new float[hb * wb * nChannls];
fill_n(H, hb * wb * nChannls, 0);
gradientMagnitude(image, M, O);
fhog(M, O, H, image.rows, image.cols, binSize, orientations, -1, 0.2f);
for (size_t i = 0; i < nChannls; i++) {
Mat tmp(Size(wb, hb), CV_32FC1);
Matlab2OpenCVC1(H + ( i * (wb * hb)), tmp);
fhogs.push_back(tmp);
}
delete[] H;
delete[] M;
delete[] O;
}
void save(Archive &ar, const cv::Mat &mat)
{
int rows, cols, type;
bool continuous;
rows = mat.rows;
cols = mat.cols;
type = mat.type();
continuous = mat.isContinuous();
ar &rows &cols &type &continuous;
if (continuous)
{
const int data_size = rows * cols * static_cast<int>(mat.elemSize());
auto mat_data = cereal::binary_data(mat.ptr(), data_size);
ar &mat_data;
}
else
{
const int row_size = cols * static_cast<int>(mat.elemSize());
for (int i = 0; i < rows; i++)
{
auto row_data = cereal::binary_data(mat.ptr(i), row_size);
ar &row_data;
}
}
}
// Save a matrix which is generated by OpenCV
int saveMat( const string& filename, const cv::Mat& M)
{
if (M.empty())
{
return 0;
}
ofstream out(filename.c_str(), ios::out|ios::binary);
if (!out)
return 0;
int cols = M.cols;
int rows = M.rows;
int chan = M.channels();
int eSiz = (M.dataend-M.datastart)/(cols*rows*chan);
// Write header
out.write((char*)&cols,sizeof(cols));
out.write((char*)&rows,sizeof(rows));
out.write((char*)&chan,sizeof(chan));
out.write((char*)&eSiz,sizeof(eSiz));
// Write data.
if (M.isContinuous())
{
out.write((char *)M.data,cols*rows*chan*eSiz);
}
else
{
return 0;
}
out.close();
return 1;
}
// Read one cv::Mat from file
bool read_one(FILE * file, cv::Mat & data)
{
bool okay = true;
int32_t rows, cols; uint32_t type;
okay &= read_one(file, rows);
okay &= read_one(file, cols);
okay &= read_one(file, type);
if (rows <= 0 || cols <= 0 || (type & ~CV_MAT_TYPE_MASK) != 0)
return false;
data.create(rows, cols, type);
// If matrix memory is continuous, we can reshape the matrix
if (data.isContinuous()) {
cols = rows*cols;
rows = 1;
}
// Currently only supports float/double matrices!
if (data.depth() == CV_32F)
for (int r = 0; r < rows; ++r)
okay &= read_n(file, data.ptr<float>(r), cols);
else if (data.depth() == CV_64F)
for (int r = 0; r < rows; ++r)
okay &= read_n(file, data.ptr<double>(r), cols);
else
return false;
return okay;
}
// using .ptr and * ++ and bitwise (continuous+channels)
void colorReduce7(cv::Mat &image, int div=64) {
int nl= image.rows; // number of lines
int nc= image.cols ; // number of columns
if (image.isContinuous()) {
// then no padded pixels
nc= nc*nl;
nl= 1; // it is now a 1D array
}
int n= static_cast<int>(log(static_cast<double>(div))/log(2.0));
// mask used to round the pixel value
uchar mask= 0xFF<<n; // e.g. for div=16, mask= 0xF0
for (int j=0; j<nl; j++) {
uchar* data= image.ptr<uchar>(j);
for (int i=0; i<nc; i++) {
// process each pixel ---------------------
*data++= *data&mask + div/2;
*data++= *data&mask + div/2;
*data++= *data&mask + div/2;
// end of pixel processing ----------------
} // end of line
}
}
//
// showPropertiesOfMat
//
// ...displays all properties of specified Mat.
//
void showPropertiesOfMat (const cv::Mat &src_mat)
{
// 行数
std::cout << "rows:" << src_mat.rows <<std::endl;
// 列数
std::cout << "cols:" << src_mat.cols << std::endl;
// 次元数
std::cout << "dims:" << src_mat.dims << std::endl;
// サイズ（2次元の場合）
std::cout << "size[]:" << src_mat.size().width << "," << src_mat.size().height << "[byte]" << std::endl;
// ビット深度ID
std::cout << "depth (ID):" << src_mat.depth() << "(=" << CV_64F << ")" << std::endl;
// チャンネル数
std::cout << "channels:" << src_mat.channels() << std::endl;
// 1要素内の1チャンネル分のサイズ [バイト単位]
std::cout << "elemSize1 (elemSize/channels):" << src_mat.elemSize1() << "[byte]" << std::endl;
// 要素の総数
std::cout << "total:" << src_mat.total() << std::endl;
// ステップ数 [バイト単位]
std::cout << "step:" << src_mat.step << "[byte]" << std::endl;
// 1ステップ内のチャンネル総数
std::cout << "step1 (step/elemSize1):" << src_mat.step1() << std::endl;
// データは連続か？
std::cout << "isContinuous:" << (src_mat.isContinuous()?"true":"false") << std::endl;
// 部分行列か？
std::cout << "isSubmatrix:" << (src_mat.isSubmatrix()?"true":"false") << std::endl;
// データは空か？
std::cout << "empty:" << (src_mat.empty()?"true":"false") << std::endl;
}
static inline void lbsp_computeImpl(const cv::Mat& oInputImg, const cv::Mat& oRefImg, const std::vector<cv::KeyPoint>& voKeyPoints,
cv::Mat& oDesc, bool bSingleColumnDesc, size_t nThreshold) {
static_assert(LBSP::DESC_SIZE==2,"bad assumptions in impl below");
CV_DbgAssert(oRefImg.empty() || (oRefImg.size==oInputImg.size && oRefImg.type()==oInputImg.type()));
CV_DbgAssert(oInputImg.type()==CV_8UC1 || oInputImg.type()==CV_8UC3);
const size_t nChannels = (size_t)oInputImg.channels();
const cv::Mat& oRefMat = oRefImg.empty()?oInputImg:oRefImg;
CV_DbgAssert(oInputImg.isContinuous() && oRefMat.isContinuous());
const size_t nKeyPoints = voKeyPoints.size();
if(nChannels==1) {
if(bSingleColumnDesc)
oDesc.create((int)nKeyPoints,1,CV_16UC1);
else
oDesc.create(oInputImg.size(),CV_16UC1);
for(size_t k=0; k<nKeyPoints; ++k) {
const int x = (int)voKeyPoints[k].pt.x;
const int y = (int)voKeyPoints[k].pt.y;
LBSP::computeDescriptor<1>(oInputImg,oRefMat.at<uchar>(y,x),x,y,0,nThreshold,oDesc.at<ushort>((int)k));
}
return;
}
else { //nChannels==3
if(bSingleColumnDesc)
oDesc.create((int)nKeyPoints,1,CV_16UC3);
else
oDesc.create(oInputImg.size(),CV_16UC3);
const std::array<size_t,3> anThreshold = {nThreshold,nThreshold,nThreshold};
for(size_t k=0; k<nKeyPoints; ++k) {
const int x = (int)voKeyPoints[k].pt.x;
const int y = (int)voKeyPoints[k].pt.y;
const uchar* acRef = oRefMat.data+oInputImg.step.p[0]*y+oInputImg.step.p[1]*x;
LBSP::computeDescriptor(oInputImg,acRef,x,y,anThreshold,((ushort*)(oDesc.data+oDesc.step.p[0]*k)));
}
}
}
void cv::write(const std::string& sFilePath, const cv::Mat& _oData, cv::MatArchiveList eArchiveType) {
lvAssert_(!sFilePath.empty() && !_oData.empty(),"output file path and matrix must both be non-empty");
cv::Mat oData = _oData.isContinuous()?_oData:_oData.clone();
if(eArchiveType==MatArchive_FILESTORAGE) {
cv::FileStorage oArchive(sFilePath,cv::FileStorage::WRITE);
lvAssert__(oArchive.isOpened(),"could not open archive at '%s' for writing",sFilePath.c_str());
oArchive << "htag" << lv::getVersionStamp();
oArchive << "date" << lv::getTimeStamp();
oArchive << "matrix" << oData;
}
else if(eArchiveType==MatArchive_PLAINTEXT) {
std::ofstream ssStr(sFilePath);
lvAssert__(ssStr.is_open(),"could not open text file at '%s' for writing",sFilePath.c_str());
ssStr << "htag " << lv::getVersionStamp() << std::endl;
ssStr << "date " << lv::getTimeStamp() << std::endl;
ssStr << "nDataType " << (int32_t)oData.type() << std::endl;
ssStr << "nDataDepth " << (int32_t)oData.depth() << std::endl;
ssStr << "nChannels " << (int32_t)oData.channels() << std::endl;
ssStr << "nElemSize " << (uint64_t)oData.elemSize() << std::endl;
ssStr << "nElemCount " << (uint64_t)oData.total() << std::endl;
ssStr << "nDims " << (int32_t)oData.dims << std::endl;
ssStr << "anSizes";
for(int nDimIdx=0; nDimIdx<oData.dims; ++nDimIdx)
ssStr << " " << (int32_t)oData.size[nDimIdx];
ssStr << std::endl << std::endl;
if(oData.depth()!=CV_64F)
_oData.convertTo(oData,CV_64F);
double* pdData = (double*)oData.data;
for(int nElemIdx=0; nElemIdx<(int)oData.total(); ++nElemIdx) {
ssStr << *pdData++;
for(int nElemPackIdx=1; nElemPackIdx<oData.channels(); ++nElemPackIdx)
ssStr << " " << *pdData++;
if(((nElemIdx+1)%oData.size[oData.dims-1])==0)
ssStr << std::endl;
else
ssStr << " ";
}
lvAssert_(ssStr,"plain text archive write failed");
}
else if(eArchiveType==MatArchive_BINARY) {
std::ofstream ssStr(sFilePath,std::ios::binary);
lvAssert__(ssStr.is_open(),"could not open binary file at '%s' for writing",sFilePath.c_str());
const int32_t nDataType = (int32_t)oData.type();
ssStr.write((const char*)&nDataType,sizeof(nDataType));
const uint64_t nElemSize = (uint64_t)oData.elemSize();
ssStr.write((const char*)&nElemSize,sizeof(nElemSize));
const uint64_t nElemCount = (uint64_t)oData.total();
ssStr.write((const char*)&nElemCount,sizeof(nElemCount));
const int32_t nDims = (int32_t)oData.dims;
ssStr.write((const char*)&nDims,sizeof(nDims));
for(int32_t nDimIdx=0; nDimIdx<nDims; ++nDimIdx) {
const int32_t nDimSize = (int32_t)oData.size[nDimIdx];
ssStr.write((const char*)&nDimSize,sizeof(nDimSize));
}
ssStr.write((const char*)(oData.data),nElemSize*nElemCount);
lvAssert_(ssStr,"binary archive write failed");
}
else
lvError("unrecognized mat archive type flag");
}
void Processor::colorReduceCheckForContinuous(const cv::Mat& image,
cv::Mat& result, int div) {
startTimer();
int lines = image.rows;
int pixels = image.cols * image.channels();
if (image.isContinuous()) {
pixels = pixels * lines;
lines = 1;
std::cout << "Image is continuous" << std::endl;
} else {
std::cout << "Image is not continuous" << std::endl;
}
result.create(image.rows, image.cols, image.type());
for (int i = 0; i < lines; i++) {
const uchar* data_in = image.ptr<uchar>(i);
uchar* data_out = result.ptr<uchar>(i);
for (int j = 0; j < pixels; j++)
*data_out++ = *data_in++ / div * div + div / 2;
}
stopTimer("ColorReduceCheckForContinous");
}
void merge(cv::Mat &in1, cv::Mat &in2,cv::Mat &out)
{
int nLines = in1.rows;
int nc = in1.cols * in1.channels();
if(in1.isContinuous())
{
nc = nc*nLines;
nLines = 1;
}
for(int j=0;j<nLines;j++)
{
uchar* dataIN1 = in1.ptr<uchar>(j); //fajne :)
uchar* dataIN2 = in2.ptr<uchar>(j);
uchar* dataOUT = out.ptr<uchar>(j);
for(int i=0;i<nc; i++)
{
if(dataIN2[i] == 0)
{
dataOUT[i] = dataIN1[i];
}
else
{
dataOUT[i] = dataIN2[i];
}
}
}
}
void TreeLiveProc::process (const cv::Mat& dmap,
cv::Mat& lmap )
{
if( dmap.depth() != CV_16U ) TLIVEEXCEPT("depth has incorrect channel type")
if( dmap.channels() != 1 ) TLIVEEXCEPT("depth has incorrect channel count")
if( dmap.size().width != 640 ) TLIVEEXCEPT("depth has incorrect width")
if( dmap.size().height != 480 ) TLIVEEXCEPT("depth has incorrect height")
if( !dmap.isContinuous() ) TLIVEEXCEPT("depth has non contiguous rows")
// alloc the buffer if it isn't done yet
lmap.create( 480, 640, CV_8UC(1) );
// copy depth to cuda
cudaMemcpy(m_dmap_device, (const void*) dmap.data,
640*480*sizeof(uint16_t), cudaMemcpyHostToDevice);
// process the dmap
CUDA_runTree( 640,480, focal,
m_tree->treeHeight(),
m_tree->numNodes(),
m_tree->nodes_device(),
m_tree->leaves_device(),
m_dmap_device,
m_lmap_device );
// download back from cuda
cudaMemcpy((Label*)(lmap.data), m_lmap_device,
640*480*sizeof(Label), cudaMemcpyDeviceToHost);
}
void ImgProc::cvtAlphaToBinary(const cv::Mat &src, cv::Mat &out) const
{
int nl= src.rows; // number of lines
int nc= src.cols * src.channels();
out = cv::Mat(src.rows, src.cols, CV_8UC1);
if (src.isContinuous())
{
// then no padded pixels
nc= nc*nl;
nl= 1; // it is now a 1D array
}
// this loop is executed only once
// in case of continuous images
for (int j=0; j<nl; j++) {
const uchar* data= src.ptr<uchar>(j);
uchar * out_data = out.ptr<uchar>(j);
for (int i=3; i<nc; i+=4) {
*out_data++ = data[i] < ALPHA_THRESSHOLD ? 0 : 255;
}
data = NULL; delete data;
out_data = NULL; delete out_data;
}
}
ConnectedComponents::ConnectedComponents( cv::Mat &img )
{
CV_Assert(img.depth() == CV_8U); // accept only uchar images
CV_Assert(img.channels() == 1); // just one channel
rows = img.rows;
cols = img.cols;
group_t *labels = new group_t [img.cols * img.rows];
int nrows = img.rows;
int ncols = img.cols;
if (img.isContinuous())
{
ncols *= nrows;
nrows = 1;
}
// populate label matrix
int i, j, ct, label = 0;
uchar* p;
for (i = 0; i < nrows; ++i)
{
group_t &q = labels[i * img.cols + j];
p = img.ptr<uchar>(i);
for (j = 0; j < ncols; ++j)
{
labels[i * img.cols + j].pixel = p[j];
labels[i * img.cols + j].label = UNASSIGNED;
labels[i * img.cols + j].parent = NULL;
}
}
}
inline std::vector<uint8_t> format_signal(const cv::Mat& oInputImage) {
CV_Assert(!oInputImage.empty() && oInputImage.isContinuous() && (oInputImage.type()==CV_8UC1 || oInputImage.type()==CV_8UC3));
std::vector<uint8_t> vSignal;
// If greyscale
if (oInputImage.type() == CV_8UC1) {
int nbPixel = oInputImage.rows * oInputImage.cols;
for (int byteCounter = 0; byteCounter < nbPixel; ++byteCounter)
{
uint8_t currentPixel = (uint8_t)*(oInputImage.data + byteCounter * sizeof(uint8_t));
vSignal.push_back(currentPixel);
}
}
// If color
else {
// For B, G, R
int nbOfColor = 3;
int nbPixel = oInputImage.rows * oInputImage.cols * nbOfColor;
for (int colorIndex = 0; colorIndex < nbOfColor; ++colorIndex)
{
for (int byteCounter = colorIndex; byteCounter < nbPixel; byteCounter+=nbOfColor)
{
uint8_t currentPixel = (uint8_t)*(oInputImage.data + byteCounter * sizeof(uint8_t));
vSignal.push_back(currentPixel);
}
}
}
return vSignal;
}
// Write one cv::Mat to file
bool write_one(FILE * file, const cv::Mat & data)
{
bool okay = true;
okay &= write_one(file, int32_t(data.rows));
okay &= write_one(file, int32_t(data.cols));
okay &= write_one(file, uint32_t(data.type()));
// If matrix memory is continuous, we can reshape the matrix
int rows = data.rows, cols = data.cols;
if (data.isContinuous()) {
cols = rows*cols;
rows = 1;
}
// Currently only supports float/double matrices!
assert(data.depth() == CV_32F || data.depth() == CV_64F);
if (data.depth() == CV_32F)
for (int r = 0; r < rows; ++r)
okay &= write_n(file, data.ptr<float>(r), cols);
else if (data.depth() == CV_64F)
for (int r = 0; r < rows; ++r)
okay &= write_n(file, data.ptr<double>(r), cols);
else
return false;
return okay;
}
static void createRawImage(const cv::Mat &src, ppr_raw_image_type &dst)
{
if (!src.isContinuous()) qFatal("PP5Context::createRawImage requires continuous data.");
else if (src.channels() == 3) ppr_raw_image_create(&dst, src.cols, src.rows, PPR_RAW_IMAGE_BGR24);
else if (src.channels() == 1) ppr_raw_image_create(&dst, src.cols, src.rows, PPR_RAW_IMAGE_GRAY8);
else qFatal("PP5Context::createRawImage invalid channel count.");
memcpy(dst.data, src.data, src.channels()*src.rows*src.cols);
}
inline void opencv_copy_image_to_mat(const image2d &image,
cv::Mat &mat,
command_queue &queue = system::default_queue())
{
BOOST_ASSERT(mat.isContinuous());
BOOST_ASSERT(image.get_context() == queue.get_context());
queue.enqueue_read_image(image, image.origin(), image.size(), mat.data);
}
void UndistorterPTAM::undistort(const cv::Mat& image, cv::OutputArray result) const
{
if (!valid)
{
result.getMatRef() = image;
return;
}
if (image.rows != in_height || image.cols != in_width)
{
printf("UndistorterPTAM: input image size differs from expected input size! Not undistorting.\n");
result.getMatRef() = image;
return;
}
if (in_height == out_height && in_width == out_width && inputCalibration[4] == 0)
{
// No transformation if neither distortion nor resize
result.getMatRef() = image;
return;
}
result.create(out_height, out_width, CV_8U);
cv::Mat resultMat = result.getMatRef();
assert(result.getMatRef().isContinuous());
assert(image.isContinuous());
uchar* data = resultMat.data;
for(int idx = out_width*out_height-1;idx>=0;idx--)
{
// get interp. values
float xx = remapX[idx];
float yy = remapY[idx];
if(xx<0)
data[idx] = 0;
else
{
// get integer and rational parts
int xxi = xx;
int yyi = yy;
xx -= xxi;
yy -= yyi;
float xxyy = xx*yy;
// get array base pointer
const uchar* src = (uchar*)image.data + xxi + yyi * in_width;
// interpolate (bilinear)
data[idx] = xxyy * src[1+in_width]
+ (yy-xxyy) * src[in_width]
+ (xx-xxyy) * src[1]
+ (1-xx-yy+xxyy) * src[0];
}
}
}
LocalBinaryPattern::LocalBinaryPattern( const cv::Mat& img)
: RFeatures::FeatureOperator( img.size()), _imgRct( 0, 0, img.cols, img.rows)
{
assert( img.channels() == 1);
assert( img.isContinuous());
cv::Mat iimg;
img.convertTo( iimg, CV_32F);
cv::integral( iimg, _intImg, CV_64F);
} // end ctor
double getThreshVal_Otsu_8u( const cv::Mat& _src )
{
cv::Size size = _src.size();
if ( _src.isContinuous() )
{
size.width *= size.height;
size.height = 1;
}
const int N = 256;
int i, j, h[N] = {0};
for ( i = 0; i < size.height; i++ )
{
const uchar* src = _src.data + _src.step*i;
for ( j = 0; j <= size.width - 4; j += 4 )
{
int v0 = src[j], v1 = src[j+1];
h[v0]++; h[v1]++;
v0 = src[j+2]; v1 = src[j+3];
h[v0]++; h[v1]++;
}
for ( ; j < size.width; j++ )
h[src[j]]++;
}
double mu = 0, scale = 1./(size.width*size.height);
for ( i = 0; i < N; i++ )
mu += i*h[i];
mu *= scale;
double mu1 = 0, q1 = 0;
double max_sigma = 0, max_val = 0;
for ( i = 0; i < N; i++ )
{
double p_i, q2, mu2, sigma;
p_i = h[i]*scale;
mu1 *= q1;
q1 += p_i;
q2 = 1. - q1;
if ( std::min(q1,q2) < FLT_EPSILON || std::max(q1,q2) > 1. - FLT_EPSILON )
continue;
mu1 = (mu1 + i*p_i)/q1;
mu2 = (mu - q1*mu1)/q2;
sigma = q1*q2*(mu1 - mu2)*(mu1 - mu2);
if ( sigma > max_sigma )
{
max_sigma = sigma;
max_val = i;
}
}
return max_val;
}
/**
*Decodes and returns the grabbed video frame.
*/
bool RaspiCam_Cv::retrieve ( cv::Mat& image ) {
image.create ( _height,_width,CV_8UC3 );
if ( image.isContinuous() )
memcpy ( image.ptr<uchar> ( 0 ),_imgData.ptr,_imgData.size );
else {
for ( int y=0; y<_height; y++ )
memcpy ( image.ptr<uchar> ( y ),_imgData.ptr+_width*y*3,_width*3 );
}
}
void convertMatToVector(cv::Mat &mat, std::vector<float> &array) {
if (mat.isContinuous()) {
array.assign((float*)mat.datastart, (float*)mat.dataend);
}
else {
for (int i = 0; i < mat.rows; ++i) {
array.insert(array.end(), (float*)mat.ptr<uchar>(i), (float*)mat.ptr<uchar>(i) + mat.cols);
}
}
}
inline void opencv_copy_buffer_to_mat(const buffer_iterator<T> buffer,
cv::Mat &mat,
command_queue &queue = system::default_queue())
{
BOOST_ASSERT(mat.isContinuous());
::boost::compute::copy_n(
buffer, mat.cols * mat.rows, reinterpret_cast<T *>(mat.data), queue
);
}
//===========================================================================
void sum2one(cv::Mat &M)
{
assert(M.type() == CV_64F);
double sum = 0; int cols = M.cols, rows = M.rows;
if(M.isContinuous()){cols *= rows;rows = 1;}
for(int i = 0; i < rows; i++){
const double* Mi = M.ptr<double>(i);
for(int j = 0; j < cols; j++)sum += *Mi++;
}
M /= sum; return;
}
template <typename Tp, int cn> KDTree <Tp, cn>::
KDTree(const cv::Mat &img, const int _leafNumber, const int _zeroThresh)
: height(img.rows), width(img.cols),
leafNumber(_leafNumber), zeroThresh(_zeroThresh)
///////////////////////////////////////////////////
{
int imgch = img.channels();
CV_Assert( img.isContinuous() && imgch <= cn);
for(size_t i = 0; i < img.total(); i++)
{
cv::Vec<Tp, cn> v = cv::Vec<Tp, cn>::all((Tp)0);
for (int c = 0; c < imgch; c++)
{
v[c] = *((Tp*)(img.data) + i*imgch + c);
}
data.push_back(v);
}
generate_seq( std::back_inserter(idx), 0, int(data.size()) );
std::fill_n( std::back_inserter(nodes),
int(data.size()), cv::Point2i(0, 0) );
std::stack <int> left, right;
left.push( 0 );
right.push( int(idx.size()) );
while ( !left.empty() )
{
int _left = left.top(); left.pop();
int _right = right.top(); right.pop();
if ( _right - _left <= leafNumber)
{
for (int i = _left; i < _right; ++i)
nodes[idx[i]] = cv::Point2i(_left, _right);
continue;
}
int nth = _left + (_right - _left)/2;
int dimIdx = getMaxSpreadN(_left, _right);
KDTreeComparator comp( this, dimIdx );
std::nth_element(/**/
idx.begin() + _left,
idx.begin() + nth,
idx.begin() + _right, comp
/**/);
left.push(_left); right.push(nth + 1);
left.push(nth + 1); right.push(_right);
}
}
cv::Mat connectedComponentsFilter(cv::Mat& curFrame, cv::Mat& img) {
if (!img.isContinuous()) {
throwError("Parammeter 'img' in 'connectedComponentsFilter' must be continuous");
}
//morphology_(img);
maskContours.clear();
// Отрисовать найденные области обратно в маску
cv::Mat result(img.size(), img.type());
result.setTo(0);
cv::findContours(img, maskContours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);
size_t i = 0;
while (i < maskContours.size()) {
Contour& contour = maskContours[i];
cv::Mat contourMat(contour, false);
double len = cv::arcLength(contourMat, true);
if (len * PERIM_SCALE < img.size().height + img.size().width) {
// Отбрасываем контуры со слишком маленьким периметром.
maskContours.erase(maskContours.begin() + i);
} else {
// Достаточно большие контуры аппроксимируем указанным методом.
Contour newContour;
// Методы аппроксимации.
//cv::approxPolyDP(contourMat, newContour, CHAIN_APPROX_SIMPLE, true);
cv::convexHull(contourMat, newContour, true);
cv::Mat newContourMat(newContour, false);
Rect boundingRect = cv::boundingRect(newContourMat);
cv::rectangle(curFrame, boundingRect, cv::Scalar(255));
maskContours[i] = newContour;
i++;
//points.push_back(CvPoint(boundingRect.x + boundingRect.width / 2, boundingRect.y + boundingRect.height / 2));
}
}
if (!maskContours.empty()) { // Обходим баг OpenCV 2.1.0; в 2.3.1 он уже исправлен.
cv::drawContours(result, maskContours, -1, cv::Scalar(255), FILLED);
}
return result;
}