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

OdometryF2F: Fixed broken OpticalFlow approach

Browse Source
This commit is contained in:
matlabbe
2016-02-24 17:42:23 -05:00
parent 172e9857c7
commit a8d1b91083
8 changed files with 55 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@@ -283,6 +283,7 @@ ENDIF(APPLE AND BUILD_AS_BUNDLE)
SET(NONFREE 0) SET(NONFREE 0)
SET(G2O 0) SET(G2O 0)
SET(GTSAM 0) SET(GTSAM 0)
SET(OPENCV3 0)
IF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND) IF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)
SET(NONFREE 1) SET(NONFREE 1)
ENDIF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND) ENDIF(OPENCV_NONFREE_FOUND OR OPENCV_XFEATURES2D_FOUND)
@@ -292,6 +293,9 @@ ENDIF(G2O_FOUND)
IF(GTSAM_FOUND) IF(GTSAM_FOUND)
SET(GTSAM 1) SET(GTSAM 1)
ENDIF(GTSAM_FOUND) ENDIF(GTSAM_FOUND)
IF(OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3)
SET(OPENCV3 1)
ENDIF(OpenCV_FOUND AND OpenCV_VERSION_MAJOR EQUAL 3)
CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h) CONFIGURE_FILE(Version.h.in ${PROJECT_SOURCE_DIR}/corelib/include/${PROJECT_PREFIX}/core/Version.h)
ADD_SUBDIRECTORY( utilite ) ADD_SUBDIRECTORY( utilite )

1
Version.h.in
View File

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#define RTABMAP_NONFREE @NONFREE@ #define RTABMAP_NONFREE @NONFREE@
#define RTABMAP_G2O @G2O@ #define RTABMAP_G2O @G2O@
#define RTABMAP_GTSAM @GTSAM@ #define RTABMAP_GTSAM @GTSAM@
#define RTABMAP_OPENCV3 @OPENCV3@
#endif /* VERSION_H_ */ #endif /* VERSION_H_ */

2
corelib/include/rtabmap/core/Parameters.h
View File

@@ -376,7 +376,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, RefineIterations, int, 10, "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined."); RTABMAP_PARAM(Vis, RefineIterations, int, 10, "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Vis, PnPReprojError, float, 2.0, "[Vis/EstimationType = 1] PnP reprojection error."); RTABMAP_PARAM(Vis, PnPReprojError, float, 2.0, "[Vis/EstimationType = 1] PnP reprojection error.");
RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, "[Vis/EstimationType = 1] Refine iterations."); RTABMAP_PARAM_COND(Vis, PnPRefineIterations, int, RTABMAP_OPENCV3, 0, 1, "[Vis/EstimationType = 1] Refine iterations.");
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation."); RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
RTABMAP_PARAM(Vis, MinInliers, int, 10, "Minimum feature correspondences to compute/accept the transformation."); RTABMAP_PARAM(Vis, MinInliers, int, 10, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform."); RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform.");

2
corelib/src/Memory.cpp
View File

