10000 Add functionality for yaw correction with template matching · geromidg/GA_SLAM@b6d6fc3 · GitHub
[go: up one dir, main page]
Skip to content

Commit b6d6fc3

Browse files
geromidggeromidg
committed
Add functionality for yaw correction with template matching
1 parent 64e5b82 commit b6d6fc3

File tree

6 files changed

+83
-16
lines changed

6 10000 files changed

+83
-16
lines changed

‎ga_slam/GaSlam.cc

+2
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,7 @@ void GaSlam::configure(
5353
double predictSigmaX, double predictSigmaY, double predictSigmaYaw,
5454
double traversedDistanceThreshold, double minSlopeThreshold,
5555
double slopeSumThresholdMultiplier, double matchAcceptanceThreshold,
56+
double matchYawRange, double matchYawStep,
5657
double globalMapLength, double globalMapResolution) {
5758
voxelSize_ = voxelSize;
5859
depthSigmaCoeff1_ = depthSigmaCoeff1;
@@ -65,6 +66,7 @@ void GaSlam::configure(
6566

6667
poseCorrection_.configure(traversedDistanceThreshold, minSlopeThreshold,
6768
slopeSumThresholdMultiplier, matchAcceptanceThreshold,
69+
matchYawRange, matchYawStep,
6870
globalMapLength, globalMapResolution);
6971

7072
dataRegistration_.configure(mapLength, mapResolution, minElevation,

‎ga_slam/GaSlam.h

+5
Original file line numberDiff line numberDiff line change
@@ -117,6 +117,10 @@ class GaSlam {
117117
* @param[in] matchAcceptanceThreshold minimum score the matched position
118118
* from template matching must have, in order for the matching
119119
* to be accepted
120+
* @param[in] matchYawRange scan range of yaw angle in radians for the
121+
* template matching
122+
* @param[in] matchYawStep scan step of yaw angle in radians for the
123+
* template matching
120124
* @param[in] globalMapLength size of one dimension of the global map
121125
* @param[in] globalMapResolution resolution of the global map in meters
122126
*/
@@ -129,6 +133,7 @@ class GaSlam {
129133
double predictSigmaX, double predictSigmaY, double predictSigmaYaw,
130134
double traversedDistan 10000 ceThreshold, double minSlopeThreshold,
131135
double slopeSumThresholdMultiplier, double matchAcceptanceThreshold,
136+
double matchYawRange, double matchYawStep,
132137
double globalMapLength, double globalMapResolution);
133138

134139
/** Handles the input delta pose data from odometry. The delta pose is

‎ga_slam/localization/PoseCorrection.cc

+8-3
Original file line numberDiff line numberDiff line change
@@ -48,12 +48,16 @@ void PoseCorrection::configure(
4848
double minSlopeThreshold,
4949
double slopeSumThresholdMultiplier,
5050
double matchAcceptanceThreshold,
51+
double matchYawRange,
52+
double matchYawStep,
5153
double globalMapLength,
5254
double globalMapResolution) {
5355
traversedDistanceThreshold_ = traversedDistanceThreshold;
5456
minSlopeThreshold_ = minSlopeThreshold;
5557
slopeSumThresholdMultiplier_ = slopeSumThresholdMultiplier;
5658
matchAcceptanceThreshold_ = matchAcceptanceThreshold;
59+
matchYawRange_ = matchYawRange;
60+
matchYawStep_ = matchYawStep;
5761

5862
globalDataRegistration_.configure(globalMapLength, globalMapResolution);
5963
}
@@ -115,17 +119,18 @@ bool PoseCorrection::matchMaps(
115119
cv::resize(localImage, localImage, cv::Size(), resolutionRatio,
116120
resolutionRatio, cv::INTER_NEAREST);
117121

118-
cv::Point2d matchedPosition;
122+
cv::Point3d matchedPosition;
119123
const bool matchFound = ImageProcessing::findBestMatch(globalImage,
120-
localImage, matchedPosition, matchAcceptanceThreshold_);
124+
localImage, matchedPosition, matchAcceptanceThreshold_,
125+
matchYawRange_, matchYawStep_);
121126

122127
if (matchFound) {
123128
ImageProcessing::convertPositionToMapCoordinates(matchedPosition,
124129
globalImage, globalMapResolution);
125130

126131
const Eigen::Vector2d mapXY = globalMapPose_.translation().head(2);
127132
const Eigen::Vector2d currentXY = currentPose.translation().head(2);
128-
correctionDeltaPose = Eigen::Translation3d(
133+
const auto correctionDeltaPose = Eigen::Translation3d(
129134
mapXY.x() + matchedPosition.x - currentXY.x(),
130135
mapXY.y() + matchedPosition.y - currentXY.y(), 0.);
131136

‎ga_slam/localization/PoseCorrection.h

+12
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,10 @@ class PoseCorrection {
7373
* @param[in] matchAcceptanceThreshold the minimum score the matched
7474
* position from template matching must have, in order for the
7575
* matching to be accepted
76+
* @param[in] matchYawRange scan range of yaw angle in radians of the
77+
* template matching
78+
* @param[in] matchYawStep scan step of yaw angle in radians of the
79+
* template matching
7680
* @param[in] globalMapLength the size of one dimension of the global map
7781
* @param[in] globalMapResolution the resolution of the global map
7882
* in meters
@@ -82,6 +86,8 @@ class PoseCorrection {
8286
double minSlopeThreshold,
8387
double slopeSumThresholdMultiplier,
8488
double matchAcceptanceThreshold,
89+
double matchYawRange,
90+
double matchYawStep,
8591
double globalMapLength,
8692
double globalMapResolution);
8793

@@ -145,6 +151,12 @@ class PoseCorrection {
145151

146152
/// Minimum score in template matching a match must have to be accepted
147153
double matchAcceptanceThreshold_;
154+
155+
/// Scan range of yaw angle in radians for the template matching
156+
double matchYawRange_;
157+
158+
/// Scan step of yaw angle in radians for the template matching
159+
double matchYawStep_;
148160
};
149161

150162
} // namespace ga_slam

‎ga_slam/processing/ImageProcessing.cc

+35-9
Original file line numberDiff line numberDiff line change
@@ -121,11 +121,13 @@ void ImageProcessing::calculateLaplacianImage(
121121
bool ImageProcessing::findBestMatch(
122122
const Image& sourceImage,
123123
const Image& templateImage,
124-
cv::Point2d& matchedPosition,
124+
cv::Point3d& matchedPosition,
125125
double matchAcceptanceThreshold,
126+
double matchYawRange,
127+
double matchYawStep,
126128
bool matchImageGradients,
127129
bool displayMatch) {
128-
Image sourceInput, templateInput, resultOutput;
130+
Image sourceInput, templateInput;
129131

130132
if (matchImageGradients) {
131133
calculateGradientImage(sourceImage, sourceInput);
@@ -138,19 +140,33 @@ bool ImageProcessing::findBestMatch(
138140
replaceNanWithZero(sourceInput);
139141
replaceNanWithZero(templateInput);
140142

141-
const auto method = CV_TM_CCORR_NORMED;
142-
cv::matchTemplate(sourceInput, templateInput, resultOutput, method);
143+
constexpr int method = CV_TM_CCORR_NORMED;
143144

145+
Image warpedTemplate, resultMatrix, bestResultMatrix;
146+
cv::Point3d bestPosition;
144147
cv::Point2i maxPosition;
145148
double maxValue;
146-
cv::minMaxLoc(resultOutput, nullptr, &maxValue, nullptr, &maxPosition);
149+
double bestValue = 0.;
150+
151+
for (double yaw = - matchYawRange / 2.; yaw <= matchYawRange / 2;
152+
yaw += matchYawStep) {
153+
warpImage(templateInput, warpedTemplate, yaw);
154+
155+
cv::matchTemplate(sourceInput, warpedTemplate, resultMatrix, method);
156+
cv::minMaxLoc(resultMatrix, nullptr, &maxValue, nullptr, &maxPosition);
157+
158+
if (maxValue > bestValue) {
159+
bestPosition = cv::Point3d(maxPosition.x, maxPosition.y, yaw);
160+
bestResultMatrix = resultMatrix;
161+
}
162+
}
147163

148164
const bool matchFound = maxValue > matchAcceptanceThreshold;
149-
if (matchFound) matchedPosition = cv::Point2d(maxPosition.x, maxPosition.y);
165+
if (matchFound) matchedPosition = bestPosition;
150166

151167
if (displayMatch && matchFound)
152-
displayMatchedPosition(sourceInput, templateInput, resultOutput,
153-
matchedPosition);
168+
displayMatchedPosition(sourceInput, warpedTemplate, bestResultMatrix,
169+
cv::Point2d(matchedPosition.x, matchedPosition.y));
154170

155171
return matchFound;
156172
}
@@ -179,7 +195,7 @@ void ImageProcessing::displayMatchedPosition(
179195
}
180196

181197
void ImageProcessing::convertPositionToMapCoordinates(
182-
cv::Point2d& imagePosition,
198+
cv::Point3d& imagePosition,
183199
const Image& image,
184200
double mapResolution) {
185201
double mapPositionX = std::round(image.rows / 2.) - imagePosition.y;
@@ -193,5 +209,15 @@ void ImageProcessing::replaceNanWithZero(Image& image) {
193209
image.setTo(0., image != image);
194210
}
195211

212+
void ImageProcessing::warpImage(
213+
const Image& inputImage,
214+
Image& outputImage,
215+
double angle) {
216+
const cv::Point2f center(inputImage.cols / 2.f, inputImage.rows / 2.f);
217+
Image rotationMatrix = cv::getRotationMatrix2D(center, angle, 1.);
218+
219+
cv::warpAffine(inputImage, outputImage, rotationMatrix, inputImage.size());
220+
}
221+
196222
} // namespace ga_slam
197223

‎ga_slam/processing/ImageProcessing.h

+21-4
Original file line numberDiff line numberDiff line change
@@ -102,21 +102,28 @@ class ImageProcessing {
102102

103103
/** Find the best match given an source and a template image using
104104
* template matching. The matching can be done using the images as they are
105-
* or using their gradients
105+
* or using their gradients. The template image is rotated to maximize
106+
* the match score and find a correction in yaw
106107
* @param[in] sourceImage the source image (search space)
107108
* @param[in] templateImage the image to be matched
108-
* @param[out] matchedPosition the position of the match
109+
* @param[out] matchedPosition the position and orientation of the match
109110
* @param[in] matchAcceptanceThreshold the minimum score the matched
110111
* position must have, in order for the matching to be accepted
112+
* @param[in] matchYawRange scan range of yaw angle in radians for the
113+
* template matching
114+
* @param[in] matchYawStep scan step of yaw angle in radians for the
115+
* template matching
111116
* @param[in] matchImageGradients whether to match the images' gradients
112117
* @param[in] displayImage whether to display the found match
113118
* @return true if a match was found
114119
*/
115120
static bool findBestMatch(
116121
const Image& sourceImage,
117122
const Image& templateImage,
118-
cv::Point2d& matchedPosition,
123+
cv::Point3d& matchedPosition,
119124
double matchAcceptanceThreshold,
125+
double matchYawRange,
126+
double matchYawStep,
120127
bool matchImageGradients = true,
121128
bool displayMatch = true);
122129

@@ -143,14 +150,24 @@ class ImageProcessing {
143150
* @param[in] mapResolution the resolution of the map
144151
*/
145152
static void convertPositionToMapCoordinates(
146-
cv::Point2d& imagePosition,
153+
cv::Point3d& imagePosition,
147154
const Image& image,
148155
double mapResolution);
149156

150157
/** Replaces the NaN values of an image with 0
151158
* @param[in] image the image to be processed
152159
*/
153160
static void replaceNanWithZero(Image& image);
161+
162+
/** Rotates an image around its center
163+
* @param[in] inputImage the image source
164+
* @param[out] outputImage the image destination
165+
* @param[in] angle the yaw value of rotation
166+
*/
167+
static void warpImage(
168+
const Image& inputImage,
169+
Image& outputImage,
170+
double angle);
154171
};
155172

156173
} // namespace ga_slam

0 commit comments

Comments
 (0)
0