void PlaneSegmenter::computeNormals()
{
int step_sz = STEP_SIZE;
int line_step_sz = LINE_STEP_SIZE;
n_points.clear();
for ( unsigned int l= step_sz; l<depth_img.rows - step_sz; l+=step_sz )
{
for( unsigned int j= step_sz; j<depth_img.cols - step_sz; j+=step_sz )
{
cv::Point3f w_p = img2world(cv::Point2f(j,l));
//get 3 points
cv::Point3f w_p1 = img2world(cv::Point2f(j,l+line_step_sz/2));
cv::Point3f w_p2 = img2world(cv::Point2f(j+line_step_sz/2, l-line_step_sz/2));
cv::Point3f w_p3 = img2world(cv::Point2f(j-line_step_sz/2, l-line_step_sz/2));
//compute norm
cv::Point3f norm = normalize( crossProduct(w_p2 - w_p1, w_p3 - w_p1) );
if( sqrt(norm.x*norm.x + norm.y*norm.y + norm.z*norm.z) != 0
&& !isnan(norm.x) && !isnan(norm.y) && !isnan(norm.z) )
n_points.push_back( PlaneSegmenter::Point( n_points.size(), w_p, norm) );
}
}
}
bool setSunAngleEx(std::vector<XgyLine> lineArray)
{
if(lineArray.size()==0)
return false;
XgyPlane P1; //确定方位角
P1.setPlane(0,0,1,0);
cv::Point3f startP,endP; //方位角指示线的起点和终点
if(!img2world(lineArray[0].x2,P1,startP))
return false;
if(!img2world(lineArray[0].x1,P1,endP))
return false;
XgyVector3D v1;
v1=setVector3D(startP,endP);
normalizeVector3D(v1);
double beijian=PI/180*90;
if(v1.y>=0)
{sunAzimuth=acos(v1.x);
}
else
{sunAzimuth=-1.0*acos(v1.x);
}
if(lineArray.size()==1)
return false;
else
{
XgyPlane P2;
getPlanefromLine(lineArray[0], P2);
cv::Point3f endZP; //高度角指示线终点
if(!img2world(lineArray[1].x2,P2,endZP))
return false;
XgyVector3D v2;
v2=setVector3D(startP,endZP);
normalizeVector3D(v2);
if(v2.z<0)
return false;
else
{
sunZenith=acos(v2.z);//sunZenith为太阳方向与z轴的夹角
sunAltitude=beijian- sunZenith;//sunAltitude为太阳方向与y轴的夹角
}
}
return true;
}
bool getPlanefromLine(XgyLine l, XgyPlane &pl)
{
XgyPlane floor;
floor.setPlane(0,0,1,0);
//计算线段两端点的3维位置
cv::Point3f wP1;
if(!(img2world(l.x1,floor,wP1)))
{
//cout<<"Can not create plane!"<<endl;
return false;
}
cv::Point3f wP2;
if(!(img2world(l.x2,floor,wP2)))
{
//cout<<"Can not create plane!"<<endl;
return false;
}
XgyVector3D tempNormal; //平面法向垂直于直线wP1wP2，且平行于z=0
tempNormal.x=wP1.y-wP2.y;
tempNormal.y=wP2.x-wP1.x;
tempNormal.z=0;
normalizeVector3D(tempNormal);
XgyVector3D view;
getcamPose(camPos);
view=setVector3D(camPos,wP1);
normalizeVector3D(view);
XgyVector3D normal;
if(dotProduct3D(tempNormal,view)>0) //若法向与视线同向
scalarMul(-1.0,tempNormal,normal); //反向
else
scalarMul(1.0,tempNormal,normal);
double d=-1*(normal.x*wP1.x+normal.y*wP1.y+normal.z*wP1.z);
pl.setPlane(normal.x,normal.y,normal.z,d);
return true;
}
