Skip to content

Instantly share code, notes, and snippets.

Created Jun 16, 2016
Embed
What would you like to do?
// 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