-
-
Save anonymous/6cc8fbd9e7c1e685f2382b13a73f1509 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// The SLAM algorithm | |
// Step 1 | |
void slam() | |
{ | |
// Covariances | |
double sigma_x = pow(0.0001, 2.0); | |
double sigma_y = pow(0.0001, 2.0); | |
double sigma_theta = pow(0.00001, 2.0); | |
double sigma_laser_range = pow(0.0056, 2.0); | |
double sigma_laser_angle = pow(0.004, 2.0); | |
// Step 2 | |
int N = world->corners.size(); | |
std::vector<Corner> estimatedCorners; | |
estimatedCorners.clear(); | |
std::vector<cv::Mat> allH; | |
allH.clear(); | |
std::vector<cv::Mat> allK; | |
allK.clear(); | |
std::vector<cv::Mat> allLandmarkErrors; | |
allLandmarkErrors.clear(); | |
// Step 3 nonexistent | |
// Step 4 | |
Position estimatedPosition; | |
estimatedPosition.x = world->position.x + odometryIncrement.x; | |
estimatedPosition.y = world->position.y + odometryIncrement.y; | |
estimatedPosition.theta = world->position.theta + odometryIncrement.theta; | |
// Step 5 | |
cv::Mat gradient = cv::Mat::eye(3 + 2*N, 3 + 2*N, cv::DataType<double>::type); | |
gradient.at<double>(0, 2) = odometryIncrement.x / passedSeconds; | |
gradient.at<double>(1, 2) = odometryIncrement.y / passedSeconds; | |
// Step 6 | |
cv::Mat estimatedCovariance = cv::Mat::zeros(3 + 2*N, 3 + 2*N, cv::DataType<double>::type); | |
estimatedCovariance = gradient * sigma * gradient.t(); | |
estimatedCovariance.at<double>(0, 0) += sigma_x; | |
estimatedCovariance.at<double>(1, 1) += sigma_y; | |
estimatedCovariance.at<double>(2, 2) += sigma_theta; | |
// Step 7 | |
cv::Mat Q = cv::Mat::zeros(2, 2, cv::DataType<double>::type); | |
Q.at<double>(0, 0) = sigma_laser_range; | |
Q.at<double>(1, 1) = sigma_laser_angle; | |
for (Corner corner : world->corners) | |
{ | |
corner.isVisible = false; | |
} | |
//Step 8 | |
for (int i = 0; i < detectedEdges.size(); i++) | |
{ | |
if (detectedEdges[i].range > 0.3 && (detectedEdges[i].type == SharpCorner || detectedEdges[i].type == HollowCorner)) | |
{ | |
double relativeRange = detectedEdges[i].range; | |
double relativeAngle = detectedEdges[i].angle; | |
double relativeX = relativeRange * cos(relativeAngle); | |
double relativeY = relativeRange * sin(relativeAngle); | |
// Step 9 | |
double absoluteX = estimatedPosition.x + relativeX * cos(estimatedPosition.theta) - relativeY * sin(estimatedPosition.theta); | |
double absoluteY = estimatedPosition.y + relativeX * sin(estimatedPosition.theta) + relativeY * cos(estimatedPosition.theta); | |
cv::Mat z_i = cv::Mat::zeros(2, 1, cv::DataType<double>::type); | |
z_i.at<double>(0, 0) = relativeRange; | |
z_i.at<double>(1, 0) = relativeAngle; | |
std::vector<double> distances; | |
distances.clear(); | |
std::vector<cv::Mat> H_j; | |
H_j.clear(); | |
std::vector<cv::Mat> psi_j; | |
psi_j.clear(); | |
std::vector<cv::Mat> landmarkErrors; | |
landmarkErrors.clear(); | |
// Step 10 | |
for (int k = 0; k < world->corners.size(); k++) | |
{ | |
// Step 11 | |
Point delta = Point(world->corners[k].x - estimatedPosition.x, world->corners[k].y - estimatedPosition.y); | |
// Step 12 | |
double q = pow(delta.x, 2.0) + pow(delta.y, 2.0); | |
// Step 13 | |
cv::Mat z_k = cv::Mat::zeros(2, 1, cv::DataType<double>::type); | |
double deltaAngle = atan2(delta.y, delta.x) - estimatedPosition.theta; | |
if (deltaAngle < - PI) | |
{ | |
deltaAngle = 2 * PI + deltaAngle; | |
} | |
else if(deltaAngle > PI) | |
{ | |
deltaAngle = - 2 * PI + deltaAngle; | |
} | |
z_k.at<double>(0, 0) = sqrt(q); | |
z_k.at<double>(1, 0) = deltaAngle; | |
// Step 14 nonexistent | |
// Step 15 | |
cv::Mat H_k = cv::Mat::zeros(2, 3 + 2*N, cv::DataType<double>::type); | |
H_k.at<double>(0, 0) = - delta.x / sqrt(q); | |
H_k.at<double>(1, 0) = delta.y / q; | |
H_k.at<double>(0, 1) = - delta.y / sqrt(q); | |
H_k.at<double>(1, 1) = - delta.x / q; | |
H_k.at<double>(0, 2) = 0; | |
H_k.at<double>(1, 2) = - 1; | |
H_k.at<double>(0, 3 + 2 * world->corners[k].id) = delta.x / sqrt(q); | |
H_k.at<double>(1, 3 + 2 * world->corners[k].id) = - delta.y / q; | |
H_k.at<double>(0, 4 + 2 * world->corners[k].id) = delta.y / sqrt(q); | |
H_k.at<double>(1, 4 + 2 * world->corners[k].id) = delta.x / q; | |
H_j.emplace_back(H_k); | |
// Step 16 | |
cv::Mat psi_k = cv::Mat::zeros(2, 2, cv::DataType<double>::type); | |
psi_k = H_k * estimatedCovariance * H_k.t() + Q; | |
psi_j.emplace_back(psi_k); | |
// Step 17a : Calculate (z^i - z_hat^k); | |
cv::Mat landmarkError = cv::Mat::zeros(2, 1, cv::DataType<double>::type); | |
landmarkError = z_i - z_k; | |
landmarkErrors.emplace_back(landmarkError); | |
// Step 17b : Get distance from observed feature i to landmark k | |
cv::Mat tempMat; | |
tempMat = landmarkError.t() * psi_k.inv() * landmarkError; | |
distances.emplace_back(tempMat.at<double>(0, 0)); | |
// Step 18 | |
} | |
int cornerIndex = N; | |
// Step 19 | |
double minRangeError = 0.2; | |
double minAngleError = 0.2; | |
for (int k = 0; k < landmarkErrors.size(); k++) | |
{ | |
double rangeError = landmarkErrors[k].at<double>(0, 0); | |
double angleError = landmarkErrors[k].at<double>(1, 0); | |
if (fabs(rangeError) < minRangeError && fabs(angleError) < minAngleError) | |
{ | |
minRangeError = rangeError; | |
cornerIndex = angleError; | |
cornerIndex = k; | |
} | |
} | |
if (cornerIndex >= 0 && cornerIndex < N) | |
{ | |
world->corners[cornerIndex].isVisible = true; | |
// Step 21.5 : Extract variables for use in steps 22, 24 and 25 | |
allLandmarkErrors.emplace_back(landmarkErrors[cornerIndex]); | |
allH.emplace_back(H_j[cornerIndex]); | |
// Step 22 | |
allK.emplace_back(estimatedCovariance * H_j[cornerIndex].t() * psi_j[cornerIndex].inv()); | |
} | |
else | |
{ | |
std::cout << "Adding corner " << N << ":\t" << absoluteX << "\t\t" << absoluteY << std::endl; | |
world->corners.emplace_back(Corner(absoluteX, absoluteY, detectedEdges[i].type, N)); | |
world->corners[N].isVisible = true; | |
N++; | |
Point delta = Point(relativeX, relativeY); | |
double q = pow(relativeRange, 2.0); | |
// Step 15 | |
cv::Mat H = cv::Mat::zeros(2, 3 + 2*N, cv::DataType<double>::type); | |
H.at<double>(0, 0) = - delta.x / sqrt(q); | |
H.at<double>(1, 0) = delta.y / q; | |
H.at<double>(0, 1) = - delta.y / sqrt(q); | |
H.at<double>(1, 1) = - delta.x / q; | |
H.at<double>(0, 2) = 0; | |
H.at<double>(1, 2) = - 1; | |
H.at<double>(0, 3 + 2 * (N-1)) = delta.x / sqrt(q); | |
H.at<double>(1, 3 + 2 * (N-1)) = - delta.y / q; | |
H.at<double>(0, 4 + 2 * (N-1)) = delta.y / sqrt(q); | |
H.at<double>(1, 4 + 2 * (N-1)) = delta.x / q; | |
cv::Mat tempMat = cv::Mat::zeros(3 + 2*N, 3 + 2*N, cv::DataType<double>::type); | |
for (int it1 = 0; it1 < estimatedCovariance.rows; it1++) | |
{ | |
for (int it2 = 0; it2 < estimatedCovariance.cols; it2++) | |
{ | |
tempMat.at<double>(it1, it2) = estimatedCovariance.at<double>(it1, it2); | |
} | |
} | |
tempMat.at<double>(1+2*N, 1+2*N) = sigma_laser_range; | |
tempMat.at<double>(2+2*N, 2+2*N) = sigma_laser_angle; | |
tempMat.copyTo(estimatedCovariance); | |
} | |
} | |
// Step 23 | |
} | |
// Step 23.5 : Calculate mean and covariance corrections for use in step 24 and 25 | |
cv::Mat meanCorrection = cv::Mat::zeros(3 + 2*N, 1, cv::DataType<double>::type); | |
cv::Mat covarianceCorrection = cv::Mat::eye(3 + 2*N, 3 + 2*N, cv::DataType<double>::type); | |
for (int i = 0; i < allH.size(); i++) | |
{ | |
cv::Mat tempH = cv::Mat::zeros(2, 3 + 2*N, cv::DataType<double>::type); | |
cv::Mat tempK = cv::Mat::zeros(3 + 2*N, 2, cv::DataType<double>::type); | |
for (int it = 0; it < allH[i].cols; it++) | |
{ | |
tempH.at<double>(0, it) = allH[i].at<double>(0, it); | |
tempH.at<double>(1, it) = allH[i].at<double>(1, it); | |
tempK.at<double>(it, 0) = allK[i].at<double>(it, 0); | |
tempK.at<double>(it, 1) = allK[i].at<double>(it, 1); | |
} | |
meanCorrection += tempK * allLandmarkErrors[i]; | |
covarianceCorrection -= tempK * tempH; | |
} | |
// Step 24 part 1 : Update robot pose | |
world->position.x = estimatedPosition.x + meanCorrection.at<double>(0, 0); | |
world->position.y = estimatedPosition.y + meanCorrection.at<double>(1, 0); | |
world->position.theta = estimatedPosition.theta + meanCorrection.at<double>(2, 0); world->position = estimatedPosition; | |
// Step 24 part 2 : Update landmark positions | |
for (int i = 0; i < world->corners.size(); i++) | |
{ | |
world->corners[i].x += meanCorrection.at<double>(3 + (2 * world->corners[i].id), 0); | |
world->corners[i].y += meanCorrection.at<double>(4 + (2 * world->corners[i].id), 0); | |
} | |
// Step 25 | |
cv::Mat tempMat = covarianceCorrection * estimatedCovariance; | |
tempMat.copyTo(sigma); | |
// Step 26 | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment