Add implementation for computing costmap using PCA

RajPShinde · Nov 19, 2020 · 180a751 · 180a751
1 parent 192beef
commit 180a751
Show file tree

Hide file tree

Showing 3 changed files with 257 additions and 2 deletions.
diff --git a/include/data.hpp b/include/data.hpp
@@ -0,0 +1,19 @@
+#include <Eigen/Dense>
+#include <map>
+
+namespace costmap
+{
+
+struct TerrainData
+{
+    Eigen::Vector3d meanPosition;
+    Eigen::Vector3d centroidPosition;
+    Eigen::Vector3d normal;
+    double curvature;
+    Eigen::Matrix3d covarianceMatrix;
+    std::map<std::pair<double, double>, double> heightMap;
+    std::map<double, std::map<double, double>> costMap;
+    double minHeight;
+    double resolution;
+};
+}
diff --git a/include/footstepAffordance.hpp b/include/footstepAffordance.hpp
@@ -19,6 +19,14 @@ class FootstepAffordance
 
         ~FootstepAffordance();
 
+        unsigned int computeMeanAndCovariance(Eigen::Vector3d& meanPosition, Eigen::Matrix3d& covarianceMatrix, const std::vector<Eigen::Vector3d>& cloud);
+
+        void solvePlaneParameters(Eigen::Vector3d &normal, double &curvature, const Eigen::Matrix3d &covarianceMatrix);
+
+        void computeRoots(Eigen::Vector3d& roots, const Eigen::Matrix3d& m);
+
+        void computeRoots2(Eigen::Vector3d& roots, const Eigen::Matrix3d::Scalar& b, const Eigen::Matrix3d::Scalar& c);
+
         void regionOfInterest();
 
         void run(octomap::OcTree* octomap, 
@@ -33,9 +41,9 @@ class FootstepAffordance
 
         void addToCostmap(Eigen::Vector3d surfaceCellXYZ, double &totalCost, std::map<std::pair<double, double>, double> &cMap);
 
-
     private:    
 
+        const double distanceFromRobot = 0.3;
         double maxX = 2; 
         double minX = 1;
         double maxY = 0.15;
@@ -45,8 +53,9 @@ class FootstepAffordance
         TerrainData terrainParameters;
         int depth_ = 16;
         bool firstRun_ = false;
+        int neighbourMaxX, neighbourMaxY, neighbourMaxZ = 2;
+        int neighbourMinX, neighbourMinY, neighbourMinZ = -2;
         terrainFeature::Feature features;
-
 };
 
 }

diff --git a/src/footstepAffordance.cpp b/src/footstepAffordance.cpp
@@ -11,6 +11,168 @@ FootstepAffordance::~FootstepAffordance() {
 
 }
 
