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

Refactored 3DTo2D and 3DTo3D motion estimations (Memory and OdometryBOW are now using the same methods)

Browse Source
This commit is contained in:
matlabbe
2015-06-27 00:08:52 -04:00
parent 6df403ed42
commit e5447be23a
8 changed files with 492 additions and 412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -55,7 +55,7 @@ void RTABMAP_EXP findCorrespondences(
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
std::set<int> * uniqueCorrespondences = 0);
std::vector<int> * uniqueCorrespondences = 0);
// remove depth by z axis
void RTABMAP_EXP extractXYZCorrespondences(const std::multimap<int, pcl::PointXYZ> & words1,

72
corelib/include/rtabmap/core/util3d_motion_estimation.h Normal file
View File

@@ -0,0 +1,72 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTIL3D_MOTION_ESTIMATION_H_
#define UTIL3D_MOTION_ESTIMATION_H_
#include <rtabmap/core/RtabmapExp.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/core/CameraModel.h>
namespace rtabmap
{
namespace util3d
{
Transform estimateMotion3DTo2D(
const std::multimap<int, pcl::PointXYZ> & words3A,
const std::multimap<int, cv::KeyPoint> & words2B,
const CameraModel & cameraModel,
int minInliers = 10,
int iterations = 100,
double reprojError = 5.,
int flagsPnP = 0,
const Transform & guess = Transform::getIdentity(),
const std::multimap<int, pcl::PointXYZ> & words3B = std::multimap<int, pcl::PointXYZ>(),
double * varianceOut = 0,
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
Transform estimateMotion3DTo3D(
const std::multimap<int, pcl::PointXYZ> & words3A,
const std::multimap<int, pcl::PointXYZ> & words3B,
int minInliers = 10,
double inliersDistance = 0.1,
int iterations = 100,
int refineIterations = 5,
double * varianceOut = 0,
std::vector<int> * matchesOut = 0,
std::vector<int> * inliersOut = 0);
} // namespace util3d
} // namespace rtabmap
#endif /* UTIL3D_TRANSFORMS_H_ */

1
corelib/src/CMakeLists.txt
View File

@@ -37,6 +37,7 @@ SET(SRC_FILES
util3d_surface.cpp
util3d_features.cpp
util3d_correspondences.cpp
util3d_motion_estimation.cpp
SensorData.cpp
Graph.cpp

327
corelib/src/Memory.cpp
View File

@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_surface.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/Statistics.h"
@@ -2009,186 +2010,46 @@ Transform Memory::computeVisualTransform(
const Signature & oldS,
const Signature & newS,
std::string * rejectedMsg,
int * inliers,
int * inliersOut,
double * varianceOut) const
{
Transform transform;
std::string msg;
// Guess transform from visual words
if(_bowPnPEstimation)
{
if(_bowEpipolarGeometry)
{
UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used.");
}
int inliersCount= 0;
double variance = 1.0;
if((!newS.sensorData().rightRaw().empty() ||
!newS.sensorData().stereoCameraModel().isValid()) &&
(!newS.sensorData().depthRaw().empty() ||
newS.sensorData().cameraModels().size() != 1 ||
if(_bowEpipolarGeometry && !_bowPnPEstimation)
{
if(!newS.sensorData().stereoCameraModel().isValid() &&
(newS.sensorData().cameraModels().size() != 1 ||
!newS.sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else
else if((int)oldS.getWords().size() >= _bowMinInliers &&
(int)newS.getWords().size() >= _bowMinInliers)
{
cv::Mat K;
Transform localTransform;
if(newS.sensorData().cameraModels().size())
{
K = newS.sensorData().cameraModels()[0].K();
localTransform = newS.sensorData().cameraModels()[0].localTransform();
}
else
{
K = newS.sensorData().stereoCameraModel().left().K();
localTransform = newS.sensorData().stereoCameraModel().left().localTransform();
}
UASSERT(!K.empty() && !localTransform.isNull());
// 2D -> 3D
if(!oldS.getWords3().empty() && !newS.getWords().empty())
{
// find correspondences
std::vector<int> ids = uListToVector(uUniqueKeys(newS.getWords()));
std::vector<cv::Point3f> objectPoints(ids.size());
std::vector<cv::Point2f> imagePoints(ids.size());
int oi=0;
std::vector<int> matches(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
if(oldS.getWords3().count(ids[i]) == 1)
{
pcl::PointXYZ pt = oldS.getWords3().find(ids[i])->second;
if(pcl::isFinite(pt))
{
objectPoints[oi].x = pt.x;
objectPoints[oi].y = pt.y;
objectPoints[oi].z = pt.z;
imagePoints[oi] = newS.getWords().find(ids[i])->second.pt;
matches[oi++] = ids[i];
}
}
}
UASSERT(oldS.sensorData().stereoCameraModel().isValid() || (oldS.sensorData().cameraModels().size() == 1 && oldS.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = oldS.sensorData().stereoCameraModel().isValid()?oldS.sensorData().stereoCameraModel().left():oldS.sensorData().cameraModels()[0];
objectPoints.resize(oi);
imagePoints.resize(oi);
matches.resize(oi);
if((int)matches.size() >= _bowMinInliers)
{
//PnPRansac
Transform guess = localTransform.inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<int> inliersV;
cv::solvePnPRansac(objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
_bowIterations,
_bowPnPReprojError,
0,
inliersV,
_bowPnPFlags);
if(inliers)
{
*inliers = (int)inliersV.size();
}
if((int)inliersV.size() >= _bowMinInliers)
{
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
transform = localTransform * pnp;
UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
// compute variance (like in PCL computeVariance() method of sac_model.h)
if(varianceOut)
{
std::vector<float> errorSqrdDists(inliersV.size());
oi = 0;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
std::multimap<int, pcl::PointXYZ>::const_iterator iter = newS.getWords3().find(matches[inliersV[i]]);
if(iter != newS.getWords3().end() && pcl::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliersV[i]];
pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(oi);
*varianceOut= 0;
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
*varianceOut = 2.1981 * median_error_sqr;
}
}
}
else
{
msg = uFormat("PnP not enough inliers (%d[%d] < %d), rejecting the transform...",
(int)inliersV.size(), (int)matches.size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)matches.size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("Not enough features in the new image (old=%d new=%d min=%d)",
(int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
}
else if(_bowEpipolarGeometry)
{
// we only need the camera transform, send guess words3 for scale estimation
if(oldS.getWords3().size() && oldS.sensorData().cameraModels().size())
{
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
double variance = 1;
std::multimap<int, pcl::PointXYZ> inliers3D = util3d::generateWords3DMono(
oldS.getWords(),
newS.getWords(),
oldS.sensorData().cameraModels()[0],
cameraModel,
cameraTransform,
100,
4.0f,
0, // cv::SOLVEPNP_ITERATIVE
_bowIterations,
_bowPnPReprojError,
_bowPnPFlags, // cv::SOLVEPNP_ITERATIVE
1.0f,
0.99f,
oldS.getWords3(),
oldS.getWords3(), // for scale estimation
&variance);
if(varianceOut)
{
*varianceOut = variance;
}
if(inliers)
{
*inliers = (int)inliers3D.size();
}
inliersCount = (int)inliers3D.size();
if(!cameraTransform.isNull())
{
@@ -2197,13 +2058,6 @@ Transform Memory::computeVisualTransform(
if(variance <= _bowEpipolarGeometryVar)
{
transform = cameraTransform.inverse();
if(_bowForce2D)
{
UDEBUG("Forcing 2D...");
float x,y,z,r,p,yaw;
transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
transform = Transform(x,y,0, 0, 0, yaw);
}
}
else
{
@@ -2234,87 +2088,105 @@ Transform Memory::computeVisualTransform(
UWARN(msg.c_str());
}
}
else
else if(_bowPnPEstimation)
{
// 3D -> 3D
if(!oldS.getWords3().empty() && !newS.getWords3().empty())
if(_bowEpipolarGeometry)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersOld(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr inliersNew(new pcl::PointCloud<pcl::PointXYZ>);
util3d::findCorrespondences(
oldS.getWords3(),
newS.getWords3(),
*inliersOld,
*inliersNew,
_bowMaxDepth);
UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used.");
}
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs2d;
EpipolarGeometry::findPairsUnique(oldS.getWords(), newS.getWords(), pairs2d);
UDEBUG("3D unique Correspondences = %d (2D unique pairs=%d) words=%d and %d",
(int)inliersOld->size(), (int)pairs2d.size(), (int)oldS.getWords3().size(), (int)newS.getWords3().size());
if((int)inliersOld->size() >= _bowMinInliers)
if(!newS.sensorData().stereoCameraModel().isValid() &&
(newS.sensorData().cameraModels().size() != 1 ||
!newS.sensorData().cameraModels()[0].isValid()))
{
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else
{
// 3D to 2D
if((int)oldS.getWords3().size() >= _bowMinInliers &&
(int)newS.getWords().size() >= _bowMinInliers)
{
UASSERT(newS.sensorData().stereoCameraModel().isValid() || (newS.sensorData().cameraModels().size() == 1 && newS.sensorData().cameraModels()[0].isValid()));
const CameraModel & cameraModel = newS.sensorData().stereoCameraModel().isValid()?newS.sensorData().stereoCameraModel().left():newS.sensorData().cameraModels()[0];
int inliersCount = 0;
std::vector<int> inliersV;
Transform t = util3d::transformFromXYZCorrespondences(
inliersOld,
inliersNew,
_bowInlierDistance,
transform = util3d::estimateMotion3DTo2D(
oldS.getWords3(),
newS.getWords(),
cameraModel,
_bowMinInliers,
_bowIterations,
true, 3.0, 10,
&inliersV,
varianceOut);
_bowPnPReprojError,
_bowPnPFlags,
Transform::getIdentity(),
newS.getWords3(),
&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(!t.isNull() && inliersCount >= _bowMinInliers)
if(transform.isNull())
{
transform = t;
if(_bowForce2D)
{
UDEBUG("Forcing 2D...");
float x,y,z,r,p,yaw;
transform.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
transform = Transform(x,y,0, 0, 0, yaw);
}
}
else if(inliersCount < _bowMinInliers)
{
msg = uFormat("Not enough inliers (after RANSAC) %d/%d between %d and %d", inliersCount, _bowMinInliers, oldS.id(), newS.id());
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _bowMinInliers, oldS.id(), newS.id());
UINFO(msg.c_str());
}
else if(inliersCount == (int)inliersOld->size())
else
{
msg = uFormat("Rejected identity with full inliers.");
UINFO(msg.c_str());
}
if(inliers)
{
*inliers = inliersCount;
transform = transform.inverse();
}
}
else
{
msg = uFormat("Not enough inliers %d/%d between %d and %d", (int)inliersOld->size(), _bowMinInliers, oldS.id(), newS.id());
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
else if(!oldS.isBadSignature() && !newS.isBadSignature() && (oldS.getWords3().size()==0 || newS.getWords3().size()==0))
}
else
{
// 3D -> 3D
if((int)oldS.getWords3().size() >= _bowMinInliers &&
(int)newS.getWords3().size() >= _bowMinInliers)
{
msg = uFormat("Words 3D empty?!? olds=%d=%d newS=%d=%d",
oldS.id(), (int)oldS.getWords3().size(),
newS.id(), (int)newS.getWords3().size());
UWARN(msg.c_str());
std::vector<int> inliersV;
transform = util3d::estimateMotion3DTo3D(
oldS.getWords3(),
newS.getWords3(),
_bowMinInliers,
_bowInlierDistance,
_bowIterations,
10,
&variance,
0,
&inliersV);
inliersCount = (int)inliersV.size();
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d between %d and %d",
inliersCount, _bowMinInliers, oldS.id(), newS.id());
UINFO(msg.c_str());
}
else
{
transform = transform.inverse();
}
}
else
{
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)oldS.getWords3().size(), (int)newS.getWords3().size(), _bowMinInliers);
UINFO(msg.c_str());
}
}
if(!transform.isNull())
{
// verify if it is a 180 degree transform, well verify > 90
float roll,pitch,yaw;
transform.getEulerAngles(roll, pitch, yaw);
float x,y,z, roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(fabs(roll) > CV_PI/2 ||
fabs(pitch) > CV_PI/2 ||
fabs(yaw) > CV_PI/2)
@@ -2324,12 +2196,25 @@ Transform Memory::computeVisualTransform(
roll, pitch, yaw);
UWARN(msg.c_str());
}
else if(_bowForce2D)
{
UDEBUG("Forcing 2D...");
transform = Transform(x,y,0, 0, 0, yaw);
}
}
if(rejectedMsg)
{
*rejectedMsg = msg;
}
if(inliersOut)
{
*inliersOut = inliersCount;
}
if(varianceOut)
{
*varianceOut = variance;
}
UDEBUG("transform=%s", transform.prettyPrint().c_str());
return transform;
}

229
corelib/src/OdometryBOW.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/Graph.h"
#include "rtabmap/core/VWDictionary.h"
#include "rtabmap/utilite/ULogger.h"
@@ -208,7 +209,7 @@ Transform OdometryBOW::computeTransform(
}
double variance = 0;
int inliers = 0;
int inliersCount = 0;
int correspondences = 0;
int nFeatures = 0;
@@ -229,8 +230,11 @@ Transform OdometryBOW::computeTransform(
Transform transform;
if((int)localMap_.size() >= this->getMinInliers())
{
std::vector<int> matches, inliers;
Transform t;
if(this->isPnPEstimationUsed())
{
// 3D to 2D
if(data.cameraModels().size() > 1)
{
UERROR("PnP cannot be used on multi-cameras setup.");
@@ -240,117 +244,19 @@ Transform OdometryBOW::computeTransform(
UASSERT(data.stereoCameraModel().isValid() || (data.cameraModels().size() == 1 && data.cameraModels()[0].isValid()));
const CameraModel & cameraModel = data.stereoCameraModel().isValid()?data.stereoCameraModel().left():data.cameraModels()[0];
// find correspondences
std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
std::vector<cv::Point3f> objectPoints(ids.size());
std::vector<cv::Point2f> imagePoints(ids.size());
int oi=0;
std::vector<int> matches(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
if(localMap_.count(ids[i]) == 1)
{
pcl::PointXYZ pt = localMap_.find(ids[i])->second;
objectPoints[oi].x = pt.x;
objectPoints[oi].y = pt.y;
objectPoints[oi].z = pt.z;
imagePoints[oi] = newSignature->getWords().find(ids[i])->second.pt;
matches[oi++] = ids[i];
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
matches.resize(oi);
if(this->isInfoDataFilled() && info)
{
info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
}
correspondences = (int)matches.size();
if((int)matches.size() >= this->getMinInliers())
{
//PnPRansac
cv::Mat K = cameraModel.K();
Transform guess = (this->getPose() * cameraModel.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
std::vector<int> inliersV;
cv::solvePnPRansac(objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
this->getIterations(),
this->getPnPReprojError(),
0,
inliersV,
this->getPnPFlags());
inliers = (int)inliersV.size();
if((int)inliersV.size() >= this->getMinInliers())
{
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
// make it incremental
transform = (cameraModel.localTransform() * pnp * this->getPose()).inverse();
UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
// compute variance (like in PCL computeVariance() method of sac_model.h)
std::vector<float> errorSqrdDists(inliersV.size());
oi = 0;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
std::multimap<int, pcl::PointXYZ>::const_iterator iter = newSignature->getWords3().find(matches[inliersV[i]]);
if(iter != newSignature->getWords3().end() && pcl::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliersV[i]];
pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
variance = 2.1981 * median_error_sqr;
}
else
{
variance = 1;
}
}
else
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
}
if(this->isInfoDataFilled() && info && inliersV.size())
{
info->wordInliers.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->wordInliers[i] = matches[inliersV[i]];
}
}
}
else
{
UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
}
t = util3d::estimateMotion3DTo2D(
localMap_,
newSignature->getWords(),
cameraModel,
this->getMinInliers(),
this->getIterations(),
this->getPnPReprojError(),
this->getPnPFlags(),
this->getPose(),
newSignature->getWords3(),
&variance,
&matches,
&inliers);
}
else
{
@@ -359,76 +265,51 @@ Transform OdometryBOW::computeTransform(
}
else
{
// 3D to 3D
if((int)newSignature->getWords3().size() >= this->getMinInliers())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
// No need to set max depth here, it is already applied in extractKeypointsAndDescriptors() above.
// Also! the localMap_ have points not in camera frame anymore (in local map frame), so filtering
// by depth here is wrong!
std::set<int> uniqueCorrespondences;
util3d::findCorrespondences(
t = util3d::estimateMotion3DTo3D(
localMap_,
newSignature->getWords3(),
*inliers1,
*inliers2,
0,
&uniqueCorrespondences);
UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
if(this->isInfoDataFilled() && info)
{
info->wordMatches.insert(info->wordMatches.end(), uniqueCorrespondences.begin(), uniqueCorrespondences.end());
}
correspondences = (int)inliers1->size();
if((int)inliers1->size() >= this->getMinInliers())
{
// the transform returned is global odometry pose, not incremental one
std::vector<int> inliersV;
Transform t = util3d::transformFromXYZCorrespondences(
inliers2,
inliers1,
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
&inliersV,
&variance);
inliers = (int)inliersV.size();
if(!t.isNull() && inliers >= this->getMinInliers())
{
// make it incremental
transform = this->getPose().inverse() * t;
UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
}
else
{
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
}
if(this->isInfoDataFilled() && info && inliersV.size())
{
info->wordInliers.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->wordInliers[i] = info->wordMatches[inliersV[i]];
}
}
}
else
{
UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers());
}
this->getMinInliers(),
this->getInlierDistance(),
this->getIterations(),
this->getRefineIterations(),
&variance,
&matches,
&inliers);
}
else
{
UWARN("Not enough 3D features in the new image (%d < %d)", (int)newSignature->getWords3().size(), this->getMinInliers());
}
}
correspondences = matches.size();
inliersCount = inliers.size();
if(this->isInfoDataFilled() && info)
{
info->wordMatches = matches;
info->wordInliers = inliers;
}
if(!t.isNull())
{
// make it incremental
transform = this->getPose().inverse() * t;
}
else if(correspondences < this->getMinInliers())
{
UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
}
else if(inliersCount < this->getMinInliers())
{
UWARN("Not enough inliers (%d < %d)", inliersCount, this->getMinInliers());
}
else
{
UWARN("Unknown estimation error");
}
}
else
{
@@ -534,7 +415,7 @@ Transform OdometryBOW::computeTransform(
if(info)
{
info->variance = variance;
info->inliers = inliers;
info->inliers = inliersCount;
info->matches = correspondences;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
@@ -544,7 +425,7 @@ Transform OdometryBOW::computeTransform(
timer.elapsed(),
output.isNull()?"true":"false",
nFeatures,
inliers,
inliersCount,
correspondences,
variance,
(int)localMap_.size(),

14
corelib/src/util3d_correspondences.cpp
View File

@@ -355,12 +355,16 @@ void findCorrespondences(
pcl::PointCloud<pcl::PointXYZ> & inliers1,
pcl::PointCloud<pcl::PointXYZ> & inliers2,
float maxDepth,
std::set<int> * uniqueCorrespondences)
std::vector<int> * uniqueCorrespondences)
{
std::list<int> ids = uUniqueKeys(words1);
// Find pairs
inliers1.resize(ids.size());
inliers2.resize(ids.size());
if(uniqueCorrespondences)
{
uniqueCorrespondences->resize(ids.size());
}
int oi=0;
for(std::list<int>::iterator iter=ids.begin(); iter!=ids.end(); ++iter)
@@ -375,16 +379,20 @@ void findCorrespondences(
(inliers2[oi].x != 0 || inliers2[oi].y != 0 || inliers2[oi].z != 0) &&
(maxDepth <= 0 || (inliers1[oi].x > 0 && inliers1[oi].x <= maxDepth && inliers2[oi].x>0 &&inliers2[oi].x<=maxDepth)))
{
++oi;
if(uniqueCorrespondences)
{
uniqueCorrespondences->insert(*iter);
uniqueCorrespondences->at(oi) = *iter;
}
++oi;
}
}
}
inliers1.resize(oi);
inliers2.resize(oi);
if(uniqueCorrespondences)
{
uniqueCorrespondences->resize(oi);
}
}
}

24
corelib/src/util3d_features.cpp
View File

@@ -351,18 +351,6 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
}
}
if(!useCameraTransformGuess)
{
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
}
if(refGuess3D.size())
{
// scale estimation
@@ -489,7 +477,6 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
else
{
UWARN("No inliers after PnP!");
cameraTransform = Transform();
}
}
}
@@ -498,6 +485,17 @@ std::multimap<int, pcl::PointXYZ> generateWords3DMono(
UWARN("Cannot compute the scale, no points corresponding between the generated ref words and words guess");
}
}
else if(!useCameraTransformGuess)
{
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2));
cameraTransform = (cameraModel.localTransform() * t).inverse() * cameraModel.localTransform();
}
}
}
}

235
corelib/src/util3d_motion_estimation.cpp Normal file
View File

@@ -0,0 +1,235 @@
/*
Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
namespace rtabmap
{
namespace util3d
{
Transform estimateMotion3DTo2D(
const std::multimap<int, pcl::PointXYZ> & words3A,
const std::multimap<int, cv::KeyPoint> & words2B,
const CameraModel & cameraModel,
int minInliers,
int iterations,
double reprojError,
int flagsPnP,
const Transform & guess,
const std::multimap<int, pcl::PointXYZ> & words3B,
double * varianceOut,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
Transform transform;
std::vector<int> matches, inliers;
if(varianceOut)
{
*varianceOut = 1.0;
}
// find correspondences
std::vector<int> ids = uListToVector(uUniqueKeys(words2B));
std::vector<cv::Point3f> objectPoints(ids.size());
std::vector<cv::Point2f> imagePoints(ids.size());
int oi=0;
matches.resize(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
if(words3A.count(ids[i]) == 1)
{
pcl::PointXYZ pt = words3A.find(ids[i])->second;
objectPoints[oi].x = pt.x;
objectPoints[oi].y = pt.y;
objectPoints[oi].z = pt.z;
imagePoints[oi] = words2B.find(ids[i])->second.pt;
matches[oi++] = ids[i];
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
matches.resize(oi);
if((int)matches.size() >= minInliers)
{
//PnPRansac
cv::Mat K = cameraModel.K();
Transform guessCameraFrame = (guess * cameraModel.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraFrame.r11(), (double)guessCameraFrame.r12(), (double)guessCameraFrame.r13(),
(double)guessCameraFrame.r21(), (double)guessCameraFrame.r22(), (double)guessCameraFrame.r23(),
(double)guessCameraFrame.r31(), (double)guessCameraFrame.r32(), (double)guessCameraFrame.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) <<
(double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());
cv::solvePnPRansac(
objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
iterations,
reprojError,
0,
inliers,
flagsPnP);
if((int)inliers.size() >= minInliers)
{
cv::Rodrigues(rvec, R);
Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
transform = (cameraModel.localTransform() * pnp).inverse();
// compute variance (like in PCL computeVariance() method of sac_model.h)
if(varianceOut && words3B.size())
{
std::vector<float> errorSqrdDists(inliers.size());
oi = 0;
for(unsigned int i=0; i<inliers.size(); ++i)
{
std::multimap<int, pcl::PointXYZ>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(iter != words3B.end() && pcl::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform);
errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
}
}
errorSqrdDists.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
*varianceOut = 2.1981 * median_error_sqr;
}
}
}
}
if(matchesOut)
{
*matchesOut = matches;
}
if(inliersOut)
{
inliersOut->resize(inliers.size());
for(unsigned int i=0; i<inliers.size(); ++i)
{
inliersOut->at(i) = matches[inliers[i]];
}
}
return transform;
}
Transform estimateMotion3DTo3D(
const std::multimap<int, pcl::PointXYZ> & words3A,
const std::multimap<int, pcl::PointXYZ> & words3B,
int minInliers,
double inliersDistance,
int iterations,
int refineIterations,
double * varianceOut,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
Transform transform;
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
std::vector<int> matches;
util3d::findCorrespondences(
words3A,
words3B,
*inliers1,
*inliers2,
0,
&matches);
if(varianceOut)
{
*varianceOut = 1.0;
}
if((int)inliers1->size() >= minInliers)
{
std::vector<int> inliers;
Transform t = util3d::transformFromXYZCorrespondences(
inliers2,
inliers1,
inliersDistance,
iterations,
refineIterations>0,
3.0,
refineIterations,
&inliers,
varianceOut);
if(!t.isNull() && (int)inliers.size() >= minInliers)
{
transform = t;
}
if(matchesOut)
{
*matchesOut = matches;
}
if(inliersOut)
{
inliersOut->resize(inliers.size());
for(unsigned int i=0; i<inliers.size(); ++i)
{
inliersOut->at(i) = matches[inliers[i]];
}
}
}
return transform;
}
}
}