mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-13 05:19:51 +08:00
Adding doc and tests (#1492)
* added doc and tests for util2d.h * updated cmake-ros ci * Added util3d.h doc and tests * util3d_transforms.h: Added doc and tests * util3d_filtering.h: started doc and test * util3d_filtering.h: more tests and doc * Added more doc/tests * finished util3d_filtering doc and tests * added test for util2d::depthBleedingFiltering * Added util3d_registration tests * Added util3d_features.h doc/tests * added doc/tests for util3d_correspondences.h * added doc/gtest for util3d_mapping.h (missing hpp functions) * finished testing util3d_mapping.hpp * Added util3d_motion_estimation.h tests (2D->3D done) * finished util3d_motion_estimation.h tests * minimal util3d_surface.h * Added Transform and VisualWord tests * Added doc for CameraModel and StereoCameraModel * Added more logs in ros ci * Passing tests on fical * improved all devcontainer * added devcontainer kilted, fixed source setup.bash, removed ldconfig in ros-cmake workflow * cleanup * source ros * Added utilite tests * Added testing to appveyor, github actions cancellable on re-commit on same branch * appveyor testing without all targets * appveyor: specifying ALL_BUILD target * Fixed Util2dTest.NMSImageBoundsRespected test * Fixing PCL Indices error on old pcl * Added VWDictionary tests and doc. Fixed LSH not working (fix from https://github.com/flann-lib/flann/pull/472 * fixing some appveyor CI errors, added test to check dictionary serialization against all type * Added StereoDense, StereoBM and StereoSGBM doc and tests * Added Stereo tests * Added CameraModel and StereoCameraModel tests * Added doc and test for Statistics * Added doc/tests for Signature * Added doc/test for SensorEvent, added doc for SensorCaptureInfo * Added doc to SensorData * Added SensorData tests * Added SensorCapture and SensorCaptureThread doc and tests * fixed sensordata test * updated SSC test and doc * Added doc and tests for BayesFilter class * Enabled testing on mac, updated windows testing like on linux * added test_link * fixed unresolved on windows * fixed ThreadHandle error on macos ci * Added GPS and GeodeticCoords tests * Added tests for compression * Added Odometry tests (base class only) * Added DBDriver tests * Added coverage report * uniformized test names * fixing concurancy and coverage ci * dont built tools, examples and app for coverage build * fixed report tool rebuilt without qt compilation error * updated coverage option * updated coverage config * added doc CI job * fixing windows and mac ci errors * Added DBDriverSqlite3 tests * Added IMU tests * Added Graph tests * fixing flaky macos test * Added IMUThread and IMUFilter tests * Added Landmarks tests * Added LASWriter tests * fixing seed flaky test * fixing flaky macos timing tests * Added LocalGrid tests * Added LocalGridMaker tests * fixing ci errors * Added GlobalMap tests * Added doc for EnvSensor * Added Features2D tests * Added Registration tests * Added RegistrationVis tests * Added doc for Rtabmap and Memory classes * Added Memory and Rtabmap tests * making some tests less flaky * lcov 1.14 support * updated compatible tool arguments * Added integration tests (RGB-D, Stereo, Lidar2d, Lidar3d) * More octomap checks * Refactored how/when python interpretor is created to simplify library usage * Added python tests * fixed some flaky tests * suppressed some third party related warnings * fixed ceres tests * more flaky fixes * Fixing tests without libpointmatcher * Added RANSAC rejection filter to PCL ICP * fixing multi platform flakiness * Added test to detect regression * Fixing windows pcl link error * fixed some macos flakiness * bigger 2D2D registration error on opencv 4.6.0 * flakiness * fixing flaky tests on windows and mac * flaky thread test on slow mac VM * windows slow test * fixing more ci erros * fxing temp dir on windows * Added Optimizer tests and discovered some bugs (fixed) * fixing flaky tests in mac and windows * Added Optimizer doc * Added GTSAM BA, updated Ceres to use g2o ba parameters. Renamed g2o's ba related parameters to Optimizer group and used by both gtsam and ceres. * fixing build without gtsam * fixing home dir * fixing python ci isssues * Added multicam ba tests * Added Ceres multicam BA support * Aligned BundleAdjustment parameters with Optimizer/Strategy to avoid confusion in the code * Added BA integration test * Added robust graph optimization integration test * Added loop3it test * Added stereo20Hz test * Added smartfactor gtsam * Fixed bugged check and warn if python didn't return any descriptors * Fixing gtsam version build issues * fixing tilt on windows ci * loosing ceres integration test for ci * mac ci flakiness * updating missing param in gui * updating test bound for mac * added appearance-based tests, set min gftt quality to quality level * testing more stuff * improving features2d tests * ci flakiness * fixing flaky ci * ci fixes * flaky fixes * Added RegistrationIcp tests * Added icp integration test with real-worl corridor like env * intermediate nodes * fixing enum * Updated test to catch #1714 * Fixed 2d corridor failing on pcl * flaky pnp test * flaky brisk test * Set rtabmap_integration test as long * updating loop closure test * flaky ci tests * TEsting roundtrip g2o/toro save/load * loosing test bound * fixed cuda capable checks * flaky tests * Debugging test hanging * more debugging stuff * updating limit * windows: disabled cuda on ci to avoid incompatible driver issue. Fixing a bad test mem allocation * trying fixing cuda hanging issue * fixing ci flakyness * flaky tests * Updated BOW flaky tests by checking min precision/recall instead of recall@100precision. Fixed signature test * CameraModel::load() test initRectificationMap param * test dbdriver load dictionary idsOnly * Memory: test keepLinkedInDb param * added dummyDictionary tests * test intermediate nodes count * Added MarkerDetector tests * reverted breaking change of UMutex and USemaphore * Features2d: fixed compiltion warnings with clang about override * clang warnings * fixing test build with pcl 1.8 * g2o and gtsam build errors on android * opencv5 test fixes * disabled testing for ios and android builds * normalized endline characters for easier diff * added LF CRLF rule * bump 0.23.10. fixing doc version * Publish rtabmap website doc from ci * fixing MSCVC build error * macos icp flaky test * fixing ceres macos test bound * ficing more flaky tests * fixing opencv5 related test errors. Also fixed an actual bug in ENU_WGS84ToGeocentric_WGS84() * added comment about mrpt change * removed rosdoc2 (will add it for rtabmap_ros later) * fixing website style * updated download links * locally deployable website with api * sweep doxygen issues * improved/revised doxygen main pages * removed examples empty page * Updated doxygen style * more concise doxygen groups * added api link on main readme * fixing utilite test error * fixing CommonFilteringGroundNormalsUp test * updated precisionRecall test bounds for Freak and brief descriptors * fixing scale check in ba tests * disabled tests on windows cuda build (missing dlls amd runner cannot test cuda anyway) * ceres: missing suitesparse dep in windows ci * adjusting recall thr for fast/freak * ficing more flaky tests * fixing flaky tests * disabled coverage in ros ci * Enable integration tests for ros ci jobs * loosing up some threshold for failing tests * trigger cache * fixing test data in ros ci. Updated flaky test for mac * slaking some test limit * Fixed rtabmap-detectMoreLoopClosures inverted output value * loosing up sift recall on mac * optimizer re-ordered distribution for reproducible results (mac g2o) * macos dump test crash log * combining all tests to save time on shared library reload. Also fixed Logs with missing arguments. * Added ENABLE_FORMAT_ERRORS cmake option * do test only one time * fixed all format warnings * format security android build errors * less verbose tests * updated ImuUThread test * fixed a log * Fixed libpointmatcher 2d normals eigen issue * Fixing libpointmatcher conversion issues * fixing libpointmatcher test on windows ci * cleanup comments, relax some test thr * disabled sequoia-intel ci build (too flaky, would need extensive testing directly on that machine)
This commit is contained in:
matlabbe
+105
-43
@@ -142,6 +142,9 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
|
||||
UDEBUG("iterations=%d", iterations);
|
||||
UDEBUG("ssdApproach=%d", ssdApproach?1:0);
|
||||
UASSERT(minDisparityF >= 0.0f && minDisparityF <= maxDisparityF);
|
||||
UASSERT(!leftImage.empty() && !rightImage.empty());
|
||||
UASSERT(leftImage.size() == rightImage.size());
|
||||
UASSERT(leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1);
|
||||
|
||||
// window should be odd
|
||||
if(winSize.width%2 == 0)
|
||||
@@ -217,7 +220,8 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
|
||||
cv::Range(center.y-halfWin.height,center.y+halfWin.height+1),
|
||||
cv::Range(center.x+d-halfWin.width,center.x+d+halfWin.width+1));
|
||||
scores[oi] = ssdApproach?ssd(windowLeft, windowRight):sad(windowLeft, windowRight);
|
||||
if(scores[oi] > 0 && (bestScore < 0.0f || scores[oi] < bestScore))
|
||||
// score: the lower the better
|
||||
if(scores[oi] >= 0 && (bestScore < 0.0f || scores[oi] < bestScore))
|
||||
{
|
||||
bestScoreIndex = oi;
|
||||
bestScore = scores[oi];
|
||||
@@ -361,17 +365,6 @@ typedef float acctype;
|
||||
typedef float itemtype;
|
||||
#define CV_DESCALE(x,n) (((x) + (1 << ((n)-1))) >> (n))
|
||||
|
||||
//
|
||||
// Adapted from OpenCV cv::calcOpticalFlowPyrLK() to force
|
||||
// only optical flow on x-axis (assuming that prevImg is the left
|
||||
// image and nextImg is the right image):
|
||||
// https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L1088
|
||||
//
|
||||
// The difference is on this line:
|
||||
// https://github.com/Itseez/opencv/blob/ddf82d0b154873510802ef75c53e628cd7b2cb13/modules/video/src/lkpyramid.cpp#L683-L684
|
||||
// - cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), (float)((A12*b1 - A11*b2) * D));
|
||||
// + cv::Point2f delta( (float)((A12*b2 - A22*b1) * D), 0); //<--- note the 0 for y
|
||||
//
|
||||
void calcOpticalFlowPyrLKStereo( cv::InputArray _prevImg, cv::InputArray _nextImg,
|
||||
cv::InputArray _prevPts, cv::InputOutputArray _nextPts,
|
||||
cv::OutputArray _status, cv::OutputArray _err,
|
||||
@@ -1197,7 +1190,7 @@ cv::Rect computeRoi(const cv::Size & imageSize, const std::vector<float> & roiRa
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
|
||||
UERROR("Image is null or _roiRatios(=%d) != 4", (int)roiRatios.size());
|
||||
return cv::Rect();
|
||||
}
|
||||
}
|
||||
@@ -1362,7 +1355,6 @@ cv::Mat interpolate(const cv::Mat & image, int factor, float depthErrorRatio)
|
||||
return out;
|
||||
}
|
||||
|
||||
// Registration Depth to RGB (return registered depth image)
|
||||
cv::Mat registerDepth(
|
||||
const cv::Mat & depth,
|
||||
const cv::Mat & depthK,
|
||||
@@ -1481,9 +1473,9 @@ cv::Mat fillDepthHoles(const cv::Mat & depth, int maximumHoleSize, float errorRa
|
||||
UASSERT(maximumHoleSize > 0);
|
||||
cv::Mat output = depth.clone();
|
||||
bool isMM = depth.type() == CV_16UC1;
|
||||
for(int y=0; y<depth.rows-2; ++y)
|
||||
for(int y=0; y<depth.rows-1; ++y)
|
||||
{
|
||||
for(int x=0; x<depth.cols-2; ++x)
|
||||
for(int x=0; x<depth.cols-1; ++x)
|
||||
{
|
||||
float a, bRight, bDown;
|
||||
if(isMM)
|
||||
@@ -1877,7 +1869,7 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
|
||||
if (!found_finite)
|
||||
{
|
||||
UWARN("Given an empty depth image. Doing nothing.");
|
||||
return cv::Mat();
|
||||
return output;
|
||||
}
|
||||
UDEBUG("base_min=%f base_max=%f", base_min, base_max);
|
||||
|
||||
@@ -2089,8 +2081,16 @@ cv::Mat brightnessAndContrastAuto(const cv::Mat &src, const cv::Mat & mask, floa
|
||||
// current range
|
||||
float inputRange = maxGray - minGray;
|
||||
|
||||
alpha = (histSize - 1) / inputRange; // alpha expands current range to histsize range
|
||||
beta = -minGray * alpha; // beta shifts current range so that minGray will go to 0
|
||||
if(inputRange == 0.0f)
|
||||
{
|
||||
alpha = 1.0f;
|
||||
beta = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
alpha = (histSize - 1) / inputRange; // alpha expands current range to histsize range
|
||||
beta = -minGray * alpha; // beta shifts current range so that minGray will go to 0
|
||||
}
|
||||
|
||||
UINFO("minGray=%f maxGray=%f alpha=%f beta=%f", minGray, maxGray, alpha, beta);
|
||||
|
||||
@@ -2147,6 +2147,10 @@ void HSVtoRGB( float *r, float *g, float *b, float h, float s, float v )
|
||||
*r = *g = *b = v;
|
||||
return;
|
||||
}
|
||||
if(h/360.0f >= 1.0f)
|
||||
{
|
||||
h -= floor(h/360.0f)*360.0f;
|
||||
}
|
||||
h /= 60; // sector 0 to 5
|
||||
i = floor( h );
|
||||
f = h - i; // factorial part of h
|
||||
@@ -2191,17 +2195,30 @@ void NMS(
|
||||
const std::vector<cv::KeyPoint> & ptsIn,
|
||||
const cv::Mat & descriptorsIn,
|
||||
std::vector<cv::KeyPoint> & ptsOut,
|
||||
cv::Mat & descriptorsOut,
|
||||
int border, int dist_thresh, int img_width, int img_height)
|
||||
cv::Mat & descriptorsOut,
|
||||
int dist_thresh, int img_width, int img_height)
|
||||
{
|
||||
std::vector<cv::Point2f> pts_raw;
|
||||
if(ptsIn.empty())
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
std::vector<cv::Point2i> pts_raw;
|
||||
|
||||
for (size_t i = 0; i < ptsIn.size(); i++)
|
||||
{
|
||||
int u = (int) ptsIn[i].pt.x;
|
||||
int v = (int) ptsIn[i].pt.y;
|
||||
|
||||
pts_raw.emplace_back(cv::Point2f(u, v));
|
||||
if(u<0 || u>img_width || v<0 || v>img_height)
|
||||
{
|
||||
UERROR("Point (%f,%f) is outside the image size (%dx%d), this point is ignored!", ptsIn[i].pt.x, ptsIn[i].pt.y, img_width, img_height);
|
||||
pts_raw.emplace_back(cv::Point2i(-1, -1));
|
||||
}
|
||||
else
|
||||
{
|
||||
pts_raw.emplace_back(cv::Point2i(u, v));
|
||||
}
|
||||
}
|
||||
|
||||
//Grid Value Legend:
|
||||
@@ -2220,9 +2237,14 @@ void NMS(
|
||||
|
||||
for (size_t i = 0; i < pts_raw.size(); i++)
|
||||
{
|
||||
int uu = (int) pts_raw[i].x;
|
||||
int vv = (int) pts_raw[i].y;
|
||||
int uu = pts_raw[i].x;
|
||||
int vv = pts_raw[i].y;
|
||||
|
||||
if(uu < 0) {
|
||||
// skip invalid points
|
||||
continue;
|
||||
}
|
||||
|
||||
grid.at<unsigned char>(vv, uu) = 100;
|
||||
inds.at<unsigned short>(vv, uu) = i;
|
||||
|
||||
@@ -2236,9 +2258,14 @@ void NMS(
|
||||
|
||||
for (size_t i = 0; i < pts_raw.size(); i++)
|
||||
{
|
||||
if(pts_raw[i].x < 0)
|
||||
{
|
||||
// skip invlaid points
|
||||
continue;
|
||||
}
|
||||
// account for top left padding
|
||||
int uu = (int) pts_raw[i].x + dist_thresh;
|
||||
int vv = (int) pts_raw[i].y + dist_thresh;
|
||||
int uu = pts_raw[i].x + dist_thresh;
|
||||
int vv = pts_raw[i].y + dist_thresh;
|
||||
float c = confidence.at<float>(vv-dist_thresh, uu-dist_thresh);
|
||||
|
||||
if (grid.at<unsigned char>(vv, uu) == 100) // If not yet suppressed.
|
||||
@@ -2299,7 +2326,9 @@ void NMS(
|
||||
std::vector<int> SSC(
|
||||
const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows, const std::vector<int> & indx)
|
||||
{
|
||||
bool useIndx = keypoints.size() == indx.size();
|
||||
UASSERT(keypoints.empty() || indx.empty() || keypoints.size() == indx.size());
|
||||
|
||||
bool useIndx = !indx.empty();
|
||||
maxKeypoints = maxKeypoints - round(maxKeypoints * tolerance); // Just the make sure the solution will always be <= input maxKeypoints
|
||||
|
||||
// several temp expression variables to simplify solution equation
|
||||
@@ -2379,7 +2408,7 @@ bool rotateImagesUpsideUpIfNecessary(
|
||||
Transform localTransform = model.localTransform()*CameraModel::opticalRotation().inverse();
|
||||
localTransform.getEulerAngles(roll, pitch, yaw);
|
||||
UDEBUG("roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
|
||||
if(fabs(pitch > M_PI/4))
|
||||
if(fabs(pitch) > M_PI/4)
|
||||
{
|
||||
// Return original because of ambiguity for what would be considered up...
|
||||
UDEBUG("Ignoring image rotation as pitch(%f)>Pi/4", pitch);
|
||||
@@ -2391,29 +2420,49 @@ bool rotateImagesUpsideUpIfNecessary(
|
||||
}
|
||||
if(roll >= M_PI/4 && roll < 3*M_PI/4)
|
||||
{
|
||||
UDEBUG("ROTATION_90 (roll=%f)", roll);
|
||||
// Body roll near +pi/2 (right side down): the world-up direction projects to
|
||||
// the image's left, so rotate the image 90 degrees clockwise to bring it
|
||||
// upright (transpose + horizontal flip). Image dimensions HxW become WxH.
|
||||
//
|
||||
// Marker X moves from top-left to top-right quadrant:
|
||||
// before (3x6): after (6x3):
|
||||
// . X . . . . . . .
|
||||
// . . . . . . . . X
|
||||
// . . . . . . . . .
|
||||
// . . .
|
||||
// . . .
|
||||
// . . .
|
||||
UDEBUG("Rotating image 90 deg clockwise to correct body roll (roll=%f)", roll);
|
||||
if(!rgb.empty())
|
||||
{
|
||||
cv::flip(rgb,rgb,1);
|
||||
cv::transpose(rgb,rgb);
|
||||
cv::flip(rgb,rgb,1);
|
||||
}
|
||||
if(!depth.empty())
|
||||
{
|
||||
cv::flip(depth,depth,1);
|
||||
cv::transpose(depth,depth);
|
||||
cv::flip(depth,depth,1);
|
||||
}
|
||||
cv::Size sizet(model.imageHeight(), model.imageWidth());
|
||||
model = CameraModel(
|
||||
model.fy(),
|
||||
model.fx(),
|
||||
model.cy(),
|
||||
model.cx()>0?model.imageWidth()-model.cx():0,
|
||||
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
|
||||
model.cy()>0?model.imageHeight()-model.cy():0,
|
||||
model.cx(),
|
||||
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
|
||||
model.setImageSize(sizet);
|
||||
}
|
||||
else if(roll >= 3*M_PI/4 && roll < 5*M_PI/4)
|
||||
{
|
||||
UDEBUG("ROTATION_180 (roll=%f)", roll);
|
||||
// Body roll near pi (upside down): rotate the image 180 degrees (horizontal
|
||||
// flip + vertical flip). Image dimensions unchanged.
|
||||
//
|
||||
// Marker X moves from top-left to bottom-right quadrant:
|
||||
// before (3x6): after (3x6):
|
||||
// . X . . . . . . . . . .
|
||||
// . . . . . . . . . . . .
|
||||
// . . . . . . . . . . X .
|
||||
UDEBUG("Rotating image 180 deg to correct body roll (roll=%f)", roll);
|
||||
if(!rgb.empty())
|
||||
{
|
||||
cv::flip(rgb,rgb,1);
|
||||
@@ -2435,29 +2484,42 @@ bool rotateImagesUpsideUpIfNecessary(
|
||||
}
|
||||
else if(roll >= 5*M_PI/4 && roll < 7*M_PI/4)
|
||||
{
|
||||
UDEBUG("ROTATION_270 (roll=%f)", roll);
|
||||
// Body roll near -pi/2 / +3*pi/2 (left side down): the world-up direction
|
||||
// projects to the image's right, so rotate the image 90 degrees counter-
|
||||
// clockwise to bring it upright (horizontal flip + transpose). Image
|
||||
// dimensions HxW become WxH.
|
||||
//
|
||||
// Marker X moves from top-left to bottom-left quadrant:
|
||||
// before (3x6): after (6x3):
|
||||
// . X . . . . . . .
|
||||
// . . . . . . . . .
|
||||
// . . . . . . . . .
|
||||
// . . .
|
||||
// X . .
|
||||
// . . .
|
||||
UDEBUG("Rotating image 90 deg counter-clockwise to correct body roll (roll=%f)", roll);
|
||||
if(!rgb.empty())
|
||||
{
|
||||
cv::transpose(rgb,rgb);
|
||||
cv::flip(rgb,rgb,1);
|
||||
cv::transpose(rgb,rgb);
|
||||
}
|
||||
if(!depth.empty())
|
||||
{
|
||||
cv::transpose(depth,depth);
|
||||
cv::flip(depth,depth,1);
|
||||
cv::transpose(depth,depth);
|
||||
}
|
||||
cv::Size sizet(model.imageHeight(), model.imageWidth());
|
||||
model = CameraModel(
|
||||
model.fy(),
|
||||
model.fx(),
|
||||
model.cy()>0?model.imageHeight()-model.cy():0,
|
||||
model.cx(),
|
||||
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
|
||||
model.cy(),
|
||||
model.cx()>0?model.imageWidth()-model.cx():0,
|
||||
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
|
||||
model.setImageSize(sizet);
|
||||
}
|
||||
else
|
||||
{
|
||||
UDEBUG("ROTATION_0 (roll=%f)", roll);
|
||||
UDEBUG("Not rotating image, body roll within +/- pi/4 of upright (roll=%f)", roll);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
||||
Reference in New Issue
Block a user