+unsigned int FootstepAffordance::computeMeanAndCovariance(Eigen::Vector3d& meanPosition, Eigen::Matrix3d& covarianceMatrix, const std::vector<Eigen::Vector3d>& cloud) {
+
+	Eigen::VectorXd accu = Eigen::VectorXd::Zero(9, 1);
+
+	for (size_t i = 0; i < cloud.size(); ++i) {
+		accu[0] += cloud[i](0) * cloud[i](0);  // summation x.x
+		accu[1] += cloud[i](0) * cloud[i](1);  // summation x.y
+		accu[2] += cloud[i](0) * cloud[i](2);  // summation x.z
+		accu[3] += cloud[i](1) * cloud[i](1);  // summation y.y
+		accu[4] += cloud[i](1) * cloud[i](2);  // summation y.z
+		accu[5] += cloud[i](2) * cloud[i](2);  // summation z.z
+		accu[6] += cloud[i](0);                // summation x
+		accu[7] += cloud[i](1);                // summation y
+		accu[8] += cloud[i](2);                // summation x
+	}
+
+	accu /= (cloud.size());    
+
+	// Meam and Covariance                
+	if (cloud.size() != 0) {
+		meanPosition[0] = accu[6];
+		meanPosition[1] = accu[7];
+		meanPosition[2] = accu[8];
+
+		covarianceMatrix.coeffRef(0) = accu[0] - accu[6] * accu[6];
+		covarianceMatrix.coeffRef(1) = accu[1] - accu[6] * accu[7];
+		covarianceMatrix.coeffRef(2) = accu[2] - accu[6] * accu[8];
+		covarianceMatrix.coeffRef(4) = accu[3] - accu[7] * accu[7];
+		covarianceMatrix.coeffRef(5) = accu[4] - accu[7] * accu[8];
+		covarianceMatrix.coeffRef(8) = accu[5] - accu[8] * accu[8];
+		covarianceMatrix.coeffRef(3) = covarianceMatrix.coeff(1);
+		covarianceMatrix.coeffRef(6) = covarianceMatrix.coeff(2);
+		covarianceMatrix.coeffRef(7) = covarianceMatrix.coeff(5);
+	}
+
+	return (static_cast<unsigned int> (cloud.size()));
+}
+
+void FootstepAffordance::solvePlaneParameters(Eigen::Vector3d &normal, double &curvature, const Eigen::Matrix3d &covarianceMatrix){
+
+	// Extract the smallest eigenvalue and its eigenvector
+	EIGEN_ALIGN16 Eigen::Vector3d::Scalar eigenvalue;
+	EIGEN_ALIGN16 Eigen::Vector3d eigenvector;
+
+	//typedef typename Eigen::Matrix3f::Scalar Scalar;
+	// Scale the matrix so its entries are in [-1,1]. The scaling is applied
+	// only when at least one matrix entry has magnitude larger than 1.
+	Eigen::Matrix3d::Scalar scale = covarianceMatrix.cwiseAbs().maxCoeff();
+	if (scale <= std::numeric_limits<Eigen::Matrix3d::Scalar>::min())
+		scale = Eigen::Matrix3d::Scalar(1.0);
+
+	Eigen::Matrix3d scaledMat = covarianceMatrix / scale;
+
+	Eigen::Vector3d eigenvalues;
+	computeRoots(eigenvalues, scaledMat);
+
+	eigenvalue = eigenvalues(0) * scale;
+
+	scaledMat.diagonal().array() -= eigenvalues(0);
+
+	Eigen::Vector3d vec1 = scaledMat.row(0).cross(scaledMat.row(1));
+	Eigen::Vector3d vec2 = scaledMat.row(0).cross(scaledMat.row(2));
+	Eigen::Vector3d vec3 = scaledMat.row(1).cross(scaledMat.row(2));
+
+	Eigen::Matrix3d::Scalar len1 = vec1.squaredNorm();
+	Eigen::Matrix3d::Scalar len2 = vec2.squaredNorm();
+	Eigen::Matrix3d::Scalar len3 = vec3.squaredNorm();
+
+	if (len1 >= len2 && len1 >= len3)
+		eigenvector = vec1 / std::sqrt(len1);
+	else if (len2 >= len1 && len2 >= len3)
+		eigenvector = vec2 / std::sqrt(len2);
+	else
+		eigenvector = vec3 / std::sqrt(len3);
+
+	// Check K_hat. The normal vectors should point to the +ve Z direction
+	if (eigenvector(2) < 0.)
+		eigenvector *= -1;
+
+	normal= eigenvector;
+}
+
+void FootstepAffordance::computeRoots(Eigen::Vector3d& roots, const Eigen::Matrix3d& m)
+{
+	// The characteristic equation is x^3 - c2*x^2 + c1*x - c0 = 0. The
+	// eigenvalues are the roots to this equation, all guaranteed to be
+	// real-valued, because the matrix is symmetric.
+	Eigen::Matrix3d::Scalar c0 = m(0,0) * m(1,1) * m(2,2) +
+			Eigen::Matrix3d::Scalar(2) * m(0,1) * m(0,2) * m(1,2)
+	- m(0,0) * m(1,2) * m(1,2) - m(1,1) * m(0,2) * m(0,2)
+	- m(2,2) * m(0,1) * m(0,1);
+
+	Eigen::Matrix3d::Scalar c1 = m(0,0) * m(1,1) - m(0,1) * m(0,1) + m(0,0) * m(2,2) -
+			m(0,2) * m(0,2) + m(1,1) * m(2,2) - m(1,2) * m(1,2);
+
+	Eigen::Matrix3d::Scalar c2 = m(0,0) + m(1,1) + m(2,2);
+
+
+	if (fabs (c0) < Eigen::NumTraits<Eigen::Matrix3d::Scalar>::epsilon ())// one root is 0 -> quadratic equation
+		computeRoots2(roots, c2, c1);
+	else
+	{
+		const Eigen::Matrix3d::Scalar s_inv3 = Eigen::Matrix3d::Scalar(1.0 / 3.0);
+		const Eigen::Matrix3d::Scalar s_sqrt3 = std::sqrt(Eigen::Matrix3d::Scalar (3.0));
+		// Construct the parameters used in classifying the roots of the equation
+		// and in solving the equation for the roots in closed form.
+		Eigen::Matrix3d::Scalar c2_over_3 = c2*s_inv3;
+		Eigen::Matrix3d::Scalar a_over_3 = (c1 - c2 * c2_over_3) * s_inv3;
+		if (a_over_3 > Eigen::Matrix3d::Scalar(0))
+			a_over_3 = Eigen::Matrix3d::Scalar (0);
+
+		Eigen::Matrix3d::Scalar half_b = Eigen::Matrix3d::Scalar(0.5) *
+				(c0 + c2_over_3 * (Eigen::Matrix3d::Scalar(2) * c2_over_3 * c2_over_3 - c1));
+
+		Eigen::Matrix3d::Scalar q = half_b * half_b + a_over_3 * a_over_3 * a_over_3;
+		if (q > Eigen::Matrix3d::Scalar(0))
+			q = Eigen::Matrix3d::Scalar(0);
+
+		// Compute the eigenvalues by solving for the roots of the polynomial.
+		Eigen::Matrix3d::Scalar rho = sqrt(-a_over_3);
+		Eigen::Matrix3d::Scalar theta = atan2(std::sqrt(-q), half_b) * s_inv3;
+		Eigen::Matrix3d::Scalar cos_theta = cos(theta);
+		Eigen::Matrix3d::Scalar sin_theta = sin(theta);
+		roots(0) = c2_over_3 + Eigen::Matrix3d::Scalar(2) * rho * cos_theta;
+		roots(1) = c2_over_3 - rho * (cos_theta + s_sqrt3 * sin_theta);
+		roots(2) = c2_over_3 - rho * (cos_theta - s_sqrt3 * sin_theta);
+
+		// Sort in increasing order.
+		double roots0 = roots(0);
+		double roots1 = roots(1);
+		double roots2 = roots(2);
+		if (roots(0) >= roots(1))
+			std::swap(roots0, roots1);
+		if (roots(1) >= roots(2)) {
+			std::swap(roots1, roots2);
+			if (roots(0) >= roots(1))
+				std::swap(roots0, roots1);
+		}
+		roots(0) = roots0;
+		roots(1) = roots1;
+		roots(2) = roots2;
+
+		if (roots(0) <= 0) // eigenvalue for symmetric positive semi-definite matrix can not be negative! Set it to 0
+			computeRoots2(roots, c2, c1);
+	}
+}
+
+void FootstepAffordance::computeRoots2(Eigen::Vector3d& roots,
+				   const Eigen::Matrix3d::Scalar& b,
+				   const Eigen::Matrix3d::Scalar& c)
+{
+	roots(0) = Eigen::Matrix3d::Scalar(0);
+	Eigen::Matrix3d::Scalar d = Eigen::Matrix3d::Scalar(b * b - 4.0 * c);
+	if (d < 0.0) // no real roots!!!! THIS SHOULD NOT HAPPEN!
+		d = 0.0;
+
+	Eigen::Matrix3d::Scalar sd = sqrt(d);
+
+	roots(2) = 0.5f * (b + sd);
+	roots(1) = 0.5f * (b - sd);
+}
+
 void FootstepAffordance::run(octomap::OcTree* octomap, 
 							const Eigen::Vector3d& robotStateXYZ, 
 							const Eigen::Vector3d& robotStateRPY, 
@@ -84,6 +246,71 @@ void FootstepAffordance::run(octomap::OcTree* octomap,
 void FootstepAffordance::calculateCost(octomap::OcTree* octomap, const octomap::OcTreeKey& surfaceCellKey, std::map<std::pair<double, double>, double> &cMap) {
     ROS_ERROR_STREAM("calculate Cost");
 	firstRun_ = true;
+	bool atLeastOneNeighbour = false;
+
+	std::vector<Eigen::Vector3d> allNeighboursPosition;
+	octomap::OcTreeNode* surfaceCellNode = octomap->search(surfaceCellKey, depth_);
+
+	Eigen::Vector3d surfaceCellXYZ;
+	octomap::point3d surfaceCellPoint = octomap->keyToCoord(surfaceCellKey, depth_);
+	surfaceCellXYZ(0) = surfaceCellPoint(0);  // x
+	surfaceCellXYZ(1) = surfaceCellPoint(1);  // y
+	surfaceCellXYZ(2) = surfaceCellPoint(2);  // z
+	allNeighboursPosition.push_back(surfaceCellXYZ);
+
+	octomap::OcTreeKey neighbourKey;
+	octomap::OcTreeNode* neighbourNode = surfaceCellNode;
+	ROS_ERROR_STREAM("Neighbours");
+	for(int z = neighbourMinZ; z<=neighbourMaxZ; z++) {
+		for(int y = neighbourMinY; y<=neighbourMaxY; y++) {
+			for(int x = neighbourMinX; x<=neighbourMaxX; x++) {
+				neighbourKey[0] = surfaceCellKey[0] + x;
+				neighbourKey[1] = surfaceCellKey[1] + y;
+				neighbourKey[2] = surfaceCellKey[2] + z;
+				neighbourNode = octomap->search(neighbourKey, depth_);
+				ROS_ERROR_STREAM(z);
+				// Check if this neighbour exists
+				if(neighbourNode) {
+					ROS_ERROR_STREAM("Found1");
+					if(octomap->isNodeOccupied(neighbourNode)) {
+						ROS_ERROR_STREAM("Found2");
+						Eigen::Vector3d neighbourCellXYZ;
+						octomap::point3d neighbourCellPoint;
+						neighbourCellPoint = octomap->keyToCoord(neighbourKey, depth_);
+						neighbourCellXYZ(0) = neighbourCellPoint(0); 
+						neighbourCellXYZ(1) = neighbourCellPoint(1);
+						neighbourCellXYZ(2) = neighbourCellPoint(2);
+						allNeighboursPosition.push_back(neighbourCellXYZ);
+						atLeastOneNeighbour = true;
+					}
+				}
+			}
+		}
+	}
+
+    ROS_ERROR_STREAM("Found Neighbours");
+	if(atLeastOneNeighbour) {
+		//compute cost
+		EIGEN_ALIGN16 Eigen::Matrix3d covarianceMatrix;
+		if(allNeighboursPosition.size()<3) {
+			computeMeanAndCovariance(terrainParameters.meanPosition, covarianceMatrix, allNeighboursPosition);
+		}
+		else
+			return;
+		terrainParameters.centroidPosition(0) = allNeighboursPosition[0](0);
+		terrainParameters.centroidPosition(0) = allNeighboursPosition[0](1);
+		terrainParameters.centroidPosition(0) = allNeighboursPosition[0](2);
+
+		solvePlaneParameters(terrainParameters.normal, terrainParameters.curvature, covarianceMatrix);
+	}
+
+	ROS_ERROR_STREAM("Cost");
+	double cost, weight, totalCost = 0;
+	// for(auto i: features) {
+		features.computeSlopeCost(cost, terrainParameters);
+		totalCost = 1*cost;
+	// }
+
 	// put totalCost in costmap
 	addToCostmap(surfaceCellXYZ, totalCost, cMap);