void Statistics::centering(Eigen::MatrixXd& m)
{
for (int i = 0; i < m.cols(); i++)
{
centering(m, i);
}
}
void Statistics::centering(vector<vector<double> >& m)
{
for (unsigned int i = 0; i < m.size(); i++)
{
centering(m[i]);
}
}
void maze_loop()
{
while (1)
{
if (maze.started)
{
PORTAbits.RA0=0;
asm("nop");
maze.started=start_done;
collect_sensor_data();
centering();
if (is_branch())
branching();
maze_debug();
}
}
}
LoginView::LoginView(Vector2u & size)
{
//create all the ui elements here
logintext = new Textblock(Vector2f(100,100),Vector2f(500,70),"LOGIN SCREEN", 50);
logintext->setFillColor(MyColors.Red);
m_DrawV.push_back(logintext);
status= new Textblock(Vector2f(100,600),Vector2f(700,70),"", 40);
status->setFontColor(MyColors.Red);
status->setBackgroundColor(MyColors.Transparent);
m_DrawV.push_back(status);
name = new TextBox(200,"Enter name",sf::Vector2f(100,200),true,1);
name->Attach(this);
m_DrawV.push_back(name);
m_AnimateV.push_back(name);
m_ClickV.push_back(name);
m_KeyV.push_back(name);
lgoinbutton = new StandardButton(Vector2f(100,300),Vector2f(100,70),"Login",1,false);
lgoinbutton->Attach(this);
m_DrawV.push_back(lgoinbutton);
m_AnimateV.push_back(lgoinbutton);
m_ClickV.push_back(lgoinbutton);
next = Views::NOCHANGE;
bg.onResize(size);
/*if(!image.loadFromFile("Data/Images/Button.png"))
std::cout << "LoginView.cpp: couldn't load Button.png" << std::endl;/**/
centering(size);
NL = nullptr;
connected = false;
connect = new Connect();
}
Window::Window()
{
drawArea = new draw;
model = new XJRP::SLCModel;
triangleMesh = new xd::TriangleMesh;
//用来显示层的SpinBox
LayerNum = new QSpinBox;
LayerNum->setRange(0,0);
LayerNum->setSpecialValueText(tr("0 (No Model)"));
connect(LayerNum, SIGNAL(valueChanged(int)),
this, SLOT(LayerChanged()));
LayerNumLabel = new QLabel(tr("Layer Number:"));
LayerNumLabel->setBuddy(LayerNum);
//用来设置填充模式的combobox
infillPatternLabel = new QLabel(tr("Infill Pattern:"));
infillPatternComboBox = new QComboBox;
infillPatternComboBox->addItem(tr("line"));
infillPatternComboBox->addItem(tr("skin_core"));
connect(infillPatternComboBox,SIGNAL(currentIndexChanged(int)),
this,SLOT(infillPatternChanged(int)));
//用来设置参数的LineEdit
interval_numEdit = new QLineEdit(tr("2"));
interval_numLabel = new QLabel(tr("Interval Number:"));
interval_numLabel->setBuddy(interval_numEdit);
spaceEdit = new QLineEdit(tr("0.1"));
spaceLabel = new QLabel(tr("Space:"));
spaceLabel->setBuddy(spaceEdit);
shrinkDistanceEdit = new QLineEdit(tr("0.1"));
shrinkDistanceLabel = new QLabel(tr("Shrink Distance:"));
shrinkDistanceLabel->setBuddy(shrinkDistanceEdit);
angle_startEdit = new QLineEdit(tr("0"));
angle_startLabel = new QLabel(tr("Angle Start:"));
angle_startLabel->setBuddy(angle_startEdit);
angle_deltaEdit = new QLineEdit(tr("90"));
angle_deltaLabel = new QLabel(tr("angle Delta:"));
angle_deltaLabel->setBuddy(angle_deltaEdit);
LaserPowerEdit = new QLineEdit(tr("0.079"));
LaserPowerLabel = new QLabel(tr("Laser Power:"));
LaserPowerLabel->setBuddy(LaserPowerEdit);
ScanSpeedEdit = new QLineEdit(tr("3000"));
ScanSpeedLabel = new QLabel(tr("Scan Speed:"));
ScanSpeedLabel->setBuddy(ScanSpeedEdit);
contourNumLabel = new QLabel(tr("Contour Number:"));
contourNumEdit = new QLineEdit(tr("1"));
contourSpaceLabel = new QLabel("Contour Space:");
contourSpaceEdit = new QLineEdit(tr("0.1"));
thickness = new QLabel("thickness:");
thicknessEdit = new QLineEdit(tr("0.1"));
//按钮
open = new QPushButton(tr("&open"));
save = new QPushButton(tr("&save"));
clear = new QPushButton(tr("&clear"));
infill = new QPushButton(tr("&infill"));
centering = new QPushButton(tr("center"));
openSTL = new QPushButton(tr("openSTL"));
slice = new QPushButton(tr("slice"));
connect(open,SIGNAL(clicked()),
this,SLOT(openTrigger()));
connect(save,SIGNAL(clicked()),
this,SLOT(saveTrigger()));
connect(clear,SIGNAL(clicked()),
this,SLOT(clearTrigger()));
connect(infill,SIGNAL(clicked()),
this,SLOT(infillTrigger()));
connect(centering,SIGNAL(clicked()),
drawArea,SLOT(centering()));
connect(openSTL,SIGNAL(clicked()),
this,SLOT(openSTLTrigger()));
connect(slice,SIGNAL(clicked()),
this,SLOT(sliceTrigger()));
//布局
QGridLayout *mainLayout = new QGridLayout;
mainLayout->addWidget(drawArea, 0, 0, 1, 16);
mainLayout->addWidget(open,2, 0);
mainLayout->addWidget(clear,2,1);
mainLayout->addWidget(LayerNumLabel, 2, 2);
mainLayout->addWidget(LayerNum, 2, 3);
mainLayout->addWidget(centering,2,4);
mainLayout->addWidget(infillPatternLabel,2,5);
mainLayout->addWidget(infillPatternComboBox,2,6);
mainLayout->addWidget(infill, 2, 7);
mainLayout->addWidget(save, 2, 8);
mainLayout->addWidget(openSTL,2,11);
mainLayout->addWidget(thicknessEdit,2,12);
mainLayout->addWidget(thickness,2,13);
mainLayout->addWidget(slice,2,14);
//第3行
mainLayout->addWidget(contourNumLabel,3,0);
mainLayout->addWidget(contourNumEdit,3,1);
mainLayout->addWidget(contourSpaceLabel,3,2);
mainLayout->addWidget(contourSpaceEdit,3,3);
mainLayout->addWidget(spaceLabel,3,4);
mainLayout->addWidget(spaceEdit,3,5);
mainLayout->addWidget(shrinkDistanceLabel,3,6);
mainLayout->addWidget(shrinkDistanceEdit,3,7);
mainLayout->addWidget(angle_startLabel,3,8);
mainLayout->addWidget(angle_startEdit,3,9);
mainLayout->addWidget(angle_deltaLabel,3,10);
mainLayout->addWidget(angle_deltaEdit,3,11);
mainLayout->addWidget(LaserPowerLabel,3,12);
mainLayout->addWidget(LaserPowerEdit,3,13);
mainLayout->addWidget(ScanSpeedLabel,3,14);
mainLayout->addWidget(ScanSpeedEdit,3,15);
//试试放在第三行！
mainLayout->addWidget(interval_numLabel,3,0);
mainLayout->addWidget(interval_numEdit,3,1);
setLayout(mainLayout);
//LayerChanged();
setWindowTitle(tr("SLC Read And Infill"));
infill->hide();
save->hide();
centering->hide();
interval_numEdit->hide();
interval_numLabel->hide();
}
/******************************************************************
* 函数功能：几何校正
*
* 待写：H_final的值也应该返回去
*/
arma::uvec geometricVerification(const arma::mat &frames1, const arma::mat &frames2,
const arma::mat &matches, const superluOpts &opts){
// 测试载入是否准确
/*std::cout<< "element测试: " << " x: " << frames1(0,1) << " y: " << frames1(1,1) << std::endl;
std::cout << " 行数： " << frames1.n_rows << " 列数：" << frames1.n_cols << std::endl;
std::cout << "==========================================================" << std::endl;*/
int numMatches = matches.n_cols;
// 测试匹配数目是否准确
/*std::cout << "没有RANSAC前匹配数目： " << numMatches << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::field<arma::uvec> inliers(1, numMatches);
arma::field<arma::mat> H(1, numMatches);
arma::uvec v = arma::linspace<arma::uvec>(0,1,2);
arma::mat onesVector = arma::ones(1, matches.n_cols);
arma::uvec matchedIndex_Query = arma::conv_to<arma::uvec>::from(matches.row(0)-1);
arma::uvec matchedIndex_Object = arma::conv_to<arma::uvec>::from(matches.row(1)-1);
arma::mat x1 = frames1(v, matchedIndex_Query) ;
arma::mat x2 = frames2(v, matchedIndex_Object);
/*std::cout << " x1查询图像匹配行数： " << x1.n_rows << " 查询图像匹配列数：" << x1.n_cols << std::endl;
std::cout << " x2目标图像匹配行数： " << x2.n_rows << " 目标图像匹配列数：" << x2.n_cols << std::endl;
std::cout<< "x1 element测试: " << " x: " << x1(0,1) << " y: " << x1(1,1) << std::endl;
std::cout<< "x2 element测试: " << " x: " << x2(0,1) << " y: " << x2(1,1) << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::mat x1hom = arma::join_cols(x1, arma::ones<arma::mat>(1, numMatches)); //在下面添加一行，注意和join_rows的区别
arma::mat x2hom = arma::join_cols(x2, arma::ones<arma::mat>(1, numMatches));
/*std::cout << " x1hom查询图像匹配行数： " << x1hom.n_rows << " 查询图像匹配列数：" << x1hom.n_cols << std::endl;
std::cout<< "x1hom element测试: " << " x: " << x1hom(0,1) << " y: " << x1hom(1,1) << " z: " << x1hom(2,1) << std::endl;
std::cout << "==========================================================" << std::endl;*/
arma::mat x1p, H21; //作用域
double tol;
for(int m = 0; m < numMatches; ++m){
//cout << "m: " << m << endl;
for(unsigned int t = 0; t < opts.numRefinementIterations; ++t){
//cout << "t: " << t << endl;
if (t == 0){
arma::mat tmp1 = frames1.col(matches(0, m)-1);
arma::mat A1 = toAffinity(tmp1);
//A1.print("A1 =");
arma::mat tmp2 = frames2.col(matches(1, m)-1);
arma::mat A2 = toAffinity(tmp2);
//A2.print("A2 =");
H21 = A2 * inv(A1);
//H21.print("H21 =");
x1p = H21.rows(0, 1) * x1hom ;
//x1p.print("x1p =");
tol = opts.tolerance1;
}else if(t !=0 && t <= 3){
arma::mat A1 = x1hom.cols(inliers(0, m));
arma::mat A2 = x2.cols(inliers(0, m));
//A1.print("A1 =");
//A2.print("A2 =");
H21 = A2*pinv(A1);
//H21.print("H21 =");
x1p = H21.rows(0, 1) * x1hom ;
//x1p.print("x1p =");
arma::mat v;
v << 0 << 0 << 1 << arma::endr;
H21 = join_vert(H21, v);
//H21.print("H21 =");
//x1p.print("x1p =");
tol = opts.tolerance2;
}else{
arma::mat x1in = x1hom.cols(inliers(0, m));
arma::mat x2in = x2hom.cols(inliers(0, m));
arma::mat S1 = centering(x1in);
arma::mat S2 = centering(x2in);
arma::mat x1c = S1 * x1in;
//x1c.print("x1c =");
arma::mat x2c = S2 * x2in;
//x2c.print("x2c =");
arma::mat A1 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A1.zeros();
arma::mat A2 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A2.zeros();
arma::mat A3 = arma::randu<arma::mat>(x1c.n_rows ,x1c.n_cols);
A3.zeros();
for(unsigned int i = 0; i < x1c.n_cols; ++i){
A2.col(i) = x1c.col(i)*(-x2c.row(0).col(i));
A3.col(i) = x1c.col(i)*(-x2c.row(1).col(i));
}
arma::mat T1 = join_cols(join_horiz(x1c, A1), join_horiz(A1, x1c));
arma::mat T2 = join_cols(T1, join_horiz(A2, A3));
//T2.print("T2 =");
arma::mat U;
arma::vec s;
arma::mat V;
svd_econ(U, s, V, T2);
//U.print("U =");
//V.print("V =");
arma::vec tmm = U.col(U.n_cols-1);
H21 = reshape(tmm, 3, 3).t();
H21 = inv(S2) * H21 * S1;
H21 = H21 / H21(H21.n_rows-1, H21.n_cols-1) ;
//H21.print("H21 =");
arma::mat x1phom = H21 * x1hom ;
arma::mat cc1 = x1phom.row(0) / x1phom.row(2);
arma::mat cc2 = x1phom.row(1) / x1phom.row(2);
arma::mat x1p = join_cols(cc1, cc2);
//x1p.print("x1p =");
tol = opts.tolerance3;
}
arma::mat tmp = arma::square(x2 - x1p); //精度跟matlab相比更高？
//tmp.print("tmp =");
arma::mat dist2 = tmp.row(0) + tmp.row(1);
//dist2.print("dist2 =");
inliers(0, m) = arma::find(dist2 < pow(tol, 2));
H(0, m) = H21;
//H(0, m).print("H(0, m) =");
//inliers(0, m).print("inliers(0, m) =");
//cout << inliers(0, m).size() << endl;
//cout << "==========================================================" << endl;
if (inliers(0, m).size() < opts.minInliers) break;
if (inliers(0, m).size() > 0.7 * numMatches) break;
}
}
arma::uvec scores(numMatches);
for(int i = 0; i < numMatches; ++i){
scores.at(i) = inliers(0, i).n_rows;
}
//scores.print("scores = ");
arma::uword index;
scores.max(index);
//cout << index << endl;
arma::mat H_final = inv(H(0, index));
//H_final.print("H_final = ");
arma::uvec inliers_final = inliers(0, index);
//inliers_final.print("inliers_final = ");
return inliers_final;
}
void LoginView::onResize(Vector2u & size)
{
centering(size);
}
void make_header(int mon, int ye, char h[HEADER_LEN]){
char *months[] = {"January", "February", "March", "April", "May", "June", "July", "August", "September", "October", "November", "December"};
sprintf(h, "%s %04d", months[mon-1], ye);
centering(h, HEADER_LEN);
}
