void Cities::writeToXMLFile(const std::string &filename) const {
XmlWriter xw(filename);
for (const auto &pair : _container) {
auto e = xw.addChild("city");
xw.setAttr(e, "name", pair.first);
const City &city = pair.second;
for (const std::string &member : city.members()) {
auto memberE = xw.addChild("member", e);
xw.setAttr(memberE, "username", member);
}
}
xw.publish();
}
Decimal32 DecimalUtil::round(Decimal32 x)
{
#if BDLDFP_DECIMALPLATFORM_C99_TR
return roundd32(x.value());
#elif BDLDFP_DECIMALPLATFORM_DECNUMBER
Decimal64 xw(x);
Decimal64 rv;
decDoubleToIntegralValue(rv.data(),
xw.data(),
getContext(),
DEC_ROUND_HALF_UP);
return Decimal32(rv);
#endif
}
bool Ckeyb::exit(void)
{
#ifdef EMSCRIPTEN
return true;
#endif
if (! modified) return true;
int ret = QMessageBox::warning(mainwindow, QObject::tr("PockEmul"),
QObject::tr("The keyboard layout has been modified.\n"
"Do you want to save your changes?"),
QMessageBox::Save | QMessageBox::Discard,
QMessageBox::Save);
if (ret == QMessageBox::Discard) return true;
// save the XML definition file
QFile file(fn_KeyMap);
file.open( QIODevice::WriteOnly);
XmlWriter xw( &file );
xw.setAutoNewLine( true );
xw.writeRaw( "<!DOCTYPE KEYBMAP><KEYBMAP version=\"1.0\">" );
xw.newLine();
xw.writeTaggedString( "model", QString(pPC->getcfgfname()) );
xw.writeOpenTag( "Keyboard" );
QList<CKey>::iterator j;
for (j = Keys.begin(); j != Keys.end(); ++j)
{
AttrMap attrs;
attrs.insert( "description", j->Description.trimmed () );
attrs.insert( "scancode", QString("0x%1").arg(j->ScanCode,2,16,QChar('0')) );
attrs.insert( "masterscancode", QString("0x%1").arg(j->MasterScanCode,2,16,QChar('0')) );
attrs.insert( "left", QString("%1").arg(j->Rect.left()) );
attrs.insert( "top", QString("%1").arg(j->Rect.top() ));
attrs.insert( "width", QString("%1").arg(j->Rect.width() ) );
attrs.insert( "height", QString("%1").arg(j->Rect.height() ) );
attrs.insert( "modifier", j->Modifier );
xw.writeAtomTag( "KEY", attrs );
}
xw.writeCloseTag( "Keyboard" );
xw.writeRaw( "</KEYBMAP>" );
return true;
}
static void analop_esil(RAnal *a, RAnalOp *op, ut64 addr, const ut8 *buf) {
r_strbuf_init (&op->esil);
r_strbuf_set (&op->esil, "");
switch (buf[0]) {
// Irregulars sorted by lower nibble
case 0x00: /* nop */
e (",");
break;
case 0x10: /* jbc bit, offset */
bit_r; e ("?{,"); bit_mask; xi (bit, "&"); jmp; e (",}");
break;
case 0x20: /* jb bit, offset */
bit_r; cjmp;
break;
case 0x30: /* jnb bit, offset */
bit_r; e ("!,"); cjmp;
break;
case 0x40: /* jc offset */
e ("c,1,&,"); cjmp;
break;
case 0x50: /* jnc offset */
e ("c,1,&,!,"); cjmp;
break;
case 0x60: /* jz offset */
e ("a,0,==,"); cjmp;
break;
case 0x70: /* jnz offset */
e ("a,0,==,!,"); cjmp;
break;
case 0x11: case 0x31: case 0x51: case 0x71:
case 0x91: case 0xB1: case 0xD1: case 0xF1: /* acall addr11 */
case 0x12: /* lcall addr16 */
call;
break;
case 0x01: case 0x21: case 0x41: case 0x61:
case 0x81: case 0xA1: case 0xC1: case 0xE1: /* ajmp addr11 */
case 0x02: /* ljmp addr16 */
case 0x80: /* sjmp offset */
jmp;
break;
case 0x22: /* ret */
case 0x32: /* reti */
xr (sp2); e ("pc,=");
break;
case 0x03: /* rr a */
e ("1,a,0x101,*,>>,a,=," flag_p);
break;
case 0x04: /* inc a */
xi (a, "++"); e (flag_p);
break;
case 0x05: /* inc direct */
xi (dir1, "++");
break;
case 0x06: case 0x07: /* inc @Ri */
xi (ri, "++");
break;
case 0x08: case 0x09: case 0x0A: case 0x0B:
case 0x0C: case 0x0D: case 0x0E: case 0x0F: /* inc @Rn */
xi (rn, "++");
break;
case 0x13: /* rrc a */
e ("7,c,<<,1,a,&,c,=,0x7f,1,a,>>,&,+,a,=," flag_p);
break;
case 0x14: /* dec a */
xi (a, "--"); e (flag_p);
break;
case 0x15: /* dec direct */
xi (dir1, "--"); e (flag_p);
break;
case 0x16: case 0x17: /* dec @Ri */
xi (ri, "--");
break;
case 0x18: case 0x19: case 0x1A: case 0x1B:
case 0x1C: case 0x1D: case 0x1E: case 0x1F: /* dec @Rn */
xi (rn, "--");
break;
case 0x23: /* rl a */
e ("7,a,0x101,*,>>,a,=," flag_p);
break;
template_alu4 (0x20, "+", flag_c flag_ac flag_ov flag_p) /* 0x24..0x2f add a,.. */
case 0x33: /* rlc a */
e ("c,1,&,a,a,+=,$c7,c,=,a,+=," flag_p);
break;
template_alu4_c (0x30, "+", flag_c flag_ac flag_ov flag_p) /* 0x34..0x3f addc a,.. */
template_alu2 (0x40, "|") /* 0x42..0x43 orl direct,.. */
template_alu4 (0x40, "|", flag_p) /* 0x44..0x4f orl a,.. */
template_alu2 (0x50, "&") /* 0x52..0x53 anl direct,.. */
template_alu4 (0x50, "&", flag_p) /* 0x54..0x5f anl a,.. */
template_alu2 (0x60, "^") /* 0x62..0x63 xrl direct,.. */
template_alu4 (0x60, "^", flag_p) /* 0x64..0x6f xrl a,.. */
case 0x72: /* orl C, bit */
bit_r; xi (c, "|");
break;
case 0x73: /* jmp @a+dptr */
e ("dptr,a,+,pc,=");
break;
case 0x74: /* mov a, imm */
xr (imm1); xw (a); e (flag_p);
break;
case 0x75: /* mov direct, imm */
xr (imm2); xw (dir1);
break;
case 0x76: case 0x77: /* mov @Ri, imm */
xr (imm1); xw (ri);
break;
case 0x78: case 0x79: case 0x7A: case 0x7B:
case 0x7C: case 0x7D: case 0x7E: case 0x7F: /* mov Rn, imm */
xr (imm1); xw (rn);
break;
case 0x82: /* anl C, bit */
bit_r; xi (c, "&");
break;
case 0x83: /* movc a, @a+pc */
e ("a,pc,--,+,[1],a,=," flag_p);
break;
case 0x84: /* div ab */
// note: escape % if this becomes a format string
e ("b,0,==,ov,=,b,a,%,b,a,/=,b,=,0,c,=," flag_p);
break;
case 0x85: /* mov direct, direct */
xr (dir1); xw (dir2);
break;
case 0x86: case 0x87: /* mov direct, @Ri */
xr (ri); xw (dir1);
break;
case 0x88: case 0x89: case 0x8A: case 0x8B:
case 0x8C: case 0x8D: case 0x8E: case 0x8F: /* mov direct, Rn */
xr (rn); xw (dir1);
break;
case 0x90: /* mov dptr, imm */
xr (imm16); xw (dp);
break;
case 0x92: /* mov bit, C */
bit_c; bit_mask; xr (bit); e ("&,|,"); xw(bit);
break;
case 0x93: /* movc a, @a+dptr */
e ("a,dptr,+,[1],a,=," flag_p);
break;
template_alu4_c (0x90, "-", flag_b flag_ab flag_ob flag_p) /* 0x94..0x9f subb a,.. */
case 0xA0: /* orl C, /bit */
bit_r; e ("!,"); xi (c, "|");
break;
case 0xA2: /* mov C, bit */
bit_r; xw (c);
break;
case 0xA3: /* inc dptr */
xi (dp, "++");
break;
case 0xA4: /* mul ab */
e ("8,a,b,*,DUP,a,=,>>,DUP,b,=,0,==,!,ov,=,0,c,=," flag_p);
break;
case 0xA5: /* "reserved" */
e ("0,trap");
break;
case 0xA6: case 0xA7: /* mov @Ri, direct */
xr (dir1); xw (ri);
break;
case 0xA8: case 0xA9: case 0xAA: case 0xAB:
case 0xAC: case 0xAD: case 0xAE: case 0xAF: /* mov Rn, direct */
xr (dir1); xw (rn);
break;
case 0xB0: /* anl C, /bit */
bit_r; e ("!,"); xi (c, "&");
break;
case 0xB2: /* cpl bit */
bit_set; xi (bit, "^");
break;
case 0xB3: /* cpl C */
e ("1,"); xi (c, "^");
break;
case 0xB4: /* cjne a, imm, offset */
xr (imm1); xr (a); e ("-," flag_b); cjmp;
break;
case 0xB5: /* cjne a, direct, offset */
xr (dir1); xr (a); e ("-," flag_b); cjmp;
break;
case 0xB6: case 0xB7: /* cjne @ri, imm, offset */
xr (imm1); xr (ri); e ("-," flag_b); cjmp;
break;
case 0xB8: case 0xB9: case 0xBA: case 0xBB:
case 0xBC: case 0xBD: case 0xBE: case 0xBF: /* cjne Rn, imm, offset */
xr (imm1); xr (rn); e ("-," flag_b); cjmp;
break;
case 0xC0: /* push direct */
xr (dir1); xw (sp1);
break;
case 0xC2: /* clr bit */
bit_mask; xi (bit, "&");
break;
case 0xC3: /* clr C */
e ("0,"); xw (c);
break;
case 0xC4: /* swap a */
e ("0xff,4,a,0x101,*,>>,&,a,=," flag_p);
break;
case 0xC5: /* xch a, direct */
xr (a); e ("0,+,"); xr (dir1); xw (a); xw (dir1); e (flag_p);
break;
case 0xC6: case 0xC7: /* xch a, @Ri */
xr (a); e ("0,+,"); xr (ri); xw (a); xw (ri); e (flag_p);
break;
case 0xC8: case 0xC9: case 0xCA: case 0xCB:
case 0xCC: case 0xCD: case 0xCE: case 0xCF: /* xch a, Rn */
xr (a); e ("0,+,"); xr (rn); xw (a); xw (rn); e (flag_p);
break;
case 0xD0: /* pop direct */
xr (sp1); xw (dir1);
break;
case 0xD2: /* setb bit */
bit_set; xi (bit, "|");
break;
case 0xD3: /* setb C */
e ("1,"); xw (c);
break;
case 0xD4: /* da a */
// BCD adjust after add:
// if (lower nibble > 9) or (AC == 1) add 6
// if (higher nibble > 9) or (C == 1) add 0x60
// carry |= carry caused by this operation
e ("a,0x0f,&,9,<,ac,|,?{,6,a,+=,$c7,c,|=,},a,0xf0,&,0x90,<,c,|,?{,0x60,a,+=,$c7,c,|=,}," flag_p);
break;
case 0xD5: /* djnz direct, offset */
xi (dir1, "--"); xr (dir1); e ("0,==,!,"); cjmp;
break;
case 0xD6:
case 0xD7: /* xchd a, @Ri*/
xr (a); e ("0xf0,&,"); xr (ri); e ("0x0f,&,|,");
xr (ri); e ("0xf0,&,"); xr (a); e ("0x0f,&,|,");
xw (ri); xw (a); e (flag_p);
break;
case 0xD8: case 0xD9: case 0xDA: case 0xDB:
case 0xDC: case 0xDD: case 0xDE: case 0xDF: /* djnz Rn, offset */
xi (rn, "--"); xr (rn); e ("0,==,!,"); cjmp;
break;
case 0xE0: /* movx a, @dptr */
xr (dpx); xw (a); e (flag_p);
break;
case 0xE2: case 0xE3: /* movx a, @Ri */
xr (rix); xw (a); e (flag_p);
break;
case 0xE4: /* clr a */
e ("0,"); xw (a); e (flag_p);
break;
case 0xE5: /* mov a, direct */
xr (dir1); xw (a); e (flag_p);
break;
case 0xE6: case 0xE7: /* mov a, @Ri */
xr (ri); xw (a); e (flag_p);
break;
case 0xE8: case 0xE9: case 0xEA: case 0xEB:
case 0xEC: case 0xED: case 0xEE: case 0xEF: /* mov a, Rn */
xr (rn); xw (a); e (flag_p);
break;
case 0xF0: /* movx @dptr, a */
xr (a); xw (dpx);
break;
case 0xF2: case 0xF3: /* movx @Ri, a */
xr (a); xw (rix);
break;
case 0xF4: /* cpl a */
e ("255,"); xi (a, "^"); e (flag_p);
break;
case 0xF5: /* mov direct, a */
xr (a); xw (dir1);
break;
case 0xF6: case 0xF7: /* mov @Ri, a */
xr (a); xw (ri);
break;
case 0xF8: case 0xF9: case 0xFA: case 0xFB:
case 0xFC: case 0xFD: case 0xFE: case 0xFF: /* mov Rn, a */
xr (a); xw (rn);
break;
default:
break;
}
}
void Lu_method(MatrixXf x3d, MatrixXf x2d, MatrixXf &R,Vector3f &t, MatrixXf &xcam_, MatrixXf K,
const PointCloud<PointXYZRGB>::Ptr& cloudRGB,//projected data
const PointCloud<PointXYZ>::Ptr& cloud_laser_cord,//laser coordinates
MatrixXf& points_projected, Mat image,PointCloud<PointXYZ>::ConstPtr cloud ){
Lu obpose;
obpose.Initialize(x3d,x2d,K);
//compute method
obpose.compute();
//getting results
R=obpose.GetR();// rotation
t=obpose.Gett();// translation
xcam_=obpose.Getxcam(); // points of the pose
cout<<"pasa?"<<endl;
points_projected=MatrixXf::Zero(2,cloud->points.size ());
cout<<"paso"<<endl;
for (int j=0;j<cloud->points.size ();j++){
PointCloud<PointXYZRGB> PointAuxRGB;
PointAuxRGB.points.resize(1);
Vector3f xw=Vector3f(cloud->points[j].x,
cloud->points[j].y,
cloud->points[j].z);
xw=K*( R*xw+t);
xw= xw/xw(2);
int col,row;
col=(int)xw(0);
row=(int)xw(1);
points_projected(0,j)=(int)xw(0);
points_projected(1,j)=(int)xw(1);
int b,r,g;
if ((col<0 || row<0) || (col>image.cols || row>image.rows)){
r=0;
g=0;
b=0;
}else{
// b = img->imageData[img->widthStep * row+ col * 3];
// g = img->imageData[img->widthStep * row+ col * 3 + 1];
//r = img->imageData[img->widthStep * row+ col * 3 + 2];
r=image.at<cv::Vec3b>(row,col)[0];
g=image.at<cv::Vec3b>(row,col)[1];
b=image.at<cv::Vec3b>(row,col)[2];
//std::cout << image.at<cv::Vec3b>(row,col)[0] << " " << image.at<cv::Vec3b>(row,col)[1] << " " << image.at<cv::Vec3b>(row,col)[2] << std::endl;
}
//std::cout << "( "<<r <<","<<g<<","<<b<<")"<<std::endl;
uint8_t r_i = r;
uint8_t g_i = g;
uint8_t b_i = b;
uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
//int32_t rgb = (r_i << 16) | (g_i << 8) | b_i;
PointAuxRGB.points[0].x=cloud->points[j].x;
PointAuxRGB.points[0].y=cloud->points[j].y;
PointAuxRGB.points[0].z=cloud->points[j].z;
PointAuxRGB.points[0].rgb=*reinterpret_cast<float*>(&rgb);
cloudRGB->points.push_back (PointAuxRGB.points[0]);
//project point to the image
}
}
void SolvePNP_method(vector <Point3f> points3d,
vector <Point2f> imagePoints,
MatrixXf &R,Vector3f &t, MatrixXf &xcam_,
MatrixXf K,
const PointCloud<PointXYZRGB>::Ptr& cloudRGB,//projected data
const PointCloud<PointXYZ>::Ptr& cloud_laser_cord,//laser coordinates
MatrixXf& points_projected,Mat image,
PointCloud<PointXYZ>::ConstPtr cloud)
{
Mat_<float> camera_matrix (3, 3);
camera_matrix(0,0)=K(0,0);
camera_matrix(0,1)=K(0,1);
camera_matrix(0,2)=K(0,2);
camera_matrix(1,0)=K(1,0);
camera_matrix(1,1)=K(1,1);
camera_matrix(1,2)=K(1,2);
camera_matrix(2,0)=K(2,0);
camera_matrix(2,1)=K(2,1);
camera_matrix(2,2)=K(2,2);
vector<double> rv(3), tv(3);
Mat rvec(rv),tvec(tv);
Mat_<float> dist_coef (5, 1);
dist_coef = 0;
cout <<camera_matrix<<endl;
cout<< imagePoints<<endl;
cout<< points3d <<endl;
solvePnP( points3d, imagePoints, camera_matrix, dist_coef, rvec, tvec);
//////////////////////////////////////////////////7
//convert data
//////////////////////////////////////////////////7
double rot[9] = {0};
Mat R_2(3,3,CV_64FC1,rot);
//change results only debugging
Rodrigues(rvec, R_2);
R=Matrix3f::Zero(3,3);
t=Vector3f(0,0,0);
double* _r = R_2.ptr<double>();
double* _t = tvec.ptr<double>();
t(0)=_t[0];
t(1)=_t[1];
t(2)=_t[2];
R(0,0)=_r[0];
R(0,1)=_r[1];
R(0,2)=_r[2];
R(1,0)=_r[3];
R(1,1)=_r[4];
R(1,2)=_r[5];
R(2,0)=_r[6];
R(2,1)=_r[7];
R(2,2)=_r[8];
points_projected=MatrixXf::Zero(2,cloud->points.size ());
for (int j=0;j<cloud->points.size ();j++){
PointCloud<PointXYZRGB> PointAuxRGB;
PointAuxRGB.points.resize(1);
Vector3f xw=Vector3f(cloud->points[j].x,
cloud->points[j].y,
cloud->points[j].z);
xw=K*( R*xw+t);
xw= xw/xw(2);
int col,row;
col=(int)xw(0);
row=(int)xw(1);
points_projected(0,j)=(int)xw(0);
points_projected(1,j)=(int)xw(1);
int b,r,g;
if ((col<0 || row<0) || (col>image.cols || row>image.rows)){
r=0;
g=0;
b=0;
}else{
// b = img->imageData[img->widthStep * row+ col * 3];
// g = img->imageData[img->widthStep * row+ col * 3 + 1];
//r = img->imageData[img->widthStep * row+ col * 3 + 2];
r=image.at<cv::Vec3b>(row,col)[0];
g=image.at<cv::Vec3b>(row,col)[1];
b=image.at<cv::Vec3b>(row,col)[2];
//std::cout << image.at<cv::Vec3b>(row,col)[0] << " " << image.at<cv::Vec3b>(row,col)[1] << " " << image.at<cv::Vec3b>(row,col)[2] << std::endl;
}
//std::cout << "( "<<r <<","<<g<<","<<b<<")"<<std::endl;
uint8_t r_i = r;
uint8_t g_i = g;
uint8_t b_i = b;
uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
//int32_t rgb = (r_i << 16) | (g_i << 8) | b_i;
PointAuxRGB.points[0].x=cloud->points[j].x;
PointAuxRGB.points[0].y=cloud->points[j].y;
PointAuxRGB.points[0].z=cloud->points[j].z;
PointAuxRGB.points[0].rgb=*reinterpret_cast<float*>(&rgb);
cloudRGB->points.push_back (PointAuxRGB.points[0]);
//project point to the image
}
}
