Skip to content

Commit

Permalink
finally optimizing
Browse files Browse the repository at this point in the history
  • Loading branch information
@udaysankar01
udaysankar01 committed Aug 28, 2024
1 parent dc153e9 commit 8744b8f
Show file tree
Hide file tree
Showing 9 changed files with 149 additions and 91 deletions.
14 changes: 11 additions & 3 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,8 @@ LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
set_property(GLOBAL PROPERTY USE_FOLDERS ON)

# for libtorch
set(CMAKE_PREFIX_PATH "${PROJECT_SOURCE_DIR}/thirdparty/pytorch/torch")
# set(CMAKE_PREFIX_PATH "${PROJECT_SOURCE_DIR}/thirdparty/pytorch/torch")
set(CMAKE_PREFIX_PATH "/home/uday/projects/xfeat_cpp/thirdparty/pytorch/torch")
find_package(Torch REQUIRED)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${TORCH_CXX_FLAGS}")
message("-- Torch Version: " ${Torch_VERSION})
Expand Down Expand Up @@ -132,7 +133,8 @@ target_link_libraries(${PROJECT_NAME}
${Pangolin_LIBRARIES}
${TORCH_LIBRARIES}

${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o${CMAKE_SHARED_LIBRARY_SUFFIX}
# ${PROJECT_SOURCE_DIR}/thirdparty/g2o/lib/libg2o${CMAKE_SHARED_LIBRARY_SUFFIX}
/home/uday/projects/ORB_SLAM3/Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/thirdparty/DBoW2/lib/libDBoW2${CMAKE_SHARED_LIBRARY_SUFFIX}
-lboost_serialization
-lcrypto
Expand All @@ -156,4 +158,10 @@ add_executable(test_stereo_xf examples/test/test_stereo_xf.cc)
target_link_libraries(test_stereo_xf ${PROJECT_NAME})

add_executable(test_stereo_orb examples/test/test_stereo_orb.cc)
target_link_libraries(test_stereo_orb ${PROJECT_NAME})
target_link_libraries(test_stereo_orb ${PROJECT_NAME})

add_executable(test_matcher_xf examples/test/test_matcher_xf.cc)
target_link_libraries(test_matcher_xf ${PROJECT_NAME})

add_executable(test_matcher_orb examples/test/test_matcher_orb.cc)
target_link_libraries(test_matcher_orb ${PROJECT_NAME})
Binary file modified examples/RGB-D/rgbd_tum
View file
Binary file not shown.
Binary file modified examples/test/test_extractor
View file
Binary file not shown.
29 changes: 17 additions & 12 deletions examples/test/test_matcher_xf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,20 +12,21 @@ using namespace ORB_SLAM3;
// Function to compute Euclidean distance between two descriptors
float computeDescriptorDistance(const Mat& desc1, const Mat& desc2)
{
float dist = 0.0;
for (int i = 0; i < desc1.cols; i++)
{
float diff = desc1.at<float>(0, i) - desc2.at<float>(0, i);
dist += diff * diff;
}
return std::sqrt(dist);
// float dist = 0.0;
// for (int i = 0; i < desc1.cols; i++)
// {
// float diff = desc1.at<float>(0, i) - desc2.at<float>(0, i);
// dist += diff * diff;
// }
// return std::sqrt(dist);
return norm(desc1, desc2, NORM_L2SQR);
}

int main(int argc, char** argv)
{
if (argc != 3)
if (argc != 4)
{
cerr << "Usage: ./test_stereo path_to_left_image path_to_right_image" << endl;
cerr << "Usage: ./test_matcher_xf path_to_left_image path_to_right_image threshold" << endl;
return -1;
}

Expand All @@ -39,6 +40,9 @@ int main(int argc, char** argv)
return -1;
}

// Parse the threshold argument
float threshold = std::stof(argv[3]);

// Initialize XFextractor
int nFeatures = 1000;
float scaleFactor = 1.2f;
Expand Down Expand Up @@ -73,15 +77,15 @@ int main(int argc, char** argv)
// Compute distance between descriptors
float dist = computeDescriptorDistance(descriptorsLeft.row(i), descriptorsRight.row(j));

// Check if this is the best match so far
if (dist < bestMatchDistance)
// Check if this is the best match so far and below the threshold
if (dist < bestMatchDistance && dist < threshold)
{
bestMatchDistance = dist;
bestMatchIndex = j;
}
}

// Store the best match
// Store the best match if below the threshold
if (bestMatchIndex >= 0)
{
matches.push_back(DMatch(i, bestMatchIndex, bestMatchDistance));
Expand All @@ -102,6 +106,7 @@ int main(int argc, char** argv)
cout << "Descriptor Sizes:" << endl;
cout << "Left Descriptors: " << descriptorsLeft.rows << " x " << descriptorsLeft.cols << endl;
cout << "Right Descriptors: " << descriptorsRight.rows << " x " << descriptorsRight.cols << endl;
cout << "Number of Matches: " << matches.size() << endl;

return 0;
}
Binary file modified examples/test/test_stereo_orb
View file
Binary file not shown.
132 changes: 70 additions & 62 deletions examples/test/test_stereo_orb.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,30 @@ using namespace std;
using namespace cv;
using namespace ORB_SLAM3;

int DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
const int *pa = a.ptr<int32_t>();
const int *pb = b.ptr<int32_t>();

int dist=0;

for(int i=0; i<8; i++, pa++, pb++)
{
unsigned int v = *pa ^ *pb;
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
}

return dist;
}

