FeatureMatcher::FeatureMatcher(cv::Mat& sourceImage, cv::Mat& targetImage, PointCloudPtr& sourceCloud, PointCloudPtr& targetCloud) {
ParameterReader paramReader;
paramReader.get("descriptor_matches", matcherType_);
sourceCloudPtr_ = sourceCloud;
targetCloudPtr_ = targetCloud;
sourceImage_ = sourceImage;
targetImage_ = targetImage;
}
FeatureMatcher::FeatureMatcher() {
ParameterReader paramReader;
paramReader.get("descriptor_matches", matcherType_);
}
int FeatureMatcher::outlierRemoval( std::vector<cv::DMatch>& rawMatches, std::vector< cv::DMatch >& goodMatches) {
ParameterReader paramReader;
int ransac, minInliers;
paramReader.get ("RANSAC", ransac);
paramReader.get ("minimum_inliers", minInliers);
if (ransac) {
RansacTransformation ransacTransForm;
Eigen::Matrix4f resTransForm;
float rmse = 0.0;
//print3DMatch (sourceFeatureLocation3f_, targetFeatureLocation3f_);
ransacTransForm.getRelativeTransformationTo(sourceFeatureLocation3f_, targetFeatureLocation3f_, &rawMatches, resTransForm, rmse, goodMatches, minInliers );
// Eigen::Quaterniond q (resTransForm.block <3,3>(0, 0) );
Eigen::Vector3f tran;
tran = resTransForm.block<3, 1>(0, 3);
/*
tran (0) = resTransForm(3,0);
tran (1) = resTransForm(3,1);
tran (2) = resTransForm(3,2);
*/
Eigen::Matrix3d rotMax = resTransForm.block<3 ,3 > (0, 0).cast<double> ();
//Eigen::Matrix3d rotMaxd = rotMaxf.cast<>
Eigen::Quaterniond q(rotMax);
std::cout << "T : " << tran(0) << ", " << tran(1) << ", " << tran(2) << std::endl;
std::cout << "Q: " << q.w() << ", " << q.x() << ", " << q.y() << ", "<< q.z() << std::endl;
Eigen::Affine3d aff;
} else {
// Outlier detection
double max_dist = 0;
double min_dist = 100;
//-- Quick calculation of max and min distances between keypoints
for (uint i = 0; i < rawMatches.size (); i++)
{
double dist = rawMatches[i].distance;
if (dist < min_dist)
min_dist = dist;
if (dist > max_dist)
max_dist = dist;
}
printf ("-- Max dist : %f \n", max_dist);
printf ("-- Min dist : %f \n", min_dist);
//-- Find only "good" matches (i.e. whose distance is less than 2*min_dist )
//-- PS.- radiusMatch can also be used here.
for (uint i = 0; i < rawMatches.size (); i++)
{
if (rawMatches[i].distance < 4 * min_dist)
goodMatches.push_back (rawMatches[i]);
}
for (uint i = 0; i < goodMatches.size (); i++)
{
printf ("-- Good Match [%d] Keypoint 1: %d -- Keypoint 2: %d \n", i,
goodMatches[i].queryIdx, goodMatches[i].trainIdx);
}
}
return 0;
}
void NucleusGridFunction::
readParameterFile( ParameterReader ¶ms )
{
//read:
// nucleus type=none, file, box, sphere
// nucleus file= <file.cwn>
// nucleus corners= x0 y0 z0 x1 y1 z1
// nucleus center= x0 y0 z0
// nucleus radius= r0
// nucleus boundary thickness= t0
const std::string keyBoundaryThickness="nucleus boundary thickness";
double thickness;
params.get( "nucleus boundary thickness", thickness, -1. );
if ( thickness>=0. ) nucleusBoundaryThickness=thickness;
typedef boost::tokenizer<>::iterator TokIterator;
typedef std::vector<double> DoubleVector;
bool noNucleus=false;
std::string nucleusType;
params.get( "nucleus type", nucleusType, "");
if ( (nucleusType == "box" ) || (nucleusType == "cubic")) {
Nucleus::NucleusShape nucleusShape=Nucleus::BoxNucleus;
std::string doubleList="";
params.get("nucleus corners", doubleList );
DPrintf(DebugSolver,"..NucleusGridFunction: nucleus type=box.\n");
DPrintf(DebugSolver,".... nucleus corners = '%s'\n", doubleList.c_str());
boost::tokenizer<> tok(doubleList);
DoubleVector corners;
DPrintf(DebugSolver," corners are = ");
corners.clear();
for (TokIterator it=tok.begin(); it!=tok.end(); ++it) {
double q;
sscanf(it->c_str(), "%lf", &q);
DPrintf(DebugSolver," %lf ", q);
corners.push_back( q );
}
DPrintf(DebugSolver,"\n");
//..create the nucleic info
if( corners.size() >5 ) {
Nucleus thisNuc;
thisNuc.setID(1);
thisNuc.setBoundaryThickness( nucleusBoundaryThickness );
thisNuc.setCorners(corners[0], corners[1], corners[2],
corners[3], corners[4], corners[5]);
thisNuc.setShape( nucleusShape );
nucleus.push_back( thisNuc );
}
}
else if ( (nucleusType == "sphere") || ( nucleusType == "spherical")) {
Nucleus::NucleusShape nucleusShape=Nucleus::SphericalNucleus;
double radius;
params.get("nucleus radius",radius, 20.);
std::string doubleList="";
params.get("nucleus center", doubleList );
DPrintf(DebugSolver,"..NucleusGridFunction: nucleus type=sphere not supported yet:\n");
DPrintf(DebugSolver,".. nucleus center = '%s'\n", doubleList.c_str());
boost::tokenizer<> tok(doubleList);
DoubleVector center;
DPrintf(DebugSolver," center is = ");
center.clear();
for (TokIterator it=tok.begin(); it!=tok.end(); ++it) {
double q;
sscanf(it->c_str(), "%lf", &q);
DPrintf(DebugSolver," %lf ", q);
center.push_back( q );
}
DPrintf(DebugSolver,"\n");
//..create the nucleic info
double x0=0., y0=0., z0=0.;
if( center.size() >1) {
x0=center[0];
y0=center[1];
}
if( center.size()>2) {
z0=center[2];
}
Nucleus thisNuc;
thisNuc.setID(1);
thisNuc.setBoundaryThickness( nucleusBoundaryThickness );
thisNuc.setCenter( x0, y0, z0);
thisNuc.setRadius( radius);
thisNuc.setShape( nucleusShape );
nucleus.push_back( thisNuc );
}
else if ( nucleusType == "file" ) {
std::string nucleusFileName="";
params.get("nucleus file",nucleusFileName);
DPrintf(DebugSolver,"nuclei read in from file='%s'\n", nucleusFileName.c_str());
readCellNucleusFile( nucleusFileName );
}
else { // all other options mean--> no nucleus
noNucleus=true;
}
}
int main(int argc, char** argv)
{
/*
if (argc != 3) {
usage();
return -1;
}
int startIndex = std::atoi(argv[1]);
int endIndex = std::atoi(argv[2]);
*/
int startIndex = 1;
int endIndex = 500;
std::vector<FRAME> keyFrame;
ParameterReader pd;
int check_loop_closure, nearby_loops, random_loops;
pd.get( "check_loop_closure", check_loop_closure );
pd.get( "nearby_loops", nearby_loops );
pd.get( "random_loops", random_loops );
//ofstream fout("/home/exbot/catkin_ws/dataset/xyz/odometry.txt", ios::trunc);
FRAME lastFrame;
FRAME currentFrame;
FeatureDection featDection;
readFrame(startIndex, lastFrame);
imagesToPointCloud(lastFrame.depImg, lastFrame.rgbImg, " ", lastFrame.cloud);
int kpSize = featDection.computeKeyPointsAndDesp(lastFrame.rgbImg, lastFrame.kp, lastFrame.desp);
if (kpSize < 10) {
std::cout << "the frame id %d key points size is : %d, too little!\n" << startIndex << kpSize;
return -1;
}
/*初始化g2o*/
SlamEnd slamEnd;
slamEnd.addVertex( lastFrame.frameID, true);
keyFrame.push_back(lastFrame);
for (int index = startIndex +1; index < endIndex; index ++) {
readFrame (index, currentFrame);
imagesToPointCloud(currentFrame.depImg, currentFrame.rgbImg, " ", currentFrame.cloud);
int kpSize = featDection.computeKeyPointsAndDesp(currentFrame.rgbImg, currentFrame.kp, currentFrame.desp);
if (kpSize < 10) {
std::cout << "the frame id %d key points size is : %d, too little!\n" << startIndex << kpSize;
return -1;
}
CHECK_RESULT result = slamEnd.checkKeyframes(keyFrame.back(), currentFrame);
switch (result ) {
case NOT_MATCHED:
std::cout << "Not enough linliers." << std::endl;
break;
case TOO_FAR_AWAY:
std::cout << "Too far away, may be an error" << std::endl;
break;
case TOO_CLOSE:
std::cout<< "This is too close" << std::endl;
break;
case KEYFRAME:
std::cout << " This is a new keyframe " << std::endl;
if ( check_loop_closure ) {
slamEnd.checkNearbyLoops(keyFrame, currentFrame);
slamEnd.checkRandomLoops(keyFrame, currentFrame);
}
keyFrame.push_back(currentFrame);
break;
default:
break;
}
/*
//to do the computer
Eigen::Vector7d pos;
pos = Maching(lastFrame, currentFrame);
fout << lastFrame.frameID <<"\t"<<currentFrame.frameID
<< "\t"<< pos(0)<< "\t"<< pos(1)<< "\t"<< pos(2)<< "\t"<< pos(3)
<< "\t"<< pos(4)<< "\t"<< pos(5)<< "\t"<< pos(6) << std::endl;
*/
// lastFrame =currentFrame;
// cv::imshow("rgb", lastFrame->rgbImg);
// cv::waitKey(1000);
}
std::string before = "result_before.g2o";
std::string after = "result_after.g2o";
slamEnd.save(before);
slamEnd.optimize(100);
slamEnd.save(after);
slamEnd.clear();
//fout.close();
return 0;
}
int main(int argc, char** argv) {
//"minimum_inliers"
ros::init(argc, argv, "registration");
std::string rgb1,rgb2, depth1, depth2;
ParameterReader paramReader;
paramReader.get( "rgb1", rgb1 );
paramReader.get( "rgb2", rgb2 );
paramReader.get( "depth1", depth1 );
paramReader.get( "depth2", depth2 );
cv::Mat sourceRgbImage = cv::imread( rgb1, CV_LOAD_IMAGE_ANYCOLOR );
cv::Mat targetRgbImage = cv::imread( rgb2, CV_LOAD_IMAGE_ANYCOLOR );
cv::Mat sourceDepthImage = cv::imread( depth1, CV_LOAD_IMAGE_ANYDEPTH );
cv::Mat targetDepthImage = cv::imread( depth2, CV_LOAD_IMAGE_ANYDEPTH );
pcl::PointCloud< pcl::PointXYZRGB >::Ptr sourcePointCloud(new pcl::PointCloud< pcl::PointXYZRGB>() );
pcl::PointCloud< pcl::PointXYZRGB >::Ptr targetPointCloud(new pcl::PointCloud< pcl::PointXYZRGB>() );
imagesToPointCloud( sourceDepthImage, sourceRgbImage, " ", sourcePointCloud);
imagesToPointCloud( targetDepthImage, targetRgbImage, " ", targetPointCloud);
cv::Mat img1Gray, img2Gray;
cv::cvtColor( sourceRgbImage, img1Gray, CV_RGB2GRAY );
cv::cvtColor( targetRgbImage, img2Gray, CV_RGB2GRAY );
FeatureMatcher featureMatcher;
featureMatcher.setSourceCloud(sourcePointCloud);
featureMatcher.setTargetCloud(targetPointCloud);
featureMatcher.setSourceImage(img1Gray);
featureMatcher.setTargetImage(img2Gray);
Matches matches;
cv::Mat matchesImg;
featureMatcher.getMatches( matches );
featureMatcher.drawMatches( matches.matches, matchesImg );
/*
FeatureMatcher featureMatcher;
featur
*/
/*
FeatureDection featureDection;
ParameterServer * param = ParameterServer::instance();
ParameterReader paramReader;
std::string detector;
double cx;
paramReader.get("detector", detector);
paramReader.get("camera.cx", cx);
std::cout << "detector: " << detector <<" cx: " << cx << std::endl;
int inlier;
inlier = param->get<int>("minimum_inliers");
//inlier = ParameterServer::instance ()->get<int> ("minimum_inliers");
*/
/*
std::vector<KeyPoint> keyPoints;
cv::Mat descriptor;
cv::Mat keyPointsImg;
FeatureDection featureDection;
cv::Mat img = cv::imread(rgb1.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
featureDection.DetectFeature(img, keyPoints);
std::cout << "keyPoints size: " << keyPoints.size() << std::endl;
featureDection.ExtractFeature(img, keyPoints, descriptor);
featureDection.drawFeature(img, keyPoints, keyPointsImg);
*/
// std::cout << "minimum_inliers: " << inlier << std::endl;
cv::imshow("keyPoints", matchesImg );
//cv::imshow("keyPoints", img);
cv::waitKey(-1);
return 0;
}
