表面法向量surface normal(源碼)
這篇文章介紹 如何從Depth（點雲）數據獲得surface normal的過程。本文提供C++代碼。
在meshlab上顯示結果：
void Frame::EstimateSurfaceNormalGradient( const cv::Mat &imDepth, const double &timeStamp,cv::Mat &K)
{
//ofstream surfaceNormal;
//surfaceNormal.open("surfaceN.txt", ios::out | ios::trunc );
//PointCloud::Ptr tmp( new PointCloud() );
vector<SurfaceNormal> surfaceNormalVector;
int cellsize=10;
cv::Mat mPointCLoud=cv::Mat::zeros(imDepth.rows,imDepth.cols,CV_32FC3);//x,y,z
for ( int m=0; m<imDepth.rows; m++ )
{
for ( int n=0; n<imDepth.cols; n++ )
{
float d = imDepth.ptr<float>(m)[n];
if(d<0.001){ continue;}
mPointCLoud.at<cv::Vec3f>(m,n)[2]=d;
//cout<<n<<","<<m<<","<<mZPointCLoud.at<float>(n,m)<<","<<imDepth.at<float>(n,m)<<endl;
mPointCLoud.at<cv::Vec3f>(m,n)[0]= (n - K.at<float>(0,2))*d / K.at<float>(0,0);
//cout<<mXPointCLoud.at<float>(n,m)<<","<<(n - K.at<float>(0,2)) / K.at<float>(0,0)<<endl;
mPointCLoud.at<cv::Vec3f>(m,n)[1]= (m - K.at<float>(1,2))*d / K.at<float>(1,1);
}
}
//cout<<"finish"<<endl;
//讀取中心點 和周圍的四個點
//tmp->width = imDepth.cols; tmp->height = imDepth.rows;tmp->is_dense = false;
//創建Mat
cv::Mat mTangeMask=cv::Mat::zeros(imDepth.rows,imDepth.cols,CV_8U);
cv::Mat tangeMaskIntegralTemp;cv:: Mat mUTangeMapIntegralTemp;Mat mVTangeMapIntegralTemp;
cv::Mat tangeMaskIntegral;cv:: Mat mUTangeMapIntegral;Mat mVTangeMapIntegral;
cv::Mat mUTangeMap=cv::Mat::zeros(imDepth.rows,imDepth.cols,CV_32FC3);
/*cv::Mat mUYTangeMap=cv::Mat::zeros(imDepth.rows-10,imDepth.cols-10,CV_32F);
cv::Mat mUZTangeMap=cv::Mat::zeros(imDepth.rows-10,imDepth.cols-10,CV_32F);*/
cv::Mat mVTangeMap=cv::Mat::zeros(imDepth.rows,imDepth.cols,CV_32FC3);
/*cv::Mat mVYTangeMap=cv::Mat::zeros(imDepth.rows-10,imDepth.cols-10,CV_32F);
cv::Mat mVZTangeMap=cv::Mat::zeros(imDepth.rows-10,imDepth.cols-10,CV_32F);*/
for ( int m=1; m<imDepth.rows-1; m++ )
{
for ( int n=1; n<imDepth.cols-1; n++ )
{
//float d = imDepth.ptr<float>(m)[n];
//if (d < 0.01 || d>10)
// continue;
//pcl::PointXYZRGB center;
auto p_center=cv::Point3f(mPointCLoud.at<cv::Vec3f>(m, n)[0], mPointCLoud.at<cv::Vec3f>(m, n)[1], mPointCLoud.at<cv::Vec3f>(m, n)[2]);
//cout<<n<<","<<m<<","<< p_center.x<<","<<p_center.y<<p_center.z<<endl;
auto xNext=cv::Point3f(mPointCLoud.at<cv::Vec3f>(m,n+1)[0],mPointCLoud.at<cv::Vec3f>(m,n+1)[1],mPointCLoud.at<cv::Vec3f>(m,n+1)[2]);
//cout<<n<<",-"<<m<<","<< xNext.x<<","<<xNext.y<<endl;
auto xPrev=cv::Point3f(mPointCLoud.at<cv::Vec3f>(m,n-1)[0],mPointCLoud.at<cv::Vec3f>(m,n-1)[1],mPointCLoud.at<cv::Vec3f>(m,n-1)[2]);
//cout<<n<<",--"<<m<<","<< xPrev.x<<","<<xPrev.y<<","<<xPrev.z<<endl;
auto yNext=cv::Point3f(mPointCLoud.at<cv::Vec3f>(m+1,n)[0],mPointCLoud.at<cv::Vec3f>(m+1,n)[1],mPointCLoud.at<cv::Vec3f>(m+1,n)[2]);
//cout<<n<<",---"<<m<<","<< yNext.x<<","<<yNext.y<<","<<yNext.z<<endl;
auto yPrev=cv::Point3f(mPointCLoud.at<cv::Vec3f>(m-1,n)[0],mPointCLoud.at<cv::Vec3f>(m-1,n)[1],mPointCLoud.at<cv::Vec3f>(m-1,n)[2]);
//cout<<n<<",---"<<m<<","<< yPrev.x<<","<<yPrev.y<<","<<yPrev.z<<endl;
//根據深度，獲取有效點
//cout<<"center point"<<endl;
if(0.001<p_center.z&&p_center.z<8&&0.001<xNext.z&&xNext.z<8&&0.001<xPrev.z&&xPrev.z<8&&0.001<yPrev.z&&yPrev.z<8&&0.001<yNext.z&&yNext.z<8)
{
mTangeMask.at<int>(m,n)=1;//1表示五個數字都有效
mUTangeMap.at<cv::Vec3f>(m,n)[0]=xNext.x-xPrev.x;
mUTangeMap.at<cv::Vec3f>(m,n)[1]=xNext.y-xPrev.y;
mUTangeMap.at<cv::Vec3f>(m,n)[2]=xNext.z-xPrev.z;
mVTangeMap.at<cv::Vec3f>(m,n)[0]=yNext.x-yPrev.x;
mVTangeMap.at<cv::Vec3f>(m,n)[1]=yNext.y-yPrev.y;
mVTangeMap.at<cv::Vec3f>(m,n)[2]=yNext.z-yPrev.z;
//cout<<mVTangeMap.at<cv::Vec3f>(m,n)[0]<<","<<mVTangeMap.at<cv::Vec3f>(m,n)[1]<<","<<mVTangeMap.at<cv::Vec3f>(m,n)[2]<<endl;
//cout<<yNext.x<<","<<yNext.y<<","<<yNext.z<<endl;
//cout<<yPrev.x<<","<<yPrev.y<<","<<yPrev.z<<endl;
}
}
}
//cout<<"tange"<<mTangeMask<<endl;
//求intergral
integral(mTangeMask,tangeMaskIntegralTemp);
//cout<<"tangeIntegra"<<tangeMaskIntegralTemp<<endl;
//cout<<","<<mTangeMask.cols<<","<<mTangeMask.rows<<","<<tangeMaskIntegralTemp.cols<<","<<tangeMaskIntegralTemp.rows<<endl;
integral(mUTangeMap,mUTangeMapIntegralTemp,CV_32FC3);
integral(mVTangeMap,mVTangeMapIntegralTemp,CV_32FC3);
//去掉積分圖的padding
tangeMaskIntegralTemp(cv::Range(1,imDepth.rows-3), cv::Range(1,imDepth.cols-3)).copyTo(tangeMaskIntegral);
mUTangeMapIntegralTemp(cv::Range(1,imDepth.rows-3), cv::Range(1,imDepth.cols-3)).copyTo(mUTangeMapIntegral);
mVTangeMapIntegralTemp(cv::Range(1,imDepth.rows-3), cv::Range(1,imDepth.cols-3)).copyTo(mVTangeMapIntegral);
//cout<<"tange"<<mVTangeMapIntegral<<endl;
//
int cellCount=0;
for ( int v=cellsize+1; v<imDepth.rows-3;v++ )
{
for ( int u=cellsize+1; u<imDepth.cols-3;u++ )
{
if(mTangeMask.at<int>(v,u)==1)
{
int numPts = tangeMaskIntegral.at<int>(v,u) - tangeMaskIntegral.at<int>(v-cellsize,u) - tangeMaskIntegral.at<int>(v,u-cellsize) + tangeMaskIntegral.at<int>(v-cellsize,u-cellsize);
if(numPts==0) continue;
cv::Point3f uVector =cv::Point3f( mUTangeMapIntegral.at<cv::Vec3f>(v,u)[0] - mUTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u)[0] - mUTangeMapIntegral.at<cv::Vec3f>(v,u-cellsize)[0] + mUTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u-cellsize)[0],
mUTangeMapIntegral.at<cv::Vec3f>(v,u)[1] - mUTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u)[1] - mUTangeMapIntegral.at<cv::Vec3f>(v,u-cellsize)[1] + mUTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u-cellsize)[1],
mUTangeMapIntegral.at<cv::Vec3f>(v,u)[2] - mUTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u)[2] - mUTangeMapIntegral.at<cv::Vec3f>(v,u-cellsize)[2] + mUTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u-cellsize)[2]) / numPts;
//surfaceNormal<<"u"<<mUTangeMapIntegral.at<cv::Vec3f>(v,u)[0]<<","<<mUTangeMapIntegral.at<cv::Vec3f>(v,u)[1]<<" "<<mUTangeMapIntegral.at<cv::Vec3f>(v,u)[2]<<endl;
cv::Point3f vVector = cv::Point3f(mVTangeMapIntegral.at<cv::Vec3f>(v,u)[0] - mVTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u)[0] - mVTangeMapIntegral.at<cv::Vec3f>(v,u-cellsize)[0] + mVTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u-cellsize)[0],
mVTangeMapIntegral.at<cv::Vec3f>(v,u)[1] - mVTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u)[1] - mVTangeMapIntegral.at<cv::Vec3f>(v,u-cellsize)[1] + mVTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u-cellsize)[1],
mVTangeMapIntegral.at<cv::Vec3f>(v,u)[2] - mVTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u)[2] - mVTangeMapIntegral.at<cv::Vec3f>(v,u-cellsize)[2] + mVTangeMapIntegral.at<cv::Vec3f>(v-cellsize,u-cellsize)[2])/ numPts;
//surfaceNormal<<"v"<<mVTangeMapIntegral.at<cv::Vec3f>(v,u)[0]<<","<<mVTangeMapIntegral.at<cv::Vec3f>(v,u)[1]<<" "<<mVTangeMapIntegral.at<cv::Vec3f>(v,u)[2]<<endl;
cellCount = cellCount + 1;
cv::Point3f normalVector;
normalVector.x = vVector.y * uVector.z - vVector.z * uVector.y;
normalVector.y = vVector.z * uVector.x - vVector.x * uVector.z;
normalVector.z = vVector.x * uVector.y - vVector.y * uVector.x;
cv::Point3f normalVectort=normalVector/norm(normalVector);
/*normalVector.x=isnan(normalVector.x);//normalVector.x
normalVector.y=isnan(normalVector.y);
normalVector.z=isnan(normalVector.z);*/
//surfaceNormal<<mPointCLoud.at<cv::Vec3f>(v, u)[0]<<" "<<mPointCLoud.at<cv::Vec3f>(v, u)[1]<<" "<<mPointCLoud.at<cv::Vec3f>(v, u)[2]<<" "<<normalVectort.x<<" "<<normalVectort.y<<" "<<normalVectort.z<<endl;
//surfaceNormalVector.push_back(normalVector/norm(normalVector));
surfacenomal.normal=normalVectort;
surfacenomal.cameraPosition=cv::Point3f(mPointCLoud.at<cv::Vec3f>(v, u)[0], mPointCLoud.at<cv::Vec3f>(v, u)[1], mPointCLoud.at<cv::Vec3f>(v,u)[2]);
surfacenomal.FramePosition=cv::Point2i(u,v);
surfaceNormalVector.push_back(surfacenomal);
}
}
}
vSurfaceNormal=surfaceNormalVector;
推薦閱讀：