i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Adding doc and tests (#1492)

Browse Source 
* 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
2026-08-06 13:32:20 -07:00
committed by GitHub
parent bcdb4b4546
commit ee49beaf4f
309 changed files with 67468 additions and 3069 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/BayesFilter.cpp
+8 -6
View File
@@ -188,7 +188,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
{
((float*)posterior.data)[j++] = (*i).second;
}
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior=%d, _posterior size=%d", posterior.rows, _posterior.size());
ULOGGER_DEBUG("STEP1-update posterior=%fs, posterior rows=%d, _posterior size=%d", timer.ticks(), posterior.rows, (int)_posterior.size());
//std::cout << "LastPosterior=" << posterior << std::endl;
// Multiply prediction matrix with the last posterior
@@ -269,7 +269,6 @@ float addNeighborProb(cv::Mat & prediction,
return sum;
}
cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector<int> & ids)
{
std::vector<int> oldIds = uKeys(_posterior);
@@ -314,7 +313,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
int cols = prediction.cols;
// Each prior is a column vector
UDEBUG("_predictionLC.size()=%d",_predictionLC.size());
UDEBUG("_predictionLC.size()=%d",(int)_predictionLC.size());
std::set<int> idsDone;
for(unsigned int i=0; i<ids.size(); ++i)
@@ -421,7 +420,10 @@ unsigned long BayesFilter::getMemoryUsed() const
{
long memoryUsage = sizeof(BayesFilter);
memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::map<int, float>::iterator)) + sizeof(std::map<int, float>);
memoryUsage += _prediction.total() * _prediction.elemSize();
if(!_prediction.empty())
{
memoryUsage += _prediction.total() * _prediction.elemSize();
}
memoryUsage += _predictionLC.size() * sizeof(double);
memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::map<int, std::map<int, int> >::iterator)) + sizeof(std::map<int, std::map<int, int> >);
for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
@@ -621,7 +623,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
UDEBUG("From added id %d, %d neighbors to update.", newIds[i], count);
}
}
UDEBUG("time getting %d ids to update = %fs", idsToUpdate.size(), timer.restart());
UDEBUG("time getting %d ids to update = %fs", (int)idsToUpdate.size(), timer.restart());
UTimer t1;
double e0=0,e1=0, e2=0, e3=0, e4=0;
@@ -649,7 +651,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
e4+=t1.ticks();
}
}
UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
UDEBUG("time updating modified/added %d ids = %fs (e0=%f e1=%f e2=%f e3=%f e4=%f)", (int)idsToUpdate.size(), timer.restart(), e0, e1, e2, e3, e4);
int copied = 0;
if(!oldAllCopied)
corelib/src/CMakeLists.txt
+41 -3
View File
@@ -882,11 +882,49 @@ target_include_directories(rtabmap_core PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR};${CMAKE_CURRENT_SOURCE_DIR}/../include;${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/include>"
"$<INSTALL_INTERFACE:${INSTALL_INCLUDE_DIR}>")
target_include_directories(rtabmap_core SYSTEM PUBLIC
target_include_directories(rtabmap_core SYSTEM PUBLIC
"$<BUILD_INTERFACE:${PUBLIC_INCLUDE_DIRS};${INCLUDE_DIRS}>"
"$<INSTALL_INTERFACE:${PUBLIC_INCLUDE_DIRS}>")
TARGET_LINK_LIBRARIES(rtabmap_core
# GCC 12 false positives from PCL/Eigen template instantiations (SSE codepath
# unaligned-loads 16 bytes from a 3-element Eigen vector). Eigen knows the
# over-read is safe; GCC 12 doesn't. Fixed in GCC 13. PCL itself doesn't
# trigger this in its own build because the offending code is in templated
# headers and only fires in downstream TUs. Same scope/approach as
# pybind11 PR #5355.
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU"
AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 12
AND CMAKE_CXX_COMPILER_VERSION VERSION_LESS 13)
target_compile_options(rtabmap_core PRIVATE -Wno-array-bounds -Wno-stringop-overread)
# GCC 12 -Wuse-after-free false positive on the vendored TORO library's
# reference-counted DMatrix destructor: when multiple ~DMatrix<double>()
# inline at the same closing brace, GCC's cross-call flow analysis can't
# prove the freed and re-read 'shares' pointers belong to different
# objects. Scope to the TORO sources only.
if(WITH_TORO)
set_source_files_properties(
optimizer/toro3d/posegraph3.cpp
optimizer/toro3d/treeoptimizer3_iteration.cpp
optimizer/toro3d/treeoptimizer3.cpp
optimizer/toro3d/posegraph2.cpp
optimizer/toro3d/treeoptimizer2.cpp
PROPERTIES COMPILE_OPTIONS "-Wno-use-after-free")
endif()
# GCC 12 -Wmaybe-uninitialized false positive: Eigen's SSE codepath
# _mm_loadu_pd's an uninitialized-by-design Eigen::Matrix<double,3,3>
# inside g2o::SBACam. GCC follows the SIMD load through and flags it as
# potentially uninitialized; Eigen relies on the caller to assign before
# reading. Only OptimizerG2O.cpp instantiates SBACam.
if(G2O_FOUND)
set_source_files_properties(
optimizer/OptimizerG2O.cpp
PROPERTIES COMPILE_OPTIONS "-Wno-maybe-uninitialized")
endif()
endif()
TARGET_LINK_LIBRARIES(rtabmap_core
PUBLIC
rtabmap_utilite
${PUBLIC_LIBRARIES}
corelib/src/CameraModel.cpp
+1 -1
View File
@@ -386,7 +386,7 @@ bool CameraModel::load(const std::string & filePath, bool initRectificationMaps)
}
catch(const cv::Exception & e)
{
UERROR("Error reading calibration file \"%s\": %s (Make sure the first line of the yaml file is \"%YAML:1.0\")", filePath.c_str(), e.what());
UERROR("Error reading calibration file \"%s\": %s (Make sure the first line of the yaml file is \"%%YAML:1.0\")", filePath.c_str(), e.what());
}
}
else
corelib/src/DBDriver.cpp
+3 -3
View File
@@ -332,7 +332,7 @@ void DBDriver::emptyTrashes(bool async)
std::map<int, VisualWord*> visualWords;
_trashesMutex.lock();
{
ULOGGER_DEBUG("signatures=%d, visualWords=%d", _trashSignatures.size(), _trashVisualWords.size());
ULOGGER_DEBUG("signatures=%d, visualWords=%d", (int)_trashSignatures.size(), (int)_trashVisualWords.size());
signatures = _trashSignatures;
visualWords = _trashVisualWords;
_trashSignatures.clear();
@@ -1368,7 +1368,7 @@ void DBDriver::generateGraph(
if(idsInput.size() == 0)
{
this->getAllNodeIds(ids);
UDEBUG("ids.size()=%d", ids.size());
UDEBUG("ids.size()=%d", (int)ids.size());
for(std::map<int, Signature*>::const_iterator iter=otherSignatures.begin(); iter!=otherSignatures.end(); ++iter)
{
ids.insert(iter->first);
@@ -1382,7 +1382,7 @@ void DBDriver::generateGraph(
const char * colorG = "green";
const char * colorP = "pink";
const char * colorNM = "blue";
UINFO("Generating map with %d locations", ids.size());
UINFO("Generating map with %d locations", (int)ids.size());
fprintf(fout, "digraph G {\n");
for(std::set<int>::iterator i=ids.begin(); i!=ids.end(); ++i)
{
corelib/src/DBDriverSqlite3.cpp
+17 -10
View File
@@ -3532,9 +3532,9 @@ void DBDriverSqlite3::loadLastNodesQuery(std::list<Signature *> & nodes, bool lo
rc = sqlite3_finalize(ppStmt);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Loading %d signatures...", ids.size());
ULOGGER_DEBUG("Loading %d signatures...", (int)ids.size());
this->loadSignaturesQuery(ids, nodes, loadWordIdsOnly);
ULOGGER_DEBUG("loaded=%d, Time=%fs", nodes.size(), timer.ticks());
ULOGGER_DEBUG("loaded=%d, Time=%fs", (int)nodes.size(), timer.ticks());
}
}
@@ -3653,8 +3653,15 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly, b
dataSize = sqlite3_column_bytes(ppStmt, index++);
if(dataSize>4 && data)
{
UDEBUG("A flann index was saved in the database (size=%ld).", dataSize);
dictionary.deserializeIndex((const unsigned char*)data, dataSize);
UDEBUG("A flann index was saved in the database (size=%ld).", (long)dataSize);
if(!dictionary.deserializeIndex((const unsigned char*)data, dataSize))
{
UERROR("Failed to deserialize dictionary's index! See previous logs for reason.");
}
else
{
UINFO("Sucessfully loaded dictionary's index.");
}
}
else {
UDEBUG("No flann index was saved in the database.");
@@ -3676,7 +3683,7 @@ void DBDriverSqlite3::loadQuery(VWDictionary & dictionary, bool lastStateOnly, b
//may be slower than the previous version but don't have a limit of words that can be loaded at the same time
void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const
{
ULOGGER_DEBUG("size=%d", wordIds.size());
ULOGGER_DEBUG("size=%d", (int)wordIds.size());
if(_ppDb && wordIds.size())
{
std::string type;
@@ -3764,7 +3771,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
UDEBUG("Not found word %d", *iter);
}
}
UERROR("Query (%d) doesn't match loaded words (%d)", wordIds.size(), loaded.size());
UERROR("Query (%d) doesn't match loaded words (%d)", (int)wordIds.size(), (int)loaded.size());
}
}
}
@@ -4350,7 +4357,7 @@ void DBDriverSqlite3::loadLinksQuery(std::list<Signature *> & signatures) const
void DBDriverSqlite3::updateQuery(const std::list<Signature *> & nodes, bool updateTimestamp) const
{
UDEBUG("nodes = %d, updateTimestamp = %s", nodes.size(), updateTimestamp?"true":"false");
UDEBUG("nodes = %d, updateTimestamp = %s", (int)nodes.size(), updateTimestamp?"true":"false");
if(_ppDb && nodes.size())
{
UTimer timer;
@@ -4658,7 +4665,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
query = queryStepSensorData();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Saving %d images", signatures.size());
UDEBUG("Saving %d images", (int)signatures.size());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
@@ -4686,7 +4693,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
query = queryStepImage();
rc = sqlite3_prepare_v2(_ppDb, query.c_str(), -1, &ppStmt, 0);
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
UDEBUG("Saving %d images", signatures.size());
UDEBUG("Saving %d images", (int)signatures.size());
for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
{
@@ -4725,7 +4732,7 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
void DBDriverSqlite3::saveQuery(const std::list<VisualWord *> & words) const
{
UDEBUG("visualWords size=%d", words.size());
UDEBUG("visualWords size=%d", (int)words.size());
if(_ppDb)
{
std::string type;
corelib/src/DBReader.cpp
+5 -7
View File
@@ -148,9 +148,8 @@ void DBReader::checkArguments()
_cameraIndices.size() != _cameraLocalTransformOverrides.size())
{
UERROR("Camera local transform overrides (%d) are not the same size than the camera indices (%d). The overrides are ignored.",
_cameraLocalTransformOverrides.size(),
_cameraIndices.size()
);
(int)_cameraLocalTransformOverrides.size(),
(int)_cameraIndices.size());
_cameraLocalTransformOverrides.clear();
}
for(size_t i=0; i<_cameraLocalTransformOverrides.size(); ++i)
@@ -625,9 +624,8 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
_cameraIndices.size() != _cameraLocalTransformOverrides.size())
{
UERROR("Camera local transform overrides (%d) are not the same size than the camera indices (%d). The overrides are ignored.",
_cameraLocalTransformOverrides.size(),
_cameraIndices.size()
);
(int)_cameraLocalTransformOverrides.size(),
(int)_cameraIndices.size());
_cameraLocalTransformOverrides.clear();
}
@@ -643,7 +641,7 @@ SensorData DBReader::getNextData(SensorCaptureInfo * info)
for(size_t i=0; i<_cameraIndices.size(); ++i)
{
UASSERT_MSG(_cameraIndices[i] < dbModels.size(), uFormat("DBReader: camera index %ld is not valid (should be between 0 and %ld)",
_cameraIndices[i], dbModels.size()-1).c_str());
(long)_cameraIndices[i], dbModels.size()-1).c_str());
int addedCameras = std::max(combinedModels.size(), combinedStereoModels.size());
corelib/src/EpipolarGeometry.cpp
+2 -2
View File
@@ -94,12 +94,12 @@ bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
int inliers = uSum(status);
if(inliers < _matchCountMinAccepted)
{
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, pairs.size(), _matchCountMinAccepted);
ULOGGER_DEBUG("Epipolar constraint failed A : not enough inliers (%d/%d), min is %d", inliers, (int)pairs.size(), _matchCountMinAccepted);
return false;
}
else
{
UDEBUG("inliers = %d/%d", inliers, pairs.size());
UDEBUG("inliers = %d/%d", inliers, (int)pairs.size());
return true;
}
}
corelib/src/Features2d.cpp
+158 -17
View File
@@ -302,7 +302,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
cv::Mat descriptorsTmp;
if(ssc)
{
ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());
ULOGGER_DEBUG("too many words (%d), removing words with SSC", (int)keypoints.size());
// Sorting keypoints by deacreasing order of strength
std::vector<float> responseVector;
@@ -354,7 +354,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
}
else
{
ULOGGER_DEBUG("too many words (%d), removing words with the hessian threshold", keypoints.size());
ULOGGER_DEBUG("too many words (%d), removing words with the hessian threshold", (int)keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
@@ -418,7 +418,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
inliers.resize(keypoints.size(), false);
if(ssc)
{
ULOGGER_DEBUG("too many words (%d), removing words with SSC", keypoints.size());
ULOGGER_DEBUG("too many words (%d), removing words with SSC", (int)keypoints.size());
// Sorting keypoints by deacreasing order of strength
std::vector<float> responseVector;
@@ -445,7 +445,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
}
else
{
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", (int)keypoints.size());
// Remove words under the new hessian threshold
// Sort words by hessian
@@ -465,7 +465,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
minimumHessian = iter->first;
}
}
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size()-removed, minimumHessian);
ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)(keypoints.size()-removed), minimumHessian);
ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
}
else
@@ -613,6 +613,69 @@ Feature2D * Feature2D::create(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kKpDetectorStrategy(), type);
return create((Feature2D::Type)type, parameters);
}
bool Feature2D::isAvailable(Feature2D::Type type)
{
// kFeatureUndef is a sentinel ("strategy not specified"); create() falls
// through to a default backend, so the type isn't really "available" as
// requested.
if(type == kFeatureUndef)
{
return false;
}
// SURF / SIFT / SURF-FREAK / SURF-DAISY require either OpenCV < 3.4.11
// (built-in) OR the xfeatures2d module + RTABMAP_NONFREE for OpenCV >= 3.4.11.
#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
#ifndef RTABMAP_NONFREE
if(type == kFeatureSurf || type == kFeatureSift || type == kFeatureSurfFreak || type == kFeatureSurfDaisy)
{
return false;
}
#endif
#else
#ifndef RTABMAP_NONFREE
if(type == kFeatureSurf || type == kFeatureSurfFreak || type == kFeatureSurfDaisy)
{
return false;
}
#endif
#endif
#if !defined(HAVE_OPENCV_XFEATURES2D) && CV_MAJOR_VERSION >= 3
if(type == kFeatureFastBrief ||
type == kFeatureFastFreak ||
type == kFeatureGfttBrief ||
type == kFeatureGfttFreak ||
type == kFeatureSurfFreak ||
type == kFeatureGfttDaisy ||
type == kFeatureSurfDaisy)
{
return false;
}
#elif CV_MAJOR_VERSION < 3
if(type == kFeatureKaze ||
type == kFeatureGfttDaisy ||
type == kFeatureSurfDaisy)
{
return false;
}
#endif
#ifndef RTABMAP_ORB_OCTREE
if(type == kFeatureOrbOctree) return false;
#endif
#ifndef RTABMAP_TORCH
if(type == kFeatureSuperPointTorch) return false;
#endif
#if !defined(RTABMAP_TORCH) || !defined(RTABMAP_PYTHON)
if(type == kFeatureSuperPointRpautrat) return false;
#endif
#ifndef RTABMAP_PYTHON
if(type == kFeaturePyDetector) return false;
#endif
return true;
}
Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parameters)
{
@@ -855,7 +918,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
}
}
UDEBUG("Keypoints extraction time = %f s, keypoints extracted = %d (grid=%dx%d, mask empty=%d)",
timer.ticks(), keypoints.size(), gridCols_, gridRows_, mask.empty()?1:0);
timer.ticks(), (int)keypoints.size(), gridCols_, gridRows_, mask.empty()?1:0);
if(keypoints.size() && _subPixWinSize > 0 && _subPixIterations > 0)
{
@@ -1147,13 +1210,13 @@ void SURF::parseParameters(const ParametersMap & parameters)
#ifdef RTABMAP_NONFREE
#if CV_MAJOR_VERSION < 3
if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
if(gpuVersion_ && cv::gpu::getCudaEnabledDeviceCount() <= 0)
{
UWARN("GPU version of SURF not available! Using CPU version instead...");
gpuVersion_ = false;
}
#else
if(gpuVersion_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
if(gpuVersion_ && cv::cuda::getCudaEnabledDeviceCount() <= 0)
{
UWARN("GPU version of SURF not available! Using CPU version instead...");
gpuVersion_ = false;
@@ -1176,6 +1239,19 @@ void SURF::parseParameters(const ParametersMap & parameters)
#endif
}
bool SURF::isGpuAvailable() const
{
#ifdef RTABMAP_NONFREE
#if CV_MAJOR_VERSION < 3
return cv::gpu::getCudaEnabledDeviceCount() > 0;
#else
return cv::cuda::getCudaEnabledDeviceCount() > 0;
#endif
#else
return false;
#endif
}
std::vector<cv::KeyPoint> SURF::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -1340,6 +1416,15 @@ void SIFT::parseParameters(const ParametersMap & parameters)
}
bool SIFT::isGpuAvailable() const
{
#ifdef RTABMAP_CUDASIFT
return cv::cuda::getCudaEnabledDeviceCount() > 0;
#else
return false;
#endif
}
std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -1444,7 +1529,7 @@ std::vector<cv::KeyPoint> SIFT::generateKeypointsImpl(const cv::Mat & image, con
}
if(k < keypoints.size())
{
UDEBUG("keypoints extracted = %d, valid=%d", keypoints.size(), k);
UDEBUG("keypoints extracted = %d, valid=%d", (int)keypoints.size(), (int)k);
keypoints.resize(k);
cudaSiftDescriptors_.resize(k);
}
@@ -1564,7 +1649,7 @@ void ORB::parseParameters(const ParametersMap & parameters)
#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() <= 0)
{
UWARN("GPU version of ORB not available! Using CPU version instead...");
gpu_ = false;
@@ -1584,7 +1669,7 @@ void ORB::parseParameters(const ParametersMap & parameters)
gpu_ = false;
}
#endif
if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() <= 0)
{
UWARN("GPU version of ORB not available (no GPU found)! Using CPU version instead...");
gpu_ = false;
@@ -1617,6 +1702,15 @@ void ORB::parseParameters(const ParametersMap & parameters)
}
}
bool ORB::isGpuAvailable() const
{
#ifdef HAVE_OPENCV_CUDAFEATURES2D
return cv::cuda::getCudaEnabledDeviceCount() > 0;
#else
return false;
#endif
}
std::vector<cv::KeyPoint> ORB::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -1811,7 +1905,7 @@ void FAST::parseParameters(const ParametersMap & parameters)
#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() == 0)
if(gpu_ && cv::gpu::getCudaEnabledDeviceCount() <= 0)
{
UWARN("GPU version of FAST not available! Using CPU version instead...");
gpu_ = false;
@@ -1825,7 +1919,7 @@ void FAST::parseParameters(const ParametersMap & parameters)
#endif
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() == 0)
if(gpu_ && cv::cuda::getCudaEnabledDeviceCount() <= 0)
{
UWARN("GPU version of FAST not available! Using CPU version instead...");
gpu_ = false;
@@ -1881,6 +1975,12 @@ void FAST::parseParameters(const ParametersMap & parameters)
}
}
bool FAST::isGpuAvailable() const
{
// Not implemented
return false;
}
std::vector<cv::KeyPoint> FAST::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -2114,7 +2214,7 @@ void GFTT::parseParameters(const ParametersMap & parameters)
#endif
#ifdef HAVE_OPENCV_CUDAIMGPROC
if(_gpu && cv::cuda::getCudaEnabledDeviceCount() == 0)
if(_gpu && cv::cuda::getCudaEnabledDeviceCount() <= 0)
{
UWARN("GPU version of GFTT not available! Using CPU version instead...");
_gpu = false;
@@ -2144,6 +2244,15 @@ void GFTT::parseParameters(const ParametersMap & parameters)
}
}
bool GFTT::isGpuAvailable() const
{
#ifdef HAVE_OPENCV_CUDAIMGPROC
return cv::cuda::getCudaEnabledDeviceCount() > 0;
#else
return false;
#endif
}
std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
{
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
@@ -2172,7 +2281,21 @@ std::vector<cv::KeyPoint> GFTT::generateKeypointsImpl(const cv::Mat & image, con
{
_gftt->detect(imgRoi, keypoints, maskRoi); // Opencv keypoints
}
if(!_useHarrisDetector && _qualityLevel>0.0)
{
std::vector<cv::KeyPoint> bestKeypoints;
bestKeypoints.reserve(keypoints.size());
for(size_t i=0; i<keypoints.size(); ++i)
{
if(keypoints[i].response > _qualityLevel)
{
bestKeypoints.push_back(keypoints[i]);
}
}
return bestKeypoints;
}
return keypoints;
}
@@ -2600,6 +2723,15 @@ SuperPointTorch::~SuperPointTorch()
{
}
bool SuperPointTorch::isGpuAvailable() const
{
#ifdef RTABMAP_TORCH
return torch::cuda::is_available();
#else
return false;
#endif
}
void SuperPointTorch::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
@@ -2634,7 +2766,7 @@ std::vector<cv::KeyPoint> SuperPointTorch::generateKeypointsImpl(const cv::Mat &
{
#ifdef RTABMAP_TORCH
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
if(roi.x!=0 || roi.y !=0)
if(roi.x!=0 || roi.y !=0 || roi.width!=image.cols || roi.height!=image.rows)
{
UERROR("SuperPoint: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
roi.x, roi.y, roi.width, roi.height,
@@ -2708,6 +2840,15 @@ SuperPointRpautrat::~SuperPointRpautrat()
{
}
bool SuperPointRpautrat::isGpuAvailable() const
{
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
return torch::cuda::is_available();
#else
return false;
#endif
}
void SuperPointRpautrat::parseParameters(const ParametersMap & parameters)
{
Feature2D::parseParameters(parameters);
@@ -2760,7 +2901,7 @@ std::vector<cv::KeyPoint> SuperPointRpautrat::generateKeypointsImpl(const cv::Ma
{
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
if(roi.x!=0 || roi.y !=0)
if(roi.x!=0 || roi.y !=0 || roi.width!=image.cols || roi.height!=image.rows)
{
UERROR("SuperPoint Rpautrat: Not supporting ROI (%d,%d,%d,%d). Make sure %s, %s, %s, %s, %s, %s are all set to default values.",
roi.x, roi.y, roi.width, roi.height,
corelib/src/FlannIndex.cpp
+34 -4
View File
@@ -123,6 +123,22 @@ std::vector<unsigned char> FlannIndex::serializeIndex(bool computeChecksum) cons
if(bytes_written < long(indexData.size()-headerSizeBytes))
{
//Expected data size and type
//
// dataType is stored in the header as a raw cv::Mat type and
// compared against features.type() on load. That value is NOT
// stable across OpenCV major versions: OpenCV 5 changed
// CV_CN_SHIFT from 3 to 5, so a multi-channel type serializes to
// a different integer than under OpenCV 4 (see the encoding
// helpers in Compression.cpp, which normalize it for the data
// blobs stored in the database).
//
// It is safe here only because descriptors are always
// single-channel (asserted CV_32FC1 or CV_8UC1 in buildKDTreeIndex()
// and friends), and 1-channel types have the same value in both
// versions. A mismatch would only make loadIndex() refuse the
// index and rebuild it, never corrupt data -- but if descriptors
// ever become multi-channel, this field needs the same
// normalization as Compression.cpp.
int dataRows = 0;
int dataCols = 0;
int dataType = -1;
@@ -302,6 +318,7 @@ void FlannIndex::buildIndex(
break;
case FLANN_INDEX_LSH:
UASSERT(features.type() == CV_8UC1);
UASSERT_MSG(features.cols >= 8, "LSH requires a minimum of 8 dimensions to provide valid results.");
params = rtflann::LshIndexParams(12, 20, 2);
break;
default:
@@ -452,6 +469,10 @@ bool FlannIndex::loadIndex(
}
return false;
}
// Raw cv::Mat type comparison: safe only because descriptors are always
// single-channel, whose type value is identical under OpenCV 4 and 5
// (OpenCV 5 changed CV_CN_SHIFT, which only shifts multi-channel types).
// See the note where the header is written in serializeIndex().
if(savedType != features.type()) {
if(error) {
*error = uFormat("Serialized feature type (%d) doesn't match the expected one (%d).", savedType, features.type());
@@ -471,7 +492,7 @@ bool FlannIndex::loadIndex(
}
if(savedIndexSize != int(indexDataSize - headerSizeBytes)) {
if(error) {
*error = uFormat("Serialized flann index size (%ld) doesn't match the expected one (%ld).", savedIndexSize, indexDataSize - headerSizeBytes);
*error = uFormat("Serialized flann index size (%ld) doesn't match the expected one (%ld).", (long)savedIndexSize, indexDataSize - headerSizeBytes);
}
return false;
}
@@ -712,10 +733,11 @@ void FlannIndex::knnSearch(
UERROR("Flann index not yet created!");
return;
}
indices.create(query.rows, knn, sizeof(size_t)==8?CV_64F:CV_32S);
dists.create(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F);
rtflann::Matrix<size_t> indicesF((size_t*)indices.data, indices.rows, indices.cols);
dists = cv::Mat(query.rows, knn, featuresType_ == CV_8UC1?CV_32S:CV_32F, cv::Scalar(-1));
std::vector<size_t> indicesBuffer(query.rows * knn, std::numeric_limits<size_t>::max());
rtflann::Matrix<size_t> indicesF((size_t*)indicesBuffer.data(), query.rows, knn);
rtflann::SearchParams params = rtflann::SearchParams(checks, eps, sorted);
@@ -742,6 +764,14 @@ void FlannIndex::knnSearch(
((rtflann::Index<rtflann::L2<float> >*)index_)->knnSearch(queryF, indicesF, distsF, knn, params);
}
}
indices.create(query.rows, knn, CV_32S);
int * ptr = indices.ptr<int>();
for(size_t i=0 ; i<indicesBuffer.size(); i+=2)
{
ptr[i] = indicesBuffer[i] == std::numeric_limits<size_t>::max()?-1:(int)indicesBuffer[i];
ptr[i+1] = indicesBuffer[i+1] == std::numeric_limits<size_t>::max()?-1:(int)indicesBuffer[i+1];
}
}
void FlannIndex::radiusSearch(
corelib/src/GeodeticCoords.cpp
+21 -27
View File
@@ -182,36 +182,30 @@ void GeodeticCoords::fromENU_WGS84(const cv::Point3d& enu, const GeodeticCoords&
cv::Point3d GeodeticCoords::ENU_WGS84ToGeocentric_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin)
{
// Generate reference 3D point:
cv::Point3f originGeocentric;
originGeocentric = origin.toGeocentric_WGS84();
// Exact inverse of Geocentric_WGS84ToENU_WGS84(): that function rotates
// ECEF-relative coordinates into ENU with a matrix built from the origin's
// *geodetic* latitude/longitude, so the inverse is its transpose (the matrix
// is orthonormal), built from the same angles.
//
// This previously derived the local frame from normalize(P_ref), i.e. the
// *geocentric* vertical. On an ellipsoid that differs from the geodetic
// normal by up to ~0.19 deg, so the two directions were not inverses of each
// other and every ENU->geodetic conversion picked up a systematic error
// proportional to the horizontal offset (~0.11 m of altitude at 40 m out).
//
// The same defect was inherited from MRPT, which this file was adapted from,
// and was fixed upstream by the identical change in
// https://github.com/MRPT/mrpt/commit/8be4931ca7abccf5eb58da253b02660cdbdf6427
// ("mrpt_topography: increase code coverage and fix bugs", 2026-07-10).
const double clat = cos(DEG2RAD(origin.latitude())), slat = sin(DEG2RAD(origin.latitude()));
const double clon = cos(DEG2RAD(origin.longitude())), slon = sin(DEG2RAD(origin.longitude()));
cv::Vec3d P_ref(originGeocentric.x, originGeocentric.y, originGeocentric.z);
// Z axis -> In direction out-ward the center of the Earth:
cv::Vec3d REF_X, REF_Y, REF_Z;
REF_Z = cv::normalize(P_ref);
// 1st column: Starting at the reference point, move in the tangent
// direction
// east-ward: I compute this as the derivative of P_ref wrt "longitude":
// A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon); --> -Z[1]
// A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon); --> Z[0]
// A_east[2] = 0; --> 0
// ---------------------------------------------------------------------------
cv::Vec3d AUX_X(-REF_Z[1], REF_Z[0], 0);
REF_X = cv::normalize(AUX_X);
// 2nd column: The cross product:
REF_Y = REF_Z.cross(REF_X);
const cv::Point3d originGeocentric = origin.toGeocentric_WGS84();
cv::Point3d out_coords;
out_coords.x =
REF_X[0] * enu.x + REF_Y[0] * enu.y + REF_Z[0] * enu.z + originGeocentric.x;
out_coords.y =
REF_X[1] * enu.x + REF_Y[1] * enu.y + REF_Z[1] * enu.z + originGeocentric.y;
out_coords.z =
REF_X[2] * enu.x + REF_Y[2] * enu.y + REF_Z[2] * enu.z + originGeocentric.z;
out_coords.x = -slon*enu.x - clon*slat*enu.y + clon*clat*enu.z + originGeocentric.x;
out_coords.y = clon*enu.x - slon*slat*enu.y + slon*clat*enu.z + originGeocentric.y;
out_coords.z = clat*enu.y + slat*enu.z + originGeocentric.z;
return out_coords;
}
corelib/src/Graph.cpp
+49 -24
View File
@@ -673,18 +673,11 @@ int32_t lastFrameFromSegmentLength(std::vector<float> &dist,int32_t first_frame,
}
inline float rotationError(const Transform &pose_error) {
float a = pose_error(0,0);
float b = pose_error(1,1);
float c = pose_error(2,2);
float d = 0.5*(a+b+c-1.0);
return std::acos(std::max(std::min(d,1.0f),-1.0f));
return pose_error.getAngle(Transform::getIdentity());
}
inline float translationError(const Transform &pose_error) {
float dx = pose_error.x();
float dy = pose_error.y();
float dz = pose_error.z();
return sqrt(dx*dx+dy*dy+dz*dz);
return pose_error.getNorm();
}
void calcKittiSequenceErrors (
@@ -695,6 +688,14 @@ void calcKittiSequenceErrors (
UASSERT(poses_gt.size() == poses_result.size());
t_err = 0.0f;
r_err = 0.0f;
if(poses_gt.size() < 2)
{
return;
}
// error vector
std::vector<errors> err;
@@ -720,24 +721,35 @@ void calcKittiSequenceErrors (
if (last_frame==-1)
continue;
const Transform & gtFirst = poses_gt[first_frame];
const Transform & gtLast = poses_gt[last_frame];
const Transform & estFirst = poses_result[first_frame];
const Transform & estLast = poses_result[last_frame];
UASSERT_MSG(gtFirst.isInvertible() && gtLast.isInvertible() &&
estFirst.isInvertible() && estLast.isInvertible(),
uFormat("Non-invertible poses at frames %d and %d (segment length %f m)",
first_frame, last_frame, len).c_str());
// compute rotational and translational errors
Transform pose_delta_gt = poses_gt[first_frame].inverse()*poses_gt[last_frame];
Transform pose_delta_result = poses_result[first_frame].inverse()*poses_result[last_frame];
Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
float r_err = rotationError(pose_error);
float t_err = translationError(pose_error);
Transform pose_delta_gt = gtFirst.inverse()*gtLast;
Transform pose_delta_result = estFirst.inverse()*estLast;
Transform pose_error = pose_delta_result.inverse()*pose_delta_gt;
const float rotErr = rotationError(pose_error);
const float transErr = translationError(pose_error);
// compute speed
float num_frames = (float)(last_frame-first_frame+1);
float speed = len/(0.1*num_frames);
float speed = len/(0.1f*num_frames);
// write to file
err.push_back(errors(first_frame,r_err/len,t_err/len,len,speed));
err.push_back(errors(first_frame, rotErr/len, transErr/len, len, speed));
}
}
t_err = 0;
r_err = 0;
if(err.empty())
{
return;
}
// for all errors do => compute sum of t_err, r_err
for (std::vector<errors>::iterator it=err.begin(); it!=err.end(); it++)
@@ -747,11 +759,11 @@ void calcKittiSequenceErrors (
}
// save errors
float num = err.size();
const float num = float(err.size());
t_err /= num;
r_err /= num;
t_err *= 100.0f; // Translation error (%)
r_err *= 180/CV_PI; // Rotation error (deg/m)
r_err *= 180.0f/CV_PI; // Rotation error (deg/m)
}
// KITTI evaluation end
@@ -1471,7 +1483,7 @@ std::map<int, Transform> radiusPosesFiltering(
//pcl::IndicesPtr indicesOut(new std::vector<int>);
//indicesOut->insert(indicesOut->end(), indicesKept.begin(), indicesKept.end());
UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", cloud->size(), indicesKept.size(), radius, angle, keepLatest?1:0);
UINFO("Cloud filtered In = %d, Out = %d (radius=%f angle=%f keepLatest=%d)", (int)cloud->size(), (int)indicesKept.size(), radius, angle, keepLatest?1:0);
//pcl::io::savePCDFile("duplicateIn.pcd", *cloud);
//pcl::io::savePCDFile("duplicateOut.pcd", *cloud, *indicesOut);
@@ -1879,6 +1891,7 @@ std::list<std::pair<int, Transform> > computePath(
if(mapIter->second == nodeIter->first)
{
pqmap.erase(mapIter);
nodeIter->second.setFromId(currentNode->id());
nodeIter->second.setCostSoFar(newCostSoFar);
pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
break;
@@ -1969,7 +1982,7 @@ std::list<int> computePath(
pq.push(Pair(n.id(), n.totalCost()));
}
}
else if(!useSameCostForAllLinks && updateNewCosts && nodeIter->second.isOpened())
else if(updateNewCosts && nodeIter->second.isOpened())
{
float newCostSoFar = currentNode->costSoFar() + cost;
if(nodeIter->second.costSoFar() > newCostSoFar)
@@ -1980,6 +1993,7 @@ std::list<int> computePath(
if(mapIter->second == nodeIter->first)
{
pqmap.erase(mapIter);
nodeIter->second.setFromId(currentNode->id());
nodeIter->second.setCostSoFar(newCostSoFar);
pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
break;
@@ -2149,6 +2163,7 @@ std::list<std::pair<int, Transform> > computePath(
if(mapIter->second == nodeIter->first)
{
pqmap.erase(mapIter);
nodeIter->second.setFromId(currentNode->id());
nodeIter->second.setCostSoFar(newCostSoFar);
pqmap.insert(std::make_pair(nodeIter->second.totalCost(), nodeIter->first));
break;
@@ -2427,8 +2442,18 @@ std::list<std::map<int, Transform> > getPaths(
std::map<int, Transform> path;
for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
{
std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
if(path.size() == 0 || (jter != links.end() && (jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)))
bool addPose = false;
if(path.empty())
{
addPose = true;
}
else
{
std::multimap<int, Link>::const_iterator jter = findLink(links, path.rbegin()->first, iter->first);
addPose = jter != links.end() &&
(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged);
}
if(addPose)
{
path.insert(*iter);
poses.erase(iter++);
corelib/src/IMU.cpp
+6 -4
View File
@@ -38,15 +38,17 @@ void IMU::convertToBaseFrame()
localTransform_.rotationMatrix().convertTo(rotationMatrix, CV_64FC1);
cv::transpose(rotationMatrix, rotationMatrixT);
cv::Mat_<double> v = rotationMatrix * cv::Mat(linearAcceleration_);
linearAcceleration_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
cv::Mat_<double> linearIn = (cv::Mat_<double>(3,1) << linearAcceleration_[0], linearAcceleration_[1], linearAcceleration_[2]);
cv::Mat_<double> linearOut = rotationMatrix * linearIn;
linearAcceleration_ = cv::Vec3d(linearOut(0,0), linearOut(1,0), linearOut(2,0));
if(!linearAccelerationCovariance_.empty())
{
linearAccelerationCovariance_ = rotationMatrix * linearAccelerationCovariance_ * rotationMatrixT;
}
v = rotationMatrix * cv::Mat(angularVelocity_);
angularVelocity_ = cv::Vec3d(v(0,0), v(0,1), v(0,2));
cv::Mat_<double> angularIn = (cv::Mat_<double>(3,1) << angularVelocity_[0], angularVelocity_[1], angularVelocity_[2]);
cv::Mat_<double> angularOut = rotationMatrix * angularIn;
angularVelocity_ = cv::Vec3d(angularOut(0,0), angularOut(1,0), angularOut(2,0));
if(!angularVelocityCovariance_.empty())
{
angularVelocityCovariance_ = rotationMatrix * angularVelocityCovariance_ * rotationMatrixT;
corelib/src/IMUThread.cpp
+1 -1
View File
@@ -67,7 +67,7 @@ bool IMUThread::init(const std::string & path)
int number_of_lines = 0;
while (std::getline(imuFile_, line))
++number_of_lines;
printf("No. IMU measurements: %d\n", number_of_lines-1);
UINFO("No. IMU measurements: %d", number_of_lines-1);
if (number_of_lines - 1 <= 0) {
UERROR("no imu messages present in %s", path.c_str());
return false;
corelib/src/LASWriter.cpp
+2 -1
View File
@@ -148,11 +148,12 @@ int saveLASFile(const std::string & filePath,
{
liblas::Point point(&header);
point.SetCoordinates(cloud.at(i).x, cloud.at(i).y, cloud.at(i).z);
point.SetIntensity(cloud.at(i).intensity);
if(!cameraIds.empty())
{
point.SetPointSourceID(cameraIds.at(i));
}
writer.WritePoint(point);
}
}
corelib/src/LaserScan.cpp
+16
View File
@@ -138,6 +138,22 @@ bool LaserScan::isScanHasRing(const Format & format)
return format==kXYZIRT;
}
float LaserScan::packRGB(unsigned char r, unsigned char g, unsigned char b)
{
int rgb = ((int)r) << 16 | ((int)g) << 8 | ((int)b);
float f;
std::memcpy(&f, &rgb, sizeof(float));
return f;
}
void LaserScan::unpackRGB(float rgb, unsigned char & r, unsigned char & g, unsigned char & b)
{
int * ptrInt = (int*)&rgb;
b = (unsigned char)(*ptrInt & 0xFF);
g = (unsigned char)((*ptrInt >> 8) & 0xFF);
r = (unsigned char)((*ptrInt >> 16) & 0xFF);
}
LaserScan LaserScan::backwardCompatibility(
const cv::Mat & oldScanFormat,
int maxPoints,
corelib/src/MarkerDetector.cpp
+9 -22
View File
@@ -462,25 +462,6 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
}
// deprecated
std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
{
UDEBUG("");
std::map<int, Transform> detections;
std::map<int, MarkerInfo> infos = detect(image, model, depth, std::map<int, float>(), imageWithDetections);
for(std::map<int, MarkerInfo>::iterator iter=infos.begin(); iter!=infos.end(); ++iter)
{
detections.insert(std::make_pair(iter->first, iter->second.pose()));
if(markerLengthOut)
{
*markerLengthOut = iter->second.length();
}
}
return detections;
}
#if (((CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION==4 && CV_MINOR_VERSION >=7)) && defined(HAVE_OPENCV_OBJDETECT)) || defined(HAVE_OPENCV_ARUCO)) && (CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7))
// Drop-in replacement for cv::aruco::estimatePoseSingleMarkers(), which was deprecated
// in OpenCV 4.7 and removed in OpenCV 5. It reproduces the legacy default behavior (marker
@@ -537,9 +518,14 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
rgbToDepthFactorX = float(depthMap.cols) / float(image.cols);
rgbToDepthFactorY = float(depthMap.rows) / float(image.rows);
}
else if(markerLength_ == 0)
else if(markerLength_ == 0 && markerLengths_.empty())
{
UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
// Without a depth image the length cannot be estimated, so it has to come
// from a parameter. Marker/Lengths is enough on its own: it gives an explicit
// length for every marker we accept.
UERROR("Depth image is empty, please set %s (or %s) parameter to non-null.",
Parameters::kMarkerLength().c_str(),
Parameters::kMarkerLengths().c_str());
return std::map<int, MarkerInfo>();
}
@@ -747,7 +733,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
{
if(tvecs[i].val[2] <=0)
{
UWARN("Skipping %d because its estimated pose is behind the camera %d", cvIdsPerCam[cam][i], cam);
UWARN("Skipping %d because its estimated pose is behind the camera %d", cvIdsPerCam[cam][i], (int)cam);
continue;
}
cv::Mat R;
@@ -835,6 +821,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
UWARN("Marker's length of %d is defined both in extra lengths "
"(%f m) and the parameter %s (%f m), we will use the length "
"from extra lengths.",
ids[i],
findIter->second,
Parameters::kMarkerLengths().c_str(),
paramIter->second);
corelib/src/Memory.cpp
+39 -23
View File
@@ -164,7 +164,7 @@ Memory::Memory(const ParametersMap & parameters) :
if(corRatio >= 0.5)
{
UWARN( "%s is >=0.5, which sets correspondence ratio for proximity detection using "
"laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity "
"laser scans to 100%% (2 x Ratio). You may lower the ratio to accept proximity "
"detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
}
_registrationIcpMulti = new RegistrationIcp(paramsMulti);
@@ -444,6 +444,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
else
{
_dbDriver->load(*_vwd, false, _dummyDictionary);
if(_dummyDictionary && _vwd->getVisualWords().empty())
{
_dummyDictionary = false;
}
}
}
else
@@ -451,6 +455,10 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
UDEBUG("load words");
// load the last dictionary
_dbDriver->load(*_vwd, _vwd->isIncremental(), _dummyDictionary);
if(_dummyDictionary && _vwd->getVisualWords().empty())
{
_dummyDictionary = false;
}
}
UDEBUG("%d words loaded! (type=%s, dim=%d)",
_vwd->getUnusedWordsSize(),
@@ -977,7 +985,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
if(corRatio >= 0.5)
{
UWARN( "%s is >=0.5, which sets correspondence ratio for proximity detection using "
"laser scans to 100% (2 x Ratio). You may lower the ratio to accept proximity "
"laser scans to 100%% (2 x Ratio). You may lower the ratio to accept proximity "
"detection with not full scans overlapping.", Parameters::kIcpCorrespondenceRatio().c_str());
}
_registrationIcpMulti->parseParameters(paramsMulti);
@@ -1330,7 +1338,7 @@ void Memory::addSignatureToStm(Signature * signature, const cv::Mat & covariance
}
++_signaturesAdded;
UDEBUG("%d words ref for the signature %d (weight=%d)", signature->getWords().size(), signature->id(), signature->getWeight());
UDEBUG("%d words ref for the signature %d (weight=%d)", (int)signature->getWords().size(), signature->id(), signature->getWeight());
if(signature->getWords().size())
{
signature->setEnabled(true);
@@ -2133,7 +2141,7 @@ void Memory::clear()
}
if(_stMem.size() != 0)
{
ULOGGER_ERROR("_stMem must be empty here, size=%d", _stMem.size());
ULOGGER_ERROR("_stMem must be empty here, size=%d", (int)_stMem.size());
}
_stMem.clear();
_stMemIntermediateNodesCount = 0;
@@ -2161,7 +2169,7 @@ void Memory::clear()
// this is only a safe check...not supposed to occur.
UASSERT_MSG(memSize == _signatures.size(),
uFormat("The number of signatures don't match! _workingMem=%d, _stMem=%d, _signatures=%d",
workingMemSize, _stMem.size(), _signatures.size()).c_str());
(int)workingMemSize, (int)_stMem.size(), (int)_signatures.size()).c_str());
UDEBUG("Adding statistics after run...");
if(_memoryChanged)
@@ -2205,13 +2213,13 @@ void Memory::clear()
if(_workingMem.size() != 0 && !(_workingMem.size() == 1 && _workingMem.begin()->first == kIdVirtual))
{
ULOGGER_ERROR("_workingMem must be empty here, size=%d", _workingMem.size());
ULOGGER_ERROR("_workingMem must be empty here, size=%d", (int)_workingMem.size());
}
_workingMem.clear();
_workingMemIntermediateNodesCount = 0;
if(_signatures.size()!=0)
{
ULOGGER_ERROR("_signatures must be empty here, size=%d", _signatures.size());
ULOGGER_ERROR("_signatures must be empty here, size=%d", (int)_signatures.size());
}
_signatures.clear();
@@ -2607,10 +2615,16 @@ std::map<int, Transform> Memory::loadOptimizedPoses(Transform * lastlocalization
void Memory::save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const
{
if(_dbDriver)
if(_dbDriver && !this->isReadOnly())
{
_dbDriver->save2DMap(map, xMin, yMin, cellSize);
}
else
{
UERROR("Attempting to write back 2D map but the database "
"is opened in read-only mode (%s=true), skipping.",
Parameters::kMemLocalizationReadOnly().c_str());
}
}
cv::Mat Memory::load2DMap(float & xMin, float & yMin, float & cellSize) const
@@ -2701,7 +2715,9 @@ public:
}
return false;
}
int weight, age, id;
int weight;
double age;
int id;
};
std::list<Signature *> Memory::getRemovableSignatures(int count, const std::set<int> & ignoredIds)
@@ -3173,7 +3189,7 @@ bool Memory::setUserData(int id, const cv::Mat & data)
}
else
{
UERROR("Node %d not found in RAM, failed to set user data (size=%d)!", id, data.total());
UERROR("Node %d not found in RAM, failed to set user data (size=%d)!", id, (int)data.total());
}
return false;
}
@@ -3362,7 +3378,7 @@ Transform Memory::computeTransform(
{
info->rejectedMsg = msg;
}
UWARN(msg.c_str());
UWARN("%s", msg.c_str());
}
return transform;
}
@@ -3741,7 +3757,7 @@ Transform Memory::computeTransform(
{
info->rejectedMsg = msg;
}
UWARN(msg.c_str());
UWARN("%s", msg.c_str());
}
return transform;
}
@@ -5118,7 +5134,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(this->getSignatures().empty() && isIntermediateNode)
{
UWARN("Ignoring input data with stamp %s because the first node in memory cannot be an intermediate node.", inputData.stamp());
UWARN("Ignoring input data with stamp %f because the first node in memory cannot be an intermediate node.", inputData.stamp());
return 0;
}
@@ -5409,7 +5425,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
"with the first camera (rgb=%dx%d, depth=%dx%d). Aborting upside up rotation, "
"will use original image orientation. Set parameter %s to false to avoid "
"this warning.",
i,
(int)i,
rgb.cols, rgb.rows,
depth.cols, depth.rows,
subOutputImageWidth, rotatedColorImages.rows,
@@ -6236,7 +6252,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
// Bearing/Range in 2D, set X as bearing and Y as range (see OptimizerGTSAM)
covariance(cv::Range(0,1), cv::Range(0,1)) *= _markerAngVariance;
covariance(cv::Range(1,3), cv::Range(1,3)) *= _markerLinVariance;
covariance(cv::Range(1,2), cv::Range(1,2)) *= _markerLinVariance;
}
else
{
@@ -6987,7 +7003,7 @@ void Memory::disableWordsRef(int signatureId)
void Memory::cleanUnusedWords()
{
std::vector<VisualWord*> removedWords = _vwd->getUnusedWords();
UDEBUG("Removing %d words (dictionary size=%d)...", removedWords.size(), _vwd->getVisualWords().size());
UDEBUG("Removing %d words (dictionary size=%d)...", (int)removedWords.size(), (int)_vwd->getVisualWords().size());
if(removedWords.size())
{
// remove them from the dictionary
@@ -7009,7 +7025,7 @@ void Memory::cleanUnusedWords()
void Memory::enableWordsRef(const std::list<int> & signatureIds)
{
UDEBUG("size=%d", signatureIds.size());
UDEBUG("size=%d", (int)signatureIds.size());
UTimer timer;
timer.start();
@@ -7041,7 +7057,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
UWARN("Dictionary is fixed, but some words retrieved have not been found!?");
}
UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", oldWordIds.size(), timer.ticks());
UDEBUG("oldWordIds.size()=%d, getOldIds time=%fs", (int)oldWordIds.size(), timer.ticks());
// the words were deleted, so try to match it with an active word
std::list<VisualWord *> vws;
@@ -7050,14 +7066,14 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
// get the descriptors
_dbDriver->loadWords(oldWordIds, vws);
}
UDEBUG("loading words(%d) time=%fs", oldWordIds.size(), timer.ticks());
UDEBUG("loading words(%d) time=%fs", (int)oldWordIds.size(), timer.ticks());
if(vws.size())
{
//Search in the dictionary
std::vector<int> vwActiveIds = _vwd->findNN(vws);
UDEBUG("find active ids (number=%d) time=%fs", vws.size(), timer.ticks());
UDEBUG("find active ids (number=%d) time=%fs", (int)vws.size(), timer.ticks());
int i=0;
for(std::list<VisualWord *>::iterator iterVws=vws.begin(); iterVws!=vws.end(); ++iterVws)
{
@@ -7081,7 +7097,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
}
++i;
}
UDEBUG("Added %d to dictionary, time=%fs", vws.size()-refsToChange.size(), timer.ticks());
UDEBUG("Added %d to dictionary, time=%fs", (int)(vws.size()-refsToChange.size()), timer.ticks());
//update the global references map and update the signatures reactivated
for(std::map<int, int>::const_iterator iter=refsToChange.begin(); iter != refsToChange.end(); ++iter)
@@ -7092,7 +7108,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
(*j)->changeWordsRef(iter->first, iter->second);
}
}
UDEBUG("changing ref, total=%d, time=%fs", refsToChange.size(), timer.ticks());
UDEBUG("changing ref, total=%d, time=%fs", (int)refsToChange.size(), timer.ticks());
}
int count = _vwd->getTotalActiveReferences();
@@ -7119,7 +7135,7 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
}
count = _vwd->getTotalActiveReferences() - count;
UDEBUG("%d words total ref added from %d signatures, time=%fs...", count, surfSigns.size(), timer.ticks());
UDEBUG("%d words total ref added from %d signatures, time=%fs...", count, (int)surfSigns.size(), timer.ticks());
}
std::set<int> Memory::reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess)
corelib/src/Optimizer.cpp
+2 -2
View File
@@ -811,13 +811,13 @@ void Optimizer::computeBACorrespondences(
}
else
{
UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
UWARN("Not enough inliers (%d) between %d and %d", (int)info.inliersIDs.size(), sFrom.id(), sTo.id());
}
}
}
}
}
UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, (int)links.size());
if(links.empty())
{
UERROR("No links found for BA?!");
corelib/src/Parameters.cpp
+7
View File
@@ -244,6 +244,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
// 0.23.7
removedParameters_.insert(std::make_pair("Marker/CornerRefinementMethod", std::make_pair(true, Parameters::kMarkerOpenCVCornerRefinementMethod())));
// BA tunables moved from g2o/ namespace to Optimizer/ since they
// now apply to g2o, GTSAM, and Ceres backends.
removedParameters_.insert(std::make_pair("g2o/PixelVariance", std::make_pair(true, Parameters::kOptimizerPixelVariance())));
removedParameters_.insert(std::make_pair("g2o/DisparityVariance", std::make_pair(true, Parameters::kOptimizerDisparityVariance())));
removedParameters_.insert(std::make_pair("g2o/RobustKernelDelta", std::make_pair(true, Parameters::kOptimizerRobustKernelDelta())));
removedParameters_.insert(std::make_pair("g2o/Baseline", std::make_pair(true, Parameters::kOptimizerBaseline())));
// 0.23.1
removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath", std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
corelib/src/Recovery.cpp
+1 -1
View File
@@ -243,7 +243,7 @@ bool databaseRecovery(
else if(UFile::erase(databasePath) != 0)
{
if(errorMsg)
*errorMsg = uFormat("Failed remove original database file \"%s\". Is it opened by another app? The recovered database cannot be copied back to original name.", UFile::getName(databasePath).c_str(), UFile::getName(recoveryPath).c_str());
*errorMsg = uFormat("Failed remove original database file \"%s\". Is it opened by another app? The recovered database \"%s\" cannot be copied back to original name.", UFile::getName(databasePath).c_str(), UFile::getName(recoveryPath).c_str());
return false;
}
corelib/src/RegistrationIcp.cpp
+183 -76
View File
@@ -54,9 +54,103 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
namespace {
// Strategy-1 projection used by both PointToPlane and PointToPoint branches
// in computeTransformationImpl(): when the structural complexity is below
// Icp/PointToPlaneMinComplexity (corridor-like environment), project the ICP
// translation correction onto the constrained eigenvectors of the normals
// (the first one always; the second only if its eigenvalue is itself above
// the complexity threshold). The unconstrained DoFs are then sourced from
// the guess. Returns the constrained transform.
Transform constrainLowComplexityTransform(
const Transform & icpT,
const Transform & guess,
const cv::Mat & complexityVectors,
float secondEigenValue,
float pointToPlaneMinComplexity,
float complexity)
{
const Transform guessInv = guess.inverse();
Transform t = guessInv * icpT.inverse() * guess;
Eigen::Vector3f v(t.x(), t.y(), t.z());
if(complexityVectors.cols == 2)
{
// 2D: project onto the first (and only constrained) eigenvector.
Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
Eigen::Vector3f vp = n * v.dot(n);
UWARN("Normals low complexity (%f): Limiting translation from (%f,%f) to (%f,%f)",
complexity, v[0], v[1], vp[0], vp[1]);
v = vp;
}
else if(complexityVectors.rows == 3)
{
// 3D: project onto first and second eigenvectors. Second is only
// kept when its eigenvalue is itself above the complexity threshold.
Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
Eigen::Vector3f vp = n1 * v.dot(n1);
if(secondEigenValue >= pointToPlaneMinComplexity)
{
vp += n2 * v.dot(n2);
}
UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
v[0], v[1], v[2], vp[0], vp[1], vp[2]);
v = vp;
}
else
{
UWARN("not supposed to be here!");
v = Eigen::Vector3f(0,0,0);
}
float roll, pitch, yaw;
t.getEulerAngles(roll, pitch, yaw);
t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
return guess * t.inverse() * guessInv;
}
} // namespace
bool RegistrationIcp::available(IcpStrategy strategy)
{
switch(strategy)
{
case kIcpPCL: return true;
case kIcpPointMatcher:
#ifdef RTABMAP_POINTMATCHER
return true;
#else
return false;
#endif
case kIcpCCCoreLib:
#ifdef RTABMAP_CCCORELIB
return true;
#else
return false;
#endif
default: return false;
}
}
const char * RegistrationIcp::strategyName(IcpStrategy strategy)
{
switch(strategy)
{
case kIcpPCL: return "PCL";
case kIcpPointMatcher: return "libpointmatcher";
case kIcpCCCoreLib: return "CCCoreLib";
default: return "Unknown";
}
}
RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration * child) :
RegistrationIcp(static_cast<IcpStrategy>(Parameters::defaultIcpStrategy()), parameters, child)
{
}
RegistrationIcp::RegistrationIcp(IcpStrategy strategy, const ParametersMap & parameters, Registration * child) :
Registration(parameters, child),
_strategy(Parameters::defaultIcpStrategy()),
_strategy(strategy),
_maxTranslation(Parameters::defaultIcpMaxTranslation()),
_maxRotation(Parameters::defaultIcpMaxRotation()),
_voxelSize(Parameters::defaultIcpVoxelSize()),
@@ -75,6 +169,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
_pointToPlaneGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
_pointToPlaneMinComplexity(Parameters::defaultIcpPointToPlaneMinComplexity()),
_pointToPlaneComplexityCentered(Parameters::defaultIcpPointToPlaneComplexityCentered()),
_pointToPlaneLowComplexityStrategy(Parameters::defaultIcpPointToPlaneLowComplexityStrategy()),
_libpointmatcherConfig(Parameters::defaultIcpPMConfig()),
_libpointmatcherKnn(Parameters::defaultIcpPMMatcherKnn()),
@@ -103,7 +198,11 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
{
Registration::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kIcpStrategy(), _strategy);
{
int strategyAsInt = static_cast<int>(_strategy);
Parameters::parse(parameters, Parameters::kIcpStrategy(), strategyAsInt);
_strategy = static_cast<IcpStrategy>(strategyAsInt);
}
Parameters::parse(parameters, Parameters::kIcpMaxTranslation(), _maxTranslation);
Parameters::parse(parameters, Parameters::kIcpMaxRotation(), _maxRotation);
Parameters::parse(parameters, Parameters::kIcpVoxelSize(), _voxelSize);
@@ -123,6 +222,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpPointToPlaneRadius(), _pointToPlaneRadius);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneGroundNormalsUp(), _pointToPlaneGroundNormalsUp);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneMinComplexity(), _pointToPlaneMinComplexity);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneComplexityCentered(), _pointToPlaneComplexityCentered);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneLowComplexityStrategy(), _pointToPlaneLowComplexityStrategy);
UASSERT(_pointToPlaneGroundNormalsUp >= 0.0f && _pointToPlaneGroundNormalsUp <= 1.0f);
UASSERT(_pointToPlaneMinComplexity >= 0.0f && _pointToPlaneMinComplexity <= 1.0f);
@@ -146,10 +246,10 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
bool pointToPlane = _pointToPlane;
#ifndef RTABMAP_POINTMATCHER
if(_strategy==1)
if(_strategy==kIcpPointMatcher)
{
UWARN("Parameter %s is set to 1 but RTAB-Map has not been built with libpointmatcher support. Setting to 0.", Parameters::kIcpStrategy().c_str());
_strategy = 0;
_strategy = kIcpPCL;
}
#else
delete (PM::ICP*)_libpointmatcherICP;
@@ -279,12 +379,12 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
#endif
#ifndef RTABMAP_CCCORELIB
if(_strategy==2)
if(_strategy==kIcpCCCoreLib)
{
#ifdef RTABMAP_POINTMATCHER
_strategy = 1;
_strategy = kIcpPointMatcher;
#else
_strategy = 0;
_strategy = kIcpPCL;
#endif
UWARN("Parameter %s is set to 2 but RTAB-Map has not been built with CCCoreLib support. Setting to %d.", Parameters::kIcpStrategy().c_str(), _strategy);
}
@@ -302,7 +402,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
_force4DoF = false;
}
UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", _voxelSize).c_str());
UASSERT_MSG(_voxelSize >= 0, uFormat("value=%d", (int)_voxelSize).c_str());
UASSERT_MSG(_downsamplingStep >= 0, uFormat("value=%d", _downsamplingStep).c_str());
UASSERT_MSG(_maxCorrespondenceDistance > 0.0f, uFormat("value=%f", _maxCorrespondenceDistance).c_str());
UASSERT_MSG(_maxIterations > 0, uFormat("value=%d", _maxIterations).c_str());
@@ -342,7 +442,7 @@ Transform RegistrationIcp::computeTransformationImpl(
UDEBUG("Enabled filters: from=%s to=%s", _filtersEnabled&1?"true":"false", _filtersEnabled&2?"true":"false");
UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);
UDEBUG("Strategy=%d", _strategy);
UDEBUG("IcpStrategy=%d", _strategy);
UTimer timer;
std::string msg;
@@ -471,8 +571,8 @@ Transform RegistrationIcp::computeTransformationImpl(
std::string fromPrefix = "rtabmap_icp_scan";
if(ULogger::level() == ULogger::kDebug)
{
fromPrefix+=uReplaceChar(uFormat("_%.3f_from_%d", fromSignature.id(), now), '.', '_');
toPrefix+=uReplaceChar(uFormat("_%.3f_to_%d", toSignature.id(), now), '.', '_');
fromPrefix+=uReplaceChar(uFormat("_%.3f_from_%d", now, fromSignature.id()), '.', '_');
toPrefix+=uReplaceChar(uFormat("_%.3f_to_%d", now, toSignature.id()), '.', '_');
}
else
{
@@ -525,15 +625,28 @@ Transform RegistrationIcp::computeTransformationImpl(
{
cv::Mat complexityVectorsFrom, complexityVectorsTo;
cv::Mat complexityValuesFrom, complexityValuesTo;
double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom);
double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo);
double fromComplexity = util3d::computeNormalsComplexity(fromScan, Transform::getIdentity(), &complexityVectorsFrom, &complexityValuesFrom, _pointToPlaneComplexityCentered);
double toComplexity = util3d::computeNormalsComplexity(toScan, guess, &complexityVectorsTo, &complexityValuesTo, _pointToPlaneComplexityCentered);
float complexity = fromComplexity<toComplexity?fromComplexity:toComplexity;
info.icpStructuralComplexity = complexity;
UDEBUG("structural complexity: from=%f to=%f", fromComplexity, toComplexity);
if(complexity < _pointToPlaneMinComplexity)
{
tooLowComplexityForPlaneToPlane = true;
pointToPlane = false;
// Strategy 3 keeps PointToPlane on and projects: gives
// cleaner residuals on the constrained perpendicular
// axes than PointToPoint does (point-to-plane cancels
// the random in-plane jitter via the normal dot
// product), and the in-plane DoFs are then pinned to
// the guess by the projection block in the
// PointToPlane branch. Strategy 1 (default, legacy)
// recomputes with PointToPoint then projects; Strategy
// 0 (reject) and 2 (accept PointToPoint as-is) keep
// the old behavior.
if(_pointToPlaneLowComplexityStrategy != 3)
{
pointToPlane = false;
}
if(complexity > 0.0f)
{
complexityVectors = fromComplexity<toComplexity?complexityVectorsFrom:complexityVectorsTo;
@@ -571,7 +684,7 @@ Transform RegistrationIcp::computeTransformationImpl(
msg = uFormat("Rejecting transform because too low complexity %f (%s=0)",
info.icpStructuralComplexity,
Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
UWARN(msg.c_str());
UWARN("%s", msg.c_str());
info.rejectedMsg = msg;
return Transform();
}
@@ -616,6 +729,13 @@ Transform RegistrationIcp::computeTransformationImpl(
PM::ICP & icp = *((PM::ICP*)_libpointmatcherICP);
UDEBUG("libpointmatcher icp... (if there is a seg fault here, make sure all third party libraries are built with same Eigen version.)");
T = icp(data, ref);
// CounterTransformationChecker (added first by parseParameters()) tracks
// the actual iteration count in its first condition variable.
if(!icp.transformationCheckers.empty())
{
const auto & vars = icp.transformationCheckers[0]->getConditionVariables();
if(vars.size() > 0) info.icpIterations = static_cast<int>(vars(0));
}
icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
UDEBUG("libpointmatcher icp...done! T=%s", icpT.prettyPrint().c_str());
@@ -644,11 +764,24 @@ Transform RegistrationIcp::computeTransformationImpl(
hasConverged,
*fromCloudNormalsRegistered,
_epsilon,
this->force3DoF());
this->force3DoF(),
_outlierRatio,
&info.icpIterations);
}
if(!icpT.isNull() && hasConverged)
{
// Strategy 3 with low complexity: project the
// PointToPlane correction onto the constrained
// eigenvectors (in-plane DoFs come from the guess).
if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy == 3)
{
icpT = constrainLowComplexityTransform(icpT, guess,
complexityVectors, secondEigenValue,
_pointToPlaneMinComplexity, info.icpStructuralComplexity);
fromCloudNormalsRegistered = util3d::transformPointCloud(fromCloudNormals, icpT);
}
util3d::computeVarianceAndCorrespondences(
fromCloudNormalsRegistered,
toCloudNormals,
@@ -725,10 +858,20 @@ Transform RegistrationIcp::computeTransformationImpl(
}
T = icpTmp(data, ref);
if(!icpTmp.transformationCheckers.empty())
{
const auto & vars = icpTmp.transformationCheckers[0]->getConditionVariables();
if(vars.size() > 0) info.icpIterations = static_cast<int>(vars(0));
}
}
else
{
T = icp(data, ref);
if(!icp.transformationCheckers.empty())
{
const auto & vars = icp.transformationCheckers[0]->getConditionVariables();
if(vars.size() > 0) info.icpIterations = static_cast<int>(vars(0));
}
}
UDEBUG("libpointmatcher icp...done!");
icpT = Transform::fromEigen3d(Eigen::Affine3d(Eigen::Matrix4d(eigenMatrixToDim<double>(T.template cast<double>(), 4))));
@@ -778,68 +921,32 @@ Transform RegistrationIcp::computeTransformationImpl(
hasConverged,
*fromCloudRegistered,
_epsilon,
this->force3DoF()); // icp2D
this->force3DoF(), // icp2D
_outlierRatio,
&info.icpIterations);
}
}
if(!icpT.isNull() && hasConverged)
{
if(tooLowComplexityForPlaneToPlane)
// PointToPoint branch is reached for strategies 1
// (default, legacy: recompute then project) and 2
// (accept as-is). Strategy 0 returned early; strategy
// 3 stays on the PointToPlane branch.
if(tooLowComplexityForPlaneToPlane && _pointToPlaneLowComplexityStrategy == 1)
{
if(_pointToPlaneLowComplexityStrategy == 1)
{
Transform guessInv = guess.inverse();
Transform t = guessInv * icpT.inverse() * guess;
Eigen::Vector3f v(t.x(), t.y(), t.z());
if(complexityVectors.cols == 2)
{
// limit translation in direction of the first eigen vector
Eigen::Vector3f n(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), 0.0f);
float a = v.dot(n);
Eigen::Vector3f vp = n*a;
UWARN("Normals low complexity (%f): Limiting translation from (%f,%f) to (%f,%f)",
info.icpStructuralComplexity,
v[0], v[1], vp[0], vp[1]);
v= vp;
}
else if(complexityVectors.rows == 3)
{
// limit translation in direction of the first and second eigen vectors
Eigen::Vector3f n1(complexityVectors.at<float>(0,0), complexityVectors.at<float>(0,1), complexityVectors.at<float>(0,2));
Eigen::Vector3f n2(complexityVectors.at<float>(1,0), complexityVectors.at<float>(1,1), complexityVectors.at<float>(1,2));
float a = v.dot(n1);
float b = v.dot(n2);
Eigen::Vector3f vp = n1*a;
if(secondEigenValue >= _pointToPlaneMinComplexity)
{
vp += n2*b;
}
UWARN("Normals low complexity: Limiting translation from (%f,%f,%f) to (%f,%f,%f)",
v[0], v[1], v[2], vp[0], vp[1], vp[2]);
v = vp;
}
else
{
UWARN("not supposed to be here!");
v = Eigen::Vector3f(0,0,0);
}
float roll, pitch, yaw;
t.getEulerAngles(roll, pitch, yaw);
t = Transform(v[0], v[1], v[2], roll, pitch, yaw);
icpT = guess * t.inverse() * guessInv;
fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
}
else
{
UWARN("Even if complexity is low (%f), PointToPoint transformation is accepted \"as is\" (%s=2)",
info.icpStructuralComplexity,
Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
if(fromCloudRegistered->empty())
fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
}
icpT = constrainLowComplexityTransform(icpT, guess,
complexityVectors, secondEigenValue,
_pointToPlaneMinComplexity, info.icpStructuralComplexity);
fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
}
else if(fromCloudRegistered->empty())
else if(tooLowComplexityForPlaneToPlane)
{
UWARN("Even if complexity is low (%f), PointToPoint transformation is accepted \"as is\" (%s=2)",
info.icpStructuralComplexity,
Parameters::kIcpPointToPlaneLowComplexityStrategy().c_str());
}
if(fromCloudRegistered->empty())
fromCloudRegistered = util3d::transformPointCloud(fromCloud, icpT);
util3d::computeVarianceAndCorrespondences(
@@ -871,7 +978,7 @@ Transform RegistrationIcp::computeTransformationImpl(
info.icpRotation,
_maxTranslation,
_maxRotation);
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
else
{
@@ -931,7 +1038,7 @@ Transform RegistrationIcp::computeTransformationImpl(
{
msg = uFormat("Cannot compute transform (cor=%d corrRatio=%f/%f maxLaserScans=%d)",
correspondences, correspondencesRatio, _correspondenceRatio, maxLaserScans);
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
else
{
@@ -966,19 +1073,19 @@ Transform RegistrationIcp::computeTransformationImpl(
msg = uFormat("Cannot compute transform (converged=%s var=%f)",
hasConverged?"true":"false", variance);
}
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
}
else
{
msg = "Laser scans empty ?!?";
UWARN(msg.c_str());
UWARN("%s", msg.c_str());
}
}
else if(!dataFrom.laserScanRaw().empty() && !dataTo.laserScanRaw().empty())
{
msg = "RegistrationIcp cannot do registration with a null guess.";
UERROR(msg.c_str());
UERROR("%s", msg.c_str());
}
corelib/src/RegistrationVis.cpp
+21 -18
View File
@@ -328,8 +328,8 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
UDEBUG("%s=%f", Parameters::kVisPnPVarianceMedianRatio().c_str(), _PnPVarMedianRatio);
UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), (double)_PnPSplitLinearCovarianceComponents);
UDEBUG("%s=%f", Parameters::kVisPnPVarianceMedianRatio().c_str(), (double)_PnPVarMedianRatio);
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -915,15 +915,15 @@ Transform RegistrationVis::computeTransformationImpl(
{
UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
"is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
kptsFrom.size(),
fromSignature.getWords3().size());
(int)kptsFrom.size(),
(int)fromSignature.getWords3().size());
}
else if(fromSignature.sensorData().keypoints3D().size() && kptsFrom.size() != fromSignature.sensorData().keypoints3D().size())
{
UWARN("kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
"is maybe a problem with the logic above (keypoints3D should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
kptsFrom.size(),
fromSignature.sensorData().keypoints3D().size());
(int)kptsFrom.size(),
(int)fromSignature.sensorData().keypoints3D().size());
}
kptsFrom3D = _detectorFrom->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size());
@@ -1653,26 +1653,26 @@ Transform RegistrationVis::computeTransformationImpl(
else
{
msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
}
else
{
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
}
else
{
msg = uFormat("No enough features < %s=%d (from=%d to=%d)",
Parameters::kVisMinInliers().c_str(), _minInliers, (int)fromSignature.getWords().size(), (int)toSignature.getWords().size());
UWARN(msg.c_str());
UWARN("%s", msg.c_str());
}
}
else if(_estimationType == 1) // PnP
@@ -1684,7 +1684,7 @@ Transform RegistrationVis::computeTransformationImpl(
UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
toSignature.id(),
(int)toSignature.sensorData().cameraModels().size(),
toSignature.sensorData().stereoCameraModels().size(),
(int)toSignature.sensorData().stereoCameraModels().size(),
toSignature.getWeight());
}
#ifndef RTABMAP_OPENGV
@@ -1803,7 +1803,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
(int)inliers.size(), _minInliers, (int)matches.size(), fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
else if(this->force3DoF())
{
@@ -1814,7 +1814,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)fromSignature.getWords3().size(), (int)toSignature.getWords().size(), _minInliers);
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
}
@@ -1857,7 +1857,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
(int)inliers.size(), _minInliers, (int)matches.size(), fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
else if(this->force3DoF())
{
@@ -1868,7 +1868,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)fromSignature.getWords3().size(), (int)toSignature.getWords3().size(), _minInliers);
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
}
}
@@ -1882,7 +1882,10 @@ Transform RegistrationVis::computeTransformationImpl(
(toSignature.sensorData().stereoCameraModels().size() >= 1 || toSignature.sensorData().cameraModels().size() >= 1))
{
UDEBUG("Refine with bundle adjustment");
Optimizer * sba = Optimizer::create(_bundleAdjustment==3?Optimizer::kTypeCeres:_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
// _bundleAdjustment matches the Optimizer/Strategy parameter 1:1
// (1=g2o, 2=GTSAM, 3=Ceres, 4=cvsba); 0 was filtered out above.
Optimizer * sba = Optimizer::create(
static_cast<Optimizer::Type>(_bundleAdjustment), _bundleParameters);
std::map<int, Transform> poses;
std::multimap<int, Link> links;
@@ -2056,7 +2059,7 @@ Transform RegistrationVis::computeTransformationImpl(
if((int)inliers.size() < _minInliers)
{
msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
(int)inliers.size(), _minInliers, (int)inliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
(int)inliers.size(), _minInliers, (int)((int)inliers.size()+sbaOutliers.size()), fromSignature.id(), toSignature.id());
transform.setNull();
}
else
@@ -2165,7 +2168,7 @@ Transform RegistrationVis::computeTransformationImpl(
if(info.inliersMeanDistance > _maxInliersMeanDistance)
{
msg = uFormat("The mean distance of the inliers is over %s threshold (%f)",
msg = uFormat("The mean distance of the inliers (%f) is over %s threshold (%f)",
info.inliersMeanDistance, Parameters::kVisMeanInliersDistance().c_str(), _maxInliersMeanDistance);
transform.setNull();
}
corelib/src/Rtabmap.cpp
+91 -62
View File
@@ -182,10 +182,13 @@ Rtabmap::Rtabmap() :
_pathStuckCount(0),
_pathStuckDistance(0.0f),
_dummyDictionary(false)
#ifdef RTABMAP_PYTHON
,_python(new PythonInterface())
#endif
{
#ifdef RTABMAP_PYTHON
// Ensure the embedded Python interpreter is up. The first call here will
// assert that it runs on the main thread; callers building Rtabmap on a
// worker thread should construct the singleton in main() beforehand.
PythonInterface::instance("Rtabmap");
#endif
}
Rtabmap::~Rtabmap() {
@@ -303,7 +306,7 @@ void Rtabmap::flushStatisticLogs()
{
if(_foutFloat && _bufferedLogsF.size())
{
UDEBUG("_bufferedLogsF.size=%d", _bufferedLogsF.size());
UDEBUG("_bufferedLogsF.size=%d", (int)_bufferedLogsF.size());
for(std::list<std::string>::iterator iter = _bufferedLogsF.begin(); iter!=_bufferedLogsF.end(); ++iter)
{
fprintf(_foutFloat, "%s", iter->c_str());
@@ -312,7 +315,7 @@ void Rtabmap::flushStatisticLogs()
}
if(_foutInt && _bufferedLogsI.size())
{
UDEBUG("_bufferedLogsI.size=%d", _bufferedLogsI.size());
UDEBUG("_bufferedLogsI.size=%d", (int)_bufferedLogsI.size());
for(std::list<std::string>::iterator iter = _bufferedLogsI.begin(); iter!=_bufferedLogsI.end(); ++iter)
{
fprintf(_foutInt, "%s", iter->c_str());
@@ -400,7 +403,7 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
_lastLocalizationPose = lastPose;
UINFO("Loaded optimizedPoses=%d firstPose %d=%s lastLocalizationPose=%s",
_optimizedPoses.size(),
(int)_optimizedPoses.size(),
_optimizedPoses.lower_bound(1)->first,
_optimizedPoses.lower_bound(1)->second.prettyPrint().c_str(),
_lastLocalizationPose.prettyPrint().c_str());
@@ -1317,7 +1320,7 @@ bool Rtabmap::process(
odomPose.r21(), odomPose.r22(), odomPose.r23(), odomPose.o24(),
odomPose.r31(), odomPose.r32(), odomPose.r33(), odomPose.o34());
odomPose.normalizeRotation();
UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible!\n"
UASSERT_MSG(odomPose.isInvertible(), uFormat("Odometry pose is not invertible! %s\n"
"[%f %f %f %f;\n"
" %f %f %f %f;\n"
" %f %f %f %f;\n"
@@ -2399,7 +2402,7 @@ bool Rtabmap::process(
"nbDirectNeighborsInDb=%d, "
"time=%fs (%fs %fs)",
neighborhoodSize,
reactivatedIds.size(),
(int)reactivatedIds.size(),
(int)nbLoadedFromDb,
nbDirectNeighborsInDb,
timeGetN.ticks(),
@@ -4439,8 +4442,13 @@ bool Rtabmap::process(
}
if(!_publishLastSignatureData)
{
// Keep the occupancy grid (compressed AND raw) on the published copy:
// downstream consumers rely on it to generate global occupancy grid
// in the same process (e.g. the raw occupancy grid used by the MainWindow
// or ROS rtabmap_slam) or on an external process (the compressed occupancy
// grid published over ROS rtabmap_msgs/MapData).
lastSignatureData.sensorData().clearCompressedData(true, true, true, false);
lastSignatureData.sensorData().clearRawData();
lastSignatureData.sensorData().clearRawData(true, true, true, false);
}
if(!_rawDataKept)
{
@@ -4573,7 +4581,7 @@ bool Rtabmap::process(
if(_maxMemoryAllowed != 0 && workingMemSize > _maxMemoryAllowed)
{
ULOGGER_INFO("Removing old signatures because memory limit is reached %d > %d...",
workingMemSize, _maxMemoryAllowed);
(int)workingMemSize, _maxMemoryAllowed);
}
immunizedLocations.insert(_lastLocalizationNodeId); // keep the latest localization in working memory
std::list<int> transferred = _memory->forget(immunizedLocations);
@@ -5019,7 +5027,14 @@ void Rtabmap::setMemoryThreshold(int maxMemoryAllowed)
void Rtabmap::setWorkingDirectory(std::string path)
{
path = uReplaceChar(path, '~', UDirectory::homeDir());
// Expand leading "~" to the user's home directory (shell convention).
// We do NOT replace every "~" in the path -- Windows 8.3 short names
// embed "~" in the middle (e.g. C:\Users\RUNNER~1\...), and a blanket
// uReplaceChar would corrupt those.
if(!path.empty() && path[0] == '~')
{
path = UDirectory::homeDir() + path.substr(1);
}
if(!path.empty() && UDirectory::exists(path))
{
ULOGGER_DEBUG("Comparing new working directory path \"%s\" with \"%s\"", path.c_str(), _wDir.c_str());
@@ -5695,7 +5710,7 @@ std::list<std::pair<int, int> > Rtabmap::repairGraph(
void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
{
ULOGGER_DEBUG("likelihood.size()=%d", likelihood.size());
ULOGGER_DEBUG("likelihood.size()=%d", (int)likelihood.size());
UTimer timer;
timer.start();
if(likelihood.size()==0)
@@ -5714,7 +5729,7 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
values.push_back(iter->second);
}
}
UDEBUG("values.size=%d", values.size());
UDEBUG("values.size=%d", (int)values.size());
float mean = uMean(values);
float stdDev = std::sqrt(uVariance(values, mean));
@@ -6335,11 +6350,11 @@ int Rtabmap::detectMoreLoopClosures(
links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, inf)));
loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, inf));
std::string msg = uFormat("Iteration %d/%d: Added loop closure %d->%d! (%d/%d)", n+1, iterations, from, to, i+1, (int)clusters.size());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(processState)
{
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(!processState->callback(msg))
{
return -1;
@@ -6356,7 +6371,7 @@ int Rtabmap::detectMoreLoopClosures(
if(processState)
{
std::string msg = uFormat("Iteration %d/%d: Detected %d total loop closures!", n+1, iterations, (int)addedLinks.size()/2);
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(!processState->callback(msg))
{
return -1;
@@ -6419,17 +6434,17 @@ bool Rtabmap::globalBundleAdjustment(
if(!_optimizedPoses.empty() && !_constraints.empty())
{
int iterations = Parameters::defaultOptimizerIterations();
float pixelVariance = Parameters::defaultg2oPixelVariance();
float pixelVariance = Parameters::defaultOptimizerPixelVariance();
ParametersMap params = _parameters;
Parameters::parse(params, Parameters::kOptimizerIterations(), iterations);
Parameters::parse(params, Parameters::kg2oPixelVariance(), pixelVariance);
Parameters::parse(params, Parameters::kOptimizerPixelVariance(), pixelVariance);
if(iterations > 0)
{
uInsert(params, ParametersPair(Parameters::kOptimizerIterations(), uNumber2Str(iterations)));
}
if(pixelVariance > 0.0f)
{
uInsert(params, ParametersPair(Parameters::kg2oPixelVariance(), uNumber2Str(pixelVariance)));
uInsert(params, ParametersPair(Parameters::kOptimizerPixelVariance(), uNumber2Str(pixelVariance)));
}
std::map<int, Signature> signatures;
@@ -6941,13 +6956,13 @@ void Rtabmap::addNodesToRepublish(const std::vector<int> & ids)
void Rtabmap::setDummyDictionary(bool enabled)
{
if(_memory) {
if(_memory && enabled) {
UERROR("Memory is already initialized, cannot set dummy dictionary. This "
"function can only be called after Rtabmap object is created, but "
"before init() is called.");
}
else {
_dummyDictionary = true;
_dummyDictionary = enabled;
}
}
@@ -7041,6 +7056,33 @@ bool Rtabmap::computePath(int targetNode, bool global)
{
if(iter->first > 0)
{
// Skip intermediate nodes (weight==-1). They are not navigable
// waypoints and updateGoalIndex would otherwise abort the
// plan when it sees them. The poses of the remaining real
// nodes already account for cumulative transform through any
// intermediate chain (relative poses from graph::computePath).
int weight = 0;
const Signature * s = _memory->getSignature(iter->first);
if(s)
{
weight = s->getWeight();
}
else
{
// For nodes in LTM, fetch weight from the database.
Transform p, gt;
int mapId = 0;
std::string label;
double stamp = 0.0;
std::vector<float> vel;
GPS gps;
EnvSensors envs;
_memory->getNodeInfo(iter->first, p, mapId, weight, label, stamp, gt, vel, gps, envs, true);
}
if(weight == -1)
{
continue;
}
// just keep nodes in the path
_path[oi].first = iter->first;
_path[oi++].second = t * iter->second;
@@ -7314,18 +7356,14 @@ void Rtabmap::updateGoalIndex()
if( _memory && _path.size())
{
// remove all previous virtual links
bool hasIntermediateNodes = false;
for(unsigned int i=0; i<_pathCurrentIndex && i<_path.size(); ++i)
{
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
{
UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str());
_memory->removeVirtualLinks(s->id());
}
if(s->getWeight() == -1)
{
hasIntermediateNodes = true;
}
}
// for the current index, only keep the newest virtual link
@@ -7351,51 +7389,42 @@ void Rtabmap::updateGoalIndex()
}
}
// Make sure the next signatures on the path are linked together
// Make sure the next signatures on the path are linked together.
// Intermediate nodes have been filtered out of _path by computePath, so
// every entry is a real node here.
float distanceSoFar = 0.0f;
for(unsigned int i=_pathCurrentIndex+1;
i<_path.size() && !hasIntermediateNodes;
++i)
for(unsigned int i=_pathCurrentIndex+1; i<_path.size(); ++i)
{
if(i>0)
if(_localRadius > 0.0f)
{
if(_localRadius > 0.0f)
distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
}
if(_path[i].first != _path[i-1].first)
{
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
{
distanceSoFar += _path[i-1].second.getDistance(_path[i].second);
}
if(_path[i].first != _path[i-1].first)
{
const Signature * s = _memory->getSignature(_path[i].first);
if(s)
UASSERT_MSG(s->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i, _path[i].first).c_str());
const Signature * sPrev = _memory->getSignature(_path[i-1].first);
if(sPrev)
{
if(s->getWeight() == -1)
{
hasIntermediateNodes = true;
break;
}
if(!s->hasLink(_path[i-1].first) && _memory->getSignature(_path[i-1].first) != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
UASSERT_MSG(sPrev->getWeight() != -1, uFormat("path[%u] id=%d is intermediate; computePath should have filtered it", i-1, _path[i-1].first).c_str());
}
if(!s->hasLink(_path[i-1].first) && sPrev != 0)
{
Transform virtualLoop = _path[i].second.inverse() * _path[i-1].second;
_memory->addLink(Link(_path[i].first, _path[i-1].first, Link::kVirtualClosure, virtualLoop, cv::Mat::eye(6,6,CV_64FC1)*0.01)); // on the optimized path
UINFO("Added Virtual link between %d and %d", _path[i-1].first, _path[i].first);
}
}
if(distanceSoFar > _localRadius)
{
UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
break;
}
}
}
if(hasIntermediateNodes)
{
UERROR("Cannot follow a path with a map containing intermediate nodes (not supported: don't use intermediate nodes if rtabmap's planner has to be used). Aborting current plan!");
this->clearPath(-1);
return;
if(distanceSoFar > _localRadius)
{
UDEBUG("Farthest goal=%d : %f m", _path[i].first, distanceSoFar);
break;
}
}
UDEBUG("current node = %d current goal = %d", _path[_pathCurrentIndex].first, _path[_pathGoalIndex].first);
corelib/src/SensorData.cpp
+57 -11
View File
@@ -36,6 +36,23 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap
{
namespace {
bool isOccupancyGridLayerFormatSupported(const cv::Mat & layer)
{
if(layer.empty())
{
return true;
}
return layer.type() == CV_32FC2 ||
layer.type() == CV_32FC3 ||
layer.type() == CV_32FC(4) ||
layer.type() == CV_32FC(5) ||
layer.type() == CV_32FC(6) ||
layer.type() == CV_32FC(7) ||
(layer.type() == CV_8UC1 && layer.rows == 1);
}
} // namespace
// empty constructor
SensorData::SensorData() :
_id(0),
@@ -89,6 +106,23 @@ SensorData::SensorData(
setUserData(userData);
}
// RGB-D constructor + Depth confidence
SensorData::SensorData(
const cv::Mat & rgb,
const cv::Mat & depth,
const cv::Mat & depth_confidence,
const CameraModel & cameraModel,
int id,
double stamp,
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_cellSize(0.0f)
{
setRGBDImage(rgb, depth, depth_confidence, cameraModel);
setUserData(userData);
}
// RGB-D constructor + laser scan
SensorData::SensorData(
const LaserScan & laserScan,
@@ -553,7 +587,7 @@ void SensorData::setOccupancyGrid(
(!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
(!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
{
UWARN("Occupancy grid cannot be overwritten! id=%d, Set occupancy grid of %d to null "
UWARN("Occupancy grid cannot be overwritten! Set occupancy grid of %d to null "
"before setting a new one.", this->id());
return;
}
@@ -565,6 +599,19 @@ void SensorData::setOccupancyGrid(
_emptyCellsRaw = cv::Mat();
_emptyCellsCompressed = cv::Mat();
if(!ground.empty() && !isOccupancyGridLayerFormatSupported(ground))
{
UFATAL("Unsupported local occupancy grid format for ground cells: OpenCV type=%d size=%dx%d", ground.type(), ground.cols, ground.rows);
}
if(!obstacles.empty() && !isOccupancyGridLayerFormatSupported(obstacles))
{
UFATAL("Unsupported local occupancy grid format for obstacle cells: OpenCV type=%d size=%dx%d", obstacles.type(), obstacles.cols, obstacles.rows);
}
if(!empty.empty() && !isOccupancyGridLayerFormatSupported(empty))
{
UFATAL("Unsupported local occupancy grid format for empty cells: OpenCV type=%d size=%dx%d", empty.type(), empty.cols, empty.rows);
}
CompressionThread ctGround(ground);
CompressionThread ctObstacles(obstacles);
CompressionThread ctEmpty(empty);
@@ -576,7 +623,7 @@ void SensorData::setOccupancyGrid(
_groundCellsRaw = ground;
ctGround.start();
}
else if(ground.type() == CV_8UC1)
else // CV_8UC1 && rows == 1
{
_groundCellsCompressed = ground;
}
@@ -588,7 +635,7 @@ void SensorData::setOccupancyGrid(
_obstacleCellsRaw = obstacles;
ctObstacles.start();
}
else if(obstacles.type() == CV_8UC1)
else // CV_8UC1 && rows == 1
{
_obstacleCellsCompressed = obstacles;
}
@@ -600,7 +647,7 @@ void SensorData::setOccupancyGrid(
_emptyCellsRaw = empty;
ctEmpty.start();
}
else if(empty.type() == CV_8UC1)
else // CV_8UC1 && rows == 1
{
_emptyCellsCompressed = empty;
}
@@ -1060,7 +1107,7 @@ void SensorData::clearRawData(bool images, bool scan, bool userData, bool occupa
}
bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
int SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
{
if(_cameraModels.size() >= 1)
{
@@ -1071,13 +1118,12 @@ bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _cameraModels[i].localTransform().inverse());
if(ptInCameraFrame.z > 0.0f)
{
int borderWidth = int(float(_cameraModels[i].imageWidth())* 0.2);
int u, v;
_cameraModels[i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
if(uIsInBounds(u, borderWidth, _cameraModels[i].imageWidth()-2*borderWidth) &&
uIsInBounds(v, borderWidth, _cameraModels[i].imageHeight()-2*borderWidth))
if(uIsInBounds(u, 0, _cameraModels[i].imageWidth()) &&
uIsInBounds(v, 0, _cameraModels[i].imageHeight()))
{
return true;
return i;
}
}
}
@@ -1097,7 +1143,7 @@ bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
if(uIsInBounds(u, 0, _stereoCameraModels[i].left().imageWidth()) &&
uIsInBounds(v, 0, _stereoCameraModels[i].left().imageHeight()))
{
return true;
return i;
}
}
}
@@ -1107,7 +1153,7 @@ bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
{
UERROR("no valid camera model!");
}
return false;
return -1;
}
} // namespace rtabmap
corelib/src/Stereo.cpp
+14
View File
@@ -79,8 +79,15 @@ std::vector<cv::Point2f> Stereo::computeCorrespondences(
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const
{
if(leftCorners.empty())
{
status.clear();
return std::vector<cv::Point2f>();
}
UASSERT(!leftImage.empty() && !rightImage.empty());
UASSERT(leftImage.type() == CV_8UC1);
UASSERT(rightImage.type() == CV_8UC1);
UASSERT(leftImage.size() == rightImage.size());
std::vector<cv::Point2f> rightCorners;
UDEBUG("util2d::calcStereoCorrespondences() begin");
rightCorners = util2d::calcStereoCorrespondences(
@@ -153,8 +160,15 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
const std::vector<cv::Point2f> & leftCorners,
std::vector<unsigned char> & status) const
{
if(leftCorners.empty())
{
status.clear();
return std::vector<cv::Point2f>();
}
UASSERT(!leftImage.empty() && !rightImage.empty());
UASSERT(leftImage.type() == CV_8UC1);
UASSERT(rightImage.type() == CV_8UC1);
UASSERT(leftImage.size() == rightImage.size());
std::vector<cv::Point2f> rightCorners;
std::vector<float> err;
#ifdef HAVE_OPENCV_CUDAOPTFLOW
corelib/src/StereoCameraModel.cpp
+1 -1
View File
@@ -256,7 +256,7 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
n = fs["camera_name"];
if(n.type() != cv::FileNode::NONE)
{
name_ = (int)n;
name_ = n.string();
}
else
{
corelib/src/Transform.cpp
+15 -15
View File
@@ -555,35 +555,35 @@ Transform Transform::getTransform(
const double & stamp)
{
UASSERT(!tfBuffer.empty());
std::map<double, Transform>::const_iterator imuIterB = tfBuffer.lower_bound(stamp);
std::map<double, Transform>::const_iterator imuIterA = imuIterB;
if(imuIterA != tfBuffer.begin())
std::map<double, Transform>::const_iterator iterB = tfBuffer.lower_bound(stamp);
std::map<double, Transform>::const_iterator iterA = iterB;
if(iterA != tfBuffer.begin())
{
imuIterA = --imuIterA;
iterA = --iterA;
}
if(imuIterB == tfBuffer.end())
if(iterB == tfBuffer.end())
{
imuIterB = --imuIterB;
iterB = --iterB;
}
Transform imuT;
if(imuIterB->first == stamp)
Transform t;
if(iterB->first == stamp)
{
imuT = imuIterB->second;
t = iterB->second;
}
else if(imuIterA != imuIterB)
else if(iterA != iterB)
{
//interpolate:
imuT = imuIterA->second.interpolate((stamp-imuIterA->first) / (imuIterB->first-imuIterA->first), imuIterB->second);
t = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
}
else if(stamp > imuIterB->first)
else if(stamp > iterB->first)
{
UWARN("No transform found for stamp %f! Latest is %f", stamp, imuIterB->first);
UWARN("No transform found for stamp %f! Latest is %f", stamp, iterB->first);
}
else
{
UWARN("No transform found for stamp %f! Earliest is %f", stamp, imuIterA->first);
UWARN("No transform found for stamp %f! Earliest is %f", stamp, iterA->first);
}
return imuT;
return t;
}
Transform Transform::getClosestTransform(
corelib/src/VWDictionary.cpp
+43 -46
View File
@@ -138,7 +138,7 @@ void VWDictionary::setIncrementalDictionary()
_incrementalDictionary = true;
if(_visualWords.size())
{
UWARN("Incremental dictionary set: already loaded visual words (%d) from the fixed dictionary will be included in the incremental one.", _visualWords.size());
UWARN("Incremental dictionary set: already loaded visual words (%d) from the fixed dictionary will be included in the incremental one.", (int)_visualWords.size());
}
}
_dictionaryPath = "";
@@ -259,7 +259,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
if(_visualWords.size() == 0)
{
_incrementalDictionary = _visualWords.size()==0;
UWARN("No words loaded, cannot set a fixed dictionary.", (int)_visualWords.size());
UWARN("No words loaded, cannot set a fixed dictionary.");
}
else
{
@@ -276,7 +276,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
}
else
{
UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", _visualWords.size());
UERROR("Cannot change to a fixed dictionary if there are already words (%d) in the incremental one.", (int)_visualWords.size());
}
}
else if(_incrementalDictionary && _visualWords.size())
@@ -300,7 +300,7 @@ bool VWDictionary::setNNStrategy(NNStrategy strategy)
{
#if CV_MAJOR_VERSION < 3
#ifdef HAVE_OPENCV_GPU
if(strategy == kNNBruteForceGPU && !cv::gpu::getCudaEnabledDeviceCount())
if(strategy == kNNBruteForceGPU && cv::gpu::getCudaEnabledDeviceCount() <= 0)
{
UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
strategy = kNNBruteForce;
@@ -314,7 +314,7 @@ bool VWDictionary::setNNStrategy(NNStrategy strategy)
#endif
#else
#ifdef HAVE_OPENCV_CUDAFEATURES2D
if(strategy == kNNBruteForceGPU && !cv::cuda::getCudaEnabledDeviceCount())
if(strategy == kNNBruteForceGPU && cv::cuda::getCudaEnabledDeviceCount() <= 0)
{
UERROR("Nearest neighobr strategy \"kNNBruteForceGPU\" chosen but no CUDA devices found! Doing \"kNNBruteForce\" instead.");
strategy = kNNBruteForce;
@@ -330,7 +330,7 @@ bool VWDictionary::setNNStrategy(NNStrategy strategy)
if(strategy>=kNNUndef)
{
UERROR("Nearest neighobr strategy \"%d\" chosen but this strategy cannot be used with a dictionary! Doing \"kNNBruteForce\" instead.");
UERROR("Nearest neighbor strategy \"%d\" chosen but this strategy cannot be used with a dictionary! Doing \"kNNBruteForce\" instead.", (int)strategy);
strategy = kNNBruteForce;
}
@@ -377,7 +377,10 @@ unsigned long VWDictionary::getMemoryUsed() const
{
long memoryUsage = sizeof(VWDictionary);
memoryUsage += getIndexMemoryUsed();
memoryUsage += _dataTree.total()*_dataTree.elemSize();
if(!_dataTree.empty())
{
memoryUsage += _dataTree.total()*_dataTree.elemSize();
}
if(!_visualWords.empty())
{
memoryUsage += _visualWords.size()*(sizeof(int) + _visualWords.rbegin()->second->getMemoryUsed() + sizeof(std::map<int, VisualWord *>::iterator)) + sizeof(std::map<int, VisualWord *>);
@@ -516,7 +519,7 @@ void VWDictionary::update()
{
UTimer timer;
timer.start();
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words...", (int)_notIndexedWords.size(), _byteToFloat?"true":"false");
ULOGGER_DEBUG("Incremental FLANN: Inserting %d words (byteToFloat=%s)...", (int)_notIndexedWords.size(), _byteToFloat?"true":"false");
for(std::set<int>::iterator iter=_notIndexedWords.begin(); iter!=_notIndexedWords.end(); ++iter)
{
VisualWord* w = uValue(_visualWords, *iter, (VisualWord*)0);
@@ -675,21 +678,24 @@ void VWDictionary::update()
_mapIdIndex.insert(_mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
}
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), dim);
ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",(int)_mapIndexId.size(), (int)_visualWords.size(), dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
_flannIndex->buildIndex(
_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
_dataTree,
useDistanceL1_,
_incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
if(_strategy < kNNBruteForce)
{
_flannIndex->buildIndex(
_strategy == kNNFlannNaive ? FlannIndex::FLANN_INDEX_LINEAR:
_strategy == kNNFlannLSH ? FlannIndex::FLANN_INDEX_LSH:
FlannIndex::FLANN_INDEX_KDTREE, // kNNFlannKdTree
_dataTree,
useDistanceL1_,
_incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
}
}
}
UDEBUG("Dictionary updated! (size=%d added=%d removed=%d)",
_dataTree.rows, _notIndexedWords.size(), _removedIndexedWords.size());
_dataTree.rows, (int)_notIndexedWords.size(), (int)_removedIndexedWords.size());
}
else
{
@@ -714,38 +720,38 @@ std::vector<unsigned char> VWDictionary::serializeIndex() const
return _flannIndex->serializeIndex(_serializeWithChecksum);
}
void VWDictionary::deserializeIndex(const std::vector<unsigned char> & data)
bool VWDictionary::deserializeIndex(const std::vector<unsigned char> & data)
{
deserializeIndex(data.data(), data.size());
return deserializeIndex(data.data(), data.size());
}
void VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
bool VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
{
if(data== NULL || size == 0)
{
UWARN("Trying to deserialize empty data, aborting.");
return;
return false;
}
UDEBUG("Loading flann index... (data size=%ld bytes)", size);
if(_strategy >= kNNBruteForce) {
//ignore
return;
return false;
}
if(_flannIndex->isBuilt()) {
UERROR("Flann index is already built, cannot deserialize data!");
return;
return false;
}
if(_visualWords.empty()) {
UERROR("Descriptors should be added before deserializing flann index! See VWDictionary::addWord()");
return;
return false;
}
if(!(_removedIndexedWords.empty() && _visualWords.size() == _notIndexedWords.size())) {
UERROR("State of dictionary not as expected before deserializing. (removed words=%ld, words=%ld, not indexed=%ld)",
_removedIndexedWords.size(), _visualWords.size(), _notIndexedWords.size());
return;
return false;
}
std::map<int, int> mapIndexId;
@@ -811,7 +817,7 @@ void VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
mapIdIndex.insert(mapIdIndex.end(), std::pair<int, int>(iter->second->id(), i));
}
ULOGGER_DEBUG("mapIndexId.size() = %d, words.size()=%d, dim=%d", mapIndexId.size(), _visualWords.size(), dim);
ULOGGER_DEBUG("mapIndexId.size() = %d, words.size()=%d, dim=%d", (int)mapIndexId.size(), (int)_visualWords.size(), dim);
ULOGGER_DEBUG("copying data = %f s", timer.ticks());
std::string errorMsg;
@@ -835,9 +841,11 @@ void VWDictionary::deserializeIndex(const unsigned char * data, size_t size)
else {
UWARN("Failed deserializing flann index data (error: %s), the index will be rebuilt on next update.", errorMsg.c_str());
_flannIndex->release(); // reset to initial state
return false;
}
ULOGGER_DEBUG("Time to load flann index = %f s", timer.ticks());
return true;
}
void VWDictionary::clear(bool printWarningsIfNotEmpty)
@@ -1094,14 +1102,9 @@ std::list<int> VWDictionary::addNewWords(
for(int j=0; j<dists.cols; ++j)
{
float d = dists.at<float>(i,j);
int index;
if (sizeof(size_t) == 8)
{
index = *((size_t*)&results.at<double>(i, j));
}
else
{
index = *((size_t*)&results.at<int>(i, j));
int index = results.at<int>(i, j);
if(index<0) {
continue;
}
int id = uValue(_mapIndexId, index);
if(d >= 0.0f && id != 0)
@@ -1219,7 +1222,7 @@ std::list<int> VWDictionary::addNewWords(
}
ULOGGER_DEBUG("naive search and add ref/words time = %f s", timerLocal.ticks());
ULOGGER_DEBUG("%d new words added...", _notIndexedWords.size());
ULOGGER_DEBUG("%d new words added...", (int)_notIndexedWords.size());
ULOGGER_DEBUG("%d duplicated words added (from current image = %d)...",
dupWordsCountFromDict+dupWordsCountFromLast, dupWordsCountFromLast);
UDEBUG("total time %fs", timer.ticks());
@@ -1459,15 +1462,9 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
for(int j=0; j<dists.cols; ++j)
{
float d = dists.at<float>(i,j);
int index;
if (sizeof(size_t) == 8)
{
index = *((size_t*)&results.at<double>(i, j));
}
else
{
index = *((size_t*)&results.at<int>(i, j));
int index = results.at<int>(i, j);
if(index < 0) {
continue;
}
int id = uValue(_mapIndexId, index);
if(d >= 0.0f && id != 0)
@@ -1656,7 +1653,7 @@ void VWDictionary::exportDictionary(const char * fileNameReferences, const char
}
}
UDEBUG("Export %d words...", _visualWords.size());
UDEBUG("Export %d words...", (int)_visualWords.size());
for(std::map<int, VisualWord *>::const_iterator iter=_visualWords.begin(); iter!=_visualWords.end(); ++iter)
{
// References
corelib/src/VisualWord.cpp
-1
View File
@@ -73,7 +73,6 @@ unsigned long VisualWord::getMemoryUsed() const
{
unsigned long memoryUsage = sizeof(VisualWord);
memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::map<int ,int>::iterator)) + sizeof(std::map<int ,int>);
memoryUsage += _descriptor.total() * _descriptor.elemSize();
return memoryUsage;
}
corelib/src/camera/CameraImages.cpp
+1 -1
View File
@@ -1077,7 +1077,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
img = cv::imread(imageFilePath.c_str(), -1);
#endif
UDEBUG("width=%d, height=%d, channels=%d, elementSize=%d, total=%d",
img.cols, img.rows, img.channels(), img.elemSize(), img.total());
img.cols, img.rows, img.channels(), (int)img.elemSize(), (int)img.total());
if(_isDepth)
{
corelib/src/camera/CameraK4A.cpp
+1 -1
View File
@@ -158,7 +158,7 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
}
uint64_t recording_length = k4a_playback_get_recording_length_usec((k4a_playback_t)playbackHandle_);
UINFO("Recording is %lld seconds long", recording_length / 1000000);
UINFO("Recording is %lld seconds long", (long long)(recording_length / 1000000));
k4a_record_configuration_t config;
if(k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
corelib/src/camera/CameraRealSense2.cpp
+2 -2
View File
@@ -625,11 +625,11 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
{
if (closing_)
{
UDEBUG("The device %d has been disconnected!", i);
UDEBUG("The device %d has been disconnected!", (int)i);
}
else
{
UERROR("The device %d has been disconnected!", i);
UERROR("The device %d has been disconnected!", (int)i);
}
}
}
corelib/src/camera/CameraStereoZed.cpp
+2 -2
View File
@@ -582,7 +582,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
}
#else
UINFO("Init ZED: Mode=%d Unit=%d CoordinateSystem=%d Verbose=false device=-1 minDist=-1 self-calibration=%s vflip=false",
quality_, sl::UNIT::METER, sl::COORDINATE_SYSTEM::IMAGE , selfCalibration_?"true":"false");
quality_, (int)sl::UNIT::METER, (int)sl::COORDINATE_SYSTEM::IMAGE, selfCalibration_?"true":"false");
#endif
@@ -943,7 +943,7 @@ SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
#if ZED_SDK_MAJOR_VERSION >=3
if(pose.timestamp != timestamp)
{
UWARN("Pose retrieve doesn't have same stamp (%ld) than grabbed image (%ld)", pose.timestamp, timestamp);
UWARN("Pose retrieve doesn't have same stamp (%ld) than grabbed image (%ld)", (long)pose.timestamp, (long)timestamp);
}
#endif
corelib/src/clams/discrete_depth_distortion_model.cpp
+6 -6
View File
@@ -150,10 +150,10 @@ namespace clams
UDEBUG("Frustum: max_dist=%f", max_dist_);
UDEBUG("Frustum: num_bins=%d", num_bins_);
UDEBUG("Frustum: bin_depth=%f", bin_depth_);
UDEBUG("Frustum: counts=%d", counts_.rows());
UDEBUG("Frustum: total_numerators=%d", total_numerators_.rows());
UDEBUG("Frustum: total_denominators=%d", total_denominators_.rows());
UDEBUG("Frustum: multipliers=%d", multipliers_.rows());
UDEBUG("Frustum: counts=%d", (int)counts_.rows());
UDEBUG("Frustum: total_numerators=%d", (int)total_numerators_.rows());
UDEBUG("Frustum: total_denominators=%d", (int)total_denominators_.rows());
UDEBUG("Frustum: multipliers=%d", (int)multipliers_.rows());
}
std::set<size_t> DiscreteDepthDistortionModel::getDivisors(const size_t &num)
@@ -443,13 +443,13 @@ namespace clams
UINFO("Distortion Model: bin_depth=%f", bin_depth_);
UINFO("Distortion Model: num_bins_x=%d", num_bins_x_);
UINFO("Distortion Model: num_bins_y=%d", num_bins_y_);
UINFO("Distortion Model: training_samples=%d", training_samples_);
UINFO("Distortion Model: training_samples=%d", (int)training_samples_);
deleteFrustums();
frustums_.resize(num_bins_y_);
for(size_t y = 0; y < frustums_.size(); ++y) {
frustums_[y].resize(num_bins_x_, NULL);
for(size_t x = 0; x < frustums_[y].size(); ++x) {
UDEBUG("Distortion Model: Frustum[%d][%d]", y, x);
UDEBUG("Distortion Model: Frustum[%d][%d]", (int)y, (int)x);
frustums_[y][x] = new DiscreteFrustum;
frustums_[y][x]->deserialize(in, ascii);
}
corelib/src/global_map/OctoMap.cpp
+1 -1
View File
@@ -506,7 +506,7 @@ void OctoMap::assemble(const std::list<std::pair<int, Transform> > & newPoses)
octomap::OcTreeKey tmpKey;
if (!octree_->coordToKeyChecked(sensorOrigin, tmpKey))
{
UERROR("Could not generate Key for origin ", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
UERROR("Could not generate Key for origin (%f,%f,%f)", sensorOrigin.x(), sensorOrigin.y(), sensorOrigin.z());
}
bool computeRays = rayTracing_ && emptyCells.empty();
corelib/src/icp/cccorelib.h
+13 -4
View File
@@ -125,10 +125,19 @@ rtabmap::Transform icpCC(
*finalRMS = (float)finalError;
}
if(result != 1)
if(result == CCCoreLib::ICPRegistrationTools::ICP_NOTHING_TO_DO)
{
// CCCoreLib's "nothing to do" outcome means the clouds were already
// aligned (no transform needed) -- semantically identity, not a
// failure. Return identity instead of null so callers don't have to
// treat trivial alignment as a rejection.
UDEBUG("CCCoreLib: ICP_NOTHING_TO_DO -- returning identity");
return rtabmap::Transform::getIdentity();
}
if(result != CCCoreLib::ICPRegistrationTools::ICP_APPLY_TRANSFO)
{
std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as result %d !=1", result);
UDEBUG(msg.c_str());
UDEBUG("%s", msg.c_str());
if(errorMsg)
{
*errorMsg = msg;
@@ -140,7 +149,7 @@ rtabmap::Transform icpCC(
else if(!transform.R.isValid())
{
std::string msg = uFormat("CCCoreLib has failed: Rotation matrix is invalid");
UDEBUG(msg.c_str());
UDEBUG("%s", msg.c_str());
if(errorMsg)
{
*errorMsg = msg;
@@ -170,7 +179,7 @@ rtabmap::Transform icpCC(
if(finalError > maxFinalRMS)
{
std::string msg = uFormat("CCCoreLib has failed: Rejecting transform as RMS %f > %f (%s) ", finalError, maxFinalRMS, rtabmap::Parameters::kIcpCCMaxFinalRMS().c_str());
UDEBUG(msg.c_str());
UDEBUG("%s", msg.c_str());
if(errorMsg)
{
*errorMsg = msg;
corelib/src/icp/libpointmatcher.h
+71 -24
View File
@@ -35,6 +35,22 @@ typedef PM::DataPoints DP;
namespace rtabmap {
// View spanning every feature row (x, y, [z], pad).
//
// getFeatureViewByName("x") returns a 1-row block, so the view(1,i) / view(2,i)
// accesses below are out of bounds by Eigen's contract: they only land on the
// y/z rows because the block shares the matrix' outer stride, and they trip
// Eigen's bounds assertions in any build that keeps them enabled (a Debug
// build, where NDEBUG is not defined).
inline DP::View featuresView(DP & cloud)
{
return cloud.features.block(0, 0, cloud.features.rows(), cloud.features.cols());
}
inline DP::ConstView featuresView(const DP & cloud)
{
return cloud.features.block(0, 0, cloud.features.rows(), cloud.features.cols());
}
DP pclToDP(const pcl::PointCloud<pcl::PointXYZI>::Ptr & pclCloud, bool is2D)
{
UDEBUG("");
@@ -65,7 +81,7 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZI>::Ptr & pclCloud, bool is2D)
cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud
View view(cloud.getFeatureViewByName("x"));
View view(featuresView(cloud));
View viewIntensity(cloud.getDescriptorRowViewByName("intensity",0));
for(unsigned int i=0; i<pclCloud->size(); ++i)
{
@@ -104,7 +120,9 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & pclCloud, bool is2
}
featLabels.push_back(Label("pad", 1));
descLabels.push_back(Label("normals", 3));
// The "normals" descriptor must have the same dimension as the features
// (see kNormalsDim comment in laserScanToDP()): 2 for a 2D cloud.
descLabels.push_back(Label("normals", is2D?2:3));
descLabels.push_back(Label("intensity", 1));
// create cloud
@@ -112,10 +130,10 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & pclCloud, bool is2
cloud.getFeatureViewByName("pad").setConstant(1);
// fill cloud
View view(cloud.getFeatureViewByName("x"));
View view(featuresView(cloud));
View viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
View viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
View viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
View viewNormalZ(!is2D?cloud.getDescriptorRowViewByName("normals",2):view);
View viewIntensity(cloud.getDescriptorRowViewByName("intensity",0));
for(unsigned int i=0; i<pclCloud->size(); ++i)
{
@@ -127,7 +145,10 @@ DP pclToDP(const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & pclCloud, bool is2
}
viewNormalX(0, i) = pclCloud->at(i).normal_x;
viewNormalY(0, i) = pclCloud->at(i).normal_y;
viewNormalZ(0, i) = pclCloud->at(i).normal_z;
if(!is2D)
{
viewNormalZ(0, i) = pclCloud->at(i).normal_z;
}
viewIntensity(0, i) = pclCloud->at(i).intensity;
}
@@ -157,9 +178,18 @@ DP laserScanToDP(const rtabmap::LaserScan & scan, bool ignoreLocalTransform = fa
}
featLabels.push_back(Label("pad", 1));
// libpointmatcher rotates the "normals" descriptor with the rotation block
// of the transform, whose size follows the feature dimension (2x2 for a 2D
// cloud, whose features are x/y/pad). Declaring 3 normal components on a 2D
// cloud is a dimension mismatch in that product; Eigen only catches it with
// assertions enabled, so a Release build reads/writes past the descriptor
// matrix instead -- an intermittent segfault inside
// RigidTransformation::inPlaceCompute(). For a 2D scan nz is always 0
// (see computeNormals2D()), so dropping it loses nothing.
const unsigned int kNormalsDim = scan.is2d()?2:3;
if(scan.hasNormals())
{
descLabels.push_back(Label("normals", 3));
descLabels.push_back(Label("normals", kNormalsDim));
}
if(scan.hasIntensity())
{
@@ -177,10 +207,10 @@ DP laserScanToDP(const rtabmap::LaserScan & scan, bool ignoreLocalTransform = fa
int nz = ny+1;
int offsetI = scan.getIntensityOffset();
bool hasLocalTransform = !ignoreLocalTransform && !scan.localTransform().isNull() && !scan.localTransform().isIdentity();
View view(cloud.getFeatureViewByName("x"));
View view(featuresView(cloud));
View viewNormalX(nx!=-1?cloud.getDescriptorRowViewByName("normals",0):view);
View viewNormalY(nx!=-1?cloud.getDescriptorRowViewByName("normals",1):view);
View viewNormalZ(nx!=-1?cloud.getDescriptorRowViewByName("normals",2):view);
View viewNormalZ(nx!=-1 && kNormalsDim==3?cloud.getDescriptorRowViewByName("normals",2):view);
View viewIntensity(offsetI!=-1?cloud.getDescriptorRowViewByName("intensity",0):view);
int oi = 0;
for(int i=0; i<scan.size(); ++i)
@@ -225,7 +255,10 @@ DP laserScanToDP(const rtabmap::LaserScan & scan, bool ignoreLocalTransform = fa
}
viewNormalX(0, oi) = pt.normal_x;
viewNormalY(0, oi) = pt.normal_y;
viewNormalZ(0, oi) = pt.normal_z;
if(kNormalsDim == 3)
{
viewNormalZ(0, oi) = pt.normal_z;
}
if(offsetI!=-1)
{
@@ -251,7 +284,10 @@ DP laserScanToDP(const rtabmap::LaserScan & scan, bool ignoreLocalTransform = fa
{
viewNormalX(0, oi) = ptr[nx];
viewNormalY(0, oi) = ptr[ny];
viewNormalZ(0, oi) = ptr[nz];
if(kNormalsDim == 3)
{
viewNormalZ(0, oi) = ptr[nz];
}
}
if(offsetI!=-1)
{
@@ -292,7 +328,7 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZI> & pclCloud)
bool hasIntensity = cloud.descriptorExists("intensity");
// fill cloud
ConstView view(cloud.getFeatureViewByName("x"));
ConstView view(featuresView(cloud));
ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
bool is3D = cloud.featureExists("z");
for(unsigned int i=0; i<pclCloud.size(); ++i)
@@ -319,11 +355,13 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZINormal> & pclClou
bool hasIntensity = cloud.descriptorExists("intensity");
// fill cloud
ConstView view(cloud.getFeatureViewByName("x"));
ConstView view(featuresView(cloud));
bool is3D = cloud.featureExists("z");
// A 2D cloud carries 2 normal components only (see laserScanToDP()).
bool hasNormalZ = cloud.getDescriptorDimension("normals") == 3;
ConstView viewNormalX(cloud.getDescriptorRowViewByName("normals",0));
ConstView viewNormalY(cloud.getDescriptorRowViewByName("normals",1));
ConstView viewNormalZ(cloud.getDescriptorRowViewByName("normals",2));
ConstView viewNormalZ(hasNormalZ?cloud.getDescriptorRowViewByName("normals",2):view);
ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
for(unsigned int i=0; i<pclCloud.size(); ++i)
{
@@ -332,7 +370,7 @@ void pclFromDP(const DP & cloud, pcl::PointCloud<pcl::PointXYZINormal> & pclClou
pclCloud.at(i).z = is3D?view(2, i):0;
pclCloud.at(i).normal_x = viewNormalX(0, i);
pclCloud.at(i).normal_y = viewNormalY(0, i);
pclCloud.at(i).normal_z = viewNormalZ(0, i);
pclCloud.at(i).normal_z = hasNormalZ?viewNormalZ(0, i):0;
if(hasIntensity)
pclCloud.at(i).intensity = viewIntensity(0, i);
}
@@ -356,10 +394,12 @@ rtabmap::LaserScan laserScanFromDP(const DP & cloud, const rtabmap::Transform &
bool is3D = cloud.featureExists("z");
bool hasNormals = cloud.descriptorExists("normals");
bool hasIntensity = cloud.descriptorExists("intensity");
ConstView view(cloud.getFeatureViewByName("x"));
// A 2D cloud carries 2 normal components only (see laserScanToDP()).
bool hasNormalZ = hasNormals && cloud.getDescriptorDimension("normals") == 3;
ConstView view(featuresView(cloud));
ConstView viewNormalX(hasNormals?cloud.getDescriptorRowViewByName("normals",0):view);
ConstView viewNormalY(hasNormals?cloud.getDescriptorRowViewByName("normals",1):view);
ConstView viewNormalZ(hasNormals?cloud.getDescriptorRowViewByName("normals",2):view);
ConstView viewNormalZ(hasNormalZ?cloud.getDescriptorRowViewByName("normals",2):view);
ConstView viewIntensity(hasIntensity?cloud.getDescriptorRowViewByName("intensity",0):view);
int channels = 2+(is3D?1:0) + (hasNormals?3:0) + (hasIntensity?1:0);
cv::Mat data(1, cloud.features.cols(), CV_32FC(channels));
@@ -375,7 +415,7 @@ rtabmap::LaserScan laserScanFromDP(const DP & cloud, const rtabmap::Transform &
if(hasNormals) {
pt.normal_x = viewNormalX(0, i);
pt.normal_y = viewNormalY(0, i);
pt.normal_z = viewNormalZ(0, i);
pt.normal_z = hasNormalZ?viewNormalZ(0, i):0;
}
if(transformValid)
pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
@@ -395,14 +435,21 @@ rtabmap::LaserScan laserScanFromDP(const DP & cloud, const rtabmap::Transform &
}
}
UASSERT(data.channels() >= 2 && data.channels() <=7);
// Derive the format from the fields written above, not from the channel
// count: with normals a 2D scan has the same count as a 3D one
// (kXYINormal and kXYZNormal are both 6, kXYNormal is 5 like kXYZIT), so
// counting alone labelled 2D scans with normals as 3D.
const rtabmap::LaserScan::Format format =
is3D?
(hasNormals?
(hasIntensity?rtabmap::LaserScan::kXYZINormal:rtabmap::LaserScan::kXYZNormal):
(hasIntensity?rtabmap::LaserScan::kXYZI:rtabmap::LaserScan::kXYZ)):
(hasNormals?
(hasIntensity?rtabmap::LaserScan::kXYINormal:rtabmap::LaserScan::kXYNormal):
(hasIntensity?rtabmap::LaserScan::kXYI:rtabmap::LaserScan::kXY));
UASSERT(rtabmap::LaserScan::channels(format) == data.channels());
return rtabmap::LaserScan(data, 0, 0,
data.channels()==2?rtabmap::LaserScan::kXY:
data.channels()==3?(hasIntensity?rtabmap::LaserScan::kXYI:rtabmap::LaserScan::kXYZ):
data.channels()==4?rtabmap::LaserScan::kXYZI:
data.channels()==5?rtabmap::LaserScan::kXYINormal:
data.channels()==6?rtabmap::LaserScan::kXYZNormal:
rtabmap::LaserScan::kXYZINormal,
format,
localTransform);
}
corelib/src/odometry/OdometryF2F.cpp
+1 -1
View File
@@ -291,7 +291,7 @@ Transform OdometryF2F::computeTransform(
}
else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanRaw().maxPoints() != 0 && float(newFrame.sensorData().laserScanRaw().size())/float(newFrame.sensorData().laserScanRaw().maxPoints())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
{
UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().size())/float(newFrame.sensorData().laserScanRaw().maxPoints()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
UWARN("Too low scan points ratio (%f < %f), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().size())/float(newFrame.sensorData().laserScanRaw().maxPoints()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
}
}
}
corelib/src/odometry/OdometryF2M.cpp
+6 -4
View File
@@ -114,13 +114,15 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
ParametersMap bundleParameters = parameters;
if(bundleAdjustment_ > 0)
{
if((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
(bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA)) ||
(bundleAdjustment_==3 && Optimizer::isAvailable(Optimizer::kTypeCeres)))
// The BundleAdjustment int matches the Optimizer/Strategy parameter
// 1:1 (g2o=1, GTSAM=2, Ceres=3, CVSBA=4). 0 = "disabled" -- it's
// the TORO slot, which isn't BA-capable.
const Optimizer::Type sbaType = static_cast<Optimizer::Type>(bundleAdjustment_);
if(Optimizer::isAvailable(sbaType))
{
// disable bundle in RegistrationVis as we do it already here
uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0"));
sba_ = Optimizer::create(bundleAdjustment_==3?Optimizer::kTypeCeres:bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters);
sba_ = Optimizer::create(sbaType, bundleParameters);
}
else
{
corelib/src/opencv/ORBextractor.cc
+9 -4
View File
@@ -550,8 +550,10 @@ void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNo
vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)
{
// Compute how many initial nodes
const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));
// Compute how many initial nodes. For regions taller than wide the
// raw ratio rounds down to 0; clamp to at least 1 so hX stays finite
// and vpIniNodes is non-empty.
const int nIni = std::max(1, (int)round(static_cast<float>(maxX-minX)/(maxY-minY)));
const float hX = static_cast<float>(maxX-minX)/nIni;
@@ -573,11 +575,14 @@ vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>&
vpIniNodes[i] = &lNodes.back();
}
//Associate points to childs
//Associate points to childs. A keypoint sitting exactly on the right
//border has kp.pt.x == maxX-minX, so kp.pt.x/hX == nIni -- one past
//the last bucket. Clamp to nIni-1 to keep it in range.
for(size_t i=0;i<vToDistributeKeys.size();i++)
{
const cv::KeyPoint &kp = vToDistributeKeys[i];
vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
const int bucket = std::min(static_cast<int>(kp.pt.x/hX), nIni-1);
vpIniNodes[bucket]->vKeys.push_back(kp);
}
list<ExtractorNode>::iterator lit = lNodes.begin();
corelib/src/optimizer/OptimizerCeres.cpp
+380 -63
View File
@@ -58,6 +58,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "ceres/pose_graph_3d/pose_graph_3d_error_term.h"
#include "ceres/bundle/BAProblem.h"
#include "ceres/bundle/snavely_reprojection_error.h"
#include "ceres/bundle/snavely_stereo_reprojection_error.h"
#include "ceres/bundle/between_cameras_error.h"
#include "ceres/bundle/planar_constraint_error.h"
#if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
#include "ceres/pose_graph_3d/eigen_quaternion_manifold.h"
@@ -94,6 +97,28 @@ bool OptimizerCeres::available()
#endif
}
OptimizerCeres::OptimizerCeres(const ParametersMap & parameters) :
Optimizer(parameters),
pixelVariance_(Parameters::defaultOptimizerPixelVariance()),
disparityVariance_(Parameters::defaultOptimizerDisparityVariance()),
robustKernelDelta_(Parameters::defaultOptimizerRobustKernelDelta()),
baseline_(Parameters::defaultOptimizerBaseline())
{
parseParameters(parameters);
}
void OptimizerCeres::parseParameters(const ParametersMap & parameters)
{
Optimizer::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kOptimizerPixelVariance(), pixelVariance_);
Parameters::parse(parameters, Parameters::kOptimizerDisparityVariance(), disparityVariance_);
Parameters::parse(parameters, Parameters::kOptimizerRobustKernelDelta(), robustKernelDelta_);
Parameters::parse(parameters, Parameters::kOptimizerBaseline(), baseline_);
UASSERT(pixelVariance_ > 0.0);
UASSERT(disparityVariance_ > 0.0);
UASSERT(baseline_ >= 0.0);
}
std::map<int, Transform> OptimizerCeres::optimize(
int rootId,
const std::map<int, Transform> & poses,
@@ -184,7 +209,7 @@ std::map<int, Transform> OptimizerCeres::optimize(
}
float yaw_radians = ceres::examples::NormalizeAngle(iter->second.transform().theta());
const Eigen::Matrix3d sqrt_information = information.llt().matrixL();
const Eigen::Matrix3d sqrt_information = information.llt().matrixU();
// Ceres will take ownership of the pointer.
ceres::CostFunction* cost_function = ceres::examples::PoseGraph2dErrorTerm::Create(
@@ -225,7 +250,7 @@ std::map<int, Transform> OptimizerCeres::optimize(
t.p.z() = iter->second.transform().z();
t.q = iter->second.transform().getQuaterniond();
const Eigen::Matrix<double, 6, 6> sqrt_information = information.llt().matrixL();
const Eigen::Matrix<double, 6, 6> sqrt_information = information.llt().matrixU();
// Ceres will take ownership of the pointer.
ceres::CostFunction* cost_function = ceres::examples::PoseGraph3dErrorTerm::Create(t, sqrt_information);
problem.AddResidualBlock(cost_function, loss_function,
@@ -280,8 +305,31 @@ std::map<int, Transform> OptimizerCeres::optimize(
UINFO("Ceres optimizing begin (iterations=%d)", iterations());
ceres::Solver::Options options;
options.linear_solver_type = ceres::ITERATIVE_SCHUR;
options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
// SPARSE_NORMAL_CHOLESKY is the standard linear solver for
// pose-graph SLAM: the Hessian is a sparse symmetric positive
// definite matrix over pose blocks, which Cholesky factors
// directly and robustly.
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
// If the build has no usable sparse backend at all, fall back to a
// dense factorization, which is always available. Slower on big graphs,
// but a pose graph that optimizes beats one that silently does not.
#if CERES_VERSION_MAJOR > 1 || \
(CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 14)
if(options.sparse_linear_algebra_library_type == ceres::NO_SPARSE ||
!ceres::IsSparseLinearAlgebraLibraryTypeAvailable(
options.sparse_linear_algebra_library_type))
{
static bool warned = false;
if(!warned)
{
warned = true;
UWARN("Ceres was built without a usable sparse linear algebra "
"library, falling back to DENSE_NORMAL_CHOLESKY for graph "
"optimization (slower on large graphs).");
}
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
}
#endif
options.max_num_iterations = iterations();
options.function_tolerance = this->epsilon();
ceres::Solver::Summary summary;
@@ -372,7 +420,18 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
ceres::BAProblem baProblem;
baProblem.num_cameras_ = poses.size();
// Multi-camera support: each (pose, camera-in-rig) pair becomes its
// own parameter block. Total camera blocks = sum over poses of rig
// size. Single-cam rigs (the common case) reduce to 1 block per pose.
int totalCameras = 0;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && !iterModel->second.empty());
totalCameras += static_cast<int>(iterModel->second.size());
}
baProblem.num_cameras_ = totalCameras;
baProblem.num_points_ = points3DMap.size();
baProblem.num_observations_ = 0;
for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
@@ -388,43 +447,45 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
baProblem.cameras_ = new double[6 * baProblem.num_cameras_];
baProblem.points_ = new double[3 * baProblem.num_points_];
// Each camera is a set of 6 parameters: R and t. The rotation R is specified as a Rodrigues' vector.
// Each camera is a set of 6 parameters: R and t. The rotation R is
// specified as a Rodrigues' vector. The map is keyed on
// (poseId, camIdx-in-rig) so multi-camera observations can look up
// the correct vertex.
int oi=0;
int camIndex=0;
std::map<int, int> camIdToIndex;
std::map<std::pair<int,int>, int> camIdxByKey;
for(std::map<int, Transform>::const_iterator iter=poses.begin();
iter!=poses.end();
++iter)
{
// Get camera model
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end());
if(iterModel->second.size() != 1)
for(size_t c = 0; c < iterModel->second.size(); ++c)
{
UERROR("Multi-camera BA not implemented for Ceres, only single camera.");
return std::map<int, Transform>();
const CameraModel & m = iterModel->second[c];
UASSERT(m.isValidForProjection());
const Transform t = (iter->second * m.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(),
(double)t.r31(), (double)t.r32(), (double)t.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
UASSERT(oi+6 <= baProblem.num_cameras_*6);
baProblem.cameras_[oi++] = rvec.at<double>(0,0);
baProblem.cameras_[oi++] = rvec.at<double>(0,1);
baProblem.cameras_[oi++] = rvec.at<double>(0,2);
baProblem.cameras_[oi++] = t.x();
baProblem.cameras_[oi++] = t.y();
baProblem.cameras_[oi++] = t.z();
camIdxByKey.insert(std::make_pair(std::make_pair(iter->first, (int)c), camIndex++));
}
UASSERT(iterModel->second[0].isValidForProjection());
const Transform & t = (iter->second * iterModel->second[0].localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(),
(double)t.r31(), (double)t.r32(), (double)t.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
UASSERT(oi+6 <= baProblem.num_cameras_*6);
baProblem.cameras_[oi++] = rvec.at<double>(0,0);
baProblem.cameras_[oi++] = rvec.at<double>(0,1);
baProblem.cameras_[oi++] = rvec.at<double>(0,2);
baProblem.cameras_[oi++] = t.x();
baProblem.cameras_[oi++] = t.y();
baProblem.cameras_[oi++] = t.z();
camIdToIndex.insert(std::make_pair(iter->first, camIndex++));
}
UASSERT(oi == baProblem.num_cameras_*6);
@@ -443,6 +504,12 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
}
UASSERT(oi == baProblem.num_points_*3);
// Per-observation stereo metadata. For mono observations
// observed_disparity[i] = 0 and baseline_fx[i] = 0 -- the second-loop
// branch picks the mono cost function in that case.
std::vector<double> observed_disparity(baProblem.num_observations_, 0.0);
std::vector<double> baseline_fx(baProblem.num_observations_, 0.0);
oi = 0;
for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
iter!=wordReferences.end();
@@ -452,21 +519,43 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
jter!=iter->second.end();
++jter)
{
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(jter->first);
const int poseId = jter->first;
const int camIdx = jter->second.cameraIndex;
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(poseId);
UASSERT(iterModel != models.end());
if(iterModel->second.size() != 1)
{
UERROR("Multi-camera BA not implemented for Ceres, only single camera.");
return std::map<int, Transform>();
}
UASSERT(iterModel->second[0].isValidForProjection());
UASSERT(camIdx >= 0 && camIdx < (int)iterModel->second.size());
const CameraModel & m = iterModel->second[camIdx];
UASSERT(m.isValidForProjection());
baProblem.camera_index_[oi] = camIdToIndex.at(jter->first);
std::map<std::pair<int,int>, int>::const_iterator camIt =
camIdxByKey.find(std::make_pair(poseId, camIdx));
UASSERT(camIt != camIdxByKey.end());
baProblem.camera_index_[oi] = camIt->second;
baProblem.point_index_[oi] = pointIdToIndex.at(iter->first);
baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second[0].cx();
baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second[0].cy();
baProblem.observations_[4*oi+2] = iterModel->second[0].fx();
baProblem.observations_[4*oi+3] = iterModel->second[0].fy();
baProblem.observations_[4*oi] = jter->second.kpt.pt.x - m.cx();
baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - m.cy();
baProblem.observations_[4*oi+2] = m.fx();
baProblem.observations_[4*oi+3] = m.fy();
// Stereo path: if a baseline is encoded in the camera model
// (Tx<0, the rtabmap convention) AND we have a finite positive
// depth from the observation, derive the observed disparity
// and cache baseline*fx for the cost function. For RGB-D /
// mono-with-depth (Tx==0) we fall back on the configurable
// Optimizer/Baseline -- a "fake baseline" that lets BA treat
// depth observations as stereo disparity. depth==0 or
// effective baseline==0 -> mono observation; the second loop
// will pick SnavelyReprojectionError instead.
const double Tx = m.Tx();
const double fx = m.fx();
const double depth = jter->second.depth;
const double baseline = Tx < 0.0 ? (-Tx / fx) : baseline_;
if(baseline > 0.0 && uIsFinite(depth) && depth > 0.0)
{
baseline_fx[oi] = baseline * fx;
observed_disparity[oi] = baseline_fx[oi] / depth;
}
++oi;
}
}
@@ -478,22 +567,222 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
// parameters for cameras and points are added automatically.
ceres::Problem problem;
// Per-axis weighting mirrors g2o's stereo information matrix:
// 1/pixelVariance on u/v, 1/disparityVariance on disparity. Pure
// loop-invariant, hoisted out.
const double inv_sigma_uv = 1.0 / std::sqrt(pixelVariance_);
const double inv_sigma_d = 1.0 / std::sqrt(disparityVariance_);
int monoObsCount = 0;
int stereoObsCount = 0;
for (int i = 0; i < baProblem.num_observations(); ++i) {
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
ceres::SnavelyReprojectionError::Create(
observations[4 * i], //u
observations[4 * i + 1], //v
observations[4 * i + 2], //fx
observations[4 * i + 3]); //fy
ceres::LossFunction* loss_function = new ceres::HuberLoss(8.0);
const double u = observations[4 * i];
const double v = observations[4 * i + 1];
const double fx = observations[4 * i + 2];
const double fy = observations[4 * i + 3];
ceres::CostFunction* cost_function = 0;
if(baseline_fx[i] > 0.0 && observed_disparity[i] > 0.0)
{
// Stereo (3 residuals: u, v, disparity). The disparity channel
// pins z relative to the observing camera, which collapses the
// mono BA gauge from 7 DOF to 6 (scale becomes observable).
cost_function = ceres::SnavelyStereoReprojectionError::Create(
u, v, observed_disparity[i], fx, fy, baseline_fx[i],
inv_sigma_uv, inv_sigma_d);
++stereoObsCount;
}
else
{
// Mono (2 residuals: u, v).
cost_function = ceres::SnavelyReprojectionError::Create(u, v, fx, fy);
++monoObsCount;
}
// Pass nullptr when robustKernelDelta_ <= 0 -- Ceres treats that as
// identity (no kernel). A new loss instance per block is required:
// Ceres takes ownership and deletes each.
ceres::LossFunction* loss_function =
robustKernelDelta_ > 0.0 ? new ceres::HuberLoss(robustKernelDelta_) : nullptr;
problem.AddResidualBlock(cost_function,
loss_function,
baProblem.mutable_camera_for_observation(i),
baProblem.mutable_point_for_observation(i));
}
UDEBUG("Ceres BA: %d mono + %d stereo observations", monoObsCount, stereoObsCount);
// Pose-graph constraints (kNeighbor / etc.) between cameras. Same role
// as the EdgeSBACam edges in OptimizerG2O and the BetweenFactor<Pose3>
// factors in OptimizerGTSAM -- folds the relative-pose chain into the
// BA cost so chains pulled by odometry don't drift freely.
int linkObsCount = 0;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
const Link & link = iter->second;
if(link.from() <= 0 || link.to() <= 0 || link.from() == link.to())
{
continue;
}
std::map<std::pair<int,int>, int>::const_iterator itA =
camIdxByKey.find(std::make_pair(link.from(), 0));
std::map<std::pair<int,int>, int>::const_iterator itB =
camIdxByKey.find(std::make_pair(link.to(), 0));
if(itA == camIdxByKey.end() || itB == camIdxByKey.end())
{
continue;
}
// Convert link from body-to-body into camera-to-camera using the
// localTransforms of both endpoints (same idiom as g2o / GTSAM).
const Transform camLink = models.at(link.from())[0].localTransform().inverse() *
link.transform() *
models.at(link.to())[0].localTransform();
// Decompose camLink (cam_b in cam_a's frame) into (angle-axis, translation).
const cv::Mat R = (cv::Mat_<double>(3, 3) <<
(double)camLink.r11(), (double)camLink.r12(), (double)camLink.r13(),
(double)camLink.r21(), (double)camLink.r22(), (double)camLink.r23(),
(double)camLink.r31(), (double)camLink.r32(), (double)camLink.r33());
cv::Mat rvec(1, 3, CV_64FC1);
cv::Rodrigues(R, rvec);
const Eigen::Vector3d aa_meas(rvec.at<double>(0,0), rvec.at<double>(0,1), rvec.at<double>(0,2));
const Eigen::Vector3d t_meas(camLink.x(), camLink.y(), camLink.z());
// Information matrix from the link covariance. rtabmap stores it as
// [linear|angular] -- same axis order as our residual [t; rot], so
// no block swap is needed (unlike the GTSAM path which expects
// [angular|linear]). cv::Mat is row-major; copying into Eigen
// (column-major) effectively transposes, which is a no-op for the
// symmetric info matrix.
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), link.infMatrix().data, link.infMatrix().total()*sizeof(double));
}
// Whitening matrix: we want sqrt_info such that
// sqrt_info^T * sqrt_info = info. LLT gives info = L*L^T with L lower
// triangular, so the upper-triangular U = L^T (matrixU()) satisfies
// U^T*U = info. Then ||U*r||² = r^T*info*r as desired.
const Eigen::Matrix<double, 6, 6> sqrt_info =
information.llt().matrixU();
ceres::CostFunction * cost = ceres::BetweenCamerasError::Create(t_meas, aa_meas, sqrt_info);
// No robust kernel on between-camera constraints (matches g2o/GTSAM:
// only projection edges carry the Huber kernel in BA).
problem.AddResidualBlock(cost,
nullptr,
baProblem.cameras_ + itA->second * 6,
baProblem.cameras_ + itB->second * 6);
++linkObsCount;
}
if(linkObsCount > 0)
{
UDEBUG("Ceres BA: %d pose-graph links", linkObsCount);
}
// Multi-camera rigid edges: for each pose with >1 cameras in its rig,
// constrain cam 0 -> cam i with a high-info BetweenCamerasError (same
// idiom as g2o's Identity*1e7 edge and GTSAM's high-info BetweenFactor
// inside their multicam loops). The measurement is the constant
// cam0->cami transform derived from the localTransforms.
int rigEdgeCount = 0;
for(std::map<int, std::vector<CameraModel> >::const_iterator iter=models.begin(); iter!=models.end(); ++iter)
{
if(!uContains(poses, iter->first))
{
continue;
}
if(iter->second.size() < 2)
{
continue;
}
std::map<std::pair<int,int>, int>::const_iterator cam0It =
camIdxByKey.find(std::make_pair(iter->first, 0));
if(cam0It == camIdxByKey.end())
{
continue;
}
const Transform & lt0 = iter->second[0].localTransform();
// Tight info matrix (matches g2o's 9999999 diagonal).
Eigen::Matrix<double, 6, 6> rigInfo = Eigen::Matrix<double, 6, 6>::Identity() * 9999999.0;
const Eigen::Matrix<double, 6, 6> rigSqrtInfo = rigInfo.llt().matrixU();
for(size_t c = 1; c < iter->second.size(); ++c)
{
std::map<std::pair<int,int>, int>::const_iterator camCIt =
camIdxByKey.find(std::make_pair(iter->first, (int)c));
if(camCIt == camIdxByKey.end())
{
continue;
}
const Transform camLink = lt0.inverse() * iter->second[c].localTransform();
const cv::Mat R = (cv::Mat_<double>(3, 3) <<
(double)camLink.r11(), (double)camLink.r12(), (double)camLink.r13(),
(double)camLink.r21(), (double)camLink.r22(), (double)camLink.r23(),
(double)camLink.r31(), (double)camLink.r32(), (double)camLink.r33());
cv::Mat rvec(1, 3, CV_64FC1);
cv::Rodrigues(R, rvec);
const Eigen::Vector3d aa(rvec.at<double>(0,0), rvec.at<double>(0,1), rvec.at<double>(0,2));
const Eigen::Vector3d tm(camLink.x(), camLink.y(), camLink.z());
ceres::CostFunction * cost = ceres::BetweenCamerasError::Create(tm, aa, rigSqrtInfo);
problem.AddResidualBlock(cost,
nullptr,
baProblem.cameras_ + cam0It->second * 6,
baProblem.cameras_ + camCIt->second * 6);
++rigEdgeCount;
}
}
if(rigEdgeCount > 0)
{
UDEBUG("Ceres BA: %d multi-cam rigid edges", rigEdgeCount);
}
// 2D / planar BA mode: lock each non-root pose's primary (cam 0)
// vertex to its initial body-z (lateral motion + yaw stay free).
// Other cameras of a multi-cam rig follow via the rigid edges above.
// Root pose's cam 0 is fixed entirely so the gauge has no remaining
// z-DOF. Mirrors the g2o EdgeSBACamPrior path.
if(isSlam2d())
{
const double sqrtInfo = std::sqrt(1e9); // matches g2o pinfo(2,2) = 1e9
int planarObsCount = 0;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
std::map<std::pair<int,int>, int>::const_iterator camIt =
camIdxByKey.find(std::make_pair(iter->first, 0));
if(camIt == camIdxByKey.end())
{
continue;
}
double * cam_block = baProblem.cameras_ + camIt->second * 6;
const bool fixNode = (rootId >= 0 && iter->first == rootId) ||
(rootId < 0 && iter->first != -rootId);
if(fixNode)
{
problem.SetParameterBlockConstant(cam_block);
continue;
}
// Unary planar constraint on the BODY z (the camera vertex is in
// world-to-camera; we extract body z by composing with the
// inverse localTransform inside the cost function).
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
if(iterModel == models.end() || iterModel->second.empty())
{
continue;
}
const Transform & localTransform = iterModel->second[0].localTransform();
const Eigen::Matrix3d R_bc = (Eigen::Matrix3d() <<
(double)localTransform.r11(), (double)localTransform.r12(), (double)localTransform.r13(),
(double)localTransform.r21(), (double)localTransform.r22(), (double)localTransform.r23(),
(double)localTransform.r31(), (double)localTransform.r32(), (double)localTransform.r33()).finished();
const Eigen::Vector3d t_bc(localTransform.x(), localTransform.y(), localTransform.z());
ceres::CostFunction * planar = ceres::PlanarConstraintError::Create(
R_bc, t_bc, iter->second.z(), sqrtInfo);
problem.AddResidualBlock(planar, nullptr, cam_block);
++planarObsCount;
}
if(planarObsCount > 0)
{
UDEBUG("Ceres BA: %d planar-constraint blocks (2D mode)", planarObsCount);
}
}
// SBA
// Make Ceres automatically detect the bundle structure. Note that the
@@ -501,10 +790,16 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
// for standard bundle adjustment problems.
ceres::Solver::Options options;
options.linear_solver_type = ceres::ITERATIVE_SCHUR;
options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
options.max_num_iterations = iterations();
//options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.function_tolerance = this->epsilon();
options.function_tolerance = this->epsilon();
// Force the LM to run to max_num_iterations rather than stopping early
// on Ceres' default parameter/gradient tolerances. Matters in particular
// for high-weight unary constraints (e.g. the 2D planar lock at sqrt(1e9))
// whose residual can stall the parameter step below the default 1e-8
// before the constraint is fully satisfied.
options.parameter_tolerance = 0.0;
options.gradient_tolerance = 0.0;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
if(ULogger::level() == ULogger::kDebug)
@@ -518,11 +813,18 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
return poses;
}
//update poses
//update poses (read back from cam 0 of each rig -- the other cameras
//are rigidly constrained to it).
std::map<int, Transform> newPoses = poses;
oi=0;
for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
{
std::map<std::pair<int,int>, int>::const_iterator camIt =
camIdxByKey.find(std::make_pair(iter->first, 0));
if(camIt == camIdxByKey.end())
{
continue;
}
const int oi = camIt->second * 6;
cv::Mat rvec = (cv::Mat_<double>(1,3) <<
baProblem.cameras_[oi], baProblem.cameras_[oi+1], baProblem.cameras_[oi+2]);
@@ -532,13 +834,28 @@ std::map<int, Transform> OptimizerCeres::optimizeBA(
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), baProblem.cameras_[oi+4],
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), baProblem.cameras_[oi+5]);
oi+=6;
if(this->isSlam2d())
{
t = (models.at(iter->first)[0].localTransform() * t).inverse();
t = iter->second.inverse() * t;
iter->second *= t.to3DoF();
// Body pose recovered by BA (with the planar constraint that locks
// each non-root body z to its initial value). If the constraint held,
// z is already within ~mm of the initial; snap it back exactly.
// Otherwise the optimizer's planar lock didn't bite -- fall back to
// projecting the BA delta onto SE(2) and applying it to the initial.
Transform body = (models.at(iter->first)[0].localTransform() * t).inverse();
if(std::fabs(body.z() - iter->second.z()) < 0.001f)
{
body.z() = iter->second.z();
iter->second = body;
}
else
{
UWARN("Planar constraints didn't work!? original pose (%d), pose %s -> %s. Falling back to old approach.",
iter->first,
iter->second.prettyPrint().c_str(),
body.prettyPrint().c_str());
const Transform delta = iter->second.inverse() * body;
iter->second = iter->second * delta.to3DoF();
}
}
else
{
corelib/src/optimizer/OptimizerG2O.cpp
+35 -17
View File
@@ -160,9 +160,10 @@ OptimizerG2O::OptimizerG2O(const ParametersMap & parameters) :
Optimizer(parameters),
solver_(Parameters::defaultg2oSolver()),
optimizer_(Parameters::defaultg2oOptimizer()),
pixelVariance_(Parameters::defaultg2oPixelVariance()),
robustKernelDelta_(Parameters::defaultg2oRobustKernelDelta()),
baseline_(Parameters::defaultg2oBaseline())
pixelVariance_(Parameters::defaultOptimizerPixelVariance()),
disparityVariance_(Parameters::defaultOptimizerDisparityVariance()),
robustKernelDelta_(Parameters::defaultOptimizerRobustKernelDelta()),
baseline_(Parameters::defaultOptimizerBaseline())
{
#ifdef RTABMAP_G2O
// Issue on android, have to explicitly register this type when using fixed root prior below
@@ -184,10 +185,12 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
Parameters::parse(parameters, Parameters::kOptimizerPixelVariance(), pixelVariance_);
Parameters::parse(parameters, Parameters::kOptimizerDisparityVariance(), disparityVariance_);
Parameters::parse(parameters, Parameters::kOptimizerRobustKernelDelta(), robustKernelDelta_);
Parameters::parse(parameters, Parameters::kOptimizerBaseline(), baseline_);
UASSERT(pixelVariance_ > 0.0);
UASSERT(disparityVariance_ > 0.0);
UASSERT(baseline_ >= 0.0);
#ifdef RTABMAP_ORB_SLAM
@@ -361,7 +364,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(!priorsIgnored() && iter->second.type() == Link::kPosePrior)
{
if(rootId!=0) {
UDEBUG("Removed rootId=%d because there are priors.");
UDEBUG("Removed rootId=%d because there are priors.", rootId);
}
rootId = 0;
break;
@@ -1187,7 +1190,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(i>0 && optimizer.activeRobustChi2() > 1000000000000.0)
{
UERROR("g2o: Large optimization error detected (%f), aborting optimization!");
UERROR("g2o: Large optimization error detected (%f), aborting optimization!",
optimizer.activeRobustChi2());
return optimizedPoses;
}
@@ -1236,7 +1240,8 @@ std::map<int, Transform> OptimizerG2O::optimize(
if(optimizer.activeRobustChi2() > 1000000000000.0)
{
UERROR("g2o: Large optimization error detected (%f), aborting optimization!");
UERROR("g2o: Large optimization error detected (%f), aborting optimization!",
optimizer.activeRobustChi2());
return optimizedPoses;
}
@@ -1861,14 +1866,22 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
double variance = pixelVariance_;
if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0)
{
// Stereo edge: per-axis info -- u, v use pixelVariance,
// disparity (u - u_right) uses disparityVariance.
// This keeps the depth measurement channel from being
// over-trusted relative to the u/v feature detector
// precision (or vice versa).
Eigen::Matrix3d stereoInfo = Eigen::Matrix3d::Zero();
stereoInfo(0, 0) = 1.0 / variance;
stereoInfo(1, 1) = 1.0 / variance;
stereoInfo(2, 2) = 1.0 / disparityVariance_;
// stereo edge
#ifdef RTABMAP_ORB_SLAM
g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
float disparity = baseline * iterModel->second[camIndex].fx() / depth;
Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
es->setMeasurement(obs);
//variance *= log(exp(1)+disparity);
es->setInformation(Eigen::Matrix3d::Identity() / variance);
es->setInformation(stereoInfo);
es->fx = iterModel->second[camIndex].fx();
es->fy = iterModel->second[camIndex].fy();
es->cx = iterModel->second[camIndex].cx();
@@ -1880,8 +1893,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
float disparity = baseline * vcam->estimate().Kcam(0,0) / depth;
Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
es->setMeasurement(obs);
//variance *= log(exp(1)+disparity);
es->setInformation(Eigen::Matrix3d::Identity() / variance);
es->setInformation(stereoInfo);
e = es;
#endif
}
@@ -1961,7 +1973,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
{
UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
UWARN("g2o: Large optimization error detected (%f), aborting optimization!",
optimizer.activeRobustChi2());
return optimizedPoses;
}
@@ -2022,7 +2035,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(optimizer.activeRobustChi2() > 1000000000000.0)
{
UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
UWARN("g2o: Large optimization error detected (%f), aborting optimization!",
optimizer.activeRobustChi2());
return optimizedPoses;
}
@@ -2683,9 +2697,13 @@ bool OptimizerG2O::saveGraph(
{
continue;
}
// Look up landmark info by landmark id (the negative endpoint),
// not by the multimap key -- callers may key landmark links
// either by from() or by the landmark id.
const int landmarkId = iter->second.from() < 0 ? iter->second.from() : iter->second.to();
if(isSlam2d())
{
if(uValue(isLandmarkWithRotation, iter->first, false))
if(uValue(isLandmarkWithRotation, landmarkId, false))
{
// EDGE_SE2 observed_vertex_id observing_vertex_id x y qx qy qz qw inf_11 inf_12 inf_13 inf_22 inf_23 inf_33
fprintf(file, "EDGE_SE2 %d %d %f %f %f %f %f %f %f %f %f\n",
@@ -2716,7 +2734,7 @@ bool OptimizerG2O::saveGraph(
}
else
{
if(uValue(isLandmarkWithRotation, iter->first, false))
if(uValue(isLandmarkWithRotation, landmarkId, false))
{
// EDGE_SE3 observed_vertex_id observing_vertex_id x y z qx qy qz qw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
corelib/src/optimizer/OptimizerGTSAM.cpp
+536
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/core/util3d.h>
#include <set>
#include <rtabmap/core/optimizer/OptimizerGTSAM.h>
@@ -38,22 +39,31 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifdef RTABMAP_GTSAM
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/sam/BearingFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/navigation/AttitudeFactor.h>
#include <optimizer/gtsam/XYFactor.h>
#include <optimizer/gtsam/XYZFactor.h>
#include <optimizer/gtsam/PlanarBodyZFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#ifdef RTABMAP_VERTIGO
@@ -67,6 +77,10 @@ namespace rtabmap {
OptimizerGTSAM::OptimizerGTSAM(const ParametersMap & parameters) :
Optimizer(parameters),
internalOptimizerType_(Parameters::defaultGTSAMOptimizer()),
pixelVariance_(Parameters::defaultOptimizerPixelVariance()),
disparityVariance_(Parameters::defaultOptimizerDisparityVariance()),
robustKernelDelta_(Parameters::defaultOptimizerRobustKernelDelta()),
baseline_(Parameters::defaultOptimizerBaseline()),
isam2_(0),
lastSwitchId_(1000000000)
{
@@ -95,6 +109,13 @@ void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
Optimizer::parseParameters(parameters);
#ifdef RTABMAP_GTSAM
Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), internalOptimizerType_);
Parameters::parse(parameters, Parameters::kOptimizerPixelVariance(), pixelVariance_);
Parameters::parse(parameters, Parameters::kOptimizerDisparityVariance(), disparityVariance_);
Parameters::parse(parameters, Parameters::kOptimizerRobustKernelDelta(), robustKernelDelta_);
Parameters::parse(parameters, Parameters::kOptimizerBaseline(), baseline_);
UASSERT(pixelVariance_ > 0.0);
UASSERT(disparityVariance_ > 0.0);
UASSERT(baseline_ >= 0.0);
bool incremental = isam2_;
double threshold = Parameters::defaultGTSAMIncRelinearizeThreshold();
@@ -1118,4 +1139,519 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
return optimizedPoses;
}
// Multi-camera offset: same convention as OptimizerG2O.cpp so per-rig camera
// vertex keys stay disjoint from pose keys (max 10 cameras per pose).
#define GTSAM_BA_MULTICAM_OFFSET 10
#ifdef RTABMAP_GTSAM
// Build a gtsam::Symbol for a 3D point. Word ids can be negative,
// but gtsam symbol cannot.
static inline gtsam::Symbol point3dSymbol(int id)
{
return id < 0
? gtsam::Symbol('L', static_cast<std::uint64_t>(-id))
: gtsam::Symbol('l', static_cast<std::uint64_t>(id));
}
#endif
std::map<int, Transform> OptimizerGTSAM::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, std::vector<CameraModel> > & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
std::set<int> * outliers)
{
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_GTSAM
UDEBUG("Optimizing BA graph...");
if(!(poses.size() >= 2 && iterations() > 0 && (models.size() == poses.size() || poses.begin()->first < 0)))
{
UWARN("GTSAM BA: nothing to optimize (poses=%d models=%d iterations=%d)",
(int)poses.size(), (int)models.size(), iterations());
return optimizedPoses;
}
gtsam::NonlinearFactorGraph graph;
gtsam::Values initialEstimate;
// Cache per-frame, per-camera intrinsics. Note that GTSAM's
// GenericProjectionFactor/GenericStereoFactor hold a shared_ptr to the
// calibration -- we have to keep these alive for the lifetime of the
// graph, hence storing them by map.
std::map<std::pair<int,int>, gtsam::Cal3_S2::shared_ptr> calMono;
std::map<std::pair<int,int>, gtsam::Cal3_S2Stereo::shared_ptr> calStereo;
std::map<std::pair<int,int>, double> baselineByCam;
// 1) Add pose variables (in CAMERA frame: pose * localTransform).
UDEBUG("GTSAM BA: adding %d poses... (rootId=%d)", (int)poses.size(), rootId);
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first <= 0)
{
continue;
}
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
if(iterModel == models.end() || iterModel->second.empty())
{
UERROR("GTSAM BA: missing camera model for pose %d", iter->first);
return optimizedPoses;
}
for(size_t i=0; i<iterModel->second.size(); ++i)
{
const CameraModel & m = iterModel->second[i];
if(!m.isValidForProjection())
{
UERROR("GTSAM BA: model %d.%d is invalid for projection", iter->first, (int)i);
return optimizedPoses;
}
const Transform camPose = iter->second * m.localTransform();
if(camPose.isNull())
{
UERROR("GTSAM BA: null camera pose for %d.%d", iter->first, (int)i);
return optimizedPoses;
}
const gtsam::Symbol xkey('x', iter->first * GTSAM_BA_MULTICAM_OFFSET + (int)i);
initialEstimate.insert(xkey, gtsam::Pose3(camPose.toEigen4d()));
// Intrinsics: skew=0 (no shear in any CameraModel rtabmap supports).
gtsam::Cal3_S2::shared_ptr K(new gtsam::Cal3_S2(m.fx(), m.fy(), 0.0, m.cx(), m.cy()));
calMono[std::make_pair(iter->first, (int)i)] = K;
const double baseline = m.Tx() < 0.0 ? (-m.Tx() / m.fx()) : baseline_;
if(baseline > 0.0)
{
gtsam::Cal3_S2Stereo::shared_ptr Ks(new gtsam::Cal3_S2Stereo(m.fx(), m.fy(), 0.0, m.cx(), m.cy(), baseline));
calStereo[std::make_pair(iter->first, (int)i)] = Ks;
baselineByCam[std::make_pair(iter->first, (int)i)] = baseline;
}
// Fix the root pose (or fix everyone else if rootId<0). GTSAM has
// no equivalent of g2o's setFixed(); the standard idiom is a
// near-zero-sigma prior on each axis. We add this only to the
// primary camera (i==0) of a multi-cam rig -- the others are
// rigidly linked via the multi-cam BetweenFactors below.
const bool fixNode = (rootId >= 0 && iter->first == rootId) ||
(rootId < 0 && iter->first != -rootId);
if(fixNode && i == 0)
{
gtsam::noiseModel::Diagonal::shared_ptr priorNoise =
gtsam::noiseModel::Diagonal::Sigmas(
(gtsam::Vector(6) << 1e-9, 1e-9, 1e-9, 1e-9, 1e-9, 1e-9).finished());
graph.add(gtsam::PriorFactor<gtsam::Pose3>(xkey, gtsam::Pose3(camPose.toEigen4d()), priorNoise));
}
else if(isSlam2d() && i == 0)
{
// 2D / planar BA: lock the body-frame z of each non-root
// camera to its initial value (mirrors g2o's EdgeSBACamPrior
// with pinfo(2,2) = 1e9). Lateral motion and yaw stay free.
const gtsam::Pose3 cam_to_body(m.localTransform().inverse().toEigen4d());
gtsam::SharedNoiseModel planarNoise =
gtsam::noiseModel::Isotropic::Sigma(1, std::sqrt(1.0 / 1e9));
graph.add(PlanarBodyZFactor(xkey, cam_to_body, iter->second.z(), planarNoise));
}
}
}
// 2) Pose-graph BetweenFactors (same role as the g2o EdgeSBACam edges).
// Expressed in camera frame: cam_from^{-1} * world * cam_to where
// cam = body * localTransform.
UDEBUG("GTSAM BA: adding %d links...", (int)links.size());
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
const Link & link = iter->second;
if(link.from() <= 0 || link.to() <= 0)
{
continue;
}
if(link.from() == link.to())
{
continue;
}
if(!uContains(poses, link.from()) || !uContains(poses, link.to()))
{
continue;
}
UASSERT(!link.transform().isNull());
const Transform camLink = models.at(link.from())[0].localTransform().inverse() *
link.transform() *
models.at(link.to())[0].localTransform();
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), link.infMatrix().data, link.infMatrix().total()*sizeof(double));
}
// rtabmap's covariance/information convention is [linear|angular];
// GTSAM expects [angular|linear]. Swap the blocks.
Eigen::Matrix<double, 6, 6> mgtsam;
mgtsam.block<3,3>(0,0) = information.block<3,3>(3,3); // rotation
mgtsam.block<3,3>(3,3) = information.block<3,3>(0,0); // translation
mgtsam.block<3,3>(0,3) = information.block<3,3>(3,0);
mgtsam.block<3,3>(3,0) = information.block<3,3>(0,3);
gtsam::SharedNoiseModel noise = gtsam::noiseModel::Gaussian::Information(mgtsam);
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(
gtsam::Symbol('x', link.from() * GTSAM_BA_MULTICAM_OFFSET),
gtsam::Symbol('x', link.to() * GTSAM_BA_MULTICAM_OFFSET),
gtsam::Pose3(camLink.toEigen4d()),
noise));
}
// 3) Hard rigid edges between camera 0 and the other cameras of a
// multi-cam rig (g2o uses Identity*1e7; we mirror that here).
for(std::map<int, std::vector<CameraModel> >::const_iterator iter=models.begin(); iter!=models.end(); ++iter)
{
if(!uContains(poses, iter->first))
{
continue;
}
for(size_t i=1; i<iter->second.size(); ++i)
{
const Transform camLink = iter->second[0].localTransform().inverse() * iter->second[i].localTransform();
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity() * 9999999.0;
gtsam::SharedNoiseModel noise = gtsam::noiseModel::Gaussian::Information(information);
graph.add(gtsam::BetweenFactor<gtsam::Pose3>(
gtsam::Symbol('x', iter->first * GTSAM_BA_MULTICAM_OFFSET),
gtsam::Symbol('x', iter->first * GTSAM_BA_MULTICAM_OFFSET + (int)i),
gtsam::Pose3(camLink.toEigen4d()),
noise));
}
}
// 4) 3D points + reprojection observations.
UDEBUG("GTSAM BA: adding %d 3D points and observations...", (int)points3DMap.size());
std::set<gtsam::Key> insertedPoints;
// Track factor->word mapping so the post-optimization residual sweep can
// report which observations went over the robust-kernel threshold.
std::vector<std::pair<size_t /*factorIndex*/, int /*wordId*/> > obsFactors;
// Build the per-axis noise models once (loop-invariant). Stereo: per-axis
// sigmas matching the g2o stereo path. StereoPoint2 is (uL, uR, v); uR =
// uL - disparity. uL and v carry pixel-detector noise, uR carries
// disparity-channel noise (matches the g2o stereo edge's (u, v, u-disp)
// interpretation up to a covariance rotation that is fine for typical
// small sigmas). The robust-Huber wrapping is also invariant.
const double sigmaPixel = std::sqrt(pixelVariance_);
const double sigmaDisparity = std::sqrt(disparityVariance_);
gtsam::SharedNoiseModel stereoNoiseModel = gtsam::noiseModel::Diagonal::Sigmas(
(gtsam::Vector(3) << sigmaPixel, sigmaDisparity, sigmaPixel).finished());
// SmartProjectionFactor requires an isotropic noise model — its
// constructor rejects diagonal/robust wrappers. Keep the un-wrapped
// isotropic around for the SmartFactor path; the generic factors
// still get the Huber-wrapped version when robust is on.
gtsam::SharedNoiseModel monoIsotropicNoise =
gtsam::noiseModel::Isotropic::Sigma(2, sigmaPixel);
gtsam::SharedNoiseModel monoNoiseModel = monoIsotropicNoise;
if(robustKernelDelta_ > 0.0)
{
gtsam::noiseModel::mEstimator::Base::shared_ptr huber =
gtsam::noiseModel::mEstimator::Huber::Create(robustKernelDelta_);
stereoNoiseModel = gtsam::noiseModel::Robust::Create(huber, stereoNoiseModel);
monoNoiseModel = gtsam::noiseModel::Robust::Create(huber, monoNoiseModel);
}
// Per-landmark: if EVERY observation is mono (no usable stereo
// depth) we fold all observations into a single
// SmartProjectionPoseFactor — it triangulates the 3D point
// internally and applies Schur complement per-factor, so the
// point doesn't appear as a graph variable. That's the GTSAM-
// native way to do BA (see the SFMExample_SmartFactorPCG demo).
//
// Stereo-bearing landmarks still go through GenericStereoFactor:
// the stereo smart factor lives in gtsam_unstable which we don't
// link.
using SmartMono = gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>;
std::map<int, SmartMono::shared_ptr> smartByWord;
for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
{
const int wordId = iter->first;
if(points3DMap.find(wordId) == points3DMap.end())
{
continue;
}
const cv::Point3f pt3d = points3DMap.at(wordId);
if(!util3d::isFinite(pt3d))
{
UWARN("Ignoring 3D point %d because it has nan value(s)!", wordId);
continue;
}
// Probe whether this landmark has any usable stereo observation;
// that decides which factor type we use.
bool anyStereoForWord = false;
for(const auto & jkv : iter->second)
{
const std::pair<int,int> camKey(jkv.first, jkv.second.cameraIndex);
const double depth = jkv.second.depth;
const double baseline = baselineByCam.count(camKey) ? baselineByCam.at(camKey) : 0.0;
if(uIsFinite(depth) && depth > 0.0 && baseline > 0.0
&& calStereo.count(camKey))
{
anyStereoForWord = true;
break;
}
}
const gtsam::Symbol pkey = point3dSymbol(wordId);
SmartMono::shared_ptr smartFactor;
if(!anyStereoForWord)
{
// Mono-only landmark → SmartProjectionPoseFactor. Use the
// first observation's Cal3_S2 (the smart factor needs one
// K shared across all observations).
gtsam::Cal3_S2::shared_ptr Kshared;
for(const auto & jkv : iter->second)
{
const std::pair<int,int> camKey(jkv.first, jkv.second.cameraIndex);
if(calMono.count(camKey))
{
Kshared = calMono.at(camKey);
break;
}
}
if(Kshared)
{
smartFactor = SmartMono::shared_ptr(new SmartMono(monoIsotropicNoise, Kshared));
}
}
if(!smartFactor)
{
// Stereo path: keep per-observation factors with an
// explicit Point3 variable in initialEstimate.
initialEstimate.insert(pkey, gtsam::Point3(pt3d.x, pt3d.y, pt3d.z));
insertedPoints.insert(pkey);
}
for(std::map<int, FeatureBA>::const_iterator jter = iter->second.begin(); jter != iter->second.end(); ++jter)
{
const int poseId = jter->first;
const int camIdx = jter->second.cameraIndex;
const FeatureBA & f = jter->second;
if(poses.find(poseId) == poses.end())
{
continue;
}
const std::pair<int,int> camKey(poseId, camIdx);
if(calMono.find(camKey) == calMono.end())
{
continue;
}
const gtsam::Symbol xkey('x', poseId * GTSAM_BA_MULTICAM_OFFSET + camIdx);
if(!initialEstimate.exists(xkey))
{
continue;
}
const double depth = f.depth;
const double baseline = baselineByCam.count(camKey) ? baselineByCam.at(camKey) : 0.0;
const bool isStereo = (uIsFinite(depth) && depth > 0.0 && baseline > 0.0 && calStereo.count(camKey));
if(smartFactor)
{
smartFactor->add(gtsam::Point2(f.kpt.pt.x, f.kpt.pt.y), xkey);
}
else if(isStereo)
{
const gtsam::Cal3_S2Stereo::shared_ptr & Ks = calStereo.at(camKey);
const double disparity = baseline * Ks->fx() / depth;
const gtsam::StereoPoint2 obs(f.kpt.pt.x, f.kpt.pt.x - disparity, f.kpt.pt.y);
size_t factorIdx = graph.size();
graph.add(gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>(
obs, stereoNoiseModel, xkey, pkey, Ks));
obsFactors.push_back(std::make_pair(factorIdx, wordId));
}
else
{
if(baseline > 0.0)
{
UDEBUG("Stereo cam detected but observation (word=%d cam=%d.%d) has null depth (%f m), adding mono observation instead.",
wordId, poseId, camIdx, depth);
}
const gtsam::Cal3_S2::shared_ptr & K = calMono.at(camKey);
const gtsam::Point2 obs(f.kpt.pt.x, f.kpt.pt.y);
size_t factorIdx = graph.size();
graph.add(gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2>(
obs, monoNoiseModel, xkey, pkey, K));
obsFactors.push_back(std::make_pair(factorIdx, wordId));
}
}
if(smartFactor && smartFactor->size() >= 2)
{
graph.add(smartFactor);
smartByWord[wordId] = smartFactor;
}
}
// 5) Optimize.
UTimer timer;
gtsam::Values result;
double finalError = std::numeric_limits<double>::quiet_NaN();
try
{
// Always use Levenberg-Marquardt for BA, ignoring GTSAM/Optimizer.
// Same rationale as the g2o BA path: BA's Hessian is often
// near-singular (points near infinity, near-parallel rays), so
// Gauss-Newton's unbounded step can blow up. Dogleg works but
// offers no advantage over LM on BA. LM is what every major BA
// library (Ceres, g2o, COLMAP) defaults to.
gtsam::LevenbergMarquardtParams params;
if(epsilon() > 0.0)
{
params.relativeErrorTol = epsilon();
params.absoluteErrorTol = epsilon();
}
params.maxIterations = iterations();
// Use PCG + Block-Jacobi instead of GTSAM's default multifrontal
// Cholesky. The example in the GTSAM repo (SFMExample_SmartFactorPCG)
// confirms the inner-solve tolerances must be tight enough that
// the iterative solver doesn't bottom out before LM converges —
// 1e-10 matches the example and keeps point accuracy within
// the test bounds. On our small problems this is ~3× faster
// than the direct Cholesky path.
params.linearSolverType = gtsam::NonlinearOptimizerParams::Iterative;
gtsam::PCGSolverParameters::shared_ptr pcg(new gtsam::PCGSolverParameters());
gtsam::PreconditionerParameters::shared_ptr preconditioner(
new gtsam::BlockJacobiPreconditionerParameters());
#if GTSAM_VERSION_NUMERIC >= 40300
// 4.3+: setter removed, fields renamed (epsilon_abs_ -> epsilon_abs).
pcg->preconditioner = preconditioner;
pcg->epsilon_abs = 1e-10;
pcg->epsilon_rel = 1e-10;
#else
// Assign the member directly instead of calling setPreconditionerParams():
// the setter does exactly this but was only added after 4.0, and the
// Android build pins GTSAM 4.0.0.
pcg->preconditioner_ = preconditioner;
pcg->epsilon_abs_ = 1e-10;
pcg->epsilon_rel_ = 1e-10;
#endif
params.iterativeParams = pcg;
gtsam::NonlinearOptimizer * optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, params);
UDEBUG("GTSAM BA optimizing (max iterations=%d, robustKernel=%f)...", iterations(), robustKernelDelta_);
result = optimizer->optimize();
finalError = optimizer->error();
UDEBUG("GTSAM BA done (initialError=%f finalError=%f time=%fs)", graph.error(initialEstimate), finalError, timer.ticks());
delete optimizer;
}
catch(const gtsam::IndeterminantLinearSystemException & e)
{
UERROR("GTSAM BA: indeterminant linear system: %s", e.what());
return optimizedPoses;
}
catch(const std::exception & e)
{
UERROR("GTSAM BA failed: %s", e.what());
return optimizedPoses;
}
if(uIsNan(finalError))
{
UERROR("GTSAM BA produced a NaN error.");
return optimizedPoses;
}
// 6) Report observations whose per-factor residual exceeded the robust
// kernel delta. Unlike g2o we don't re-optimize without them -- the
// Huber kernel has already down-weighted them in the solve.
if(outliers && robustKernelDelta_ > 0.0)
{
const double thresholdSq = robustKernelDelta_ * robustKernelDelta_;
for(std::vector<std::pair<size_t, int> >::const_iterator iter = obsFactors.begin(); iter != obsFactors.end(); ++iter)
{
if(iter->first >= graph.size()) continue;
const double e = graph.at(iter->first)->error(result);
// GTSAM returns 0.5 * r^T * Σ^{-1} * r; multiply by 2 to get chi^2.
if(2.0 * e > thresholdSq)
{
outliers->insert(iter->second);
}
}
UDEBUG("GTSAM BA: %d outlier observations flagged.", (int)outliers->size());
}
// 7) Read back poses (camera frame -> body frame via localTransform^-1).
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first <= 0)
{
continue;
}
const gtsam::Symbol xkey('x', iter->first * GTSAM_BA_MULTICAM_OFFSET);
if(!result.exists(xkey))
{
continue;
}
Transform t = Transform::fromEigen4d(result.at<gtsam::Pose3>(xkey).matrix());
t *= models.at(iter->first)[0].localTransform().inverse();
if(t.isNull())
{
UERROR("GTSAM BA: optimized pose %d is null", iter->first);
optimizedPoses.clear();
return optimizedPoses;
}
if(isSlam2d())
{
// Same snap-back idiom as g2o / Ceres: PlanarBodyZFactor locks
// each non-root body z to its initial value, but tiny LM-residual
// slack can still leave a sub-mm drift. Snap z back exactly when
// within tolerance; fall back to a 2D-projected delta otherwise.
if(std::fabs(t.z() - iter->second.z()) < 0.001f)
{
t.z() = iter->second.z();
}
else
{
UWARN("Planar constraints didn't work!? original pose (%d), pose %s -> %s. Falling back to old approach.",
iter->first,
iter->second.prettyPrint().c_str(),
t.prettyPrint().c_str());
const Transform delta = iter->second.inverse() * t;
t = iter->second * delta.to3DoF();
}
}
optimizedPoses.insert(std::make_pair(iter->first, t));
}
// 8) Read back 3D points.
for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter != points3DMap.end(); ++iter)
{
// SmartFactor landmarks aren't graph variables — triangulate from
// the optimized poses instead.
std::map<int, SmartMono::shared_ptr>::const_iterator sit = smartByWord.find(iter->first);
if(sit != smartByWord.end())
{
auto p = sit->second->point(result);
if(p)
{
iter->second = cv::Point3f(
static_cast<float>(p->x()),
static_cast<float>(p->y()),
static_cast<float>(p->z()));
}
continue;
}
const gtsam::Symbol pkey = point3dSymbol(iter->first);
if(insertedPoints.count(pkey) && result.exists(pkey))
{
const gtsam::Point3 p = result.at<gtsam::Point3>(pkey);
iter->second = cv::Point3f(static_cast<float>(p.x()), static_cast<float>(p.y()), static_cast<float>(p.z()));
}
}
#else
UERROR("Not built with GTSAM support!");
(void)rootId;
(void)poses;
(void)links;
(void)models;
(void)points3DMap;
(void)wordReferences;
(void)outliers;
#endif
return optimizedPoses;
}
} /* namespace rtabmap */
corelib/src/optimizer/ceres/bundle/between_cameras_error.h
+114
View File
@@ -0,0 +1,114 @@
/*
* between_cameras_error.h
*
* Pose-graph constraint between two BA camera variables. Used to fold
* relative-pose link measurements (kNeighbor edges) into the BA cost so
* Ceres BA can use the same pose-graph chain that g2o (EdgeSBACam) and
* GTSAM (BetweenFactor<Pose3>) include in their BA paths.
*
* Camera parameterization matches SnavelyReprojectionError: 6 doubles
* per camera [angle_axis (3), translation (3)] in world-to-camera
* convention (a point X_world is mapped to X_cam = R*X_world + t).
*/
#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BETWEEN_CAMERAS_ERROR_H_
#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BETWEEN_CAMERAS_ERROR_H_
#include "Eigen/Core"
#include "ceres/autodiff_cost_function.h"
#include "ceres/rotation.h"
namespace ceres {
// Relative-pose constraint between two cameras, expressed in the SAME frame
// convention as g2o's EdgeSBACam: the measurement is the pose of camera B as
// seen from camera A's frame (i.e., the relative transform from cam_a to
// cam_b). Residual is 6-D [Δt; Δrot] in se(3) — Δt = predicted_t - measured_t
// and Δrot = log(R_meas^T * R_predicted) — then whitened by the square root
// of the information matrix.
struct BetweenCamerasError {
BetweenCamerasError(const Eigen::Vector3d & translation_measured,
const Eigen::Vector3d & angle_axis_measured,
const Eigen::Matrix<double, 6, 6> & sqrt_information)
: t_meas_(translation_measured),
aa_meas_(angle_axis_measured),
sqrt_info_(sqrt_information) {}
template <typename T>
bool operator()(const T * const cam_a,
const T * const cam_b,
T * residuals_ptr) const {
// Camera layout: [aa(3), t(3)] -- world-to-camera.
const T * aa_a = cam_a;
const T * t_a = cam_a + 3;
const T * aa_b = cam_b;
const T * t_b = cam_b + 3;
// Predicted relative transform "cam_b in cam_a's frame":
// R_ab = R_a * R_b^T
// t_ab = t_a - R_ab * t_b
T q_a[4];
T q_b[4];
ceres::AngleAxisToQuaternion(aa_a, q_a);
ceres::AngleAxisToQuaternion(aa_b, q_b);
// q_b_inv = conjugate (unit quat assumed).
T q_b_inv[4] = { q_b[0], -q_b[1], -q_b[2], -q_b[3] };
T q_ab[4];
ceres::QuaternionProduct(q_a, q_b_inv, q_ab);
T aa_ab[3];
ceres::QuaternionToAngleAxis(q_ab, aa_ab);
T R_ab_tb[3];
ceres::AngleAxisRotatePoint(aa_ab, t_b, R_ab_tb);
T t_ab[3] = { t_a[0] - R_ab_tb[0],
t_a[1] - R_ab_tb[1],
t_a[2] - R_ab_tb[2] };
// Translation residual: predicted - measured.
T t_residual[3] = { t_ab[0] - T(t_meas_(0)),
t_ab[1] - T(t_meas_(1)),
t_ab[2] - T(t_meas_(2)) };
// Rotation residual: angle-axis of (R_meas^T * R_predicted).
T q_meas[4];
const T aa_meas_T[3] = { T(aa_meas_(0)), T(aa_meas_(1)), T(aa_meas_(2)) };
ceres::AngleAxisToQuaternion(aa_meas_T, q_meas);
T q_meas_inv[4] = { q_meas[0], -q_meas[1], -q_meas[2], -q_meas[3] };
T q_err[4];
ceres::QuaternionProduct(q_meas_inv, q_ab, q_err);
T aa_err[3];
ceres::QuaternionToAngleAxis(q_err, aa_err);
Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
residuals(0) = t_residual[0];
residuals(1) = t_residual[1];
residuals(2) = t_residual[2];
residuals(3) = aa_err[0];
residuals(4) = aa_err[1];
residuals(5) = aa_err[2];
residuals.applyOnTheLeft(sqrt_info_.template cast<T>());
return true;
}
static ceres::CostFunction * Create(
const Eigen::Vector3d & translation_measured,
const Eigen::Vector3d & angle_axis_measured,
const Eigen::Matrix<double, 6, 6> & sqrt_information) {
return new ceres::AutoDiffCostFunction<BetweenCamerasError, 6, 6, 6>(
new BetweenCamerasError(translation_measured, angle_axis_measured, sqrt_information));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
const Eigen::Vector3d t_meas_;
const Eigen::Vector3d aa_meas_;
const Eigen::Matrix<double, 6, 6> sqrt_info_;
};
} // namespace ceres
#endif // CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BETWEEN_CAMERAS_ERROR_H_
corelib/src/optimizer/ceres/bundle/planar_constraint_error.h
+68
View File
@@ -0,0 +1,68 @@
/*
* planar_constraint_error.h
*
* Unary "lock the robot to a horizontal plane" constraint for 2D BA.
* Mirrors g2o's EdgeSBACamPrior with pinfo(2,2) = 1e9: constrains the
* BODY-frame z coordinate of the camera vertex to its initial value, so
* the recovered trajectory stays on the floor. Only the z translation
* axis carries weight; everything else is free.
*
* Snavely camera parameterization is [aa_cw (3), t_cw (3)] (world-to-
* camera). Given the body-to-camera (= localTransform) rotation R_bc and
* translation t_bc, the body position in world coordinates is:
*
* body_world = R_cw^T * (-R_bc^T * t_bc - t_cw)
*
* Only z is extracted and compared to the initial body z.
*/
#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_PLANAR_CONSTRAINT_ERROR_H_
#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_PLANAR_CONSTRAINT_ERROR_H_
#include "Eigen/Core"
#include "ceres/autodiff_cost_function.h"
#include "ceres/rotation.h"
namespace ceres {
struct PlanarConstraintError {
PlanarConstraintError(const Eigen::Matrix3d & R_bc,
const Eigen::Vector3d & t_bc,
double initial_body_z,
double sqrt_information)
: neg_Rbc_T_tbc_(-R_bc.transpose() * t_bc),
initial_body_z_(initial_body_z),
sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T * const camera, T * residuals) const {
// camera = [aa_cw (3), t_cw (3)] (Snavely / world-to-camera).
// body_world = R_cw^T * (-R_bc^T * t_bc - t_cw)
const T diff[3] = { T(neg_Rbc_T_tbc_(0)) - camera[3],
T(neg_Rbc_T_tbc_(1)) - camera[4],
T(neg_Rbc_T_tbc_(2)) - camera[5] };
const T inv_aa[3] = { -camera[0], -camera[1], -camera[2] };
T body_world[3];
ceres::AngleAxisRotatePoint(inv_aa, diff, body_world);
residuals[0] = (body_world[2] - T(initial_body_z_)) * T(sqrt_information_);
return true;
}
static ceres::CostFunction * Create(const Eigen::Matrix3d & R_bc,
const Eigen::Vector3d & t_bc,
double initial_body_z,
double sqrt_information) {
return new ceres::AutoDiffCostFunction<PlanarConstraintError, 1, 6>(
new PlanarConstraintError(R_bc, t_bc, initial_body_z, sqrt_information));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
const Eigen::Vector3d neg_Rbc_T_tbc_; // = -R_bc^T * t_bc (constant)
const double initial_body_z_;
const double sqrt_information_;
};
} // namespace ceres
#endif // CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_PLANAR_CONSTRAINT_ERROR_H_
corelib/src/optimizer/ceres/bundle/snavely_stereo_reprojection_error.h
+104
View File
@@ -0,0 +1,104 @@
/*
* snavely_stereo_reprojection_error.h
*
* Stereo analogue of SnavelyReprojectionError: outputs 3 residuals
* (u, v, u-disparity) so the disparity (depth) measurement pins the
* z component of each landmark relative to its observing camera. With
* at least one stereo observation per point the 7-DOF mono BA gauge
* collapses to 6 -- absolute scale becomes observable.
*/
#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_STEREO_REPROJECTION_ERROR_H_
#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_STEREO_REPROJECTION_ERROR_H_
#include "ceres/ceres.h"
#include "ceres/rotation.h"
namespace ceres {
// Templated pinhole stereo camera model. Camera parameterization is
// the same as SnavelyReprojectionError (3 angle-axis + 3 translation).
// The stereo channel is encoded via the right-image x coordinate, i.e.
// u_right = u_left - disparity, where disparity = baseline * fx / Z.
// observed_disparity is the measured disparity (precomputed from the
// observation's depth: disparity = baseline * fx / depth).
//
// inv_sigma_uv and inv_sigma_d are 1/sigma_pixel and 1/sigma_disparity --
// they let the cost function reproduce g2o's per-axis information matrix
// (1/pixelVariance on u/v, 1/disparityVariance on disparity). Pass
// inv_sigma_uv = inv_sigma_d = 1 for isotropic behavior.
struct SnavelyStereoReprojectionError {
SnavelyStereoReprojectionError(
double observed_x,
double observed_y,
double observed_disparity,
double fx,
double fy,
double baseline_fx,
double inv_sigma_uv,
double inv_sigma_d)
: observed_x(observed_x),
observed_y(observed_y),
observed_disparity(observed_disparity),
fx(fx),
fy(fy),
baseline_fx(baseline_fx),
inv_sigma_uv(inv_sigma_uv),
inv_sigma_d(inv_sigma_d) {}
template <typename T>
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Pinhole projection.
T xp = p[0] / p[2];
T yp = p[1] / p[2];
T predicted_x = fx * xp;
T predicted_y = fy * yp;
// disparity = baseline * fx / Z
T predicted_disparity = T(baseline_fx) / p[2];
residuals[0] = (predicted_x - observed_x) * T(inv_sigma_uv);
residuals[1] = (predicted_y - observed_y) * T(inv_sigma_uv);
residuals[2] = (predicted_disparity - observed_disparity) * T(inv_sigma_d);
return true;
}
static ceres::CostFunction* Create(double observed_x,
double observed_y,
double observed_disparity,
double fx,
double fy,
double baseline_fx,
double inv_sigma_uv,
double inv_sigma_d) {
return (new ceres::AutoDiffCostFunction<SnavelyStereoReprojectionError, 3, 6, 3>(
new SnavelyStereoReprojectionError(
observed_x, observed_y, observed_disparity, fx, fy, baseline_fx,
inv_sigma_uv, inv_sigma_d)));
}
double observed_x;
double observed_y;
double observed_disparity;
double fx;
double fy;
double baseline_fx; // baseline * fx (cached)
double inv_sigma_uv; // 1 / sqrt(pixelVariance)
double inv_sigma_d; // 1 / sqrt(disparityVariance)
};
} // namespace ceres
#endif // CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_STEREO_REPROJECTION_ERROR_H_
corelib/src/optimizer/g2o/edge_sbacam_prior.h
+4 -4
View File
@@ -84,7 +84,7 @@ class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, VertexCam> {
_inverseMeasurement = m.inverse();
}
virtual bool setMeasurementData(const double* d) override {
virtual bool setMeasurementData(const double* d) {
Eigen::Map<const Eigen::Matrix<double, 7, 1, Eigen::ColMajor> > v(d);
// SE3Quat expects [x, y, z, qx, qy, qz, qw]
_measurement.fromVector(v);
@@ -92,7 +92,7 @@ class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, VertexCam> {
return true;
}
virtual bool getMeasurementData(double* d) const override {
virtual bool getMeasurementData(double* d) const {
Eigen::Map<Eigen::Matrix<double, 7, 1, Eigen::ColMajor> > v(d);
// Returns [x, y, z, qx, qy, qz, qw]
v = _measurement.toVector();
@@ -124,8 +124,8 @@ class EdgeSBACamPrior : public g2o::BaseUnaryEdge<6, g2o::SE3Quat, VertexCam> {
v->setEstimate(newEstimate);
}
virtual bool read(std::istream& is) override { return true; }
virtual bool write(std::ostream& os) const override { return true; }
virtual bool read(std::istream& is) { return true; }
virtual bool write(std::ostream& os) const { return true; }
protected:
g2o::SE3Quat _inverseMeasurement;
g2o::SE3Quat _cameraInvLocalTransform;
corelib/src/optimizer/gtsam/PlanarBodyZFactor.h
+59
View File
@@ -0,0 +1,59 @@
/**
* Author: Mathieu Labbe
*/
/**
* Unary planar constraint mirroring g2o's EdgeSBACamPrior (pinfo(2,2) = 1e9):
* locks the BODY-frame z of the camera vertex to its initial value, leaving
* lateral motion + yaw free. Used when isSlam2d() is true in BA.
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
namespace rtabmap {
class PlanarBodyZFactor: public gtsam::NoiseModelFactor1<gtsam::Pose3> {
gtsam::Pose3 camera_to_body_;
double initial_body_z_;
public:
PlanarBodyZFactor(gtsam::Key poseKey,
const gtsam::Pose3 & camera_to_body,
double initial_body_z,
const gtsam::SharedNoiseModel & model):
gtsam::NoiseModelFactor1<gtsam::Pose3>(model, poseKey),
camera_to_body_(camera_to_body),
initial_body_z_(initial_body_z) {}
gtsam::Vector evaluateError(const gtsam::Pose3 & camPose,
#if GTSAM_VERSION_NUMERIC >= 40300
gtsam::OptionalMatrixType H = OptionalNone) const override
#else
boost::optional<gtsam::Matrix &> H = boost::none) const override
#endif
{
const auto error_fn = [this](const gtsam::Pose3 & p) {
gtsam::Vector1 e;
e(0) = p.compose(camera_to_body_).translation().z() - initial_body_z_;
return e;
};
if(H)
{
*H = gtsam::numericalDerivative11<gtsam::Vector1, gtsam::Pose3>(error_fn, camPose);
}
return error_fn(camPose);
}
}; // PlanarBodyZFactor
} /// namespace rtabmap
corelib/src/optimizer/gtsam/XYZFactor.h
+4
View File
@@ -59,6 +59,10 @@ public:
#else
boost::optional<gtsam::Matrix&> H = boost::none) const {
#endif
if(H)
{
*H = gtsam::Matrix::Identity(3, 3);
}
return (gtsam::Vector3() << p.x() - mx_, p.y() - my_, p.z() - mz_).finished();
}
};
corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h
+11 -2
View File
@@ -41,14 +41,23 @@ namespace vertigo {
#endif
{
// calculate error
// calculate error: f(p1, p2, s) = E_raw(p1, p2) * s
gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
// Jacobian w.r.t. the switch tangent: dE/ds = E_raw (the
// unscaled error). Must be captured BEFORE scaling `error` by
// s.value() below. Setting H3 to the scaled error (= E_raw*s)
// was a bug: at small s the switch gradient vanishes
// quadratically with s, so the optimizer stalls before driving
// the switch to 0 -- visibly worse outlier rejection than the
// g2o equivalent (EdgeSE3Switchable, which has the correct
// constant Jacobian-element).
if (H3) *H3 = error;
error *= s.value();
// handle derivatives
if (H1) *H1 = *H1 * s.value();
if (H2) *H2 = *H2 * s.value();
if (H3) *H3 = error;
return error;
};
corelib/src/pcl18/surface/impl/texture_mapping.hpp
+3 -3
View File
@@ -1074,7 +1074,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
std::vector<Eigen::Affine3f> invCamTransform(cameras.size());
std::vector<std::list<int> > faceCameras(faces.size());
std::string msg = uFormat("Computing visible faces per cam (%d faces, %d cams)", (int)faces.size(), (int)cameras.size());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
@@ -1277,7 +1277,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
}
msg = uFormat("Processed camera %d/%d: %d occluded and %d spurious polygons out of %d", (int)current_cam+1, (int)cameras.size(), (int)occludedFaces.size(), clusterFaces, (int)visibilityIndices.size());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
@@ -1287,7 +1287,7 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras2 (
}
msg = uFormat("Texturing %d polygons...", (int)faces.size());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
corelib/src/python/PyDescriptor.cpp
+23 -1
View File
@@ -23,6 +23,7 @@ PyDescriptor::PyDescriptor(
dim_(Parameters::defaultPyDescriptorDim())
{
UDEBUG("");
PythonInterface::instance("PyDescriptor");
this->parseParameters(parameters);
}
@@ -47,7 +48,10 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters)
std::string previousPath = path_;
Parameters::parse(parameters, Parameters::kPyDescriptorPath(), path_);
Parameters::parse(parameters, Parameters::kPyDescriptorDim(), dim_);
path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
if(!path_.empty() && path_[0] == '~' && (path_.size() == 1 || path_[1] == '/' || path_[1] == '\\'))
{
path_ = UDirectory::homeDir() + path_.substr(1);
}
UINFO("path = %s", path_.c_str());
UINFO("dim = %d", dim_);
UTimer timer;
@@ -77,15 +81,33 @@ void PyDescriptor::parseParameters(const ParametersMap & parameters)
{
return;
}
// Pre-validate the path: PyImport_Import on a non-existent script
// fails without setting a Python exception, after which
// getPythonTraceback() dereferences NULL pointers in Py_BuildValue
// and crashes. Mirrors the check in PyDetector::PyDetector().
if(!UFile::exists(path_) || UFile::getExtension(path_).compare("py") != 0)
{
UERROR("Cannot initialize Python descriptor, the path is not valid: \"%s\"=\"%s\"",
Parameters::kPyDescriptorPath().c_str(), path_.c_str());
return;
}
std::string matcherPythonDir = UDirectory::getDir(path_);
if(!matcherPythonDir.empty())
{
// For windows
matcherPythonDir = uReplaceChar(matcherPythonDir, '\\', '/');
PyRun_SimpleString("import sys");
PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
}
_import_array();
// Invalidate importlib's directory-listing caches so a script created
// after sys.path was first scanned in this process is still found.
// Without this, the second Py* instance pointing at a freshly-written
// script in an already-known directory fails with ModuleNotFoundError.
PyRun_SimpleString("import importlib; importlib.invalidate_caches()");
std::string scriptName = uSplit(UFile::getName(path_), '.').front();
PyObject * pName = PyUnicode_FromString(scriptName.c_str());
UDEBUG("PyImport_Import() beg");
corelib/src/python/PyDescriptor.h
+2 -1
View File
@@ -8,12 +8,13 @@
#include <rtabmap/core/GlobalDescriptorExtractor.h>
#include "rtabmap/core/PythonInterface.h"
#include "rtabmap/core/rtabmap_core_export.h"
#include <Python.h>
namespace rtabmap
{
class PyDescriptor : public GlobalDescriptorExtractor
class RTABMAP_CORE_EXPORT PyDescriptor : public GlobalDescriptorExtractor
{
public:
PyDescriptor(const ParametersMap & parameters = ParametersMap());
corelib/src/python/PyDetector.cpp
+13 -1
View File
@@ -24,6 +24,7 @@ PyDetector::PyDetector(const ParametersMap & parameters) :
path_(Parameters::defaultPyDetectorPath()),
cuda_(Parameters::defaultPyDetectorCuda())
{
PythonInterface::instance("PyDetector");
this->parseParameters(parameters);
UDEBUG("path = %s", path_.c_str());
@@ -39,12 +40,20 @@ PyDetector::PyDetector(const ParametersMap & parameters) :
std::string matcherPythonDir = UDirectory::getDir(path_);
if(!matcherPythonDir.empty())
{
// For Windows
matcherPythonDir = uReplaceChar(matcherPythonDir, '\\', '/');
PyRun_SimpleString("import sys");
PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
}
_import_array();
// Invalidate importlib's directory-listing caches so a script created
// after sys.path was first scanned in this process is still found. Without
// this, the second PyDetector instance pointing at a freshly-written
// script in an already-known directory fails with ModuleNotFoundError.
PyRun_SimpleString("import importlib; importlib.invalidate_caches()");
std::string scriptName = uSplit(UFile::getName(path_), '.').front();
PyObject * pName = PyUnicode_FromString(scriptName.c_str());
UDEBUG("PyImport_Import() beg");
@@ -81,7 +90,10 @@ void PyDetector::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kPyDetectorPath(), path_);
Parameters::parse(parameters, Parameters::kPyDetectorCuda(), cuda_);
path_ = uReplaceChar(path_, '~', UDirectory::homeDir());
if(!path_.empty() && path_[0] == '~' && (path_.size() == 1 || path_[1] == '/' || path_[1] == '\\'))
{
path_ = UDirectory::homeDir() + path_.substr(1);
}
}
std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask)
corelib/src/python/PyDetector.h
+7 -1
View File
@@ -12,12 +12,13 @@
#include <vector>
#include "rtabmap/core/PythonInterface.h"
#include "rtabmap/core/rtabmap_core_export.h"
#include <Python.h>
namespace rtabmap
{
class PyDetector : public Feature2D
class RTABMAP_CORE_EXPORT PyDetector : public Feature2D
{
public:
PyDetector(const ParametersMap & parameters = ParametersMap());
@@ -25,6 +26,11 @@ public:
virtual void parseParameters(const ParametersMap & parameters);
virtual Feature2D::Type getType() const {return kFeaturePyDetector;}
// PyDetector hands off CUDA usage to the python script -- the C++
// wrapper can't introspect it, so we report capability is "possible"
// whenever the python interpreter is built in. The actual hardware
// presence + the script's own decision is out of our hands.
virtual bool isGpuAvailable() const override {return true;}
private:
virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
corelib/src/python/PyMatcher.cpp
+16 -2
View File
@@ -30,8 +30,14 @@ PyMatcher::PyMatcher(
iterations_(iterations),
cuda_(cuda)
{
path_ = uReplaceChar(pythonMatcherPath, '~', UDirectory::homeDir());
model_ = uReplaceChar(model, '~', UDirectory::homeDir());
PythonInterface::instance("PyMatcher");
auto expandTilde = [](const std::string & p) -> std::string {
if(!p.empty() && p[0] == '~' && (p.size() == 1 || p[1] == '/' || p[1] == '\\'))
return UDirectory::homeDir() + p.substr(1);
return p;
};
path_ = expandTilde(pythonMatcherPath);
model_ = expandTilde(model);
UINFO("path = %s", path_.c_str());
UINFO("model = %s", model_.c_str());
@@ -46,12 +52,20 @@ PyMatcher::PyMatcher(
std::string matcherPythonDir = UDirectory::getDir(path_);
if(!matcherPythonDir.empty())
{
// For windows:
matcherPythonDir = uReplaceChar(matcherPythonDir, '\\', '/');
PyRun_SimpleString("import sys");
PyRun_SimpleString(uFormat("sys.path.append(\"%s\")", matcherPythonDir.c_str()).c_str());
}
_import_array();
// Invalidate importlib's directory-listing caches so a script created
// after sys.path was first scanned in this process is still found.
// Without this, the second Py* instance pointing at a freshly-written
// script in an already-known directory fails with ModuleNotFoundError.
PyRun_SimpleString("import importlib; importlib.invalidate_caches()");
std::string scriptName = uSplit(UFile::getName(path_), '.').front();
PyObject * pName = PyUnicode_FromString(scriptName.c_str());
UDEBUG("PyImport_Import");
corelib/src/python/PyMatcher.h
+2 -1
View File
@@ -10,13 +10,14 @@
#include <opencv2/core/types.hpp>
#include <opencv2/core/mat.hpp>
#include "rtabmap/core/PythonInterface.h"
#include "rtabmap/core/rtabmap_core_export.h"
#include <vector>
#include <Python.h>
namespace rtabmap
{
class PyMatcher
class RTABMAP_CORE_EXPORT PyMatcher
{
public:
PyMatcher(const std::string & pythonMatcherPath,
corelib/src/python/PythonInterface.cpp
+55 -5
View File
@@ -8,14 +8,39 @@
#include <rtabmap/core/PythonInterface.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UConversion.h>
#include <pybind11/embed.h>
#include <filesystem>
#include <thread>
namespace rtabmap {
PythonInterface::PythonInterface()
namespace {
// Captured when librtabmap_core is loaded. The dynamic loader runs static
// initializers on the main thread before main(), so this records the main
// thread id (as long as the library isn't dlopen'd from a worker thread).
const std::thread::id g_mainThreadId = std::this_thread::get_id();
}
PythonInterface & PythonInterface::instance(const std::string & caller)
{
UINFO("Initialize python interpreter");
// Meyers singleton: thread-safe construction in C++11, destroyed at exit.
// The constructor asserts it runs on the main thread; the caller tag
// from the first invocation is captured into the assertion message.
static PythonInterface inst(caller);
return inst;
}
PythonInterface::PythonInterface(const std::string & caller)
{
UASSERT_MSG(std::this_thread::get_id() == g_mainThreadId,
uFormat("PythonInterface must be created on the main thread "
"(first construction triggered by \"%s\"). Call "
"PythonInterface::instance() early in main() before "
"any worker thread touches a Python-backed class.",
caller.empty()?"<unspecified>":caller.c_str()).c_str());
UINFO("Initialize python interpreter (triggered by \"%s\")",
caller.empty()?"<unspecified>":caller.c_str());
guard_ = new pybind11::scoped_interpreter();
// Tell Python to look in this directory for DLLs
@@ -40,12 +65,28 @@ std::string getPythonTraceback()
{
// Author: https://stackoverflow.com/questions/41268061/c-c-python-exception-traceback-not-being-generated
// Early-exit when there is no active Python exception: callers commonly
// log this after any Python C-API failure, but some failures (notably
// PyImport_Import returning NULL after repeated load/unload cycles) do
// not set an exception. Without this guard, PyErr_Fetch returns NULL
// triples and Py_BuildValue("OOO", NULL, NULL, NULL) below crashes.
if(!PyErr_Occurred())
{
return "<no python exception set>";
}
PyObject* type;
PyObject* value;
PyObject* traceback;
PyErr_Fetch(&type, &value, &traceback);
PyErr_NormalizeException(&type, &value, &traceback);
// PyErr_Fetch may leave value/traceback NULL even when an exception was
// active. Py_BuildValue("OOO", ...) rejects NULL slots and raises
// SystemError, so substitute Py_None for any missing component.
if(!type) { Py_INCREF(Py_None); type = Py_None; }
if(!value) { Py_INCREF(Py_None); value = Py_None; }
if(!traceback) { Py_INCREF(Py_None); traceback = Py_None; }
std::string fcn = "";
fcn += "def get_pretty_traceback(exc_type, exc_value, exc_tb):\n";
@@ -60,13 +101,22 @@ std::string getPythonTraceback()
UASSERT(mod);
PyObject* method = PyObject_GetAttrString(mod, "get_pretty_traceback");
UASSERT(method);
PyObject* outStr = PyObject_CallObject(method, Py_BuildValue("OOO", type, value, traceback));
PyObject* args = Py_BuildValue("OOO", type, value, traceback);
PyObject* outStr = args ? PyObject_CallObject(method, args) : nullptr;
std::string pretty;
if(outStr)
pretty = PyBytes_AsString(PyUnicode_AsASCIIString(outStr));
{
PyObject* asciiStr = PyUnicode_AsASCIIString(outStr);
if(asciiStr)
{
pretty = PyBytes_AsString(asciiStr);
Py_DECREF(asciiStr);
}
}
Py_XDECREF(args);
Py_DECREF(method);
Py_DECREF(outStr);
Py_XDECREF(outStr); // outStr may be NULL if PyObject_CallObject failed
Py_DECREF(mod);
return pretty;
corelib/src/python/rtabmap_trace_superpoint.py
+42 -9
View File
@@ -1,13 +1,46 @@
"""Trace MagicLeap's SuperPoint pretrained net to TorchScript for rtabmap.
Usage:
python rtabmap_trace_superpoint.py [--weights superpoint_v1.pth]
[--output superpoint_v1.pt]
[--model-dir <dir containing demo_superpoint.py>]
`--model-dir` is prepended to sys.path so `from demo_superpoint import ...`
resolves. Defaults to the directory of `--weights` (or cwd).
"""
import argparse
import os
import sys
from pathlib import Path
import torch
import torchvision
from demo_superpoint import SuperPointNet
model = SuperPointNet()
model.load_state_dict(torch.load("superpoint_v1.pth"))
model.eval()
example = torch.rand(1, 1, 640, 480)
traced_script_module = torch.jit.trace(model, example, check_trace=False)
traced_script_module.save("superpoint_v1.pt")
def main() -> int:
parser = argparse.ArgumentParser()
parser.add_argument("--weights", default="superpoint_v1.pth")
parser.add_argument("--output", default="superpoint_v1.pt")
parser.add_argument("--model-dir", default=None,
help="directory containing demo_superpoint.py (default: dir of --weights, then cwd)")
args = parser.parse_args()
candidates = [args.model_dir, os.path.dirname(os.path.abspath(args.weights)), os.getcwd()]
for path in candidates:
if path and path not in sys.path:
sys.path.insert(0, path)
from demo_superpoint import SuperPointNet # noqa: E402
model = SuperPointNet()
model.load_state_dict(torch.load(args.weights, map_location="cpu"))
model.eval()
example = torch.rand(1, 1, 640, 480)
traced_script_module = torch.jit.trace(model, example, check_trace=False)
os.makedirs(os.path.dirname(os.path.abspath(args.output)) or ".", exist_ok=True)
traced_script_module.save(args.output)
print(f"Saved TorchScript: {args.output}")
return 0
if __name__ == "__main__":
sys.exit(main())
corelib/src/rtflann/util/result_set.h
+4 -3
View File
@@ -107,7 +107,7 @@ public:
capacity_(capacity_)
{
// reserving capacity to prevent memory re-allocations
dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),-1));
dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),std::numeric_limits<size_t>::max()));
clear();
}
@@ -210,7 +210,7 @@ public:
KNNResultSet(int capacity) : capacity_(capacity)
{
// reserving capacity to prevent memory re-allocations
dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),-1));
dist_index_.resize(capacity_, DistIndex(std::numeric_limits<DistanceType>::max(),std::numeric_limits<size_t>::max()));
clear();
}
@@ -252,7 +252,8 @@ public:
#endif
{
// Check for duplicate indices
for (size_t j = i - 1; dist_index_[j].dist_ == dist && j--;) {
// https://github.com/flann-lib/flann/pull/472
for (size_t j = i; j-- && dist_index_[j].dist_ == dist;) {
if (dist_index_[j].index_ == index) {
return;
}
corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp
+1 -1
View File
@@ -48,7 +48,7 @@ static std::string exportSuperPointTorchScript(
const std::string weightsPath = superpointWeightsPath;
const std::string modelPath = superpointModelPath;
const std::string output = std::string(outputDir + "/superpoint_v6_from_tf.pt");
// Sanity checks
if(!UFile::exists(weightsPath)) {
UERROR("Weights not found: %s", weightsPath.c_str());
corelib/src/superpoint_rpautrat/superpoint_to_torchscript.py
+27 -4
View File
@@ -5,10 +5,19 @@ Convert PyTorch weights to TorchScript format for C++ usage.
import argparse
import os
import sys
import warnings
import torch
import torch.nn as nn
from superpoint_pytorch import SuperPoint
# rpautrat's superpoint_pytorch.py uses Python control flow on tensor shapes
# (`if image.shape[1] == 3`, `if b > 1`, ...) and arithmetic on shape ints
# (`keypoints.new_tensor([w, h])`). torch.jit.trace warns on those because
# the resulting TorchScript only generalises to inputs with the same shape.
# That's exactly our use case (single grayscale image, fixed batch=1), so
# silence the warnings to keep the C++ trace output clean.
warnings.filterwarnings("ignore", category=torch.jit.TracerWarning)
def wrap_model(model: nn.Module):
@@ -56,6 +65,10 @@ def generate_model(
device = "cuda" if cuda else "cpu"
# Imported lazily so callers that pre-set sys.path via --model-dir don't
# need superpoint_pytorch on PYTHONPATH at module import time.
from superpoint_pytorch import SuperPoint # noqa: E402
# Load SuperPoint model and weights
model = SuperPoint(
nms_radius=nms_radius,
@@ -77,8 +90,8 @@ def generate_model(
scripted = torch.jit.trace(wrapped, (dummy,), strict=False)
print("Successfully traced SuperPoint model")
# Save output
os.makedirs(os.path.dirname(output_path), exist_ok=True)
# Save output (handle bare filenames where dirname == "")
os.makedirs(os.path.dirname(os.path.abspath(output_path)) or ".", exist_ok=True)
torch.jit.save(scripted, output_path)
print(f"Converted SuperPoint weights to TorchScript: {output_path}")
@@ -88,6 +101,8 @@ if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Convert SuperPoint weights to TorchScript")
parser.add_argument("--weights", required=True, help="Path to weights file")
parser.add_argument("--output", required=True, help="Output TorchScript file")
parser.add_argument("--model-dir", default=None,
help="directory containing superpoint_pytorch.py (default: dir of --weights, then cwd)")
parser.add_argument("--cuda", action="store_true", help="Use CUDA")
parser.add_argument("--width", type=int, default=1920, help="Width of the input image")
parser.add_argument("--height", type=int, default=288, help="Height of the input image")
@@ -95,7 +110,15 @@ if __name__ == "__main__":
parser.add_argument("--threshold", type=float, default=0.005, help="Confidence threshold")
args = parser.parse_args()
print(f"Generating model from weights: {args.weights} to output: {args.output}")
# Make superpoint_pytorch.py discoverable to the lazy import in
# generate_model(). Default to the weights' directory so a co-located
# model file Just Works in the fetch_test_data.sh flow.
candidates = [args.model_dir, os.path.dirname(os.path.abspath(args.weights)), os.getcwd()]
for path in candidates:
if path and path not in sys.path:
sys.path.insert(0, path)
generate_model(
weights_path=args.weights,
output_path=args.output,
corelib/src/util2d.cpp
+105 -43
View File
@@ -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;
corelib/src/util3d.cpp
+24 -95
View File
@@ -48,7 +48,7 @@ namespace rtabmap
namespace util3d
{
cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
cv::Mat rgbFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrOrder)
{
cv::Mat frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
@@ -76,13 +76,9 @@ cv::Mat bgrFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud, bool bgrO
// return float image in meter
cv::Mat depthFromCloud(
const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
float & fx,
float & fy,
bool depth16U)
{
cv::Mat frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
fx = 0.0f; // needed to reconstruct the cloud
fy = 0.0f; // needed to reconstruct the cloud
for(unsigned int h = 0; h < cloud.height; h++)
{
for(unsigned int w = 0; w < cloud.width; w++)
@@ -102,32 +98,6 @@ cv::Mat depthFromCloud(
{
frameDepth.at<float>(h,w) = depth;
}
// update constants
if(fx == 0.0f &&
uIsFinite(cloud.at(h*cloud.width + w).x) &&
uIsFinite(depth) &&
w != cloud.width/2 &&
depth > 0)
{
fx = cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth);
if(depth16U)
{
fx*=1000.0f;
}
}
if(fy == 0.0f &&
uIsFinite(cloud.at(h*cloud.width + w).y) &&
uIsFinite(depth) &&
h != cloud.height/2 &&
depth > 0)
{
fy = cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth);
if(depth16U)
{
fy*=1000.0f;
}
}
}
}
return frameDepth;
@@ -137,16 +107,12 @@ cv::Mat depthFromCloud(
void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
cv::Mat & frameBGR,
cv::Mat & frameDepth,
float & fx,
float & fy,
bool bgrOrder,
bool depth16U)
{
frameDepth = cv::Mat(cloud.height,cloud.width,depth16U?CV_16UC1:CV_32FC1);
frameBGR = cv::Mat(cloud.height,cloud.width,CV_8UC3);
fx = 0.0f; // needed to reconstruct the cloud
fy = 0.0f; // needed to reconstruct the cloud
for(unsigned int h = 0; h < cloud.height; h++)
{
for(unsigned int w = 0; w < cloud.width; w++)
@@ -181,32 +147,6 @@ void rgbdFromCloud(const pcl::PointCloud<pcl::PointXYZRGBA> & cloud,
{
frameDepth.at<float>(h,w) = depth;
}
// update constants
if(fx == 0.0f &&
uIsFinite(cloud.at(h*cloud.width + w).x) &&
uIsFinite(depth) &&
w != cloud.width/2 &&
depth > 0)
{
fx = 1.0f/(cloud.at(h*cloud.width + w).x / ((float(w) - float(cloud.width)/2.0f) * depth));
if(depth16U)
{
fx/=1000.0f;
}
}
if(fy == 0.0f &&
uIsFinite(cloud.at(h*cloud.width + w).y) &&
uIsFinite(depth) &&
h != cloud.height/2 &&
depth > 0)
{
fy = 1.0f/(cloud.at(h*cloud.width + w).y / ((float(h) - float(cloud.height)/2.0f) * depth));
if(depth16U)
{
fy/=1000.0f;
}
}
}
}
}
@@ -1464,28 +1404,25 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
float minDepth)
{
pcl::PointCloud<pcl::PointXYZ> scan;
UASSERT(!depthImages.empty() && !cameraModels.empty());
UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
int subImageWidth = depthImages.cols/cameraModels.size();
for(int i=(int)cameraModels.size()-1; i>=0; --i)
{
if(cameraModels[i].isValidForProjection())
{
cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
UASSERT(cameraModels[i].isValidForProjection());
UASSERT(cameraModels[i].imageWidth() == subImageWidth);
UASSERT(subImageWidth*(i+1) <= depthImages.cols);
cv::Mat depth = cv::Mat(depthImages, cv::Rect(subImageWidth*i, 0, subImageWidth, depthImages.rows));
scan += laserScanFromDepthImage(
depth,
cameraModels[i].fx(),
cameraModels[i].fy(),
cameraModels[i].cx(),
cameraModels[i].cy(),
maxDepth,
minDepth,
cameraModels[i].localTransform());
}
else
{
UERROR("Camera model %d is invalid", i);
}
scan += laserScanFromDepthImage(
depth,
cameraModels[i].fx(),
cameraModels[i].fy(),
cameraModels[i].cx(),
cameraModels[i].cy(),
maxDepth,
minDepth,
cameraModels[i].localTransform());
}
return scan;
}
@@ -2629,11 +2566,7 @@ pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, uns
if(laserScan.hasRGB())
{
int * ptrInt = (int*)ptr;
int indexRGB = laserScan.getRGBOffset();
output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
LaserScan::unpackRGB(ptr[laserScan.getRGBOffset()], output.r, output.g, output.b);
}
else if(laserScan.hasIntensity())
{
@@ -2695,11 +2628,7 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, in
if(laserScan.hasRGB())
{
int * ptrInt = (int*)ptr;
int indexRGB = laserScan.getRGBOffset();
output.b = (unsigned char)(ptrInt[indexRGB] & 0xFF);
output.g = (unsigned char)((ptrInt[indexRGB] >> 8) & 0xFF);
output.r = (unsigned char)((ptrInt[indexRGB] >> 16) & 0xFF);
LaserScan::unpackRGB(ptr[laserScan.getRGBOffset()], output.r, output.g, output.b);
}
else if(laserScan.hasIntensity())
{
@@ -3325,7 +3254,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
return pointToPixel;
std::string msg = uFormat("Computing visible points per cam (%d points, %d cams)", (int)cloud.size(), (int)cameraPoses.size());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
@@ -3454,11 +3383,11 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
if(count == 0)
{
registered.clear();
UINFO("No points projected in camera %d/%d", pter->first, camIndex);
UINFO("No points projected in camera %d/%d", pter->first, (int)camIndex);
}
else
{
UDEBUG("%d points projected in camera %d/%d", count, pter->first, camIndex);
UDEBUG("%d points projected in camera %d/%d", count, pter->first, (int)camIndex);
}
for(int u=0; u<imageSize.width; ++u)
{
@@ -3516,7 +3445,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
}
msg = uFormat("Processed camera %d/%d", (int)cameraProcessed+1, (int)cameraPoses.size());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
@@ -3527,7 +3456,7 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
}
msg = uFormat("Select best camera for %d points...", (int)cloud.size());
UINFO(msg.c_str());
UINFO("%s", msg.c_str());
if(state && !state->callback(msg))
{
//cancelled!
@@ -3560,8 +3489,8 @@ std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > projectCloudToCamera
}
}
msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, colorized*100/cloud.size());
UINFO(msg.c_str());
msg = uFormat("Process %d points...done! (%d [%d%%] projected in cameras)", (int)cloud.size(), colorized, (int)(colorized*100/cloud.size()));
UINFO("%s", msg.c_str());
if(state)
{
state->callback(msg);
corelib/src/util3d_correspondences.cpp
+15 -33
View File
@@ -173,15 +173,22 @@ inline void extractXYZCorrespondencesImpl(const std::list<std::pair<cv::Point2f,
const pcl::PointCloud<PointT> & cloud1,
const pcl::PointCloud<PointT> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis)
pcl::PointCloud<pcl::PointXYZ> & inliers2)
{
UASSERT(cloud1.empty() || cloud1.isOrganized());
UASSERT(cloud2.empty() || cloud2.isOrganized());
for(std::list<std::pair<cv::Point2f, cv::Point2f> >::const_iterator iter = correspondences.begin();
iter!=correspondences.end();
++iter)
{
PointT pt1 = cloud1.at(int(iter->first.y+0.5f) * cloud1.width + int(iter->first.x+0.5f));
PointT pt2 = cloud2.at(int(iter->second.y+0.5f) * cloud2.width + int(iter->second.x+0.5f));
int u1 = int(iter->first.x+0.5f);
int v1 = int(iter->first.y+0.5f);
int u2 = int(iter->second.x+0.5f);
int v2 = int(iter->second.y+0.5f);
UASSERT(v1>=0 && v1 < (int)cloud1.height && u1>=0 && u1<(int)cloud1.width);
UASSERT(v2>=0 && v2 < (int)cloud2.height && u2>=0 && u2<(int)cloud2.width);
PointT pt1 = cloud1.at(v1 * cloud1.width + u1);
PointT pt2 = cloud2.at(v2 * cloud2.width + u2);
if(pcl::isFinite(pt1) &&
pcl::isFinite(pt2))
{
@@ -195,19 +202,17 @@ void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis)
pcl::PointCloud<pcl::PointXYZ> & inliers2)
{
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2);
}
void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2f> > & correspondences,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud1,
const pcl::PointCloud<pcl::PointXYZRGB> & cloud2,
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
char depthAxis)
pcl::PointCloud<pcl::PointXYZ> & inliers2)
{
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2, depthAxis);
extractXYZCorrespondencesImpl(correspondences, cloud1, cloud2, inliers1, inliers2);
}
int countUniquePairs(const std::multimap<int, pcl::PointXYZ> & wordsA,
@@ -274,29 +279,6 @@ void filterMaxDepth(pcl::PointCloud<pcl::PointXYZ> & inliers1,
}
}
// a kdtree is constructed with cloud_target, then nearest neighbor
// is computed for each cloud_source points.
int getCorrespondencesCount(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_target,
float maxDistance)
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
kdTree->setInputCloud(cloud_target);
int count = 0;
float sqrdMaxDistance = maxDistance * maxDistance;
for(unsigned int i=0; i<cloud_source->size(); ++i)
{
std::vector<int> ind(1);
std::vector<float> dist(1);
if(kdTree->nearestKSearch(cloud_source->at(i), 1, ind, dist) && dist[0] < sqrdMaxDistance)
{
++count;
}
}
return count;
}
/**
* if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
* realPairsCount = 5
corelib/src/util3d_features.cpp
+14 -13
View File
@@ -51,19 +51,6 @@ namespace rtabmap
namespace util3d
{
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel,
float minDepth,
float maxDepth)
{
UASSERT(cameraModel.isValidForProjection());
std::vector<CameraModel> models;
models.push_back(cameraModel);
return generateKeypoints3DDepth(keypoints, depth, models, minDepth, maxDepth);
}
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
@@ -119,6 +106,19 @@ std::vector<cv::Point3f> generateKeypoints3DDepth(
return keypoints3d;
}
std::vector<cv::Point3f> generateKeypoints3DDepth(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & depth,
const CameraModel & cameraModel,
float minDepth,
float maxDepth)
{
UASSERT(cameraModel.isValidForProjection());
std::vector<CameraModel> models;
models.push_back(cameraModel);
return generateKeypoints3DDepth(keypoints, depth, models, minDepth, maxDepth);
}
std::vector<cv::Point3f> generateKeypoints3DDisparity(
const std::vector<cv::KeyPoint> & keypoints,
const cv::Mat & disparity,
@@ -416,6 +416,7 @@ std::multimap<int, cv::KeyPoint> aggregate(
const std::list<int> & wordIds,
const std::vector<cv::KeyPoint> & keypoints)
{
UASSERT(wordIds.size() == keypoints.size());
std::multimap<int, cv::KeyPoint> words;
std::vector<cv::KeyPoint>::const_iterator kpIter = keypoints.begin();
for(std::list<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
corelib/src/util3d_filtering.cpp
+261 -104
View File
@@ -87,7 +87,10 @@ LaserScan commonFiltering(
if(!scan.isEmpty())
{
// combined downsampling and range filtering step
if(downsamplingStep<=1 || scan.size() <= downsamplingStep)
if(downsamplingStep<=1 ||
scan.size() <= downsamplingStep ||
(scan.data().rows > scan.data().cols && scan.data().rows <= downsamplingStep) ||
(scan.data().cols > scan.data().rows && scan.data().cols <= downsamplingStep))
{
downsamplingStep = 1;
}
@@ -179,7 +182,7 @@ LaserScan commonFiltering(
cloud = voxelize(cloud, voxelSize);
float ratio = float(cloud->size()) / scan.size();
scanMaxPts = int(float(scanMaxPts) * ratio);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), (int)cloud->size(), scan.maxPoints(), scanMaxPts);
}
if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
{
@@ -212,7 +215,7 @@ LaserScan commonFiltering(
cloud = voxelize(cloud, voxelSize);
float ratio = float(cloud->size()) / scan.size();
scanMaxPts = int(float(scanMaxPts) * ratio);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), (int)cloud->size(), scan.maxPoints(), scanMaxPts);
}
if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
{
@@ -272,7 +275,7 @@ LaserScan commonFiltering(
cloud = voxelize(cloud, voxelSize);
float ratio = float(cloud->size()) / scan.size();
scanMaxPts = int(float(scanMaxPts) * ratio);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), cloud->size(), scan.maxPoints(), scanMaxPts);
UDEBUG("Voxel filtering scan (voxel=%f m): %d -> %d (scanMaxPts=%d->%d)", voxelSize, scan.size(), (int)cloud->size(), scan.maxPoints(), scanMaxPts);
}
if(cloud->size() && (normalK > 0 || normalRadius>0.0f))
{
@@ -325,7 +328,11 @@ LaserScan commonFiltering(
if(scan.size() && !scan.is2d() && scan.hasNormals() && groundNormalsUp>0.0f)
{
scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,10), groundNormalsUp);
// The scan is still in sensor frame here (we never apply its local transform),
// so the view point is the sensor origin. Note that this makes the ground test
// of adjustNormalsToViewPoint() use the sensor Z axis, which is vertical only
// if the sensor is mounted level.
scan = util3d::adjustNormalsToViewPoint(scan, Eigen::Vector3f(0,0,0), groundNormalsUp);
}
}
return scan;
@@ -548,27 +555,97 @@ LaserScan downsample(
int step)
{
UASSERT(step > 0);
if(step <= 1 || scan.size() <= step)
cv::Mat output;
if(step <= 1 ||
(scan.data().cols > scan.data().rows ? scan.data().cols <= step : scan.data().rows <= step))
{
// no sampling
return scan;
}
else if(scan.isOrganized())
{
// Organized LaserScan
if(scan.data().rows <= scan.data().cols/4 ||
scan.data().cols<= scan.data().rows/4)
{
// Assuming LiDAR point cloud (e.g, 2048x64 or 32x1024),
// for which the lower dimension is the number of rings.
// Downsample each ring by the step.
// Example data packed:
// <ringA-1, ringB-1, ringC-1, ringD-1;
// ringA-2, ringB-2, ringC-2, ringD-2;
// ringA-3, ringB-3, ringC-3, ringD-3;
// ringA-4, ringB-4, ringC-4, ringD-4;
// ringA-#, ringB-#, ringC-#, ringD-#>
// or
// <ringA-1, ringA-2, ringA-3, ringA-4, ringA-#;
// ringB-1, ringB-2, ringB-3, ringB-4, ringB-#;
// ringC-1, ringC-2, ringC-3, ringC-4, ringC-#;
// ringD-1, ringD-2, ringD-3, ringD-4, ringD-#;>
bool ringsOnRows = scan.data().rows < scan.data().cols;
unsigned int rings = ringsOnRows ? scan.data().rows : scan.data().cols;
unsigned int pts = ringsOnRows ? scan.data().cols : scan.data().rows;
unsigned int outputPts = pts/step;
output = cv::Mat(
ringsOnRows ? rings : outputPts,
ringsOnRows ? outputPts : rings,
scan.data().type());
if(ringsOnRows) {
for(unsigned int j=0; j<rings; ++j)
{
for(unsigned int i=0; i<outputPts; ++i)
{
cv::Mat(scan.data(), cv::Range(j,j+1), cv::Range(i*step,i*step+1)).copyTo(cv::Mat(output, cv::Range(j,j+1), cv::Range(i,i+1)));
}
}
}
else {
for(unsigned int j=0; j<outputPts; ++j)
{
for(unsigned int i=0; i<rings; ++i)
{
cv::Mat(scan.data(), cv::Range(j*step,j*step+1), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range(j,j+1), cv::Range(i,i+1)));
}
}
}
}
else
{
// assume depth image (e.g., 640x480), downsample like an image (step in both dimensions)
UASSERT_MSG(scan.data().rows % step == 0 && scan.data().cols % step == 0,
uFormat("Decimation of image-like scans should be exact! (decimation=%d, size=%dx%d)",
step, scan.data().cols, scan.data().rows).c_str());
output = cv::Mat(scan.data().rows/step, scan.data().cols/step, scan.data().type());
for(int j=0; j<output.rows; ++j)
{
for(int i=0; i<output.cols; ++i)
{
cv::Mat(scan.data(), cv::Range(j*step,j*step+1), cv::Range(i*step,i*step+1)).copyTo(cv::Mat(output, cv::Range(j,j+1), cv::Range(i,i+1)));
}
}
}
}
else
{
// Dense LaserScan
int finalSize = scan.size()/step;
cv::Mat output = cv::Mat(1, finalSize, scan.dataType());
output = cv::Mat(1, finalSize, scan.dataType());
int oi = 0;
for(int i=0; i<scan.size()-step+1; i+=step)
{
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(output, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
if(scan.angleIncrement() > 0.0f)
{
return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement()*step, scan.localTransform());
}
return LaserScan(output, scan.maxPoints()/step, scan.rangeMax(), scan.format(), scan.localTransform());
}
if(scan.angleIncrement() > 0.0f)
{
return LaserScan(output, scan.format(), scan.rangeMin(), scan.rangeMax(), scan.angleMin(), scan.angleMax(), scan.angleIncrement()*step, scan.localTransform());
}
return LaserScan(output, scan.maxPoints()/step, scan.rangeMax(), scan.format(), scan.localTransform());
}
template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
@@ -577,42 +654,62 @@ typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
{
UASSERT(step > 0);
typename pcl::PointCloud<PointT>::Ptr output(new pcl::PointCloud<PointT>);
if(step <= 1 || (int)cloud->size() <= step)
if(step <= 1 ||
(cloud->width > cloud->height ? (int)cloud->width <= step : (int)cloud->height <= step))
{
// no sampling
*output = *cloud;
}
else
else if(cloud->height > 1) // Organized point cloud
{
if(cloud->height > 1 && cloud->height < cloud->width/4)
if(cloud->height <= cloud->width/4 ||
cloud->width <= cloud->height/4)
{
// Assuming ouster point cloud (e.g, 2048x64),
// Assuming LiDAR point cloud (e.g, 2048x64 or 32x1024),
// for which the lower dimension is the number of rings.
// Downsample each ring by the step.
// Example data packed:
// <ringA-1, ringB-1, ringC-1, ringD-1;
// ringA-2, ringB-2, ringC-2, ringD-2;
// ringA-3, ringB-3, ringC-3, ringD-3;
// ringA-4, ringB-4, ringC-4, ringD-4>
unsigned int rings = cloud->height<cloud->width?cloud->height:cloud->width;
unsigned int pts = cloud->height>cloud->width?cloud->height:cloud->width;
// ringA-4, ringB-4, ringC-4, ringD-4;
// ringA-#, ringB-#, ringC-#, ringD-#>
// or
// <ringA-1, ringA-2, ringA-3, ringA-4, ringA-#;
// ringB-1, ringB-2, ringB-3, ringB-4, ringB-#;
// ringC-1, ringC-2, ringC-3, ringC-4, ringC-#;
// ringD-1, ringD-2, ringD-3, ringD-4, ringD-#;>
bool ringsOnRows = cloud->height < cloud->width;
unsigned int rings = ringsOnRows ? cloud->height : cloud->width;
unsigned int pts = ringsOnRows ? cloud->width : cloud->height;
unsigned int finalSize = rings * pts/step;
unsigned int outputPts = pts/step;
output->resize(finalSize);
output->width = rings;
output->height = pts/step;
output->height = ringsOnRows ? rings : outputPts;
output->width = ringsOnRows ? outputPts : rings;
for(unsigned int j=0; j<rings; ++j)
{
for(unsigned int i=0; i<output->height; ++i)
if(ringsOnRows) {
for(unsigned int j=0; j<rings; ++j)
{
(*output)[i*rings + j] = cloud->at(i*step*rings + j);
for(unsigned int i=0; i<outputPts; ++i)
{
(*output)[j*outputPts + i] = cloud->at(j*pts + i*step);
}
}
}
else {
for(unsigned int j=0; j<outputPts; ++j)
{
for(unsigned int i=0; i<rings; ++i)
{
(*output)[j*rings + i] = cloud->at(j*step*rings + i);
}
}
}
}
else if(cloud->height > 1)
else
{
// assume depth image (e.g., 640x480), downsample like an image
// assume depth image (e.g., 640x480), downsample like an image (step in both dimensions)
UASSERT_MSG(cloud->height % step == 0 && cloud->width % step == 0,
uFormat("Decimation of depth images should be exact! (decimation=%d, size=%dx%d)",
step, cloud->width, cloud->height).c_str());
@@ -626,19 +723,19 @@ typename pcl::PointCloud<PointT>::Ptr downsampleImpl(
{
for(unsigned int i=0; i<output->width; ++i)
{
output->at(j*output->width + i) = cloud->at(j*output->width*step + i*step);
output->at(j*output->width + i) = cloud->at(j*cloud->width*step + i*step);
}
}
}
else
}
else // Dense cloud
{
int finalSize = int(cloud->size())/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
int finalSize = int(cloud->size())/step;
output->resize(finalSize);
int oi = 0;
for(unsigned int i=0; i<cloud->size()-step+1; i+=step)
{
(*output)[oi++] = cloud->at(i);
}
(*output)[oi++] = cloud->at(i);
}
}
return output;
@@ -964,7 +1061,10 @@ pcl::IndicesPtr cropBoxImpl(
filter.setMax(max);
if(!transform.isNull() && !transform.isIdentity())
{
filter.setTransform(transform.toEigen3f());
float x,y,z,roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
filter.setTranslation(Eigen::Vector3f(x,y,z));
filter.setRotation(Eigen::Vector3f(roll,pitch,yaw));
}
filter.setInputCloud(cloud);
filter.setIndices(indices);
@@ -984,7 +1084,10 @@ pcl::IndicesPtr cropBox(const pcl::PCLPointCloud2::Ptr & cloud, const pcl::Indic
filter.setMax(max);
if(!transform.isNull() && !transform.isIdentity())
{
filter.setTransform(transform.toEigen3f());
float x,y,z,roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
filter.setTranslation(Eigen::Vector3f(x,y,z));
filter.setRotation(Eigen::Vector3f(roll,pitch,yaw));
}
filter.setInputCloud(cloud);
filter.setIndices(indices);
@@ -1069,8 +1172,8 @@ pcl::IndicesPtr frustumFilteringImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const Transform & cameraPose,
float horizontalFOV, // in degrees
float verticalFOV, // in degrees
float horizontalFOV, // in degrees, xfov = atan((image_width/2)/fx)*2
float verticalFOV, // in degrees, yfov = atan((image_height/2)/fy)*2
float nearClipPlaneDistance,
float farClipPlaneDistance,
bool negative)
@@ -1092,7 +1195,18 @@ pcl::IndicesPtr frustumFilteringImpl(
fc.setNearPlaneDistance (nearClipPlaneDistance);
fc.setFarPlaneDistance (farClipPlaneDistance);
fc.setCameraPose (cameraPose.toEigen4f());
//The pcl function assumes a coordinate system where:
// - X is forward (view direction),
// - Y is up,
// - Z is to the right.
// Convert from the traditional camera coordinate system (X right, Y down, Z forward):
Transform cam2robot(
0, 0, 1, 0,
0,-1, 0, 0,
1, 0, 0, 0);
Transform pose_new = cameraPose * cam2robot;
fc.setCameraPose (pose_new.toEigen4f());
fc.filter (*output);
return output;
@@ -1125,7 +1239,18 @@ typename pcl::PointCloud<PointT>::Ptr frustumFilteringImpl(
fc.setNearPlaneDistance (nearClipPlaneDistance);
fc.setFarPlaneDistance (farClipPlaneDistance);
fc.setCameraPose (cameraPose.toEigen4f());
//The pcl function assumes a coordinate system where:
// - X is forward (view direction),
// - Y is up,
// - Z is to the right.
// Convert from the traditional camera coordinate system (X right, Y down, Z forward):
Transform cam2robot(
0, 0, 1, 0,
0,-1, 0, 0,
1, 0, 0, 0);
Transform pose_new = cameraPose * cam2robot;
fc.setCameraPose (pose_new.toEigen4f());
fc.filter (*output);
return output;
@@ -1464,7 +1589,7 @@ pcl::IndicesPtr proportionalRadiusFilteringImpl(
{
std::vector<bool> kept(cloud->size());
tree->setInputCloud(cloud);
#pragma omp parallel for
//#pragma omp parallel for
for(int i=0; i<(int)cloud->size(); ++i)
{
std::vector<int> kIndices;
@@ -1475,6 +1600,7 @@ pcl::IndicesPtr proportionalRadiusFilteringImpl(
cv::Point3f point = cv::Point3f(cloud->at(i).x,cloud->at(i).y, cloud->at(i).z);
float radiusSearch = factor * cv::norm(viewpoint-point);
int k = tree->radiusSearch(cloud->at(i), radiusSearch, kIndices, kDistances);
printf("Found k=%d (radius=%f) for %d (factor=%f dist to viewpoint=%f)\n", k, radiusSearch, i, factor, cv::norm(viewpoint-point));
bool keep = k>0;
for(int j=0; j<k && keep; ++j)
{
@@ -1487,12 +1613,14 @@ pcl::IndicesPtr proportionalRadiusFilteringImpl(
UASSERT(viewpointIter != viewpoints.end());
viewpoint = cv::Point3f(viewpointIter->second.x(), viewpointIter->second.y(), viewpointIter->second.z());
float radiusSearchTmp = factor * cv::norm(viewpoint-pointTmp) * neighborScale;
printf("Check neighbor %d dist to %d = %f >? %f\n", kIndices[j], i, sqrt(distPtSqr), radiusSearchTmp);
if(distPtSqr > radiusSearchTmp*radiusSearchTmp)
{
keep = false;
}
}
}
printf("keep %d = %s\n", i, keep?"true":"false");
kept[i] = keep;
}
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
@@ -1570,14 +1698,26 @@ pcl::IndicesPtr proportionalRadiusFiltering(
return proportionalRadiusFilteringImpl<pcl::PointXYZINormal>(cloud, indices, viewpointIndices, viewpoints, factor, neighborScale);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & subtractCloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZ>::Ptr out(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & subtractCloud,
float radiusSearch,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, minNeighborsInRadius);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
@@ -1587,8 +1727,8 @@ template<typename PointT>
pcl::IndicesPtr subtractFilteringImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
const typename pcl::PointCloud<PointT>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearch,
int minNeighborsInRadius)
{
@@ -1599,13 +1739,13 @@ pcl::IndicesPtr subtractFilteringImpl(
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
if(subtractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
tree->setInputCloud(subtractCloud, subtractIndices);
}
else
{
tree->setInputCloud(substractCloud);
tree->setInputCloud(subtractCloud);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
@@ -1624,13 +1764,13 @@ pcl::IndicesPtr subtractFilteringImpl(
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
if(substractIndices->size())
if(subtractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
tree->setInputCloud(subtractCloud, subtractIndices);
}
else
{
tree->setInputCloud(substractCloud);
tree->setInputCloud(subtractCloud);
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
@@ -1646,52 +1786,62 @@ pcl::IndicesPtr subtractFilteringImpl(
return output;
}
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearch,
int minNeighborsInRadius)
{
return subtractFilteringImpl<pcl::PointXYZ>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, minNeighborsInRadius);
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearch,
int minNeighborsInRadius)
{
return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, substractCloud, substractIndices, radiusSearch, minNeighborsInRadius);
return subtractFilteringImpl<pcl::PointXYZRGB>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, minNeighborsInRadius);
}
pcl::PointCloud<pcl::PointNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
const pcl::PointCloud<pcl::PointNormal>::Ptr & subtractCloud,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointNormal>::Ptr out(new pcl::PointCloud<pcl::PointNormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::PointCloud<pcl::PointXYZINormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & subtractCloud,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZINormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
}
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & subtractCloud,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, substractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::IndicesPtr indicesOut = subtractFiltering(cloud, indices, subtractCloud, indices, radiusSearch, maxAngle, minNeighborsInRadius);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr out(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::copyPointCloud(*cloud, *indicesOut, *out);
return out;
@@ -1701,8 +1851,8 @@ template<typename PointT>
pcl::IndicesPtr subtractFilteringImpl(
const typename pcl::PointCloud<PointT>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const typename pcl::PointCloud<PointT>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
const typename pcl::PointCloud<PointT>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
@@ -1714,13 +1864,13 @@ pcl::IndicesPtr subtractFilteringImpl(
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
if(subtractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
tree->setInputCloud(subtractCloud, subtractIndices);
}
else
{
tree->setInputCloud(substractCloud);
tree->setInputCloud(subtractCloud);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
@@ -1737,7 +1887,7 @@ pcl::IndicesPtr subtractFilteringImpl(
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
Eigen::Vector4f v(subtractCloud->at(kIndices.at(j)).normal_x, subtractCloud->at(kIndices.at(j)).normal_y, subtractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
@@ -1771,13 +1921,13 @@ pcl::IndicesPtr subtractFilteringImpl(
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
if(substractIndices->size())
if(subtractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
tree->setInputCloud(subtractCloud, subtractIndices);
}
else
{
tree->setInputCloud(substractCloud);
tree->setInputCloud(subtractCloud);
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
@@ -1794,7 +1944,7 @@ pcl::IndicesPtr subtractFilteringImpl(
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
Eigen::Vector4f v(subtractCloud->at(kIndices.at(j)).normal_x, subtractCloud->at(kIndices.at(j)).normal_y, subtractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
@@ -1829,42 +1979,42 @@ pcl::IndicesPtr subtractFilteringImpl(
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
const pcl::PointCloud<pcl::PointNormal>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
return subtractFilteringImpl<pcl::PointNormal>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
const pcl::PointCloud<pcl::PointXYZINormal>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
return subtractFilteringImpl<pcl::PointXYZINormal>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
}
pcl::IndicesPtr subtractFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearch,
float maxAngle,
int minNeighborsInRadius)
{
return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, substractCloud, substractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
return subtractFilteringImpl<pcl::PointXYZRGBNormal>(cloud, indices, subtractCloud, subtractIndices, radiusSearch, maxAngle, minNeighborsInRadius);
}
pcl::IndicesPtr subtractAdaptiveFiltering(
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearchRatio,
int minNeighborsInRadius,
const Eigen::Vector3f & viewpoint)
@@ -1878,13 +2028,13 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
if(subtractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
tree->setInputCloud(subtractCloud, subtractIndices);
}
else
{
tree->setInputCloud(substractCloud);
tree->setInputCloud(subtractCloud);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
@@ -1910,13 +2060,13 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
if(substractIndices->size())
if(subtractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
tree->setInputCloud(subtractCloud, subtractIndices);
}
else
{
tree->setInputCloud(substractCloud);
tree->setInputCloud(subtractCloud);
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
@@ -1942,8 +2092,8 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
pcl::IndicesPtr subtractAdaptiveFiltering(
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & substractCloud,
const pcl::IndicesPtr & substractIndices,
const pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & subtractCloud,
const pcl::IndicesPtr & subtractIndices,
float radiusSearchRatio,
float maxAngle,
int minNeighborsInRadius,
@@ -1957,13 +2107,13 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
{
pcl::IndicesPtr output(new std::vector<int>(indices->size()));
int oi = 0; // output iterator
if(substractIndices->size())
if(subtractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
tree->setInputCloud(subtractCloud, subtractIndices);
}
else
{
tree->setInputCloud(substractCloud);
tree->setInputCloud(subtractCloud);
}
for(unsigned int i=0; i<indices->size(); ++i)
{
@@ -1986,7 +2136,7 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
Eigen::Vector4f v(subtractCloud->at(kIndices.at(j)).normal_x, subtractCloud->at(kIndices.at(j)).normal_y, subtractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
@@ -2022,13 +2172,13 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
{
pcl::IndicesPtr output(new std::vector<int>(cloud->size()));
int oi = 0; // output iterator
if(substractIndices->size())
if(subtractIndices->size())
{
tree->setInputCloud(substractCloud, substractIndices);
tree->setInputCloud(subtractCloud, subtractIndices);
}
else
{
tree->setInputCloud(substractCloud);
tree->setInputCloud(subtractCloud);
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
@@ -2051,7 +2201,7 @@ pcl::IndicesPtr subtractAdaptiveFiltering(
int count = k;
for(int j=0; j<count && k >= minNeighborsInRadius; ++j)
{
Eigen::Vector4f v(substractCloud->at(kIndices.at(j)).normal_x, substractCloud->at(kIndices.at(j)).normal_y, substractCloud->at(kIndices.at(j)).normal_z, 0.0f);
Eigen::Vector4f v(subtractCloud->at(kIndices.at(j)).normal_x, subtractCloud->at(kIndices.at(j)).normal_y, subtractCloud->at(kIndices.at(j)).normal_z, 0.0f);
if(uIsFinite(v[0]) &&
uIsFinite(v[1]) &&
uIsFinite(v[2]))
@@ -2535,12 +2685,19 @@ pcl::IndicesPtr extractPlane(
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (distanceThreshold);
seg.setInputCloud (cloud);
if(indices->size())
try {
seg.setInputCloud (cloud);
if(indices.get() && indices->size())
{
seg.setIndices(indices);
}
seg.segment (*inliers, *coefficients);
}
catch(const pcl::PCLException& e)
{
seg.setIndices(indices);
UWARN("PCL exception: %s", e.what());
return pcl::IndicesPtr(new std::vector<int>); // return empty indices
}
seg.segment (*inliers, *coefficients);
if(coefficientsOut)
{
corelib/src/util3d_mapping.cpp
+30 -12
View File
@@ -587,7 +587,7 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
float minMapSize,
float scanMaxRange)
{
UDEBUG("poses=%d, scans = %d scanMaxRange=%f", poses.size(), scans.size(), scanMaxRange);
UDEBUG("poses=%d, scans = %d scanMaxRange=%f", (int)poses.size(), (int)scans.size(), scanMaxRange);
// local scans contain end points of each ray in map frame (pose+localTransform)
std::map<int, std::pair<cv::Mat, cv::Mat> > localScans;
@@ -818,18 +818,29 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
ptA = start;
ptB = end;
float slope = float(ptB.y - ptA.y)/float(ptB.x - ptA.x);
// clip end point
ptB.x = std::min(std::max(ptB.x, 0), grid.cols-1);
ptB.y = std::min(std::max(ptB.y, 0), grid.rows-1);
if(ptA == ptB) {
signed char * v = &grid.at<signed char>(ptA.y, ptA.x);
if(*v == 100 && stopOnObstacle)
{
return;
}
else
{
*v = 0; // free space
}
return;
}
float slope = ptB.x - ptA.x != 0 ? float(ptB.y - ptA.y)/float(ptB.x - ptA.x) : std::numeric_limits<float>::quiet_NaN();
bool swapped = false;
if(slope<-1.0f || slope>1.0f)
if(!uIsFinite(slope) || slope<-1.0f || slope>1.0f)
{
// swap x and y
slope = 1.0f/slope;
int tmp = ptA.x;
ptA.x = ptA.y;
ptA.y = tmp;
@@ -838,6 +849,13 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
ptB.x = ptB.y;
ptB.y = tmp;
if(!uIsFinite(slope)) {
slope = 0.0f;
}
else {
slope = 1.0f/slope;
}
swapped = true;
}
@@ -860,13 +878,13 @@ void rayTrace(const cv::Point2i & start, const cv::Point2i & end, cv::Mat & grid
if(!swapped)
{
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%f grid.rows=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.rows, x, slope, b, x).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("upperbound=%f grid.rows=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.rows, x+1, slope, b, x).c_str());
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.rows, uFormat("lowerbound=%d upperbound=%d grid.rows=%d x=%d slope=%f b=%f", lowerbound, upperbound, grid.rows, x, slope, b).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.rows, uFormat("lowerbound=%d upperbound=%d grid.rows=%d x+1=%d slope=%f b=%f", lowerbound, upperbound, grid.rows, x+1, slope, b).c_str());
}
else
{
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%f grid.cols=%d x=%d slope=%f b=%f x=%f", lowerbound, grid.cols, x, slope, b, x).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("upperbound=%f grid.cols=%d x+1=%d slope=%f b=%f x=%f", upperbound, grid.cols, x+1, slope, b, x).c_str());
UASSERT_MSG(lowerbound >= 0 && lowerbound < grid.cols, uFormat("lowerbound=%d upperbound=%d grid.cols=%d x=%d slope=%f b=%f", lowerbound, upperbound, grid.cols, x, slope, b).c_str());
UASSERT_MSG(upperbound >= 0 && upperbound < grid.cols, uFormat("lowerbound=%d upperbound=%d grid.cols=%d x+1=%d slope=%f b=%f", lowerbound, upperbound, grid.cols, x+1, slope, b).c_str());
}
for(int y = lowerbound; y<=(int)upperbound; ++y)
@@ -989,9 +1007,9 @@ cv::Mat erodeMap(const cv::Mat & map)
{
UASSERT(map.type() == CV_8SC1);
cv::Mat erodedMap = map.clone();
for(int i=0; i<map.rows; ++i)
for(int i=1; i<map.rows-1; ++i)
{
for(int j=0; j<map.cols; ++j)
for(int j=1; j<map.cols-1; ++j)
{
if(map.at<signed char>(i, j) == 100)
{
corelib/src/util3d_motion_estimation.cpp
+55 -12
View File
@@ -56,6 +56,24 @@ namespace rtabmap
namespace util3d
{
namespace {
// When true, every newly-constructed OpenGV SAC problem inside this
// translation unit gets its RNG reseeded with a fixed constant so that
// RANSAC is bit-for-bit reproducible. Tests can flip this on via
// setRansacDeterministicSeed(true); production code leaves it off.
bool g_ransacDeterministicSeed = false;
} // namespace
void setRansacDeterministicSeed(bool enable)
{
g_ransacDeterministicSeed = enable;
}
bool ransacDeterministicSeedEnabled()
{
return g_ransacDeterministicSeed;
}
Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
@@ -224,11 +242,11 @@ Transform estimateMotion3DTo2D(
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () / varianceMedianRatio];
UASSERT(uIsFinite(median_error_sqr_lin));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin + 1e-6;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () / varianceMedianRatio];
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang + 1e-6;
if(splitLinearCovarianceComponents)
{
@@ -242,9 +260,9 @@ Transform estimateMotion3DTo2D(
UASSERT(uIsFinite(median_error_sqr_x));
UASSERT(uIsFinite(median_error_sqr_y));
UASSERT(uIsFinite(median_error_sqr_z));
covariance->at<double>(0,0) = median_error_sqr_x;
covariance->at<double>(1,1) = median_error_sqr_y;
covariance->at<double>(2,2) = median_error_sqr_z;
covariance->at<double>(0,0) = median_error_sqr_x + 1e-6;
covariance->at<double>(1,1) = median_error_sqr_y + 1e-6;
covariance->at<double>(2,2) = median_error_sqr_z + 1e-6;
median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
}
@@ -267,7 +285,7 @@ Transform estimateMotion3DTo2D(
err += uNormSquared(imagePoints.at(inliers[i]).x - imagePointsReproj.at(inliers[i]).x, imagePoints.at(inliers[i]).y - imagePointsReproj.at(inliers[i]).y);
}
UASSERT(uIsFinite(err));
*covariance *= std::sqrt(err/float(inliers.size()));
*covariance *= std::sqrt(err/float(inliers.size())) + 1e-6;
}
}
}
@@ -401,7 +419,7 @@ Transform estimateMotion3DTo2D(
int cameraIndex = int(kpt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)cameraModels.size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)cameraModels.size(), kpt.x, subImageWidth, cameraModels[cameraIndex].imageWidth()).c_str());
cameraIndex, (int)cameraModels.size(), kpt.x, (double)subImageWidth, cameraModels[cameraIndex].imageWidth()).c_str());
const cv::Point3f & pt = iter->second;
objectPoints[oi] = pt;
@@ -420,7 +438,7 @@ Transform estimateMotion3DTo2D(
UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d samplingPolicy=%ld",
(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
guess.prettyPrint().c_str(), reprojError, iterations, samplingPolicy);
guess.prettyPrint().c_str(), reprojError, iterations, (long)samplingPolicy);
if((int)matches.size() >= minInliers)
{
@@ -436,7 +454,7 @@ Transform estimateMotion3DTo2D(
for (size_t i=0; i<cameraModels.size(); ++i)
{
UDEBUG("Matches in Camera %d: %d", i, cc[i]);
UDEBUG("Matches in Camera %d: %d", (int)i, cc[i]);
// opengv multi ransac needs at least 2 matches/camera
if (cc[i] < 2)
{
@@ -483,6 +501,7 @@ Transform estimateMotion3DTo2D(
// convert 2d-3d correspondences into bearing vectors
std::vector<std::shared_ptr<opengv::bearingVectors_t>> multiBearingVectors;
multiBearingVectors.resize(cameraModels.size());
std::vector<std::vector<int> > localIndexToGlobalIndex(cameraModels.size());
for(size_t i=0; i<cameraModels.size();++i)
{
multiPoints[i] = std::make_shared<opengv::points_t>();
@@ -497,6 +516,7 @@ Transform estimateMotion3DTo2D(
cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
pt = cv::normalize(pt);
multiBearingVectors[cameraIndex]->push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
localIndexToGlobalIndex[cameraIndex].push_back(i);
}
//create a non-central absolute multi adapter
@@ -515,6 +535,18 @@ Transform estimateMotion3DTo2D(
std::shared_ptr<opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem> absposeproblem_ptr(
new opengv::sac_problems::absolute_pose::MultiNoncentralAbsolutePoseSacProblem(adapter));
// Opt-in deterministic RANSAC: OpenGV's default constructor seeds
// its internal mt19937 from the system clock, so without this
// override two calls with identical inputs can produce different
// inlier sets / covariances. Tests flip the toggle via
// setRansacDeterministicSeed(true).
if(g_ransacDeterministicSeed)
{
absposeproblem_ptr->rng_alg_.seed(12345u);
absposeproblem_ptr->rng_gen_.reset(new std::function<int()>(
std::bind(*absposeproblem_ptr->rng_dist_, absposeproblem_ptr->rng_alg_)));
}
ransac.sac_model_ = absposeproblem_ptr;
ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
ransac.max_iterations_ = iterations;
@@ -527,9 +559,10 @@ Transform estimateMotion3DTo2D(
UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
UDEBUG("Ransac iterations done: %d", ransac.iterations_);
for (size_t i=0; i < cameraModels.size(); ++i)
{
inliers.insert(inliers.end(), ransac.inliers_[i].begin(), ransac.inliers_[i].end());
for (size_t i=0; i < cameraModels.size(); ++i) {
for (size_t j=0; j < ransac.inliers_[i].size(); ++j) {
inliers.push_back(localIndexToGlobalIndex[i][ransac.inliers_[i][j]]);
}
}
}
else
@@ -569,6 +602,14 @@ Transform estimateMotion3DTo2D(
std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P));
// Opt-in deterministic RANSAC (see comment on MultiRansac above).
if(g_ransacDeterministicSeed)
{
absposeproblem_ptr->rng_alg_.seed(12345u);
absposeproblem_ptr->rng_gen_.reset(new std::function<int()>(
std::bind(*absposeproblem_ptr->rng_dist_, absposeproblem_ptr->rng_alg_)));
}
ransac.sac_model_ = absposeproblem_ptr;
ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
ransac.max_iterations_ = iterations;
@@ -705,6 +746,7 @@ Transform estimateMotion3DTo2D(
if(matchesOut)
{
matchesOut->clear();
matchesOut->resize(cameraModels.size());
UASSERT(matches.size() == cameraIndexes.size());
for(size_t i=0; i<matches.size(); ++i)
@@ -715,6 +757,7 @@ Transform estimateMotion3DTo2D(
}
if(inliersOut)
{
inliersOut->clear();
inliersOut->resize(cameraModels.size());
for(unsigned int i=0; i<inliers.size(); ++i)
{
corelib/src/util3d_registration.cpp
+96 -19
View File
@@ -31,7 +31,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_filtering.h"
#include "rtabmap/core/util3d.h"
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/icp.h>
// Explicitly instantiate pcl::RandomSampleConsensus and
// pcl::SampleConsensusModelRegistration for pcl::PointXYZINormal. PCL itself
// only ships precompiled symbols for the types listed in PCL_XYZ_POINT_TYPES
// when built without PCL_ONLY_CORE_POINT_TYPES; the Windows pre-built PCL
// (and any "core point types" build) omits PointXYZINormal, so without this
// rtabmap_core.dll fails to link when the rejector is used with that type.
#include <pcl/sample_consensus/impl/ransac.hpp>
#include <pcl/sample_consensus/impl/sac_model_registration.hpp>
template class pcl::RandomSampleConsensus<pcl::PointXYZINormal>;
template class pcl::SampleConsensusModelRegistration<pcl::PointXYZINormal>;
#include <pcl/registration/transformation_estimation_2D.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/sample_consensus/sac_model_registration.h>
@@ -51,6 +64,7 @@ Transform transformFromXYZCorrespondencesSVD(
const pcl::PointCloud<pcl::PointXYZ> & cloud1,
const pcl::PointCloud<pcl::PointXYZ> & cloud2)
{
UASSERT(cloud1.size() == cloud2.size());
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> svd;
// Perform the alignment
@@ -205,7 +219,7 @@ Transform transformFromXYZCorrespondences(
{
double variance = model->computeVariance();
UASSERT(uIsFinite(variance));
*covariance *= variance;
*covariance *= variance + 1e-6;
}
// get best transformation
@@ -256,7 +270,13 @@ void computeVarianceAndCorrespondencesImpl(
est->setInputTarget(target);
est->setInputSource(source);
pcl::Correspondences correspondences;
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
if(reciprocal) {
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
}
else {
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
}
if(correspondences.size())
{
@@ -340,7 +360,12 @@ void computeVarianceAndCorrespondencesImpl(
est->setInputTarget(cloudA->size()>cloudB->size()?cloudA:cloudB);
est->setInputSource(cloudA->size()>cloudB->size()?cloudB:cloudA);
pcl::Correspondences correspondences;
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
if(reciprocal) {
est->determineReciprocalCorrespondences(correspondences, maxCorrespondenceDistance);
}
else {
est->determineCorrespondences(correspondences, maxCorrespondenceDistance);
}
if(correspondences.size()>=3)
{
@@ -381,6 +406,28 @@ void computeVarianceAndCorrespondences(
computeVarianceAndCorrespondencesImpl<pcl::PointXYZI>(cloudA, cloudB, maxCorrespondenceDistance, variance, correspondencesOut, reciprocal);
}
// RANSAC-based correspondence rejector: fits a rigid transform on random
// 3-pair subsets and discards pairs that disagree. PCL's
// IterativeClosestPoint::setRANSACOutlierRejectionThreshold and
// setRANSACIterations are NOT honored by ICP itself (only by NDT and
// k-4PCS), so we install the rejector explicitly here.
template<typename PointT>
void addRansacRejector(
pcl::IterativeClosestPoint<PointT, PointT> & icp,
const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
const typename pcl::PointCloud<PointT>::ConstPtr & cloud_target,
double maxCorrespondenceDistance,
float ransacOutlierRatio)
{
typename pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>::Ptr
rejector(new pcl::registration::CorrespondenceRejectorSampleConsensus<PointT>());
rejector->setInlierThreshold(maxCorrespondenceDistance * ransacOutlierRatio);
rejector->setMaximumIterations(50);
rejector->setInputSource(cloud_source);
rejector->setInputTarget(cloud_target);
icp.addCorrespondenceRejector(rejector);
}
// return transform from source to target (All points must be finite!!!)
template<typename PointT>
Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_source,
@@ -390,7 +437,9 @@ Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_sourc
bool & hasConverged,
pcl::PointCloud<PointT> & cloud_source_registered,
float epsilon,
bool icp2D)
bool icp2D,
float ransacOutlierRatio,
int * iterationsDone)
{
pcl::IterativeClosestPoint<PointT, PointT> icp;
// Set the input source and target
@@ -404,19 +453,25 @@ Transform icpImpl(const typename pcl::PointCloud<PointT>::ConstPtr & cloud_sourc
icp.setTransformationEstimation(est);
}
// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
// Set the max correspondence distance (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
// Set the transformation epsilon (criterion 2)
icp.setTransformationEpsilon (epsilon*epsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
//icp.setEuclideanFitnessEpsilon (-std::numeric_limits<double>::max());
if(ransacOutlierRatio > 0.0f && ransacOutlierRatio < 1.0f)
{
addRansacRejector<PointT>(icp, cloud_source, cloud_target,
maxCorrespondenceDistance, ransacOutlierRatio);
}
// Perform the alignment
icp.align (cloud_source_registered);
hasConverged = icp.hasConverged();
if(iterationsDone) *iterationsDone = icp.nr_iterations_;
return Transform::fromEigen4f(icp.getFinalTransformation());
}
@@ -428,9 +483,11 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud_source,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZ> & cloud_source_registered,
float epsilon,
bool icp2D)
bool icp2D,
float ransacOutlierRatio,
int * iterationsDone)
{
return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
}
// return transform from source to target (All points must be finite!!!)
@@ -441,9 +498,11 @@ Transform icp(const pcl::PointCloud<pcl::PointXYZI>::ConstPtr & cloud_source,
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZI> & cloud_source_registered,
float epsilon,
bool icp2D)
bool icp2D,
float ransacOutlierRatio,
int * iterationsDone)
{
return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
return icpImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
}
// return transform from source to target (All points/normals must be finite!!!)
@@ -456,7 +515,9 @@ Transform icpPointToPlaneImpl(
bool & hasConverged,
pcl::PointCloud<PointNormalT> & cloud_source_registered,
float epsilon,
bool icp2D)
bool icp2D,
float ransacOutlierRatio,
int * iterationsDone)
{
pcl::IterativeClosestPoint<PointNormalT, PointNormalT> icp;
// Set the input source and target
@@ -467,7 +528,7 @@ Transform icpPointToPlaneImpl(
est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<PointNormalT, PointNormalT>);
icp.setTransformationEstimation(est);
// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
// Set the max correspondence distance (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (maxCorrespondenceDistance);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (maximumIterations);
@@ -475,17 +536,29 @@ Transform icpPointToPlaneImpl(
icp.setTransformationEpsilon (epsilon*epsilon);
// Set the euclidean distance difference epsilon (criterion 3)
//icp.setEuclideanFitnessEpsilon (1);
//icp.setRANSACOutlierRejectionThreshold(maxCorrespondenceDistance);
if(ransacOutlierRatio > 0.0f && ransacOutlierRatio < 1.0f)
{
addRansacRejector<PointNormalT>(icp, cloud_source, cloud_target,
maxCorrespondenceDistance, ransacOutlierRatio);
}
// Perform the alignment
icp.align (cloud_source_registered);
hasConverged = icp.hasConverged();
if(iterationsDone) *iterationsDone = icp.nr_iterations_;
Transform t = Transform::fromEigen4f(icp.getFinalTransformation());
if(icp2D)
{
// FIXME probably an estimation approach already 2D like in icp() version above exists.
// PCL has no 2D-aware PointToPlane estimator (only the
// point-to-point pcl::registration::TransformationEstimation2D),
// so we run the full 6DoF PointToPlaneLLS above and then snap
// the result to 3DoF here. The 6DoF LLS on planar (z=0) input
// is numerically fragile -- callers that need 2D PointToPlane
// reliably should use libpointmatcher (Icp/Strategy=1).
t = t.to3DoF();
pcl::transformPointCloudWithNormals(*cloud_source, cloud_source_registered, t.toEigen4f());
}
return t;
@@ -500,9 +573,11 @@ Transform icpPointToPlane(
bool & hasConverged,
pcl::PointCloud<pcl::PointNormal> & cloud_source_registered,
float epsilon,
bool icp2D)
bool icp2D,
float ransacOutlierRatio,
int * iterationsDone)
{
return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
}
// return transform from source to target (All points/normals must be finite!!!)
Transform icpPointToPlane(
@@ -513,9 +588,11 @@ Transform icpPointToPlane(
bool & hasConverged,
pcl::PointCloud<pcl::PointXYZINormal> & cloud_source_registered,
float epsilon,
bool icp2D)
bool icp2D,
float ransacOutlierRatio,
int * iterationsDone)
{
return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D);
return icpPointToPlaneImpl(cloud_source, cloud_target, maxCorrespondenceDistance, maximumIterations, hasConverged, cloud_source_registered, epsilon, icp2D, ransacOutlierRatio, iterationsDone);
}
}
corelib/src/util3d_surface.cpp
+81 -70
View File
@@ -45,6 +45,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/surface/mls.h>
#include <pcl18/surface/texture_mapping.h>
#include <pcl/features/integral_image_normal.h>
#include <algorithm>
#ifdef RTABMAP_ALICE_VISION
#include <aliceVision/sfmData/SfMData.hpp>
@@ -89,6 +90,61 @@ namespace rtabmap
namespace util3d
{
namespace {
float pcaEigenvalueAt(const cv::Mat & eigenvalues, int index)
{
if(eigenvalues.empty())
{
return 0.f;
}
if(eigenvalues.rows == 1)
{
index = std::min(index, eigenvalues.cols - 1);
return eigenvalues.at<float>(0, index);
}
index = std::min(index, eigenvalues.rows - 1);
return eigenvalues.at<float>(index, 0);
}
// Shared eigendecomposition of a normals matrix (each row is a normal).
// centered=true -> covariance (cv::PCA): measures spread *around the mean*.
// Misses degeneracy when ≤ dim distinct viewpoint-flipped
// normal directions sit on a single (dim-1)-D affine
// subspace (e.g. 3 perpendicular surfaces with normals
// +x/+y/+z all sum to mean (1/3,1/3,1/3) -> rank-2).
// centered=false -> uncentered second-moment matrix (1/N) * Σ nᵢ nᵢᵀ:
// smallest eigenvalue is 0 iff normals are confined to a
// (dim-1)-D linear subspace, which is the actual ICP
// observability question. Recommended for the
// PointToPlane low-complexity check.
// Returns smallest eigenvalue * dim (so values scale ~[0,1]).
float normalsComplexity(
const cv::Mat & normals,
bool centered,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
{
const int dim = normals.cols;
cv::Mat eigVals, eigVecs;
if(centered)
{
cv::PCA pca(normals, cv::Mat(), cv::PCA::DATA_AS_ROW);
eigVals = pca.eigenvalues;
eigVecs = pca.eigenvectors;
}
else
{
cv::Mat M;
cv::mulTransposed(normals, M, /*aTa=*/true);
M /= static_cast<float>(normals.rows);
cv::eigen(M, eigVals, eigVecs);
}
if(pcaEigenVectors) *pcaEigenVectors = eigVecs;
if(pcaEigenValues) *pcaEigenValues = eigVals;
return pcaEigenvalueAt(eigVals, dim - 1) * static_cast<float>(dim);
}
} // namespace
void createPolygonIndexes(
const std::vector<pcl::Vertices> & polygons,
int cloudSize,
@@ -3163,7 +3219,8 @@ float computeNormalsComplexity(
const LaserScan & scan,
const Transform & t,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
cv::Mat * pcaEigenValues,
bool centered)
{
if(!scan.isEmpty() && (scan.hasNormals()))
{
@@ -3216,19 +3273,9 @@ float computeNormalsComplexity(
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), cv::PCA::DATA_AS_ROW);
if(pcaEigenVectors)
{
*pcaEigenVectors = pca_analysis.eigenvectors;
}
if(pcaEigenValues)
{
*pcaEigenValues = pca_analysis.eigenvalues;
}
UASSERT((is2d && pca_analysis.eigenvalues.total()>=2) || (!is2d && pca_analysis.eigenvalues.total()>=3));
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
return normalsComplexity(
cv::Mat(data_normals, cv::Range(0, oi)),
centered, pcaEigenVectors, pcaEigenValues);
}
}
else if(!scan.isEmpty())
@@ -3243,7 +3290,8 @@ float computeNormalsComplexity(
const Transform & t,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
cv::Mat * pcaEigenValues,
bool centered)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(cloud.size()*2);
@@ -3277,19 +3325,9 @@ float computeNormalsComplexity(
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), cv::PCA::DATA_AS_ROW);
if(pcaEigenVectors)
{
*pcaEigenVectors = pca_analysis.eigenvectors;
}
if(pcaEigenValues)
{
*pcaEigenValues = pca_analysis.eigenvalues;
}
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
return normalsComplexity(
cv::Mat(data_normals, cv::Range(0, oi)),
centered, pcaEigenVectors, pcaEigenValues);
}
return 0.0f;
}
@@ -3299,7 +3337,8 @@ float computeNormalsComplexity(
const Transform & t,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
cv::Mat * pcaEigenValues,
bool centered)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(normals.size()*2);
@@ -3333,19 +3372,9 @@ float computeNormalsComplexity(
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), cv::PCA::DATA_AS_ROW);
if(pcaEigenVectors)
{
*pcaEigenVectors = pca_analysis.eigenvectors;
}
if(pcaEigenValues)
{
*pcaEigenValues = pca_analysis.eigenvalues;
}
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
return normalsComplexity(
cv::Mat(data_normals, cv::Range(0, oi)),
centered, pcaEigenVectors, pcaEigenValues);
}
return 0.0f;
}
@@ -3355,7 +3384,8 @@ float computeNormalsComplexity(
const Transform & t,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
cv::Mat * pcaEigenValues,
bool centered)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(cloud.size()*2);
@@ -3389,19 +3419,9 @@ float computeNormalsComplexity(
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), cv::PCA::DATA_AS_ROW);
if(pcaEigenVectors)
{
*pcaEigenVectors = pca_analysis.eigenvectors;
}
if(pcaEigenValues)
{
*pcaEigenValues = pca_analysis.eigenvalues;
}
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
return normalsComplexity(
cv::Mat(data_normals, cv::Range(0, oi)),
centered, pcaEigenVectors, pcaEigenValues);
}
return 0.0f;
}
@@ -3411,7 +3431,8 @@ float computeNormalsComplexity(
const Transform & t,
bool is2d,
cv::Mat * pcaEigenVectors,
cv::Mat * pcaEigenValues)
cv::Mat * pcaEigenValues,
bool centered)
{
//Construct a buffer used by the pca analysis
int sz = static_cast<int>(cloud.size()*2);
@@ -3445,19 +3466,9 @@ float computeNormalsComplexity(
}
if(oi>1)
{
cv::PCA pca_analysis(cv::Mat(data_normals, cv::Range(0, oi*2)), cv::Mat(), cv::PCA::DATA_AS_ROW);
if(pcaEigenVectors)
{
*pcaEigenVectors = pca_analysis.eigenvectors;
}
if(pcaEigenValues)
{
*pcaEigenValues = pca_analysis.eigenvalues;
}
// Get last eigen value, scale between 0 and 1: 0=low complexity, 1=high complexity
return pca_analysis.eigenvalues.at<float>(0, is2d?1:2)*(is2d?2.0f:3.0f);
return normalsComplexity(
cv::Mat(data_normals, cv::Range(0, oi)),
centered, pcaEigenVectors, pcaEigenValues);
}
return 0.0f;
}