void blobFromProto(const caffe::BlobProto &pbBlob, cv::Mat &dstBlob) { MatShape shape; blobShapeFromProto(pbBlob, shape); dstBlob.create((int)shape.size(), &shape[0], CV_32F); if (pbBlob.data_size()) { // Single precision floats. CV_Assert(pbBlob.data_size() == (int)dstBlob.total()); CV_DbgAssert(pbBlob.GetDescriptor()->FindFieldByLowercaseName("data")->cpp_type() == FieldDescriptor::CPPTYPE_FLOAT); Mat(dstBlob.dims, &dstBlob.size[0], CV_32F, (void*)pbBlob.data().data()).copyTo(dstBlob); } else { CV_Assert(pbBlob.has_raw_data()); const std::string& raw_data = pbBlob.raw_data(); if (pbBlob.raw_data_type() == caffe::FLOAT16) { // Half precision floats. CV_Assert(raw_data.size() / 2 == (int)dstBlob.total()); Mat halfs((int)shape.size(), &shape[0], CV_16SC1, (void*)raw_data.c_str()); convertFp16(halfs, dstBlob); } else if (pbBlob.raw_data_type() == caffe::FLOAT) { CV_Assert(raw_data.size() / 4 == (int)dstBlob.total()); Mat((int)shape.size(), &shape[0], CV_32FC1, (void*)raw_data.c_str()).copyTo(dstBlob); } else CV_Error(Error::StsNotImplemented, "Unexpected blob data type"); } }
void blobFromProto(const caffe::BlobProto &pbBlob, cv::Mat &dstBlob) { MatShape shape; blobShapeFromProto(pbBlob, shape); dstBlob.create((int)shape.size(), &shape[0], CV_32F); float *dstData = dstBlob.ptr<float>(); if (pbBlob.data_size()) { // Single precision floats. CV_Assert(pbBlob.data_size() == (int)dstBlob.total()); CV_DbgAssert(pbBlob.GetDescriptor()->FindFieldByLowercaseName("data")->cpp_type() == FieldDescriptor::CPPTYPE_FLOAT); for (int i = 0; i < pbBlob.data_size(); i++) dstData[i] = pbBlob.data(i); } else { // Half precision floats. CV_Assert(pbBlob.raw_data_type() == caffe::FLOAT16); std::string raw_data = pbBlob.raw_data(); CV_Assert(raw_data.size() / 2 == (int)dstBlob.total()); Mat halfs((int)shape.size(), &shape[0], CV_16SC1, (void*)raw_data.c_str()); convertFp16(halfs, dstBlob); } }
double __aruco_solve_pnp(const std::vector<cv::Point3f>& p3d, const std::vector<cv::Point2f>& p2d, const cv::Mat& cam_matrix, const cv::Mat& dist, cv::Mat& r_io, cv::Mat& t_io) { assert(r_io.type() == CV_32F); assert(t_io.type() == CV_32F); assert(t_io.total() == r_io.total()); assert(t_io.total() == 3); auto toSol = [](const cv::Mat& r, const cv::Mat& t) { typename LevMarq<T>::eVector sol(6); for (int i = 0; i < 3; i++) { sol(i) = r.ptr<float>(0)[i]; sol(i + 3) = t.ptr<float>(0)[i]; } return sol; }; auto fromSol = [](const typename LevMarq<T>::eVector& sol, cv::Mat& r, cv::Mat& t) { r.create(1, 3, CV_32F); t.create(1, 3, CV_32F); for (int i = 0; i < 3; i++) { r.ptr<float>(0)[i] = sol(i); t.ptr<float>(0)[i] = sol(i + 3); } }; cv::Mat Jacb; auto err_f = [&](const typename LevMarq<T>::eVector& sol, typename LevMarq<T>::eVector& err) { std::vector<cv::Point2f> p2d_rej; cv::Mat r, t; fromSol(sol, r, t); cv::projectPoints(p3d, r, t, cam_matrix, dist, p2d_rej, Jacb); err.resize(p3d.size() * 2); int err_idx = 0; for (size_t i = 0; i < p3d.size(); i++) { err(err_idx++) = p2d_rej[i].x - p2d[i].x; err(err_idx++) = p2d_rej[i].y - p2d[i].y; } }; auto jac_f = [&](const typename LevMarq<T>::eVector& sol, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& J) { (void)(sol); J.resize(p3d.size() * 2, 6); for (size_t i = 0; i < p3d.size() * 2; i++) { double* jacb = Jacb.ptr<double>(i); for (int j = 0; j < 6; j++) J(i, j) = jacb[j]; } }; LevMarq<T> solver; solver.setParams(100, 0.01, 0.01); // solver.verbose()=true; typename LevMarq<T>::eVector sol = toSol(r_io, t_io); auto err = solver.solve(sol, err_f, jac_f); fromSol(sol, r_io, t_io); return err; }
void CameraParameters::setParams(cv::Mat cameraMatrix, cv::Mat distorsionCoeff, cv::Size size) { CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3); CV_Assert(distorsionCoeff.total() >= 4 && distorsionCoeff.total() < 7); cameraMatrix.convertTo(CameraMatrix, CV_32FC1); distorsionCoeff.convertTo(Distorsion, CV_32FC1); CamSize = size; }
int Hamming::fitness(cv::Mat &A, cv::Mat &B) const { cv::Mat C; cv::bitwise_xor(A, B, C); int measure = cv::countNonZero(C); //czarne pixele int inpNonZero = A.total() - cv::countNonZero(A); //czarne pixele wejsciowe int refNonZero = B.total() - cv::countNonZero(B); //czarne pixele referencyjne if(inpNonZero < refNonZero) { measure += (refNonZero - inpNonZero) * 10; } return measure; }
/*! * */ void CameraParameters::setParams(cv::Mat &cameraMatrix, cv::Mat &distorsionCoeff, cv::Size &size) throw(cv::Exception) { if (cameraMatrix.rows!=3 || cameraMatrix.cols!=3) throw cv::Exception(9000,"invalid input cameraMatrix","CameraParameters::setParams", __FILE__,__LINE__); cameraMatrix.convertTo(_k,CV_32FC1); if ( distorsionCoeff.total()<4 || distorsionCoeff.total()>=7 ) throw cv::Exception(9000,"invalid input distorsionCoeff","CameraParameters::setParams", __FILE__,__LINE__); distorsionCoeff.convertTo(_distor, CV_32FC1); _size=size; }
void WriteToMesh::writeDisparityToMeshProcessed(cv::Mat point_cloud, cv::Mat disparity, std::string filename){ std::ofstream plyfile(filename.c_str()); if(!plyfile.is_open()){ throw std::runtime_error("File not found on saveComputedPoints"); } int failed_pixel = 0; for(int y=0; y<disparity.rows; y++) for(int x=0; x<disparity.cols; x++){ if(disparity.at<uchar>(y,x)==0){ failed_pixel++; } } int point_num = (int)point_cloud.total(); plyfile << "ply" << std::endl; plyfile << "format ascii 1.0" << std::endl; plyfile << "element vertex " << point_num - failed_pixel << std::endl; plyfile << "property float x" << std::endl; plyfile << "property float y" << std::endl; plyfile << "property float z" << std::endl; plyfile << "end_header" << std::endl; cv::Point3f point; for(int y=0; y<point_cloud.rows; y++) for(int x=0; x<point_cloud.cols; x++){ if(disparity.at<uchar>(y,x)==0){ continue; } point = point_cloud.at<cv::Point3f>(y,x); plyfile << point.x << " " << point.y << " " << point.z << std::endl; } plyfile.close(); }
std::vector<unsigned long> create_histogram(const cv::Mat& mat, int channel) { assert(channel >= 0 && channel < mat.channels()); assert(mat.depth() == CV_8U); return create_histogram(mat.data + channel, mat.total(), mat.channels()); }
void plotHistogram(const cv::Mat& histogram, cv::Mat& canvas, const cv::Scalar& color) { Check("Histogram", histogram).notEmpty().hasDepth(CV_32F); Check("Canvas", canvas).notEmpty().hasType(CV_8UC3); const auto N = histogram.total(); const auto bar_width = canvas.cols / N; const auto bar_height = canvas.rows / histogram.channels(); assert(bar_width > 0); assert(bar_height > 0); double max = 0; cv::minMaxLoc(histogram, nullptr, &max, nullptr, nullptr); std::vector<cv::Mat> histogram_channels; cv::split(histogram, histogram_channels); const cv::Scalar* COLORS = histogram.channels() > 1 ? utils::colors::BGR : &color; for (int c = 0; c < histogram.channels(); ++c) { cv::Mat roi(canvas, cv::Rect(0, (histogram.channels() - 1 - c) * bar_height, N * bar_width, bar_height)); roi.setTo(255); for (unsigned int b = 0; b < N; b++) { auto value = histogram_channels[c].at<float>(b); auto height = std::round(value * bar_height / max); if (height > 0) cv::rectangle(roi, cv::Point(b * bar_width, bar_height - 1), cv::Point((b + 1) * bar_width - 1, bar_height - 1 - height), COLORS[c], -1); } } }
void SensorData::setUserData(const cv::Mat & userData) { if(!userData.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty())) { UWARN("Cannot write new user data (%d bytes) over existing user " "data (%d bytes, %d compressed). Set user data of %d to null " "before setting a new one.", int(userData.total()*userData.elemSize()), int(_userDataRaw.total()*_userDataRaw.elemSize()), _userDataCompressed.cols, this->id()); return; } _userDataRaw = cv::Mat(); _userDataCompressed = cv::Mat(); if(!userData.empty()) { if(userData.type() == CV_8UC1) // Bytes { _userDataCompressed = userData; // assume compressed } else { _userDataRaw = userData; _userDataCompressed = compressData2(userData); } } }
cv::Mat compressData2(const cv::Mat & data) { cv::Mat bytes; if(!data.empty()) { uLong sourceLen = uLong(data.total())*uLong(data.elemSize()); uLong destLen = compressBound(sourceLen); bytes = cv::Mat(1, destLen+3*sizeof(int), CV_8UC1); int errCode = compress( (Bytef *)bytes.data, &destLen, (const Bytef *)data.data, sourceLen); bytes = cv::Mat(bytes, cv::Rect(0,0, destLen+3*sizeof(int), 1)); *((int*)&bytes.data[destLen]) = data.rows; *((int*)&bytes.data[destLen+sizeof(int)]) = data.cols; *((int*)&bytes.data[destLen+2*sizeof(int)]) = data.type(); if(errCode == Z_MEM_ERROR) { UERROR("Z_MEM_ERROR : Insufficient memory."); } else if(errCode == Z_BUF_ERROR) { UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data."); } } return bytes; }
// // 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; }
std::vector<unsigned char> compressData(const cv::Mat & data) { std::vector<unsigned char> bytes; if(!data.empty()) { uLong sourceLen = uLong(data.total())*uLong(data.elemSize()); uLong destLen = compressBound(sourceLen); bytes.resize(destLen); int errCode = compress( (Bytef *)bytes.data(), &destLen, (const Bytef *)data.data, sourceLen); bytes.resize(destLen+3*sizeof(int)); *((int*)&bytes[destLen]) = data.rows; *((int*)&bytes[destLen+sizeof(int)]) = data.cols; *((int*)&bytes[destLen+2*sizeof(int)]) = data.type(); if(errCode == Z_MEM_ERROR) { UERROR("Z_MEM_ERROR : Insufficient memory."); } else if(errCode == Z_BUF_ERROR) { UERROR("Z_BUF_ERROR : The buffer dest was not large enough to hold the uncompressed data."); } } return bytes; }
camera_packet_t(cv::Mat mat):camera_packet_t(){ rows=mat.rows; cols=mat.cols; data=mat.data; size = (unsigned int) (mat.total() * mat.elemSize()); isMat = true; }
WarpCost::WarpCost(const Warper& warper, const cv::Mat& J, const cv::Mat& I, const cv::Mat& dIdx, const cv::Mat& dIdy, const cv::Mat& mask, int interpolation, bool check_condition, double max_condition) : warper_(&warper), J_(&J), I_(&I), dIdx_(&dIdx), dIdy_(&dIdy), mask_(&mask), interpolation_(interpolation), check_condition_(check_condition), max_condition_(max_condition) { // Check that we have a square patch. CHECK(J.rows == J.cols); // Check that mask is correct size. CHECK(mask.size() == J.size()); // Set the number of inputs (configure as a single block). mutable_parameter_block_sizes()->push_back(warper_->numParams()); // Set the number of outputs. set_num_residuals(J.total()); }
cv::Mat MedianFilter::process_frame(const cv::Mat &frame) { // Get grayscale image to do sorting with cv::Mat gray_frame; cv::cvtColor(frame, gray_frame, cv::COLOR_BGR2GRAY); // Initialize the filter if we haven't yet if ( ! is_init ) init_median_lists(frame.total()); assert( median_lists.size() == frame.total() ); // Init iterators cv::MatConstIterator_<cv::Vec3b> bgr_it = frame.begin<cv::Vec3b>(), bgr_end = frame.end <cv::Vec3b>(); cv::MatConstIterator_<uchar> gray_it = gray_frame.begin<uchar>(); auto med_it = median_lists.begin(); // Matrix to stuff the "current frame" into. cv::Mat dst(frame.rows, frame.cols, CV_8UC3); cv::MatIterator_<cv::Vec3b> dst_it = dst.begin<cv::Vec3b>(); // Iterate through all pixels in each image together for ( ; bgr_it != bgr_end; bgr_it++, gray_it++, dst_it++, med_it++) { Pixel pixel(*bgr_it, *gray_it); // NOTE: because quickselect partially sorts, we're not really // getting rid of the min/max values, but this should be fine if (pop_front) (*med_it)[0] = pixel; else (*med_it)[filter_length-1] = pixel; int med_idx = (*med_it).size()/2; std::nth_element((*med_it).begin(), (*med_it).begin()+med_idx, (*med_it).end()); *dst_it = (*med_it)[med_idx].color; } pop_front = ! pop_front; return dst; }
bool write_bgr_image(const cv::Mat& output_image, const std::string& output_filename) { std::cout << "Output Image: name=" << output_filename << ", width=" << output_image.cols << ", height=" << output_image.rows << ", channels=" << output_image.channels() << ", total=" << output_image.total() << std::endl; return cv::imwrite(output_filename, output_image); }
void CameraParameters::setParams(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) { if (cameraMatrix.rows!=3 || cameraMatrix.cols!=3) throw cv::Exception(9000,"invalid input cameraMatrix","CameraParameters::setParams",__FILE__,__LINE__); cameraMatrix.convertTo(CameraMatrix,CV_32FC1); if ( distorsionCoeff.total()<4 || distorsionCoeff.total()>5 ) throw cv::Exception(9000,"invalid input distorsionCoeff","CameraParameters::setParams",__FILE__,__LINE__); cv::Mat auxD; distorsionCoeff.convertTo( auxD,CV_32FC1); //now, get only the 4 first elements Distorsion.create(1,4,CV_32FC1); for (int i=0;i<4;i++) Distorsion.ptr<float>(0)[i]=auxD.ptr<float>(0)[i]; CamSize=size; }
void printImageData(const cv::Mat& input, std::string description){ std::cout << description << std::endl; std::cout << "matrix type: " << input.type() << std::endl; std::cout << "matrix depth: " << input.depth() << std::endl; std::cout << "channels: " << input.channels() << std::endl; std::cout << "size: " << input.size() << std::endl; std::cout << "total elem: " << input.total() << std::endl; std::cout << std::endl; }
void CameraParameters::setParams(cv::Mat cameraMatrix,cv::Mat distorsionCoeff,cv::Size size) { if (cameraMatrix.rows!=3 || cameraMatrix.cols!=3) return; cameraMatrix.convertTo(CameraMatrix,CV_32FC1); if ( distorsionCoeff.total()<4 || distorsionCoeff.total()>=7 ) return; cv::Mat auxD; distorsionCoeff.convertTo( Distorsion,CV_32FC1); // Distorsion.create(1,4,CV_32FC1); // for (int i=0;i<4;i++) // Distorsion.ptr<float>(0)[i]=auxD.ptr<float>(0)[i]; CamSize=size; }
cv::Mat plotHistogram(const cv::Mat& histogram, unsigned int bar_width, unsigned int height, const cv::Scalar& color) { assert(bar_width > 0); assert(height > 0); cv::Mat canvas(height * histogram.channels(), bar_width * histogram.total(), CV_8UC3); canvas.setTo(0); plotHistogram(histogram, canvas, color); return canvas; }
cv::Mat createMultiChannelMask(const cv::Mat& img, const cv::Mat& mask) { cv::Mat out(img.size(), CV_8UC4, cv::Scalar::all(0)); for (int i = 0; i < img.total(); i++) { for (int j = 0; j < 3; j++) out.ptr<cv::Vec4b>()[i][j] = img.ptr<cv::Vec3b>()[i][j]; if (mask.size() == img.size()) out.ptr<cv::Vec4b>()[i][3] = mask.ptr<unsigned char>()[i]; } return out; }
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); } }
//! Write cv::Mat to binary file void writeMatBinary(std::ofstream& ofs, const cv::Mat& out_mat) { if(!ofs.is_open()){ throw new orArgException("not opened"); } int type = out_mat.type(); ofs.write((const char*)(&out_mat.rows), sizeof(int)); ofs.write((const char*)(&out_mat.cols), sizeof(int)); ofs.write((const char*)(&type), sizeof(int)); ofs.write((const char*)(out_mat.data), out_mat.elemSize() * out_mat.total()); }
bool Core_EigenTest::check_pairs_order(const cv::Mat& eigen_values) { switch (eigen_values.type()) { case CV_32FC1: { for (int i = 0; i < (int)(eigen_values.total() - 1); ++i) if (!(eigen_values.at<float>(i, 0) > eigen_values.at<float>(i+1, 0))) { std::cout << endl; std::cout << "Checking order of eigen values vector " << eigen_values << "..." << endl; std::cout << "Pair of indexes with non ascending of eigen values: (" << i << ", " << i+1 << ")." << endl; std::cout << endl; CV_Error(CORE_EIGEN_ERROR_ORDER, MESSAGE_ERROR_ORDER); return false; } break; } case CV_64FC1: { for (int i = 0; i < (int)(eigen_values.total() - 1); ++i) if (!(eigen_values.at<double>(i, 0) > eigen_values.at<double>(i+1, 0))) { std::cout << endl; std::cout << "Checking order of eigen values vector " << eigen_values << "..." << endl; std::cout << "Pair of indexes with non ascending of eigen values: (" << i << ", " << i+1 << ")." << endl; std::cout << endl; CV_Error(CORE_EIGEN_ERROR_ORDER, "Eigen values are not sorted in ascending order."); return false; } break; } default:; } return true; }
bool PointSelection::getDebugIndex(const size_t& level, cv::Mat& dbg_idx) { if(debug_ && storage_.size() > level) { dbg_idx = storage_[level].debug_idx; return dbg_idx.total() > 0; } else { return false; } }
void cv::CircleDetector::Context::debug_buffer(const cv::Mat& image, cv::Mat& out) { out.create(height, width, CV_8UC3); cv::Vec3b* out_ptr = out.ptr<cv::Vec3b>(0); const cv::Vec3b* im_ptr = image.ptr<cv::Vec3b>(0); out = cv::Scalar(128,128,128); for (uint i = 0; i < out.total(); i++, ++out_ptr, ++im_ptr) { /*if (buffer[i] == -1) *ptr = cv::Vec3b(0,0,0); else if (buffer[i] == -2) *ptr = cv::Vec3b(255,255,255);*/ //else if (buffer[i] < 0) *ptr = cv::Vec3b(0, 255, 0); if (buffer[i] > 0) *out_ptr = cv::Vec3b(255, 0, 255); else *out_ptr = *im_ptr; } }
//! Read cv::Mat from binary void readMatBinary(std::ifstream& ifs, cv::Mat& in_mat) { if(!ifs.is_open()){ throw new orArgException("Not opened"); } int rows, cols, type; ifs.read((char*)(&rows), sizeof(int)); ifs.read((char*)(&cols), sizeof(int)); ifs.read((char*)(&type), sizeof(int)); in_mat.release(); in_mat.create(rows, cols, type); ifs.read((char*)(in_mat.data), in_mat.elemSize() * in_mat.total()); }
void hulo::readMatBin(std::string filename, cv::Mat& mat) { std::ifstream ifs(filename, std::ios::binary); CV_Assert(ifs.is_open()); int rows, cols, type; ifs.read((char*) (&rows), sizeof(int)); if (rows == 0) { return; } ifs.read((char*) (&cols), sizeof(int)); ifs.read((char*) (&type), sizeof(int)); mat.release(); mat.create(rows, cols, type); ifs.read((char*) (mat.data), mat.elemSize() * mat.total()); }
void SensorData::setUserDataRaw(const cv::Mat & userDataRaw) { if(!userDataRaw.empty() && (!_userDataCompressed.empty() || !_userDataRaw.empty())) { UWARN("Cannot write new user data (%d bytes) over existing user " "data (%d bytes, %d compressed). Set user data of %d to null " "before setting a new one.", int(userDataRaw.total()*userDataRaw.elemSize()), int(_userDataRaw.total()*_userDataRaw.elemSize()), _userDataCompressed.cols, this->id()); return; } _userDataRaw = userDataRaw; _userDataCompressed = cv::Mat(); }