QPainterPath DiagonalWipeStrategy::clipPath( int step, const QRect &area )
{
qreal percent = static_cast<qreal>(step) / static_cast<qreal>(StepCount);
QPoint vecx( static_cast<int>(2.0 * area.width() * percent), 0 ) ;
QPoint vecy( 0, static_cast<int>( 2.0 * area.height() * percent ) );
QPainterPath path;
switch( subType() )
{
case DiagonalWipeEffectFactory::FromTopLeft:
path.moveTo( area.topLeft() );
path.lineTo( area.topLeft() + vecx );
path.lineTo( area.topLeft() + vecy );
break;
case DiagonalWipeEffectFactory::FromTopRight:
path.moveTo( area.topRight() );
path.lineTo( area.topRight() - vecx );
path.lineTo( area.topRight() + vecy );
break;
case DiagonalWipeEffectFactory::FromBottomLeft:
path.moveTo( area.bottomLeft() );
path.lineTo( area.bottomLeft() + vecx );
path.lineTo( area.bottomLeft() - vecy );
break;
case DiagonalWipeEffectFactory::FromBottomRight:
path.moveTo( area.bottomRight() );
path.lineTo( area.bottomRight() - vecx );
path.lineTo( area.bottomRight() - vecy );
break;
default:
return QPainterPath();
}
path.closeSubpath();
return path;
}
//Computes the ground plane
void GroundPlane::planar_fitting(CCloud &cpcl,
CPhotographs &photos,
float extend_boundary){
std::vector<cv::Point3f> plane_points;
int n_points = 0;
cv::Matx31f ABC, sumRight(0,0,0);
cv::Matx33f sumLeft(0,0,0,
0,0,0,
0,0,0);
//Planar surface approximation ---> output: (A, B, D) from Ax + By + D = 1z
float avgx = 0, avgy = 0, avgz = 0;
for(int i = 0; i < (int) photos.size(); i++){
cv::Matx34f P = photos[i].P;
cv::Matx33f R(P(0,0), P(0,1), P(0,2),
P(1,0), P(1,1), P(1,2),
P(2,0), P(2,1), P(2,2));
cv::Matx31f T(P(0,3),
P(1,3),
P(2,3));
cv::Matx31f C = (-R.t()) * T;
avgx += C(0,0);
avgy += C(1,0);
avgz += C(2,0);
}
avgx /= (int) photos.size();
avgy /= (int) photos.size();
avgz /= (int) photos.size();
for(int i = 0; i < (int) photos.size(); i++){
cv::Matx34f P = photos[i].P;
cv::Matx33f R(P(0,0), P(0,1), P(0,2),
P(1,0), P(1,1), P(1,2),
P(2,0), P(2,1), P(2,2));
cv::Matx31f T(P(0,3),
P(1,3),
P(2,3));
cv::Matx31f Cold = (-R.t()) * T;
cv::Matx31f C = cv::Matx31f(Cold(0,0)-avgx,
Cold(1,0)-avgy,
Cold(2,0)-avgz);
//plane_points.push_back(cv::Point3f(C(0,0),C(1,0), C(2,0)));
n_points++;
sumLeft(0,0) += pow(C(0,0),2.0);
sumLeft(0,1) += C(0,0) * C(2,0);
sumLeft(0,2) += C(0,0);
sumLeft(1,0) += C(0,0) * C(2,0);
sumLeft(1,1) += pow(C(2,0),2.0);
sumLeft(1,2) += C(2,0);
sumLeft(2,0) += C(0,0);
sumLeft(2,1) += C(2,0);
sumLeft(2,2) += 1.0;
sumRight(0,0) += C(0,0) * C(1,0);
sumRight(1,0) += C(2,0) * C(1,0);
sumRight(2,0) += C(1,0);
}
ABC = sumLeft.inv() * sumRight;
plane_normal = cv::Point3f(ABC(0,0),
-1.0,
ABC(1,0)
//(-1.0)
);
cv::Point3f vecx(1,0,0);
float angle = acos(vecx.dot(plane_normal));
if(angle < 0){
plane_normal.x = -plane_normal.x;
plane_normal.y = -plane_normal.y;
plane_normal.z = -plane_normal.z;
}
normalize_vector(plane_normal);
/*float length = sqrt(pow(plane_normal.x,2.0) + pow(plane_normal.y,2.0) + pow(plane_normal.z,2.0));
plane_normal.x /= length;
plane_normal.y /= length;
plane_normal.z /= length;
*/
//Point from plane: p.n = -D
plane_point = cv::Point3f(0.0,
ABC(2,0),
0.0
);
//Search Upper and lower bounds and center of cloud
cv::Point3f cloud_center(0,0,0);
cv::Point3f cloud_upper_bound(-9000,-9000,-9000), cloud_lower_bound(9000,9000,9000);
for(int i = 0; i < (int) cpcl.size(); i++){
cloud_center.x = cloud_center.x + cpcl[i].pos.x;
cloud_center.y = cloud_center.y + cpcl[i].pos.y;
cloud_center.z = cloud_center.z + cpcl[i].pos.z;
if(cpcl[i].pos.x < cloud_lower_bound.x)
cloud_lower_bound.x = cpcl[i].pos.x;
if(cpcl[i].pos.y < cloud_lower_bound.y)
cloud_lower_bound.y = cpcl[i].pos.y;
if(cpcl[i].pos.z < cloud_lower_bound.z)
cloud_lower_bound.z = cpcl[i].pos.z;
if(cpcl[i].pos.x > cloud_upper_bound.x)
cloud_upper_bound.x = cpcl[i].pos.x;
if(cpcl[i].pos.y > cloud_upper_bound.y)
cloud_upper_bound.y = cpcl[i].pos.y;
if(cpcl[i].pos.z > cloud_upper_bound.z)
cloud_upper_bound.z = cpcl[i].pos.z;
}
cloud_center.x /= cpcl.size();
cloud_center.y /= cpcl.size();
cloud_center.z /= cpcl.size();
//Streatch boundaries
cloud_lower_bound.x -= extend_boundary;
cloud_lower_bound.y -= extend_boundary;
cloud_lower_bound.z -= extend_boundary;
cloud_upper_bound.x += extend_boundary;
cloud_upper_bound.y += extend_boundary;
cloud_upper_bound.z += extend_boundary;
//Project both boudaries and center to the computed plane
cv::Point3f v = cloud_center - plane_point;
float distance = v.dot(plane_normal);
cloud_center = cloud_center - (distance * plane_normal);
v = cloud_lower_bound - plane_point;
distance = v.dot(plane_normal);
lower_bound = cloud_lower_bound - distance * plane_normal;
v = cloud_upper_bound - plane_point;
distance = v.dot(plane_normal);
higher_bound = cloud_upper_bound - distance * plane_normal;
}
