int main(int argc, char *argv[])
{
cout << endl
<< " iHBT_fit " << endl
<< endl
<< " Ver 1.2 ----- Chun Shen, 10/2014 " << endl;
cout << endl << "**********************************************************" << endl;
display_logo(2); // Hail to the king~
cout << endl << "**********************************************************" << endl << endl;
// Read-in parameters
ParameterReader *paraRdr = new ParameterReader;
paraRdr->readFromFile("parameters.dat");
paraRdr->echo();
string filename=argv[1];
Stopwatch sw_total;
sw_total.tic();
fit_correlation test(filename, paraRdr);
test.fit();
sw_total.toc();
cout << "Program totally finished in " << sw_total.takeTime() << " sec." << endl;
return 0;
}
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;
}
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ){
PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb,newFrame.depth,camera);
// 合并点云
PointCloud::Ptr output(new PointCloud());
pcl::transformPointCloud(*original,*output,T.matrix());
*newCloud += *output;
// Voxel grid 滤波降采样
static pcl::VoxelGrid<PointT> voxel;
static ParameterReader pd;
double gridsize = atof(pd.getData("voxel_grid").c_str());
voxel.setLeafSize(gridsize,gridsize,gridsize);
voxel.setInputCloud(newCloud);
PointCloud::Ptr tmp(new PointCloud());
voxel.filter(*tmp);
return tmp;
}
int main(int argc, char** argv)
{
if (argc!=3){
std::cerr<<"There should be two arguments"<<std::endl;
return -1;
}
std::string parameterfile = argv[1];
std::string datafile = argv[2];
ParameterReader parameters;
parameters.readParameters(parameterfile);
MolDy particleSystem(parameters);
particleSystem.readData(datafile);
particleSystem.simulate();
return 0;
}
int main(int argc,char** argv){
ParameterReader* pr = new ParameterReader("../parameters.txt");
CAMERA_INTRINSIC_PARAMETERS camera;
if(strcmp((pr->getData("camera.cx")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("camera.cy")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("camera.fx")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("camera.fy")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("camera.scale")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("detector")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("descriptor")).c_str(),"NOT_FOUND")==0 || strcmp((pr->getData("good_match_threshold")).c_str(),"NOT_FOUND")==0){
cout<<"Parameter lost."<<endl;
return -1;
}
camera.cx = atof((pr->getData("camera.cx")).c_str());
camera.cy = atof((pr->getData("camera.cy")).c_str());
camera.fx = atof((pr->getData("camera.fx")).c_str());
camera.fy = atof((pr->getData("camera.fy")).c_str());
camera.scale = atof((pr->getData("camera.scale")).c_str());
int min_good_match = atoi((pr->getData("min_good_match")).c_str());
/*
cv::Mat rgb,depth;
rgb = cv::imread("../pic/color.png");
depth = cv::imread("../pic/depth.png",-1);
PointCloud::Ptr cloud = image2PointCloud(rgb,depth,camera);
cloud->points.clear();
cv::Point3f point;
point.x = 50;
point.y = 100;
point.z = 1200;
cv::Point3f p = point2dTo3d(point,camera);
cout<<"2D point ("<<point.x<<","<<point.y<<") to "<<"3D point ("<<p.x<<","<<p.y<<","<<p.z<<")"<<endl;
*/
double good_match_threshold = atof((pr->getData("good_match_threshold")).c_str());
FRAME frame1,frame2;
frame1.rgb = cv::imread("../pic/color1.png");
frame1.depth = cv::imread("../pic/depth1.png");
frame2.rgb = cv::imread("../pic/color2.png");
frame2.depth = cv::imread("../pic/depth2.png");
string detector = pr->getData("detector");
string descriptor = pr->getData("descriptor");
computeKeyPointsAndDesp(frame1,detector,descriptor);
computeKeyPointsAndDesp(frame2,detector,descriptor);
RESULT_OF_PNP motion = estimateMotion(frame1,frame2,camera,good_match_threshold,min_good_match);
cout<<"Inliers = "<<motion.inliers<<endl;
cout<<"R = "<<motion.rvec<<endl;
cout<<"t = "<<motion.tvec<<endl;
//joinPointCloud(frame1,frame2,camera,motion);
return 0;
}
int main(int argc, char** argv) {
Stopwatch sw;
sw.tic();
ParameterReader* paraRdr = new ParameterReader();
paraRdr->readFromFile("parameters.dat");
paraRdr->readFromArguments(argc, argv);
// create integration grid along eta direction for boost-invariant medium
int neta = paraRdr->getVal("neta");
double eta_i = paraRdr->getVal("eta_i");
double eta_f = paraRdr->getVal("eta_f");
double* eta_ptr = new double[neta];
double* etaweight_ptr = new double[neta];
gauss_quadrature(neta, 1, 0.0, 0.0, eta_i, eta_f, eta_ptr, etaweight_ptr);
PhotonEmission thermalPhotons(paraRdr);
// initialize hydro medium
int hydro_flag = paraRdr->getVal("hydro_flag");
if (hydro_flag == 0) {
int bufferSize = paraRdr->getVal("HydroinfoBuffersize");
int hydroInfoVisflag = paraRdr->getVal("HydroinfoVisflag");
// hydro data file pointer
HydroinfoH5* hydroinfo_ptr = new HydroinfoH5(
"results/JetData.h5", bufferSize, hydroInfoVisflag);
// calculate thermal photons from the hydro medium
thermalPhotons.calPhotonemission(hydroinfo_ptr, eta_ptr,
etaweight_ptr);
delete hydroinfo_ptr;
} else if (hydro_flag == 1) {
Hydroinfo_MUSIC* hydroinfo_ptr = new Hydroinfo_MUSIC();
int hydro_mode = 8;
int nskip_tau = paraRdr->getVal("hydro_nskip_tau");
hydroinfo_ptr->readHydroData(hydro_mode, nskip_tau);
// calculate thermal photons from the hydro medium
thermalPhotons.calPhotonemission(hydroinfo_ptr, eta_ptr,
etaweight_ptr);
delete hydroinfo_ptr;
} else if (hydro_flag == 3) {
Hydroinfo_MUSIC* hydroinfo_ptr = new Hydroinfo_MUSIC();
int hydro_mode = 9;
int nskip_tau = paraRdr->getVal("hydro_nskip_tau");
hydroinfo_ptr->readHydroData(hydro_mode, nskip_tau);
// calculate thermal photons from the hydro medium
thermalPhotons.calPhotonemission(hydroinfo_ptr, eta_ptr,
etaweight_ptr);
delete hydroinfo_ptr;
} else if (hydro_flag == 2) {
Hydroinfo_MUSIC* hydroinfo_ptr = new Hydroinfo_MUSIC();
int hydro_mode = 10;
int nskip_tau = 1;
hydroinfo_ptr->readHydroData(hydro_mode, nskip_tau);
// calculate thermal photons from the hydro medium
thermalPhotons.calPhotonemission_3d(hydroinfo_ptr);
delete hydroinfo_ptr;
} else {
cout << "main: unrecognized hydro_flag = " << hydro_flag << endl;
exit(1);
}
// sum up all channels and compute thermal photon spectra and vn
thermalPhotons.calPhoton_SpvnpT_individualchannel();
thermalPhotons.calPhoton_total_SpMatrix();
thermalPhotons.calPhoton_total_Spvn();
// output results
thermalPhotons.outputPhotonSpvn();
sw.toc();
cout << "totally takes : " << sw.takeTime() << " seconds." << endl;
// clean up
delete [] eta_ptr;
delete [] etaweight_ptr;
return(0);
}
int main(int argc, char** args)
{
// check the range of the values in this mesh
ParameterReader pd;
bool use_mesh_file = atof(pd.getData("use_mesh_file").c_str());
if(use_mesh_file)
{
// this works for a single mesh
PangaeaMeshData inputMesh;
std::string input_file = pd.getData("input_mesh_file");
std::string output_file = pd.getData("output_mesh_file");
PangaeaMeshIO::loadfromFile(input_file, inputMesh);
vector<vector<double> > bbox;
getMeshBoundingBox(inputMesh, bbox);
double xrange = bbox[0][1] - bbox[0][0];
double yrange = bbox[1][1] - bbox[1][0];
double zrange = bbox[2][1] - bbox[2][0];
cout << "x_range: " << xrange << endl;
cout << "y_range: " << yrange << endl;
cout << "z_range: " << zrange << endl;
double factor = 400.0 / zrange;
cout << "scaling factor is: " << factor << endl;
// scale the mesh up
scaleMeshUp(inputMesh, factor);
PangaeaMeshIO::writeToFile(output_file, inputMesh);
}else
{
// this works for a heirachy of meshes
PangaeaMeshData inputMesh;
std::string input_format = pd.getData("input_mesh_format");
std::string output_format = pd.getData("output_mesh_format");
std::string input_list_str = pd.getData("input_list");
std::stringstream input_list(input_list_str);
int number; vector<int> input_list_vector;
input_list >> number;
while( !input_list.fail() )
{
input_list_vector.push_back(number);
input_list >> number;
}
double factor = 0;
char buffer[BUFFER_SIZE];
for(int i = 0; i < input_list_vector.size(); ++i)
{
stringstream input_file;
sprintf(buffer, input_format.c_str(), input_list_vector[i]);
input_file << buffer;
//memset(&buffer[0], 0, sizeof(buffer));
stringstream output_file;
sprintf(buffer, output_format.c_str(), input_list_vector[i]);
output_file << buffer;
//memset(&buffer[0], 0, sizeof(buffer));
PangaeaMeshIO::loadfromFile(input_file.str(), inputMesh);
if(factor == 0) // if this is the first frame
{
vector<vector<double> > bbox;
getMeshBoundingBox(inputMesh, bbox);
double xrange = bbox[0][1] - bbox[0][0];
double yrange = bbox[1][1] - bbox[1][0];
double zrange = bbox[2][1] - bbox[2][0];
cout << "x_range: " << xrange << endl;
cout << "y_range: " << yrange << endl;
cout << "z_range: " << zrange << endl;
factor = 400.0 / zrange;
cout << "scaling factor is: " << factor << endl;
}
// scale the mesh up
scaleMeshUp(inputMesh, factor);
PangaeaMeshIO::writeToFile(output_file.str(), inputMesh);
}
}
}
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;
}