int computeHammingDistance(const Mat& desc1, const Mat& desc2)
{
// return norm(desc1, desc2, NORM_HAMMING);
return DescriptorDistance(desc1, desc2);
}

int main(int argc, char** argv)
{
if (argc != 3)
Expand All @@ -32,83 +56,67 @@ int main(int argc, char** argv)
int iniThFAST = 20;
int minThFAST = 7;

ORBextractor ORBextractor(nFeatures, scaleFactor, nLevels, iniThFAST, minThFAST);

// Define vLapping area for stereo
std::vector<int> vLappingArea = {imLeft.cols / 4, 3 * imLeft.cols / 4};
ORBextractor orbExtractor(nFeatures, scaleFactor, nLevels, iniThFAST, minThFAST);

// Extract keypoints and descriptors for left image
vector<KeyPoint> keypointsLeft;
Mat descriptorsLeft;
int monoIndexLeft = ORBextractor(imLeft, Mat(), keypointsLeft, descriptorsLeft, vLappingArea);
vector<int> vLappingAreaLeft;
int monoIndexLeft = orbExtractor(imLeft, Mat(), keypointsLeft, descriptorsLeft, vLappingAreaLeft);

// Extract keypoints and descriptors for right image
vector<KeyPoint> keypointsRight;
Mat descriptorsRight;
int monoIndexRight = ORBextractor(imRight, Mat(), keypointsRight, descriptorsRight, vLappingArea);

// Filter keypoints to keep only those inside the overlapping area
auto filterKeypoints = [&](vector<KeyPoint>& keypoints, int monoIndex) {
vector<KeyPoint> filteredKeypoints;
for (size_t i = monoIndex; i < keypoints.size(); ++i) {
if (keypoints[i].pt.x >= vLappingArea[0] && keypoints[i].pt.x <= vLappingArea[1]) {
filteredKeypoints.push_back(keypoints[i]);
}
}
return filteredKeypoints;
};

vector<KeyPoint> filteredKeypointsLeft = filterKeypoints(keypointsLeft, monoIndexLeft);
vector<KeyPoint> filteredKeypointsRight = filterKeypoints(keypointsRight, monoIndexRight);

// Draw keypoints for left and right images within the overlapping area
Mat imLeftKeypoints, imRightKeypoints;
drawKeypoints(imLeft, filteredKeypointsLeft, imLeftKeypoints, Scalar::all(-1), DrawMatchesFlags::DEFAULT);
drawKeypoints(imRight, filteredKeypointsRight, imRightKeypoints, Scalar::all(-1), DrawMatchesFlags::DEFAULT);

// Create a single image with left and right images side by side
Mat imCombined(max(imLeftKeypoints.rows, imRightKeypoints.rows), imLeftKeypoints.cols + imRightKeypoints.cols, CV_8UC3, Scalar::all(0));
Mat left(imCombined, Rect(0, 0, imLeftKeypoints.cols, imLeftKeypoints.rows));
Mat right(imCombined, Rect(imLeftKeypoints.cols, 0, imRightKeypoints.cols, imRightKeypoints.rows));
imLeftKeypoints.copyTo(left);
imRightKeypoints.copyTo(right);

// Display the combined image
imshow("Stereo Keypoints", imCombined);
waitKey(0);
vector<int> vLappingAreaRight;
int monoIndexRight = orbExtractor(imRight, Mat(), keypointsRight, descriptorsRight, vLappingAreaRight);

// Print keypoints and descriptor information
cout << "Left Image Keypoints: " << keypointsLeft.size() << endl;
cout << "Right Image Keypoints: " << keypointsRight.size() << endl;
cout << "Left Image Keypoints in Overlapping Area: " << filteredKeypointsLeft.size() << endl;
cout << "Right Image Keypoints in Overlapping Area: " << filteredKeypointsRight.size() << endl;

cout << "Descriptor Sizes:" << endl;
cout << "Left Descriptors: " << descriptorsLeft.rows << " x " << descriptorsLeft.cols << endl;
cout << "Right Descriptors: " << descriptorsRight.rows << " x " << descriptorsRight.cols << endl;

// Print sections of descriptor values
if (!descriptorsLeft.empty() && !descriptorsRight.empty())
// Brute force matching with ratio test
vector<DMatch> matches;
for (int i = monoIndexLeft; i < descriptorsLeft.rows; i++)
{
cout << "Sample Descriptors (Left Image):" << endl;
for (int i = 0; i < min(5, descriptorsLeft.rows); ++i) {
cout << "Descriptor " << i + 1 << ": ";
for (int j = 0; j < min(5, descriptorsLeft.cols); ++j) {
cout << descriptorsLeft.at<float>(i, j) << " ";
int bestMatchIndex = -1;
int secondBestMatchIndex = -1;
int bestMatchDistance = std::numeric_limits<int>::max();
int secondBestMatchDistance = std::numeric_limits<int>::max();

for (int j = monoIndexRight; j < descriptorsRight.rows; j++)
{
// Compute Hamming distance between descriptors
int dist = computeHammingDistance(descriptorsLeft.row(i), descriptorsRight.row(j));

if (dist < bestMatchDistance)
{
secondBestMatchDistance = bestMatchDistance;
secondBestMatchIndex = bestMatchIndex;
bestMatchDistance = dist;
bestMatchIndex = j;
}
else if (dist < secondBestMatchDistance)
{
secondBestMatchDistance = dist;
secondBestMatchIndex = j;
}
cout << "..." << endl;
}

cout << "Sample Descriptors (Right Image):" << endl;
for (int i = 0; i < min(5, descriptorsRight.rows); ++i) {
cout << "Descriptor " << i + 1 << ": ";
for (int j = 0; j < min(5, descriptorsRight.cols); ++j) {
cout << descriptorsRight.at<float>(i, j) << " ";
}
cout << "..." << endl;
// Apply Lowe's ratio test to ensure the best match is sufficiently better than the second-best
if (bestMatchDistance < 0.75 * secondBestMatchDistance)
{
matches.push_back(DMatch(i, bestMatchIndex, static_cast<float>(bestMatchDistance)));
}
}

// Draw matches
Mat imMatches;
drawMatches(imLeft, keypointsLeft, imRight, keypointsRight, matches, imMatches);

// Display the matches
imshow("Stereo Keypoints Matches", imMatches);
waitKey(0);

// Print keypoints and descriptor information
cout << "Left Image Keypoints: " << keypointsLeft.size() << endl;
cout << "Right Image Keypoints: " << keypointsRight.size() << endl;
cout << "Number of Matches: " << matches.size() << endl;

return 0;
}

Binary file modified examples/test/test_stereo_xf
View file
Binary file not shown.
56 changes: 45 additions & 11 deletions src/ORBmatcher.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2241,22 +2241,56 @@ namespace ORB_SLAM3

// Bit set count operation from
// http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
// int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
// {
// const int *pa = a.ptr<int32_t>();
// const int *pb = b.ptr<int32_t>();

// int dist=0;

// for(int i=0; i<8; i++, pa++, pb++)
// {
// unsigned int v = *pa ^ *pb;
// v = v - ((v >> 1) & 0x55555555);
// v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
// dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
// }

// return dist;
// }
int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
const int *pa = a.ptr<int32_t>();
const int *pb = b.ptr<int32_t>();
{
if (std::getenv("USE_ORB") == nullptr)
{
// Calculate the squared L2 distance
float normDist = cv::norm(a, b, cv::NORM_L2SQR);

int dist=0;
// Normalize the L2 distance to be comparable to Hamming distance
// Assuming descriptors are normalized and considering a typical scale factor
int scaledDist = static_cast<int>(normDist / a.cols * 256);

for(int i=0; i<8; i++, pa++, pb++)
{
unsigned int v = *pa ^ *pb;
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
cout << "a: " << a.size() << endl;
cout << "Distance: " << scaledDist << std::endl;

return scaledDist;
}
else
{
const int *pa = a.ptr<int32_t>();
const int *pb = b.ptr<int32_t>();

int dist=0;

for(int i=0; i<8; i++, pa++, pb++)
{
unsigned int v = *pa ^ *pb;
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
}

return dist;
return dist;
}
}

} //namespace ORB_SLAM
9 changes: 6 additions & 3 deletions src/Tracking.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2748,7 +2748,8 @@ bool Tracking::TrackReferenceKeyFrame()
int nmatches;
if (std::getenv("USE_ORB") == nullptr)
{
nmatches = matcher.SearchByNN(mpReferenceKF, mCurrentFrame, vpMapPointMatches);
// nmatches = matcher.SearchByNN(mpReferenceKF, mCurrentFrame, vpMapPointMatches);
nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
}
else
{
Expand Down Expand Up @@ -3439,7 +3440,8 @@ void Tracking::SearchLocalPoints()
int matches;
if (std::getenv("USE_ORB") == nullptr)
{
matches = matcher.SearchByNN(mCurrentFrame,mvpLocalMapPoints);
// matches = matcher.SearchByNN(mCurrentFrame,mvpLocalMapPoints);
matches = matcher.SearchByProjection(mCurrentFrame, mvpLocalMapPoints, th, mpLocalMapper->mbFarPoints, mpLocalMapper->mThFarPoints);
}
else
{
Expand Down Expand Up @@ -3682,7 +3684,8 @@ bool Tracking::Relocalization()
int nmatches;
if (std::getenv("USE_ORB") == nullptr)
{
nmatches = matcher.SearchByNN(pKF,mCurrentFrame,vvpMapPointMatches[i]);
// nmatches = matcher.SearchByNN(pKF,mCurrentFrame,vvpMapPointMatches[i]);
nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
}
else
{
Expand Down

0 comments on commit 8744b8f

Please sign in to comment.