i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

SuperPoint: updated descriptor normalization process to match exactly the implementation in SuperGlue. generateWords3DMono: use variance at 25% instead of 50% of the sqrd variance vector (to do the same than with PnP), also fixed return camera transform to include local transform of the camera model. rtabmap-matcher: updated default parameters when no depth images are used, added reproj error in title bar.

Browse Source
This commit is contained in:
matlabbe
2020-05-10 14:13:41 -04:00
parent fbdc3780c7
commit cc9fd439fe
4 changed files with 35 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/RegistrationVis.cpp
View File

@@ -1351,7 +1351,7 @@ Transform RegistrationVis::computeTransformationImpl(
}
else
{
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
UINFO(msg.c_str());
}
}

23
corelib/src/superpoint_torch/SuperPoint.cc
View File

@@ -218,30 +218,35 @@ cv::Mat SPDetector::compute(const std::vector<cv::KeyPoint> &keypoints)
{
cv::Mat kpt_mat(keypoints.size(), 2, CV_32F); // [n_keypoints, 2] (y, x)
// Based on sample_descriptors() of SuperPoint implementation in SuperGlue:
// https://github.com/magicleap/SuperGluePretrainedNetwork/blob/45a750e5707696da49472f1cad35b0b203325417/models/superpoint.py#L80-L92
float s = 8;
for (size_t i = 0; i < keypoints.size(); i++) {
kpt_mat.at<float>(i, 0) = (float)keypoints[i].pt.y;
kpt_mat.at<float>(i, 1) = (float)keypoints[i].pt.x;
kpt_mat.at<float>(i, 0) = (float)keypoints[i].pt.y - s/2 + 0.5;
kpt_mat.at<float>(i, 1) = (float)keypoints[i].pt.x - s/2 + 0.5;
}
auto fkpts = torch::from_blob(kpt_mat.data, {(long int)keypoints.size(), 2}, torch::kFloat);
float w = desc_.size(3); //W/8
float h = desc_.size(2); //H/8
torch::Device device(cuda_?torch::kCUDA:torch::kCPU);
auto grid = torch::zeros({1, 1, fkpts.size(0), 2}).to(device); // [1, 1, n_keypoints, 2]
grid[0][0].slice(1, 0, 1) = 2.0 * fkpts.slice(1, 1, 2) / prob_.size(1) - 1; // x
grid[0][0].slice(1, 1, 2) = 2.0 * fkpts.slice(1, 0, 1) / prob_.size(0) - 1; // y
grid[0][0].slice(1, 0, 1) = 2.0 * fkpts.slice(1, 1, 2) / (w*s - s/2 - 0.5) - 1; // x
grid[0][0].slice(1, 1, 2) = 2.0 * fkpts.slice(1, 0, 1) / (h*s - s/2 - 0.5) - 1; // y
auto desc = torch::grid_sampler(desc_, grid, 0, 0, true); // [1, 256, 1, n_keypoints]
desc = desc.squeeze(0).squeeze(1); // [256, n_keypoints]
// normalize to 1
auto dn = torch::norm(desc, 2, 1);
desc = desc.div(torch::unsqueeze(dn, 1));
desc = torch::nn::functional::normalize(desc.reshape({1, desc_.size(1), -1})); //[1, 256, n_keypoints]
desc = desc.squeeze(); //[256, n_keypoints]
desc = desc.transpose(0, 1).contiguous(); //[n_keypoints, 256]
desc = desc.transpose(0, 1).contiguous(); // [n_keypoints, 256]
if(cuda_)
desc = desc.to(torch::kCPU);
cv::Mat desc_mat(cv::Size(desc.size(1), desc.size(0)), CV_32FC1, desc.data<float>());
cv::Mat desc_mat(cv::Size(desc.size(1), desc.size(0)), CV_32FC1, desc.data_ptr<float>());
return desc_mat.clone();
}

9
corelib/src/util3d_features.cpp
View File

@@ -377,7 +377,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
float var = 2.1981 * median_error_sqr;
//UDEBUG("scale %d = %f variance = %f", (int)i, s, variance);
@@ -397,7 +397,7 @@ std::map<int, cv::Point3f> generateWords3DMono(
errorSqrdDists[j] = uNormSquared(refPt.x-newPt.x, refPt.y-newPt.y, refPt.z-newPt.z);
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 2];
variance = 2.1981 * median_error_sqr;
}
@@ -487,7 +487,10 @@ std::map<int, cv::Point3f> generateWords3DMono(
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
UDEBUG("t (cam frame)=%s", t.prettyPrint().c_str());
UDEBUG("base->cam=%s", cameraModel.localTransform().prettyPrint().c_str());
cameraTransform = cameraModel.localTransform() * t.inverse() * cameraModel.localTransform().inverse();
UDEBUG("t (base frame)=%s", cameraTransform.prettyPrint().c_str());
}
}
}