@@ -3104,7 +3104,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
} }
std::vector<cv::Point3f> keypoints3D; std::vector<cv::Point3f> keypoints3D;
if(!_useOdometryFeatures || data.keypoints().size() == 0) if(!_useOdometryFeatures || (data.keypoints().size() != data.descriptors().rows))
{ {
if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode) if(_feature2D->getMaxFeatures() >= 0 && !data.imageRaw().empty() && !isIntermediateNode)
{ {

12
corelib/src/OdometryF2F.cpp
View File

@@ -84,11 +84,9 @@ Transform OdometryF2F::computeTransform(
output = registrationPipeline_->computeTransformationMod( output = registrationPipeline_->computeTransformationMod(
refFrame_, refFrame_,
newFrame, newFrame,
guessFromMotion_?motionSinceLastKeyFrame_*this->previousTransform():Transform(), guessFromMotion_&&!this->previousTransform().isNull()?motionSinceLastKeyFrame_*this->previousTransform():Transform(),
&regInfo); &regInfo);
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
if(info && this->isInfoDataFilled()) if(info && this->isInfoDataFilled())
{ {
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs; std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -132,18 +130,16 @@ Transform OdometryF2F::computeTransform(
if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_) if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_)
{ {
UDEBUG("Update key frame"); UDEBUG("Update key frame");
int features = newFrame.sensorData().keypoints().size(); int features = newFrame.getWordsDescriptors().size();
if(features == 0) if(features == 0)
{ {
newFrame = Signature(data); newFrame = Signature(data);
// this will generate features only for the first frame // this will generate features only for the first frame or if optical flow was used (no 3d words)
Signature dummy; Signature dummy;
registrationPipeline_->computeTransformationMod( registrationPipeline_->computeTransformationMod(
newFrame, newFrame,
dummy); dummy);
features = (int)newFrame.sensorData().keypoints().size(); features = (int)newFrame.sensorData().keypoints().size();
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
} }
if((features >= registrationPipeline_->getMinVisualCorrespondences()) && if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
@@ -183,6 +179,8 @@ Transform OdometryF2F::computeTransform(
UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str()); UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
} }
data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
if(info) if(info)
{ {
info->type = 1; info->type = 1;

68
corelib/src/RegistrationVis.cpp
View File

@@ -564,15 +564,6 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<cv::Point2f> projected; std::vector<cv::Point2f> projected;
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected); cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
/*UDEBUG("guess=%s", guess.prettyPrint().c_str());
std::vector<cv::KeyPoint> projectedKpts;
cv::KeyPoint::convert(projected, projectedKpts);
cv::Mat image = toSignature.sensorData().imageRaw().clone();
drawKeypoints(image, projectedKpts, image, cv::Scalar(255,0,0));
drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255));
cv::imwrite("projected.bmp", image);
UWARN("saved projected.bmp");*/
//remove projected points outside of the image //remove projected points outside of the image
UASSERT((int)projected.size() == descriptorsFrom.rows); UASSERT((int)projected.size() == descriptorsFrom.rows);
std::vector<cv::Point2f> cornersProjected(projected.size()); std::vector<cv::Point2f> cornersProjected(projected.size());
@@ -621,10 +612,11 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG(""); UDEBUG("");
// Process results (Nearest Neighbor Distance Ratio) // Process results (Nearest Neighbor Distance Ratio)
int notToMatchedUniqueId = descriptorsFrom.rows+descriptorsTo.rows; // make sure new words from "to" are after older one of "from" int matchedID = descriptorsFrom.rows+descriptorsTo.rows;
int notFromMatchedUniqueId = descriptorsTo.rows; int newToId = descriptorsFrom.rows;
int notMatchedFromId = 0;
std::map<int,int> addedWordsFrom; //<id, index> std::map<int,int> addedWordsFrom; //<id, index>
std::set<int> duplicates; std::map<int, int> duplicates; //<fromId, toId>
int newWords = 0; int newWords = 0;
for(unsigned int i = 0; i < pointsToMat.rows; ++i) for(unsigned int i = 0; i < pointsToMat.rows; ++i)
{ {
@@ -657,12 +649,12 @@ Transform RegistrationVis::computeTransformationImpl(
if(matchedIndex >= 0) if(matchedIndex >= 0)
{ {
matchedIndex = projectedIndexToDescIndex[matchedIndex]; matchedIndex = projectedIndexToDescIndex[matchedIndex];
int id = i; int id = matchedID++;
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end()) if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
{ {
id = addedWordsFrom.at(matchedIndex); id = addedWordsFrom.at(matchedIndex);
duplicates.insert(matchedIndex); duplicates.insert(std::make_pair(matchedIndex, id));
} }
else else
{ {
@@ -686,14 +678,14 @@ Transform RegistrationVis::computeTransformationImpl(
else else
{ {
// gen fake ids // gen fake ids
wordsTo.insert(std::make_pair(notToMatchedUniqueId, kptsTo[i])); wordsTo.insert(std::make_pair(newToId, kptsTo[i]));
wordsDescTo.insert(std::make_pair(notToMatchedUniqueId, descriptorsTo.row(i))); wordsDescTo.insert(std::make_pair(newToId, descriptorsTo.row(i)));
if(kptsTo3D.size()) if(kptsTo3D.size())
{ {
words3To.insert(std::make_pair(notToMatchedUniqueId, kptsTo3D[i])); words3To.insert(std::make_pair(newToId, kptsTo3D[i]));
} }
++notToMatchedUniqueId; ++newToId;
++newWords; ++newWords;
} }
} }
@@ -703,17 +695,6 @@ Transform RegistrationVis::computeTransformationImpl(
(int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords, (int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
(int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size()); (int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
//remove duplicates
for(std::set<int>::iterator iter=duplicates.begin(); iter!=duplicates.end(); ++iter)
{
wordsFrom.erase(*iter);
wordsDescFrom.erase(*iter);
words3From.erase(*iter);
wordsTo.erase(*iter);
wordsDescTo.erase(*iter);
words3To.erase(*iter);
}
// create fake ids for not matched words from "from" // create fake ids for not matched words from "from"
int addWordsFromNotMatched = 0; int addWordsFromNotMatched = 0;
for(unsigned int i=0; i<kptsFrom3D.size(); ++i) for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
@@ -722,16 +703,37 @@ Transform RegistrationVis::computeTransformationImpl(
{ {
if(kptsFrom.size()) if(kptsFrom.size())
{ {
wordsFrom.insert(std::make_pair(notFromMatchedUniqueId, kptsFrom[i])); wordsFrom.insert(std::make_pair(notMatchedFromId, kptsFrom[i]));
} }
wordsDescFrom.insert(std::make_pair(notFromMatchedUniqueId, descriptorsFrom.row(i))); wordsDescFrom.insert(std::make_pair(notMatchedFromId, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(notFromMatchedUniqueId, kptsFrom3D[i])); words3From.insert(std::make_pair(notMatchedFromId, kptsFrom3D[i]));
++notFromMatchedUniqueId; ++notMatchedFromId;
++addWordsFromNotMatched; ++addWordsFromNotMatched;
} }
} }
UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size()); UDEBUG("addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (int)words3From.size());
/*std::vector<cv::KeyPoint> matches(wordsTo.size());
int oi=0;
for(std::multimap<int, cv::KeyPoint>::iterator iter = wordsTo.begin(); iter!=wordsTo.end(); ++iter)
{
if(iter->first >= descriptorsFrom.rows+descriptorsTo.rows && wordsTo.count(iter->first) <= 1)
{
matches[oi++] = iter->second;
}
}
matches.resize(oi);
UDEBUG("guess=%s", guess.prettyPrint().c_str());
std::vector<cv::KeyPoint> projectedKpts;
cv::KeyPoint::convert(projected, projectedKpts);
cv::Mat image = toSignature.sensorData().imageRaw().clone();
drawKeypoints(image, projectedKpts, image, cv::Scalar(255,0,0));
drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255));
drawKeypoints(image, matches, image, cv::Scalar(0,255,0));
cv::imwrite("projected.bmp", image);
UWARN("saved projected.bmp");*/
} }
UDEBUG(""); UDEBUG("");
} }

4
corelib/src/Transform.cpp
View File

@@ -375,7 +375,7 @@ Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
Transform Transform::fromString(const std::string & string) Transform Transform::fromString(const std::string & string)
{ {
std::list<std::string> list = uSplit(string, ' '); std::list<std::string> list = uSplit(string, ' ');
UASSERT_MSG(list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12, UASSERT_MSG(list.empty() || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12,
uFormat("Cannot parse \"%s\"", string.c_str()).c_str()); uFormat("Cannot parse \"%s\"", string.c_str()).c_str());
std::vector<float> numbers(list.size()); std::vector<float> numbers(list.size());
@@ -424,7 +424,7 @@ Transform Transform::fromString(const std::string & string)
bool Transform::canParseString(const std::string & string) bool Transform::canParseString(const std::string & string)
{ {
std::list<std::string> list = uSplit(string, ' '); std::list<std::string> list = uSplit(string, ' ');
return list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12; return list.size() == 0 || list.size() == 3 || list.size() == 6 || list.size() == 7 || list.size() == 9 || list.size() == 12;
} }
} }

7
corelib/src/util3d_motion_estimation.cpp
View File

@@ -144,7 +144,12 @@ Transform estimateMotion3DTo2D(
{ {
const cv::Point3f & objPt = objectPoints[inliers[i]]; const cv::Point3f & objPt = objectPoints[inliers[i]];
cv::Point3f newPt = util3d::transformPoint(iter->second, transform); cv::Point3f newPt = util3d::transformPoint(iter->second, transform);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z); errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
//ignore very very far features (stereo)
if(errorSqrdDists[oi] < 100.0f)
{
++oi;
}
} }
} }
errorSqrdDists.resize(oi); errorSqrdDists.resize(oi);