mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-02 01:20:25 +08:00
Updated EpipolarGeometry methods
Experimental: added OdometryMono in epipolar geometry test git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1982 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/core/Features2d.h"
|
||||
#include "rtabmap/core/EpipolarGeometry.h"
|
||||
#include "rtabmap/core/VWDictionary.h"
|
||||
#include "rtabmap/core/Odometry.h"
|
||||
#include "rtabmap/gui/UCv2Qt.h"
|
||||
|
||||
#include "rtabmap/gui/ImageView.h"
|
||||
@@ -62,6 +63,570 @@ void showUsage()
|
||||
exit(1);
|
||||
}
|
||||
|
||||
class RTABMAP_EXP OdometryMono : public Odometry
|
||||
{
|
||||
public:
|
||||
OdometryMono(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap()) :
|
||||
Odometry(parameters),
|
||||
flowWinSize_(Parameters::defaultOdomFlowWinSize()),
|
||||
flowIterations_(Parameters::defaultOdomFlowIterations()),
|
||||
flowEps_(Parameters::defaultOdomFlowEps()),
|
||||
flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
|
||||
subPixWinSize_(Parameters::defaultOdomSubPixWinSize()),
|
||||
subPixIterations_(Parameters::defaultOdomSubPixIterations()),
|
||||
subPixEps_(Parameters::defaultOdomSubPixEps()),
|
||||
refCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
|
||||
{
|
||||
Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
|
||||
Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
|
||||
Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
|
||||
Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
|
||||
Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize_);
|
||||
Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations_);
|
||||
Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps_);
|
||||
|
||||
ParametersMap::const_iterator iter;
|
||||
Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
|
||||
if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
|
||||
{
|
||||
detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
|
||||
}
|
||||
feature2D_ = Feature2D::create(detectorStrategy, parameters);
|
||||
|
||||
ParametersMap customParameters;
|
||||
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uValue(parameters, Parameters::kOdomBowNNType(), uNumber2Str(Parameters::defaultOdomBowNNType()))));
|
||||
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uValue(parameters, Parameters::kOdomBowNNDR(), uNumber2Str(Parameters::defaultOdomBowNNDR()))));
|
||||
customParameters.insert(ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
|
||||
dictionary_ = new VWDictionary(customParameters);
|
||||
}
|
||||
|
||||
virtual ~OdometryMono()
|
||||
{
|
||||
delete feature2D_;
|
||||
delete dictionary_;
|
||||
}
|
||||
|
||||
private:
|
||||
virtual Transform computeTransform(const SensorData & data, int * quality = 0, int * features = 0, int * localMapSize = 0)
|
||||
{
|
||||
UTimer timer;
|
||||
Transform output;
|
||||
|
||||
int inliers = 0;
|
||||
int correspondences = 0;
|
||||
|
||||
cv::Mat newFrame;
|
||||
// convert to grayscale
|
||||
if(data.image().channels() > 1)
|
||||
{
|
||||
cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
|
||||
}
|
||||
else
|
||||
{
|
||||
newFrame = data.image().clone();
|
||||
}
|
||||
|
||||
UDEBUG("lastCorners_.size()=%d lastFrame_=%d", (int)refCorners_.size(), refFrame_.empty()?0:1);
|
||||
if(!refFrame_.empty() && refCorners_.size())
|
||||
{
|
||||
if(refCorners3D_->size())
|
||||
{
|
||||
//PnP
|
||||
UDEBUG("PnP");
|
||||
|
||||
std::vector<cv::KeyPoint> newKpts;
|
||||
std::vector<cv::Point2f> newCorners;
|
||||
cv::Mat newDescriptors;
|
||||
if(data.keypoints().size())
|
||||
{
|
||||
cv::KeyPoint::convert(data.keypoints(), newCorners);
|
||||
newKpts = data.keypoints();
|
||||
newDescriptors = data.descriptors();
|
||||
}
|
||||
else
|
||||
{
|
||||
// generate kpts
|
||||
cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
|
||||
newKpts = feature2D_->generateKeypoints(newFrame, this->getMaxFeatures(), roi);
|
||||
Feature2D::limitKeypoints(newKpts, this->getMaxFeatures());
|
||||
|
||||
if(newKpts.size())
|
||||
{
|
||||
//extract descriptors (before subpixel)
|
||||
newDescriptors = feature2D_->generateDescriptors(newFrame, newKpts);
|
||||
|
||||
cv::KeyPoint::convert(newKpts, newCorners);
|
||||
|
||||
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
|
||||
{
|
||||
UDEBUG("cv::cornerSubPix() begin");
|
||||
cv::cornerSubPix(newFrame, newCorners,
|
||||
cv::Size( subPixWinSize_, subPixWinSize_ ),
|
||||
cv::Size( -1, -1 ),
|
||||
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
|
||||
UDEBUG("cv::cornerSubPix() end");
|
||||
|
||||
for(unsigned int i=0; i<newCorners.size(); ++i)
|
||||
{
|
||||
newKpts[i].pt = newCorners[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//matching using visual words dictionary
|
||||
std::vector<int> newWordIds = uListToVector(dictionary_->addNewWords(newDescriptors, 2));
|
||||
UDEBUG("");
|
||||
UASSERT((int)newKpts.size() == newDescriptors.rows);
|
||||
UASSERT(newKpts.size() == newWordIds.size());
|
||||
std::multimap<int, cv::KeyPoint> newWords;
|
||||
for(unsigned int i=0; i<newWordIds.size(); ++i)
|
||||
{
|
||||
newWords.insert(std::make_pair(newWordIds[i], newKpts[i]));
|
||||
}
|
||||
UDEBUG("newWords=%d", (int)newWords.size());
|
||||
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
|
||||
if(EpipolarGeometry::findPairsUnique(refWords_, newWords, pairs) > this->getMinInliers())
|
||||
{
|
||||
UDEBUG("pairs = %d", (int)pairs.size());
|
||||
// now that we have correspondences, set data for PnP
|
||||
std::vector<cv::Point3f> objectPoints(pairs.size());
|
||||
std::vector<cv::Point2f> imagePoints(pairs.size());
|
||||
int i=0;
|
||||
std::vector<cv::KeyPoint> a,b;
|
||||
for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter = pairs.begin();
|
||||
iter!=pairs.end();
|
||||
++iter)
|
||||
{
|
||||
pcl::PointXYZ pt3 = refCorners3D_->at(iter->first-1); // id and index should match
|
||||
objectPoints[i] = cv::Point3f(pt3.x, pt3.y, pt3.z);
|
||||
imagePoints[i] = iter->second.second.pt;
|
||||
UDEBUG("ref (%f %f) new (%f %f) pt (%f %f %f)",
|
||||
iter->second.first.pt.x, iter->second.first.pt.y,
|
||||
iter->second.second.pt.x, iter->second.second.pt.y,
|
||||
pt3.x, pt3.y, pt3.z);
|
||||
a.push_back(iter->second.first);
|
||||
b.push_back(iter->second.second);
|
||||
++i;
|
||||
}
|
||||
|
||||
UDEBUG("");
|
||||
cv::Mat K = (cv::Mat_<double>(3,3) <<
|
||||
data.fx(), 0, data.cx(),
|
||||
0, data.fyOrBaseline(), data.cy(),
|
||||
0, 0, 1);
|
||||
cv::Mat rvec, tvec;
|
||||
std::vector<int> inliers;
|
||||
cv::solvePnPRansac(objectPoints, imagePoints, K, cv::Mat(), rvec, tvec, false, 100, 8., 100, inliers);
|
||||
UDEBUG("");
|
||||
UDEBUG("inliers=%d/%d", (int)inliers.size(), (int)objectPoints.size());
|
||||
|
||||
/*
|
||||
/// Debug draw matches
|
||||
std::vector<cv::DMatch> good_matches(inliers.size());
|
||||
for(i=0; i<(int)good_matches.size(); ++i)
|
||||
{
|
||||
good_matches[i].trainIdx = inliers[i];
|
||||
good_matches[i].queryIdx = inliers[i];
|
||||
}
|
||||
|
||||
cv::Mat imgInliers;
|
||||
cv::drawMatches( refFrame_, a, newFrame, b,
|
||||
good_matches, imgInliers, cv::Scalar::all(-1), cv::Scalar::all(-1),
|
||||
std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
|
||||
UWARN("saved test.png");
|
||||
cv::imwrite("test.png", imgInliers);
|
||||
cv::imwrite("testa.png", refFrame_);
|
||||
cv::imwrite("testb.png", newFrame);
|
||||
/// Debug draw matches
|
||||
*/
|
||||
|
||||
if((int)inliers.size() > this->getMinInliers())
|
||||
{
|
||||
cv::Mat R(3,3,CV_64FC1);
|
||||
cv::Rodrigues(rvec, R);
|
||||
|
||||
std::cout << "R: " << R << std::endl;
|
||||
std::cout << "T: " << tvec << std::endl;
|
||||
|
||||
//R = R.t(); // rotation of inverse
|
||||
//tvec = -R * tvec; // translation of inverse
|
||||
|
||||
//UDEBUG("camera movement:");
|
||||
//std::cout << "R: " << R << std::endl;
|
||||
//std::cout << "T: " << tvec << std::endl;
|
||||
|
||||
output = Transform(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));
|
||||
output = data.localTransform() * output.inverse() * data.localTransform().inverse();
|
||||
output = this->getPose().inverse() * refCorners3DPose_ * output;
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliers.size(), this->getMinInliers());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Not enough pairs found (%d)...", (int)pairs.size());
|
||||
}
|
||||
|
||||
// remove new words from dictionary
|
||||
for(unsigned int i=0; i<newWordIds.size(); ++i)
|
||||
{
|
||||
dictionary_->removeAllWordRef(newWordIds[i], 2);
|
||||
}
|
||||
dictionary_->deleteUnusedWords();
|
||||
}
|
||||
else
|
||||
{
|
||||
//flow
|
||||
|
||||
UDEBUG("flow");
|
||||
// Find features in the new left image
|
||||
std::vector<unsigned char> status;
|
||||
std::vector<float> err;
|
||||
std::vector<cv::Point2f> flowCorners = refCornersGuess_;
|
||||
std::vector<cv::Point2f> refCorners = refCorners_;
|
||||
std::vector<cv::KeyPoint> refKpts = refKpts_;
|
||||
cv::Mat refDescriptors = refDescriptors_;
|
||||
UDEBUG("cv::calcOpticalFlowPyrLK() begin (ref=%d guess=%d)", (int)refCorners.size(), (int)flowCorners.size());
|
||||
cv::calcOpticalFlowPyrLK(
|
||||
refFrame_,
|
||||
newFrame,
|
||||
refCorners,
|
||||
flowCorners,
|
||||
status,
|
||||
err,
|
||||
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
|
||||
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
|
||||
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
|
||||
UDEBUG("cv::calcOpticalFlowPyrLK() end");
|
||||
|
||||
UDEBUG("Filtering optical flow outliers...");
|
||||
std::vector<cv::Point2f> tmpFlowCorners(status.size());
|
||||
std::vector<cv::Point2f> tmpRefCorners(status.size());
|
||||
std::vector<cv::KeyPoint> tmpRefKpts(status.size());
|
||||
cv::Mat tmpRefDescriptors;
|
||||
int oi = 0;
|
||||
float flow = 0;
|
||||
float minFlow = 50;
|
||||
|
||||
UASSERT(flowCorners.size() == status.size());
|
||||
UASSERT(refCorners.size() == status.size());
|
||||
UASSERT(refKpts.size() == status.size());
|
||||
UASSERT(refDescriptors.rows == (int)status.size());
|
||||
for(unsigned int i=0; i<status.size(); ++i)
|
||||
{
|
||||
if(status[i] && refCornersMask_[i])
|
||||
{
|
||||
float dx = refCorners[i].x - flowCorners[i].x;
|
||||
float dy = refCorners[i].y - flowCorners[i].y;
|
||||
float tmp = std::sqrt(dx*dx + dy*dy);
|
||||
flow+=tmp;
|
||||
|
||||
tmpFlowCorners[oi] = flowCorners[i];
|
||||
tmpRefCorners[oi] = refCorners[i];
|
||||
tmpRefKpts[oi] = refKpts[i];
|
||||
tmpRefDescriptors.push_back(refDescriptors.row(i));
|
||||
++oi;
|
||||
|
||||
UDEBUG("%d = ref(%f %f) flow(%f %f) = %f", i,
|
||||
refCorners[i].x, refCorners[i].y,
|
||||
flowCorners[i].x, flowCorners[i].y,
|
||||
tmp);
|
||||
}
|
||||
else
|
||||
{
|
||||
refCornersMask_[i] = 0;
|
||||
}
|
||||
}
|
||||
if(oi)
|
||||
{
|
||||
flow /=float(oi);
|
||||
}
|
||||
tmpFlowCorners.resize(oi);
|
||||
tmpRefCorners.resize(oi);
|
||||
tmpRefKpts.resize(oi);
|
||||
UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)status.size());
|
||||
|
||||
if(flow > minFlow && oi > this->getMinInliers())
|
||||
{
|
||||
flowCorners = tmpFlowCorners;
|
||||
refCorners = tmpRefCorners;
|
||||
refKpts = tmpRefKpts;
|
||||
refDescriptors = tmpRefDescriptors;
|
||||
|
||||
UDEBUG("flow=%f", flow);
|
||||
// compute fundamental matrix
|
||||
UDEBUG("Find fundamental matrix");
|
||||
status.clear();
|
||||
cv::Mat F = cv::findFundamentalMat(refCorners, flowCorners, status, cv::RANSAC, 3.0, 0.99);
|
||||
std::cout << "F=" << F << std::endl;
|
||||
|
||||
if(!F.empty())
|
||||
{
|
||||
UDEBUG("Filtering fundamental matrix outliers...");
|
||||
tmpFlowCorners.resize(status.size());
|
||||
tmpRefCorners.resize(status.size());
|
||||
tmpRefKpts.resize(status.size());
|
||||
tmpRefDescriptors = cv::Mat();
|
||||
oi = 0;
|
||||
UASSERT(flowCorners.size() == status.size());
|
||||
UASSERT(refCorners.size() == status.size());
|
||||
UASSERT(refKpts.size() == status.size());
|
||||
UASSERT(refDescriptors.rows == (int)status.size());
|
||||
for(unsigned int i=0; i<status.size(); ++i)
|
||||
{
|
||||
if(status[i])
|
||||
{
|
||||
tmpFlowCorners[oi] = flowCorners[i];
|
||||
tmpRefCorners[oi] = refCorners[i];
|
||||
tmpRefKpts[oi] = refKpts[i];
|
||||
tmpRefDescriptors.push_back(refDescriptors.row(i));
|
||||
++oi;
|
||||
}
|
||||
}
|
||||
tmpFlowCorners.resize(oi);
|
||||
tmpRefCorners.resize(oi);
|
||||
tmpRefKpts.resize(oi);
|
||||
flowCorners = tmpFlowCorners;
|
||||
refCorners = tmpRefCorners;
|
||||
refKpts = tmpRefKpts;
|
||||
refDescriptors = tmpRefDescriptors;
|
||||
UDEBUG("Filtering fundamental matrix outliers...done! (inliers=%d/%d)", oi, (int)status.size());
|
||||
|
||||
if(refCorners.size())
|
||||
{
|
||||
std::vector<cv::Point2f> lastCornersRefined;
|
||||
std::vector<cv::Point2f> newCornersRefined;
|
||||
//UDEBUG("Correcting matches...");
|
||||
cv::correctMatches(F, refCorners, flowCorners, lastCornersRefined, newCornersRefined);
|
||||
refCorners = lastCornersRefined;
|
||||
flowCorners = newCornersRefined;
|
||||
//UDEBUG("Correcting matches...done!");
|
||||
|
||||
UDEBUG("Computing P...");
|
||||
cv::Mat K = (cv::Mat_<double>(3,3) <<
|
||||
data.fx(), 0, data.cx(),
|
||||
0, data.fyOrBaseline(), data.cy(),
|
||||
0, 0, 1);
|
||||
//std::cout << "K=" << K << std::endl;
|
||||
cv::Mat Kinv = K.inv();
|
||||
//std::cout << "Kinv=" << Kinv << std::endl;
|
||||
cv::Mat E = K.t()*F*K;
|
||||
std::cout << "E=" << E << std::endl;
|
||||
|
||||
//normalize coordinates
|
||||
cv::Mat x(3, refCorners.size(), CV_64FC1);
|
||||
cv::Mat xp(3, refCorners.size(), CV_64FC1);
|
||||
for(unsigned int i=0; i<refCorners.size(); ++i)
|
||||
{
|
||||
x.at<double>(0, i) = refCorners[i].x;
|
||||
x.at<double>(1, i) = refCorners[i].y;
|
||||
x.at<double>(2, i) = 1;
|
||||
|
||||
xp.at<double>(0, i) = flowCorners[i].x;
|
||||
xp.at<double>(1, i) = flowCorners[i].y;
|
||||
xp.at<double>(2, i) = 1;
|
||||
|
||||
//UDEBUG("ptA= %f %f %f", ptA.at<double>(0, i), ptA.at<double>(1, i), ptA.at<double>(2, i));
|
||||
}
|
||||
|
||||
cv::Mat x_norm = Kinv * x;
|
||||
cv::Mat xp_norm = Kinv * xp;
|
||||
x_norm = x_norm.rowRange(0,2);
|
||||
xp_norm = xp_norm.rowRange(0,2);
|
||||
x = x.rowRange(0,2);
|
||||
xp = xp.rowRange(0,2);
|
||||
|
||||
cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
|
||||
if(!P.empty())
|
||||
{
|
||||
cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
|
||||
P0.at<double>(0,0) = 1;
|
||||
P0.at<double>(1,1) = 1;
|
||||
P0.at<double>(2,2) = 1;
|
||||
|
||||
UDEBUG("Computing P...done!");
|
||||
std::cout << "P=" << P << std::endl;
|
||||
|
||||
//scale
|
||||
//P.col(3) /= 10.0;
|
||||
|
||||
cv::Mat R, T;
|
||||
EpipolarGeometry::findRTFromP(P, R, T);
|
||||
//std::cout << "R=" << R << std::endl;
|
||||
//std::cout << "T=" << T << std::endl;
|
||||
|
||||
UDEBUG("");
|
||||
//cv::Mat pts4D;
|
||||
std::vector<double> reprojErrors;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
|
||||
EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
|
||||
//cv::triangulatePoints(P0, P, x_norm, xp_norm, pts4D);
|
||||
|
||||
tmpRefCorners.resize(cloud->size());
|
||||
tmpRefKpts.resize(cloud->size());
|
||||
tmpRefDescriptors = cv::Mat();
|
||||
refCorners3D_->resize(cloud->size());
|
||||
oi = 0;
|
||||
UASSERT(refCorners.size() == cloud->size());
|
||||
UASSERT(refKpts.size() == cloud->size());
|
||||
UASSERT(refDescriptors.rows == (int)cloud->size());
|
||||
for(unsigned int i=0; i<cloud->size(); ++i)
|
||||
{
|
||||
if(cloud->at(i).z>0)
|
||||
{
|
||||
refCorners3D_->at(oi) = cloud->at(i);
|
||||
tmpRefCorners[oi] = refCorners[i];
|
||||
tmpRefKpts[oi] = refKpts[i];
|
||||
tmpRefDescriptors.push_back(refDescriptors.row(i));
|
||||
++oi;
|
||||
}
|
||||
}
|
||||
refCorners3D_->resize(oi);
|
||||
tmpRefCorners.resize(oi);
|
||||
tmpRefKpts.resize(oi);
|
||||
refCorners = tmpRefCorners;
|
||||
refKpts = tmpRefKpts;
|
||||
refDescriptors = tmpRefDescriptors;
|
||||
UDEBUG("Filtering triangulation outliers...done! (inliers=%d/%d)", oi, (int)cloud->size());
|
||||
|
||||
//refCorners3D_ = util3d::transformPointCloud<pcl::PointXYZ>(refCorners3D_, data.localTransform());
|
||||
|
||||
refCorners3DPose_ = this->getPose();
|
||||
dictionary_->clear();
|
||||
refCorners_ = refCorners;
|
||||
refKpts_ = refKpts;
|
||||
refDescriptors_ = refDescriptors;
|
||||
std::vector<int> wordsId = uListToVector(dictionary_->addNewWords(refDescriptors_, 1));
|
||||
refWords_.clear();
|
||||
UASSERT(wordsId.size() == refCorners_.size());
|
||||
for(unsigned int i=0; i<wordsId.size(); ++i)
|
||||
{
|
||||
refWords_.insert(std::make_pair(wordsId[i], refKpts[i]));
|
||||
}
|
||||
dictionary_->update();
|
||||
|
||||
output = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0)/*/T.at<double>(3)*/,
|
||||
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1)/*/T.at<double>(3)*/,
|
||||
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2)/*/T.at<double>(3)*/);
|
||||
output = data.localTransform() * output.inverse() * data.localTransform().inverse();
|
||||
}
|
||||
else
|
||||
{
|
||||
UFATAL("No valid camera matrix found!");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
|
||||
refCornersGuess_ = flowCorners;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//return Identity
|
||||
output = Transform::getIdentity();
|
||||
|
||||
std::vector<cv::KeyPoint> newKpts;
|
||||
std::vector<cv::Point2f> newCorners;
|
||||
cv::Mat newDescriptors;
|
||||
if(data.keypoints().size())
|
||||
{
|
||||
cv::KeyPoint::convert(data.keypoints(), newCorners);
|
||||
newKpts = data.keypoints();
|
||||
newDescriptors = data.descriptors();
|
||||
}
|
||||
else
|
||||
{
|
||||
// generate kpts
|
||||
cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
|
||||
newKpts = feature2D_->generateKeypoints(newFrame, this->getMaxFeatures(), roi);
|
||||
Feature2D::limitKeypoints(newKpts, this->getMaxFeatures());
|
||||
|
||||
if(newKpts.size())
|
||||
{
|
||||
//extract descriptors (before subpixel)
|
||||
newDescriptors = feature2D_->generateDescriptors(newFrame, newKpts);
|
||||
|
||||
cv::KeyPoint::convert(newKpts, newCorners);
|
||||
|
||||
if(subPixWinSize_ > 0 && subPixIterations_ > 0)
|
||||
{
|
||||
UDEBUG("cv::cornerSubPix() begin");
|
||||
cv::cornerSubPix(newFrame, newCorners,
|
||||
cv::Size( subPixWinSize_, subPixWinSize_ ),
|
||||
cv::Size( -1, -1 ),
|
||||
cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
|
||||
UDEBUG("cv::cornerSubPix() end");
|
||||
|
||||
for(unsigned int i=0; i<newCorners.size(); ++i)
|
||||
{
|
||||
newKpts[i].pt = newCorners[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if((int)newCorners.size() > this->getMinInliers())
|
||||
{
|
||||
refFrame_ = newFrame;
|
||||
refCorners_ = newCorners;
|
||||
refKpts_ = newKpts;
|
||||
refDescriptors_ = newDescriptors;
|
||||
refCornersGuess_ = newCorners;
|
||||
refCornersMask_.resize(newCorners.size(), 1);
|
||||
UASSERT(refCorners_.size() == refKpts_.size());
|
||||
UASSERT(refDescriptors_.rows == (int)refKpts_.size());
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Too low 2D corners (%d), ignoring new frame...",
|
||||
(int)newCorners.size());
|
||||
}
|
||||
}
|
||||
|
||||
UINFO("Odom update time = %fs tf=[%s] inliers=%d/%d, transform accepted=%s",
|
||||
timer.elapsed(),
|
||||
output.prettyPrint().c_str(),
|
||||
inliers,
|
||||
correspondences,
|
||||
!output.isNull()?"true":"false");
|
||||
|
||||
return output;
|
||||
|
||||
}
|
||||
private:
|
||||
//Parameters:
|
||||
int flowWinSize_;
|
||||
int flowIterations_;
|
||||
double flowEps_;
|
||||
int flowMaxLevel_;
|
||||
|
||||
int subPixWinSize_;
|
||||
int subPixIterations_;
|
||||
double subPixEps_;
|
||||
|
||||
Feature2D * feature2D_;
|
||||
VWDictionary * dictionary_;
|
||||
|
||||
cv::Mat refFrame_;
|
||||
std::vector<cv::Point2f> refCorners_;
|
||||
std::vector<cv::Point2f> refCornersGuess_;
|
||||
std::vector<unsigned char> refCornersMask_;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr refCorners3D_;
|
||||
Transform refCorners3DPose_;
|
||||
std::vector<cv::KeyPoint> refKpts_;
|
||||
cv::Mat refDescriptors_;
|
||||
std::multimap<int, cv::KeyPoint> refWords_;
|
||||
};
|
||||
|
||||
class MainWidget : public QWidget
|
||||
{
|
||||
public:
|
||||
@@ -327,7 +892,7 @@ int main(int argc, char** argv)
|
||||
std::cout<<"K=" << k << std::endl;
|
||||
timer.start();
|
||||
//std::cout<<"e=" << e << std::endl;
|
||||
cv::Mat p = EpipolarGeometry::findPFromF(e, x1, x2);
|
||||
cv::Mat p = EpipolarGeometry::findPFromE(e, x1, x2);
|
||||
cv::Mat p0 = cv::Mat::zeros(3, 4, CV_64FC1);
|
||||
p0.at<double>(0,0) = 1;
|
||||
p0.at<double>(1,1) = 1;
|
||||
|
||||
Reference in New Issue
Block a user