void getBigInterval(vector<int> inputArray, int * outputBoundary1, int * outputBoundary2, double percentage)
{
int maxVal;
int maxInd;
vector<int> left;
vector<int> right;
vectorGetMax(inputArray, &maxVal, &maxInd);
vector<double> sumLeft(&inputArray[0], &inputArray[maxInd]);
vector<double> sumRight(&inputArray[maxInd], &inputArray[inputArray.size()]);
left = ourFind(vectorTo01(sumLeft, percentage*maxVal), 1, FIND_LAST);
if (left.empty())
{
*outputBoundary1 = 0;
}
else
{
*outputBoundary1 = left[0];
}
right = ourFind(vectorTo01(sumRight, percentage*maxVal), 1, FIND_FIRST);
if (right.empty())
{
right.push_back(sumRight.size() - 1);
}
*outputBoundary2 = right[0] + maxInd;
}
//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;
}
