int main(void)
{
logger::edglog.open(std::clog, glite::wms::common::logger::debug);
configuration::Configuration config(opt_conf_file,
configuration::ModuleType::workload_manager);
configuration::WMConfiguration const* const wm_config(config.wm());
configuration::NSConfiguration const* const ns_config(config.ns());
while (true) {
purchaser::ism_ii_purchaser icp(ns_config->ii_contact(),
ns_config->ii_port(),
ns_config->ii_dn(),
ns_config->ii_timeout(),
purchaser::once);
try {
icp();
sleep(1);
ism::call_update_ism_entries()();
ism::call_dump_ism_entries()();
std::string filename(ism::get_ism_dump());
purchaser::ism_file_purchaser ifp(filename);
} catch(std::exception& e) {
std::cerr << e.what() << std::endl;
}
}
return 0;
}
IIterativeClosestPointPtr IterativeClosestPointFactory::createIterativeClosestPoint() {
IPointCorrespondence* assigner = new PointCorrespondenceKDTree();;
IRigidTransformationEstimation* estimator = new RigidTransformationEstimationSVD();
IIterativeClosestPointPtr icp(new IterativeClosestPoint(assigner, estimator));
icpConfigurator = boost::dynamic_pointer_cast<IIterativeClosestPointSetup>(icp);
return icp;
}
void testPointPlaneICP()
{
Random rnd( 0 );
Matrix4f dstToSrc = Matrix4f::translation( 0.1f, -0.42f, 0.2f ) * Matrix4f::rotateX( MathUtils::degreesToRadians( 10.f ) );
//Matrix4f dstToSrc = Matrix4f::rotateX( MathUtils::degreesToRadians( 10.f ) );
//Matrix4f dstToSrc = Matrix4f::rotateX( MathUtils::degreesToRadians( 20.f ) );
Matrix4f srcToDstGT = dstToSrc.inverse();
int nPoints = 1024;
std::vector< Vector3f > srcPoints( nPoints );
std::vector< Vector3f > dstPoints( nPoints );
std::vector< Vector3f > dstNormals( nPoints );
for( size_t i = 0; i < dstPoints.size(); ++i )
{
dstPoints[i] = Vector3f( rnd.nextFloat(), rnd.nextFloat(), rnd.nextFloat() );
dstNormals[i] = Sampling::areaSampleSphere( rnd.nextFloat(), rnd.nextFloat() );
srcPoints[i] = dstToSrc.transformPoint( dstPoints[i] );
}
PointPlaneICP icp( 6, 0.01f );
Matrix4f initialGuess = Matrix4f::identity();
Matrix4f mSolution;
bool succeeded = icp.align( srcPoints, dstPoints, dstNormals, initialGuess, mSolution );
Matrix4f diff = srcToDstGT.inverse() - mSolution;
diff.print();
}
void LVRMainWindow::alignPointClouds()
{
Matrix4f mat = m_correspondanceDialog->getTransformation();
QString name = m_correspondanceDialog->getDataName();
QString modelName = m_correspondanceDialog->getModelName();
PointBufferPtr modelBuffer = m_treeWidgetHelper->getPointBuffer(modelName);
PointBufferPtr dataBuffer = m_treeWidgetHelper->getPointBuffer(name);
float pose[6];
LVRModelItem* item = m_treeWidgetHelper->getModelItem(name);
if(item)
{
mat.toPostionAngle(pose);
// Pose ist in radians, so we need to convert p to degrees
// to achieve consistency
Pose p;
p.x = pose[0];
p.y = pose[1];
p.z = pose[2];
p.r = pose[3] * 57.295779513;
p.t = pose[4] * 57.295779513;
p.p = pose[5] * 57.295779513;
item->setPose(p);
}
updateView();
// Refine pose via ICP
if(m_correspondanceDialog->doICP() && modelBuffer && dataBuffer)
{
ICPPointAlign icp(modelBuffer, dataBuffer, mat);
icp.setEpsilon(m_correspondanceDialog->getEpsilon());
icp.setMaxIterations(m_correspondanceDialog->getMaxIterations());
icp.setMaxMatchDistance(m_correspondanceDialog->getMaxDistance());
Matrix4f refinedTransform = icp.match();
cout << "Initial: " << mat << endl;
// Apply correction to initial estimation
//refinedTransform = mat * refinedTransform;
refinedTransform.toPostionAngle(pose);
cout << "Refined: " << refinedTransform << endl;
Pose p;
p.x = pose[0];
p.y = pose[1];
p.z = pose[2];
p.r = pose[3] * 57.295779513;
p.t = pose[4] * 57.295779513;
p.p = pose[5] * 57.295779513;
item->setPose(p);
}
m_correspondanceDialog->clearAllItems();
updateView();
}
int
main (int argc, char **argv)
{
double dist = 0.1;
pcl::console::parse_argument (argc, argv, "-d", dist);
double rans = 0.1;
pcl::console::parse_argument (argc, argv, "-r", rans);
int iter = 100;
pcl::console::parse_argument (argc, argv, "-i", iter);
pcl::registration::ELCH<PointType> elch;
pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
icp->setMaximumIterations (iter);
icp->setMaxCorrespondenceDistance (dist);
icp->setRANSACOutlierRejectionThreshold (rans);
elch.setReg (icp);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
elch.addPointCloud (clouds[i].second);
}
int first = 0, last = 0;
for (size_t i = 0; i < clouds.size (); i++)
{
if (loopDetection (int (i), clouds, 3.0, first, last))
{
std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
elch.setLoopStart (first);
elch.setLoopEnd (last);
elch.compute ();
}
}
for (const auto &cloud : clouds)
{
std::string result_filename (cloud.first);
result_filename = result_filename.substr (result_filename.rfind ('/') + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(cloud.second));
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
bool CLogicLikeCal::PostFixLize(int nodenum){
if(LogicLize(nodenum)!= true)
return false;
m_sOperate.push('#');
int num = m_strExp.length();
string strtmp;
//char strresult[MAX_LEN];
//int j = 0;
char ch ,y;
for(int i = 0;i<num;i++){
ch = m_strExp[i];
if(isdigit(ch))
strtmp+= ch;
else if(ch == ')')
for(y = m_sOperate.top(),m_sOperate.pop() ;y!='(';y = m_sOperate.top(),m_sOperate.pop())
strtmp+= y;
else{
for(y = m_sOperate.top(),m_sOperate.pop();isp(y)>icp(ch);y = m_sOperate.top(),m_sOperate.pop())
strtmp+= y;
m_sOperate.push(y);
m_sOperate.push(ch);
}
}
while(!m_sOperate.empty()){
y = m_sOperate.top();
m_sOperate.pop();
strtmp+= y;
}
//strresult[--j]='\0';
string::iterator iter1 = strtmp.end();
strtmp.erase(iter1-1);
m_strExp = strtmp;
//cout<<endl<<"后缀表达式为："<<m_strExp<<endl;
return true;
}
int
main (int argc, char **argv)
{
pcl::registration::ELCH<PointType> elch;
pcl::IterativeClosestPoint<PointType, PointType>::Ptr icp (new pcl::IterativeClosestPoint<PointType, PointType>);
icp->setMaximumIterations (100);
icp->setMaxCorrespondenceDistance (0.1);
icp->setRANSACOutlierRejectionThreshold (0.1);
elch.setReg (icp);
CloudVector clouds;
for (int i = 1; i < argc; i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[i], *pc);
clouds.push_back (CloudPair (argv[i], pc));
std::cout << "loading file: " << argv[i] << " size: " << pc->size () << std::endl;
elch.addPointCloud (clouds[i-1].second);
}
int first = 0, last = 0;
for (size_t i = 0; i < clouds.size (); i++)
{
if (loopDetection (int (i), clouds, 3.0, first, last))
{
std::cout << "Loop between " << first << " (" << clouds[first].first << ") and " << last << " (" << clouds[last].first << ")" << std::endl;
elch.setLoopStart (first);
elch.setLoopEnd (last);
elch.compute ();
}
}
for (size_t i = 0; i < clouds.size (); i++)
{
std::string result_filename (clouds[i].first);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
box find_CE_via_overapprox(box const & b, unordered_set<Enode*> const & forall_vars, vector<Enode*> const & vec, bool const p, SMTConfig & config) {
vector<contractor> ctcs;
box counterexample(b, forall_vars);
if (config.nra_shrink_for_dop) {
counterexample = shrink_for_dop(counterexample);
}
auto vars = counterexample.get_vars();
unordered_set<Enode *> var_set(vars.begin(), vars.end());
for (Enode * e : vec) {
lbool polarity = p ? l_False : l_True;
if (e->isNot()) {
polarity = !polarity;
e = e->get1st();
}
contractor ctc = make_contractor(e, polarity, counterexample, var_set);
ctcs.push_back(ctc);
}
contractor fp = mk_contractor_fixpoint(default_strategy::term_cond, ctcs);
random_icp icp(fp, config);
counterexample = icp.solve(counterexample, config.nra_precision);
return counterexample;
}
main()
{
char in[30],i,len,post[30],opr[30],ch1,ch2;
int ans[20],post_top=-1,opr_top=-1,ans_top=-1;
int val;
printf("\nEnter the expression:");
gets(in);
opr[++opr_top]='#';
for(i=0;i<strlen(in);i++)
{
ch1=in[i];
printf("Token %c \n",ch1);
if(ch1=='#')
{
}
if(ch1=='+' || ch1=='-'|| ch1=='*' || ch1=='/'|| ch1=='^')
{
if(icp(ch1)>isp(opr[opr_top]))
{
printf("~");
opr[++opr_top]=ch1;
}
else
{
printf("!");
while(icp(ch1) <= isp(opr[opr_top]))
{
post[++post_top]=opr[opr_top--];
}
opr[++opr_top]=ch1;
}
}
if(ch1==')')
{
while(opr[opr_top] != '(' )
{
post[++post_top]=opr[opr_top--];
}
opr_top--;
}
if(ch1=='(')
{
opr[++opr_top]=ch1;
}
if(ch1>='a' && ch1<='z')
{
post[++post_top]=ch1;
}
post[post_top+1]='\0';
opr[opr_top+1]='\0';
puts(post);
puts(opr);
}
while(opr[opr_top] != '#')
{
post[++post_top]=opr[opr_top--];
}
opr[0] ='\0';
post[post_top+1] = '\0';
printf("\n");
puts(post);
for(i=0;i<=post_top;i++)
{
ch1=post[i];
printf("\n");
if((ch1>= 'a') && (ch1 <= 'z'))
{
printf("Enter the value of %c \n",ch1);
scanf("%d",&val);
ans[++ans_top] = val;
}
if(ch1 =='+')
{
ans[ans_top-1]=ans[ans_top-1]+ans[ans_top];
ans_top--;
}
if(ch1 == '-')
{
ans[ans_top-1] = ans[ans_top-1]-ans[ans_top];
ans_top--;
}
if(ch1 == '*')
{
ans[ans_top-1] = ans[ans_top-1]*ans[ans_top];
ans_top--;
//printf("3");
}
if(ch1=='/')
{
ans[ans_top-1] = ans[ans_top-1]/ans[ans_top];
ans_top--;
}
}
printf("Result %d\n",ans[0]);
}
Exdivssion & Postfix() //将中缀表达式转变为后缀表达式
{
First();
if (Get()->type == POSTFIX) return *this;
Stack<char> s; s.Push('=');
Exdivssion temp;
ExpNode<Type> *p = Next();
while (p != NULL)
{
switch (p->type)
{
case OPND:
temp.LastInsert(*p); p = Next(); break;
case OPTR:
while (isp(s.GetTop()) > icp(p->optr) )
{
ExpNode<Type> newoptr(s.Pop());
temp.LastInsert(newoptr);
}
if (isp(s.GetTop()) == icp(p->optr) )
{
s.Pop(); p =Next(); break;
}
s.Push(p->optr); p = Next(); break;
default: break;
}
}
*this = temp;
pGetFirst()->data.type = POSTFIX;
return *this;
}
int main(int,char **) {
int i, num_pts ;
std::vector<CvPoint2D32f> ref_points;
std::vector<CvPoint2D32f> new_points;
IplImage * image_base ;
IplImage * image ;
num_pts = 200;
image_base = cvCreateImage(cvSize(500,500),8,3);
image = cvCreateImage(cvSize(500,500),8,3);
cvZero(image_base);
double norm = 200;
float a = 0.;
for( i=0; i<num_pts; i++ ) {
float xx = (float)(norm/2.f)*cos(a);
float yy = (float)(norm)*sin(a);
float x = (float)(xx * cos(CV_PI/4) + yy *sin(CV_PI/4) +250);
float y = (float)(xx * -sin(CV_PI/4) + yy *cos(CV_PI/4)+250);
ref_points.push_back(cvPoint2D32f(x,y));
cvDrawCircle(image_base,cvPoint((int)x,(int)y),1,CV_RGB(0,255,255),1);
a += (float)(2*CV_PI/num_pts);
}
a = 0.;
for( i=0; i< num_pts/5; i++ ) {
float xx = (float)((norm/1.9)*cos(a));
float yy = (float)((norm/1.1)*sin(a));
float x = (float)(xx * cos(-CV_PI/8) + yy *sin(-CV_PI/8) +150);
float y = (float)(xx * -sin(-CV_PI/8) + yy *cos(-CV_PI/8)+250);
new_points.push_back(cvPoint2D32f(x,y));
a += (float)(2*CV_PI/(float)(num_pts/5));
cvDrawCircle(image_base,cvPoint((int)x,(int)y),1,CV_RGB(255,255,0),1);
}
for(int k = 0; k < 30 ;k++) {
float R[4] = {1.f,0.f,0.f,1.f},T[2] = {0.,0.};
CvMat r = cvMat(2,2,CV_32F,R);
CvMat t = cvMat(2,1,CV_32F,T);
cvCopy(image_base,image);
float err = icp(&new_points[0],new_points.size(),
&ref_points[0],ref_points.size(),
&r,&t,cvTermCriteria(CV_TERMCRIT_ITER,1,0.1),image);
printf("err = %f\n",err);
for(int i = 0; i < (int)new_points.size() ; i++ ) {
float x = new_points[i].x;
float y = new_points[i].y;
float X = (R[0]*x + R[1]*y + T[0]);
float Y = (R[2]*x + R[3]*y + T[1]);
new_points[i].x = X;
new_points[i].y = Y;
cvDrawCircle(image,cvPoint((int)X,(int)Y),5,CV_RGB(255,255,0),1);
}
printf("Final transformation : \n");
printf("R[0]=%f R[1]=%f T[0]=%f\n",R[0],R[1],T[0]);
printf("R[2]=%f R[3]=%f T[1]=%f\n",R[2],R[3],T[1]);
cvShowImage("image",image);
if( cvWaitKey(200)== 27) break;
}
cvReleaseImage(&image_base);
cvReleaseImage(&image);
return 0;
}
rbt::pose<double> COccupancyGridWithObstacleList::fit(rbt::pose<double> const& poseWorld, SScanLine const& scanline) {
if(m_iEndSorted<m_vecptfOccupied.size()) {
auto itptfEndSorted = m_vecptfOccupied.begin()+m_iEndSorted;
std::sort(itptfEndSorted, m_vecptfOccupied.end());
m_vecptfOccupied.erase(std::unique(itptfEndSorted, m_vecptfOccupied.end()), m_vecptfOccupied.end());
std::inplace_merge(m_vecptfOccupied.begin(), itptfEndSorted, m_vecptfOccupied.end());
m_iEndSorted = m_vecptfOccupied.size();
}
if(m_vecptfOccupied.size()<10) return poseWorld;
std::vector<rbt::point<double>> vecptfTemplate;
scanline.ForEachScan(poseWorld, [&](rbt::pose<double> const& poseScan, double fRadAngle, int nDistance) {
vecptfTemplate.emplace_back(ToGridCoordinate(Obstacle(poseScan, fRadAngle, nDistance)));
});
#ifdef ENABLE_SCANMATCH_LOG
static int c_nCount = 0;
{
std::stringstream ss;
ss << "scanmatch" << c_nCount << "_a.png";
DebugOutputScan(ObstacleMap(), vecptfTemplate, ss.str().c_str());
}
LOG(c_nCount);
#endif
rbt::pose<double> poseGrid(ToGridCoordinate(poseWorld));
// Transformation matrix from pose
Matrix R = Matrix::eye(2);
Matrix t(2,1);
static_assert(sizeof(rbt::point<double>)==2*sizeof(double), "");
// Use libicp, an iterative closest point implementation (http://www.cvlibs.net/software/libicp/)
IcpPointToPoint icp(&m_vecptfOccupied[0].x, m_vecptfOccupied.size(), 2);
icp.fit(&vecptfTemplate[0].x,vecptfTemplate.size(), R, t, 250);
#ifdef ENABLE_SCANMATCH_LOG
LOG("ICP: R = " << R << " t = " << t);
#endif
// Pose from transformation matrix
Matrix vecLastPose = (R * Matrix(2, 1, &poseGrid.m_pt.x)) + t;
rbt::pose<double> const poseWorldCorrected(
ToWorldCoordinate(
rbt::pose<double>(
rbt::point<double>(vecLastPose.val[0][0], vecLastPose.val[1][0]),
poseWorld.m_fYaw - asin(R.val[0][1])
)
));
#ifdef ENABLE_SCANMATCH_LOG
LOG(" Corrected Pose: " << poseWorldCorrected);
{
std::vector<rbt::point<double>> vecptfTemplateCorrected;
scanline.ForEachScan(poseWorldCorrected, [&](rbt::pose<double> const& poseScan, double fRadAngle, int nDistance) {
vecptfTemplateCorrected.emplace_back(ToGridCoordinate(Obstacle(poseScan, fRadAngle, nDistance)));
});
std::stringstream ss;
ss << "scanmatch" << c_nCount << "_b.png";
DebugOutputScan(ObstacleMap(), vecptfTemplateCorrected, ss.str().c_str());
}
++c_nCount;
#endif
return poseWorldCorrected;
}
void demo::makemap_ICP(){
ros::Time start, end;
Eigen::Matrix4d T_backup;
T_backup.setIdentity();
PCloud cloud_s;
PCloud cloud_d;
//take 1 frame
cloud_s = getframe();
pub_pointcloud.publish(cloud_s);
std::cout<< 0 << "\n";
*cloud_key = *cloud_s;
cloud_d = getframe();
ICP icp(cloud_s,cloud_d);
RGBFeaturesInit rgbf(cloud_s,cloud_d);
rgbf_key.setRansacThreshold(th);
rgbf_key.setDesiredP(prob);
rgbf.setRansacThreshold(th);
rgbf.setDesiredP(prob);
icp.setRThreshold (THRESH_dr);
icp.setTThreshold (THRESH_dt);
icp.setDisThreshold(D);
icp.setNiterations (max_iterations);
icp.setMaxPairs (MAXPAIRS);
icp.setMinPairs (MINPAIRS);
Eigen::Vector3d p1(0.0,0.0,0.0);
Eigen::Vector3d p2(0.0,0.0,0.1);
camera_positions.push_back(std::make_pair(p1, p2));
int i=1;
float a1,a2;
int ft;
while(ros::ok()) {
ROS_INFO("%d ", i);
start = ros::Time::now();
ft = rgbf.compute();
end = ros::Time::now();
a1 = (end-start).toSec();
start = ros::Time::now();
if ( ft > f_th ){
icp.align(rgbf.T,rgbf.getInliers());
}else{
icp.align(T_backup);
}
end = ros::Time::now();
a2 = (end-start).toSec();
//update transformation
T_backup=icp.returnTr();
T = T*icp.returnTr();
ROS_INFO("RGB: %.6f seconds | ICP: %.6f seconds | inliears= %d", a1, a2,ft);
//update camera position
p1 = T.block<3,3>(0,0)*camera_positions[0].first + T.block<3,1>(0,3);
p2 = T.block<3,3>(0,0)*camera_positions[0].second + T.block<3,1>(0,3);
camera_positions.push_back(std::make_pair(p1, p2));
if(checkIfIsNewKeyframe(cloud_d)){
ApplyRTto(cloud_d);
pub_pointcloud.publish(cloud_d);
}
cloud_d = getframe();
//update icp clouds
rgbf.setNewinputCloud(cloud_d);
icp.setNewICPinputCloud(cloud_d);
i++;
}
WriteCameraPLY(cameras_filename.data(), camera_positions);
}
void main( )
{
char express[30], num[10], theta, tp , d;//express[30]用来读入一个表达式
double a ,b , result, tps; //num[10]用来存放表达式中连接着的数字字符
int t, i, j;
int position = 0;//表达式读到的当前字符
Initstack(OPND);
Initstack(OPTR); Push(OPTR , '=');
printf("input 'b' to run the calc:\n");
scanf("%c" , &d);
getchar();
while(d == 'b')
{
printf( "请输入表达式( 可使用+、-、*、/、(、)、= ): " ) ;
gets(express);
while(express[position] != '='||Gettop(OPTR , tp) != '=' )
{
if(!Is_op(express[position]))
{ //用两个计数器t和j实现浮点数的读取
t = position;j=0;
while(!Is_op(express[position]))
{
position++;
}
for(i = t; i<position ; i++ )
{//把表达式中第t到position-1个字符赋给num[10]
num[j] = express[i];
j++;
}
Push(OPND , atof(num));
memset(num,0,sizeof(num));//将num[10]清空
}
else
{
switch(Precede(isp(Gettop(OPTR , tp)),icp(express[position])))
{
case '<':
Push(OPTR,express[position]);position++;break;
case '=':
Pop(OPTR , tp) ;position++;break;
case '>':
{
Pop(OPTR , theta) ;
Pop(OPND , b) ;
Pop(OPND , a) ;
result = Operate(a, theta ,b);
Push(OPND , result);break;
}//end sase
}//end switch
}//end else
}//end while
printf("%s%8.4f\n",express,Pop(OPND,tps));
memset(express , 0 , sizeof(express));
position = 0;
printf("input 'b' to run the calc again:\n");
scanf("%c" , &d);
getchar();
}//end while
}
