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; }