TEST(Core_IOArray, submat_create)
{
Mat1f A = Mat1f::zeros(2,2);
EXPECT_THROW( OutputArray_create1(A.row(0)), cv::Exception );
EXPECT_THROW( OutputArray_create2(A.row(0)), cv::Exception );
}
void AdaptiveManifoldFilterN::computeEigenVector(const vector<Mat>& X, const Mat1b& mask, Mat1f& vecDst, int num_pca_iterations, const Mat1f& vecRand)
{
int cnNum = (int)X.size();
int height = X[0].rows;
int width = X[0].cols;
vecDst.create(1, cnNum);
CV_Assert(vecRand.size() == Size(cnNum, 1) && vecDst.size() == Size(cnNum, 1));
CV_Assert(mask.rows == height && mask.cols == width);
const float *pVecRand = vecRand.ptr<float>();
Mat1d vecDstd(1, cnNum, 0.0);
double *pVecDst = vecDstd.ptr<double>();
Mat1f Xw(height, width);
for (int iter = 0; iter < num_pca_iterations; iter++)
{
for (int i = 0; i < height; i++)
{
const uchar *maskRow = mask.ptr<uchar>(i);
float *mulRow = Xw.ptr<float>(i);
//first multiplication
for (int cn = 0; cn < cnNum; cn++)
{
const float *srcRow = X[cn].ptr<float>(i);
const float cnVal = pVecRand[cn];
if (cn == 0)
{
for (int j = 0; j < width; j++)
mulRow[j] = cnVal*srcRow[j];
}
else
{
for (int j = 0; j < width; j++)
mulRow[j] += cnVal*srcRow[j];
}
}
for (int j = 0; j < width; j++)
if (!maskRow[j]) mulRow[j] = 0.0f;
//second multiplication
for (int cn = 0; cn < cnNum; cn++)
{
float curCnSum = 0.0f;
const float *srcRow = X[cn].ptr<float>(i);
for (int j = 0; j < width; j++)
curCnSum += mulRow[j]*srcRow[j];
//TODO: parallel reduce
pVecDst[cn] += curCnSum;
}
}
}
divide(vecDstd, norm(vecDstd), vecDst);
}
Mat DeWAFF::filter(const Mat& A, const Mat& Laplacian, int w, double sigma_d, int sigma_r){
//Pre-compute Gaussian domain weights.
Mat1i X,Y;
Tools::meshgrid(Range(-w,w),Range(-w,w),X,Y);
pow(X,2,X);
pow(Y,2,Y);
Mat1f G = X+Y;
G.convertTo(G,CV_32F,-1/(2*pow(sigma_d,2)));
exp(G,G);
//Apply bilateral filter.
Mat B = Mat(A.size(),A.type());
Mat F, H, I, L, dL, da, db;
int iMin, iMax, jMin, jMax;
vector<Mat> channels(3);
Vec3f pixel;
double norm_F;
#pragma omp target //OpenMP 4 pragma. Supported in GCC 5
#pragma omp parallel for\
private(I,iMin,iMax,jMin,jMax,pixel,channels,dL,da,db,H,F,norm_F,L)\
shared(A,B,G,Laplacian,w,sigma_d,sigma_r)
for(int i = 0; i < A.rows; i++){
for(int j = 0; j < A.cols; j++){
//Extract local region.
iMin = max(i - w,0);
iMax = min(i + w,A.rows-1);
jMin = max(j - w,0);
jMax = min(j + w,A.cols-1);
//Compute Gaussian range weights in the three channels
I = A(Range(iMin,iMax), Range(jMin,jMax));
split(I,channels);
pixel = A.at<Vec3f>(i,j);
pow(channels[0] - pixel.val[0], 2, dL);
pow(channels[1] - pixel.val[1], 2, da);
pow(channels[2] - pixel.val[2], 2, db);
exp((dL + da + db) / (-2 * pow(sigma_r,2)),H);
//Calculate bilateral filter response.
F = H.mul(G(Range(iMin-i+w, iMax-i+w), Range(jMin-j+w, jMax-j+w)));
norm_F = sum(F).val[0];
//The laplacian deceive consists on weighting the gaussian function with
//the original image, and using the image values of the laplacian image.
L = Laplacian(Range(iMin,iMax), Range(jMin,jMax));
split(L,channels);
B.at<Vec3f>(i,j)[0] = (sum(sum(F.mul(channels[0])))/norm_F).val[0];
B.at<Vec3f>(i,j)[1] = (sum(sum(F.mul(channels[1])))/norm_F).val[0];
B.at<Vec3f>(i,j)[2] = (sum(sum(F.mul(channels[2])))/norm_F).val[0];
}
}
return B;
}
void computeEigenVector(const Mat1f& X, const Mat1b& mask, Mat1f& dst, int num_pca_iterations, const Mat1f& rand_vec)
{
CV_DbgAssert( X.cols == rand_vec.cols );
CV_DbgAssert( X.rows == mask.size().area() );
CV_DbgAssert( rand_vec.rows == 1 );
dst.create(rand_vec.size());
rand_vec.copyTo(dst);
Mat1f t(X.size());
float* dst_row = dst[0];
for (int i = 0; i < num_pca_iterations; ++i)
{
t.setTo(Scalar::all(0));
for (int y = 0, ind = 0; y < mask.rows; ++y)
{
const uchar* mask_row = mask[y];
for (int x = 0; x < mask.cols; ++x, ++ind)
{
if (mask_row[x])
{
const float* X_row = X[ind];
float* t_row = t[ind];
float dots = 0.0;
for (int c = 0; c < X.cols; ++c)
dots += dst_row[c] * X_row[c];
for (int c = 0; c < X.cols; ++c)
t_row[c] = dots * X_row[c];
}
}
}
dst.setTo(0.0);
for (int k = 0; k < X.rows; ++k)
{
const float* t_row = t[k];
for (int c = 0; c < X.cols; ++c)
{
dst_row[c] += t_row[c];
}
}
}
double n = norm(dst);
divide(dst, n, dst);
}
void AdaptiveManifoldFilterN::computeOrientation(const vector<Mat>& X, const Mat1f& vec, Mat1f& dst)
{
int height = X[0].rows;
int width = X[0].cols;
int cnNum = (int)X.size();
dst.create(height, width);
CV_DbgAssert(vec.rows == 1 && vec.cols == cnNum);
const float *pVec = vec.ptr<float>();
for (int i = 0; i < height; i++)
{
float *dstRow = dst.ptr<float>(i);
for (int cn = 0; cn < cnNum; cn++)
{
const float *srcRow = X[cn].ptr<float>(i);
const float cnVal = pVec[cn];
if (cn == 0)
{
for (int j = 0; j < width; j++)
dstRow[j] = cnVal*srcRow[j];
}
else
{
for (int j = 0; j < width; j++)
dstRow[j] += cnVal*srcRow[j];
}
}
}
}
void applyMask(Mat1f& mat, const Mat1b& mask){
Mat1f old = mat.clone();
mat = Mat1f(1, countNonZero(mask));
unsigned mat_counter = 0;
for (int i = 0; i < mask.cols; ++i){
if (mask.at<char>(i) != 0){
mat.at<float>(mat_counter) = old.at<float>(i);
++mat_counter;
}
}
}
Mat DeWAFF::filter(const Mat& A, const Mat& Laplacian, int w, double sigma_d, int sigma_r){
//Pre-compute Gaussian domain weights.
Mat1i X,Y;
Tools::meshgrid(Range(-w,w),Range(-w,w),X,Y);
pow(X,2,X);
pow(Y,2,Y);
Mat1f G = X+Y;
G.convertTo(G,CV_32F,-1/(2*pow(sigma_d,2)));
exp(G,G);
//Apply bilateral filter.
Mat B = Mat(A.size(),A.type());
Mat F, H, I, L, dL, da, db;
int iMin, iMax, jMin, jMax;
vector<Mat> channels(3);
Vec3f pixel;
double norm_F;
//#pragma omp target
//#pragma omp parallel for\
private(I,iMin,iMax,jMin,jMax,pixel,channels,dL,da,db,H,F,norm_F,L)\
void AdaptiveManifoldFilterN::h_filter(const Mat1f& src, Mat& dst, float sigma)
{
CV_DbgAssert(src.depth() == CV_32F);
const float a = exp(-sqrt(2.0f) / sigma);
dst.create(src.size(), CV_32FC1);
for (int y = 0; y < src.rows; ++y)
{
const float* src_row = src[y];
float* dst_row = dst.ptr<float>(y);
dst_row[0] = src_row[0];
for (int x = 1; x < src.cols; ++x)
{
dst_row[x] = src_row[x] + a * (dst_row[x - 1] - src_row[x]);
}
for (int x = src.cols - 2; x >= 0; --x)
{
dst_row[x] = dst_row[x] + a * (dst_row[x + 1] - dst_row[x]);
}
}
for (int y = 1; y < src.rows; ++y)
{
float* dst_cur_row = dst.ptr<float>(y);
float* dst_prev_row = dst.ptr<float>(y-1);
rf_vert_row_pass(dst_cur_row, dst_prev_row, a, src.cols);
}
for (int y = src.rows - 2; y >= 0; --y)
{
float* dst_cur_row = dst.ptr<float>(y);
float* dst_prev_row = dst.ptr<float>(y+1);
rf_vert_row_pass(dst_cur_row, dst_prev_row, a, src.cols);
}
}
void AdaptiveManifoldFilterN::computeClusters(Mat1b& cluster, Mat1b& cluster_minus, Mat1b& cluster_plus)
{
Mat difEtaSrc;
{
vector<Mat> eta_difCn(jointCnNum);
for (int i = 0; i < jointCnNum; i++)
subtract(jointCn[i], etaFull[i], eta_difCn[i]);
merge(eta_difCn, difEtaSrc);
difEtaSrc = difEtaSrc.reshape(1, (int)difEtaSrc.total());
CV_DbgAssert(difEtaSrc.cols == jointCnNum);
}
Mat1f initVec(1, jointCnNum);
if (useRNG)
{
rnd.fill(initVec, RNG::UNIFORM, -0.5, 0.5);
}
else
{
for (int i = 0; i < (int)initVec.total(); i++)
initVec(0, i) = (i % 2 == 0) ? 0.5f : -0.5f;
}
Mat1f eigenVec(1, jointCnNum);
computeEigenVector(difEtaSrc, cluster, eigenVec, num_pca_iterations_, initVec);
Mat1f difOreientation;
gemm(difEtaSrc, eigenVec, 1, noArray(), 0, difOreientation, GEMM_2_T);
difOreientation = difOreientation.reshape(1, srcSize.height);
CV_DbgAssert(difOreientation.size() == srcSize);
compare(difOreientation, 0, cluster_minus, CMP_LT);
bitwise_and(cluster_minus, cluster, cluster_minus);
compare(difOreientation, 0, cluster_plus, CMP_GE);
bitwise_and(cluster_plus, cluster, cluster_plus);
}
void AdaptiveManifoldFilterN::computeClusters(Mat1b& cluster, Mat1b& cluster_minus, Mat1b& cluster_plus)
{
Mat1f difOreientation;
if (jointCnNum > 1)
{
Mat1f initVec(1, jointCnNum);
if (useRNG)
{
rnd.fill(initVec, RNG::UNIFORM, -0.5, 0.5);
}
else
{
for (int i = 0; i < (int)initVec.total(); i++)
initVec(0, i) = (i % 2 == 0) ? 0.5f : -0.5f;
}
vector<Mat> difEtaSrc(jointCnNum);
for (int i = 0; i < jointCnNum; i++)
subtract(jointCn[i], etaFull[i], difEtaSrc[i]);
Mat1f eigenVec(1, jointCnNum);
computeEigenVector(difEtaSrc, cluster, eigenVec, num_pca_iterations_, initVec);
computeOrientation(difEtaSrc, eigenVec, difOreientation);
CV_DbgAssert(difOreientation.size() == srcSize);
}
else
{
subtract(jointCn[0], etaFull[0], difOreientation);
}
compare(difOreientation, 0, cluster_minus, CMP_LT);
bitwise_and(cluster_minus, cluster, cluster_minus);
compare(difOreientation, 0, cluster_plus, CMP_GE);
bitwise_and(cluster_plus, cluster, cluster_plus);
}
TEST(Core_IOArray, submat_assignment)
{
Mat1f A = Mat1f::zeros(2,2);
Mat1f B = Mat1f::ones(1,3);
EXPECT_THROW( B.colRange(0,3).copyTo(A.row(0)), cv::Exception );
EXPECT_NO_THROW( B.colRange(0,2).copyTo(A.row(0)) );
EXPECT_EQ( 1.0f, A(0,0) );
EXPECT_EQ( 1.0f, A(0,1) );
}
void smoothDepth( const Mat1f &scale_map,
const Mat1d &ii_depth_map,
const Mat_<uint32_t>& ii_depth_count,
float base_scale,
Mat1f &depth_out )
{
float nan = std::numeric_limits<float>::quiet_NaN();
depth_out.create( ii_depth_map.rows-1, ii_depth_map.cols-1 );
for ( int y = 0; y < ii_depth_map.rows-1; y++ )
{
for ( int x = 0; x < ii_depth_map.cols-1; ++x )
{
float s = scale_map[y][x] * base_scale + 0.5f;
if ( isnan(s) )
{
depth_out(y,x) = nan;
continue;
}
if ( s < 5 ) s=5;
const int s_floor = s;
const float t = s - s_floor;
if ( !checkBounds( ii_depth_map, x, y, 1 ) )
{
depth_out(y,x) = nan;
continue;
}
depth_out(y,x) = (1.0-t) * meanDepth( ii_depth_map, ii_depth_count, x, y, s_floor )
+ t * meanDepth( ii_depth_map, ii_depth_count, x, y, s_floor+1 );
}
}
}
void callback(const CloudXYZ::ConstPtr& cld,
const sensor_msgs::ImageConstPtr& img,
const sensor_msgs::ImageConstPtr& img_seg)
{
namespace img_enc=sensor_msgs::image_encodings;
// Solve all of perception here...
mtx_.lock ();
{
cloud_.reset(new CloudXYZ(*cld)); ///< @todo avoid copy
try {
cv_bridge::CvImageConstPtr cv_img_ptr;
cv_img_ptr = cv_bridge::toCvShare(img, img_enc::BGR8);
ni::normalizeColors(cv_img_ptr->image, img_normalized_colors_);
//img_normalized_colors_.convertTo(img_normalized_colors_8bit_, CV_8UC3, 255.f);
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception for img message: %s", e.what());
return;
}
try {
cv_bridge::CvImageConstPtr cv_img_ptr;
cv_img_ptr = cv_bridge::toCvShare(img_seg, img_enc::MONO8);
cv_img_ptr->image.convertTo(img_seg_, CV_32FC1);
}
catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception for img_seg message: %s", e.what());
return;
}
// External inputs from messages extracted...
// Now to processing...
sig_.Clear();
sig_.Append(name_in_cld_, cloud_);
sig_.Append(name_in_img_, static_cast<Mat1f>(img_normalized_colors_));
sig_.Append(name_in_seg_, img_seg_);
for(size_t i=0; i<layers_.size(); i++) {
layers_[i]->Activate(sig_);
layers_[i]->Response(sig_);
}
//imshow("depth_map", ConvertTo8U(sig_.MostRecentMat1f("depth_map")));
imshow("map_gray_", ConvertTo8U(sig_.MostRecentMat1f("map_gray_")));
{
Mat1f tmp = sig_.MostRecentMat1f("map_gray_");
std::set<int> seg;
for(size_t i=0; i<tmp.total(); i++) {
seg.insert(static_cast<int>(tmp(i)));
}
VecI vtmp;
std::copy(seg.begin(), seg.end(), std::back_inserter(vtmp));
ELM_COUT_VAR(elm::to_string(vtmp));
}
imshow("img_seg_", ConvertTo8U(img_seg_));
//imshow("nes", ConvertTo8U(sig_.MostRecentMat1f("map_gray_")) != ConvertTo8U(img_seg_));
imshow("out", ConvertTo8U(sig_.MostRecentMat1f(name_out_)));
// imshow("net", ConvertTo8U(sig_.MostRecentMat1f(name_out_)) != ConvertTo8U(sig_.MostRecentMat1f("name_out_2")));
//imshow("out2", ConvertTo8U(sig_.MostRecentMat1f("name_out_2")));
Mat1f img = sig_.MostRecentMat1f(name_out_);
img(0) = 12.f;
Mat mask_not_assigned = img <= 0.f;
Mat img_color;
applyColorMap(ConvertTo8U(img),
img_color,
COLORMAP_HSV);
img_color.setTo(Scalar(0), mask_not_assigned);
imshow("img_color", img_color);
waitKey(0);
// convert in preparation to publish depth map image
sensor_msgs::ImagePtr img_msg = cv_bridge::CvImage(
std_msgs::Header(),
sensor_msgs::image_encodings::BGR8,
img_color).toImageMsg();
img_pub_.publish(img_msg);
}
mtx_.unlock (); // release mutex
}
