int SingleSLAM::fastPoseUpdate3D() {
propagateFeatureStates();
//get the feature points corresponding to the map points
std::vector<Track2DNode*> nodes;
int num = getStaticMappedTrackNodes(nodes);
if (num < 5) {
repErr(
"[camera id:%d]intra-camera pose update failed! less than five static map points (%d)",
camId, num);
return -1;
}
//choose the feature points for pose estimation
std::vector<FeaturePoint*> featPts;
int iChoose = chooseStaticFeatPts(featPts);
//test
logInfo("number of chosen features :%d\n", iChoose);
std::vector<FeaturePoint*> mappedFeatPts;
std::vector<FeaturePoint*> unmappedFeatPts;
mappedFeatPts.reserve(nRowBlk * nColBlk * 2);
unmappedFeatPts.reserve(nRowBlk * nColBlk * 2);
for (size_t i = 0; i < featPts.size(); i++) {
if (featPts[i]->mpt)
mappedFeatPts.push_back(featPts[i]);
else if (featPts[i]->preFrame)
unmappedFeatPts.push_back(featPts[i]);
}
//get the 2D-3D corresponding points
int n3D2Ds = mappedFeatPts.size();
Mat_d ms(n3D2Ds, 2), Ms(n3D2Ds, 3), repErrs(n3D2Ds, 1);
for (int i = 0; i < n3D2Ds; i++) {
FeaturePoint* fp = mappedFeatPts[i];
ms.data[2 * i] = fp->x;
ms.data[2 * i + 1] = fp->y;
Ms.data[3 * i] = fp->mpt->x;
Ms.data[3 * i + 1] = fp->mpt->y;
Ms.data[3 * i + 2] = fp->mpt->z;
repErrs.data[i] = fp->reprojErr;
}
//get the 2D-2D corresponding points
int n2D2Ds = unmappedFeatPts.size();
Mat_d ums(n2D2Ds, 2), umspre(n2D2Ds, 2), Rpre(n2D2Ds, 9), tpre(n2D2Ds, 3);
for (int i = 0; i < n2D2Ds; i++) {
FeaturePoint* fp = unmappedFeatPts[i];
ums.data[2 * i] = fp->x;
ums.data[2 * i + 1] = fp->y;
//travel back to the frame of the first appearance
//while (fp->preFrame) {
fp = fp->preFrame;
//}
assert(fp);
umspre.data[2 * i] = fp->x;
umspre.data[2 * i + 1] = fp->y;
doubleArrCopy(Rpre.data, i, fp->cam->R, 9);
doubleArrCopy(tpre.data, i, fp->cam->t, 3);
}
//estimate the camera pose using both 2D-2D and 3D-2D correspondences
double R[9], t[3];
double* cR = m_camPos.current()->R;
double* cT = m_camPos.current()->t;
// //test
// logInfo("==============start of camId:%d=================\n", camId);
// logInfo("n3D2D:%d, n2D2D:%d\n", n3D2Ds, n2D2Ds);
//
// write(K, "/home/tsou/data/K.txt");
// write(cR, 3, 3, "/home/tsou/data/%d_R0.txt", camId);
// write(cT, 3, 1, "/home/tsou/data/%d_T0.txt", camId);
// write(repErrs, "/home/tsou/data/%d_errs.txt", camId);
// write(Ms, "/home/tsou/data/%d_Ms.txt", camId);
// write(ms, "/home/tsou/data/%d_ms.txt", camId);
// write(Rpre, "/home/tsou/data/%d_Rpre.txt", camId);
// write(tpre, "/home/tsou/data/%d_tpre.txt", camId);
// write(umspre, "/home/tsou/data/%d_umspre.txt", camId);
// write(ums, "/home/tsou/data/%d_ums.txt", camId);
//
// //test
// printMat(3, 3, cR);
// printMat(3, 1, cT);
IntraCamPoseOption opt;
double R_tmp[9], t_tmp[3];
intraCamEstimate(K.data, cR, cT, n3D2Ds, repErrs.data, Ms.data, ms.data, 6,
R_tmp, t_tmp, &opt);
if (getCameraDistance(R_tmp, t_tmp, Rpre.data, tpre.data) > 1000) {
opt.verboseLM = 1;
intraCamEstimateEpi(K.data, R_tmp, t_tmp, n3D2Ds, repErrs.data, Ms.data,
ms.data, n2D2Ds, 0, Rpre.data, tpre.data, umspre.data, ums.data,
6, R, t, &opt);
} else {
doubleArrCopy(R, 0, R_tmp, 9);
doubleArrCopy(t, 0, t_tmp, 3);
}
// printMat(3, 3, cR);
// printMat(3, 1, cT);
// printMat(3, 3, R);
// printMat(3, 1, cT);
// logInfo("==============end of camId:%d=================\n", camId);
// intraCamEstimate(K.data,cR,cT,n3D2Ds, repErrs.data,Ms.data,ms.data,6.0,R,t,&opt);
// find outliers
int numOut = 0;
double rm[2], var[4], ivar[4];
for (int i = 0; i < num; i++) {
double* pM = nodes[i]->pt->mpt->M;
double* pCov = nodes[i]->pt->mpt->cov;
project(K, R, t, pM, rm);
getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
mat22Inv(var, ivar);
double err = mahaDist2(rm, nodes[i]->pt->m, ivar);
if (err < 1) { //inlier
nodes[i]->pt->reprojErr = err;
seqTriangulate(K, R, t, nodes[i]->pt->m, pM, pCov,
Const::PIXEL_ERR_VAR);
project(K, R, t, pM, rm);
getProjectionCovMat(K, R, t, pM, pCov, var, Const::PIXEL_ERR_VAR);
mat22Inv(var, ivar);
err = mahaDist2(rm, nodes[i]->pt->m, ivar);
if (err >= 1) {
nodes[i]->pt->mpt->setFalse();
}
} else {
//outliers
numOut++;
double repErr = dist2(rm, nodes[i]->pt->m);
nodes[i]->pt->reprojErr = repErr;
nodes[i]->pt->mpt->setUncertain();
}
}
CamPoseItem* camPos = m_camPos.add(currentFrame(), camId, R, t);
updateCamParamForFeatPts(K, camPos);
return num;
}
void showmem()
{
Z80 &cpu = CpuMgr.Cpu();
char line[80]; unsigned ii;
unsigned cursor_found = 0;
if (mem_dump) mem_ascii = 1, mem_sz = 32; else mem_sz = 8;
if (editor == ED_LOG || editor == ED_PHYS)
{
edited_track.clear();
edited_track.seek(comp.wd.fdd+mem_disk, mem_track/2, mem_track & 1, (editor == ED_LOG)? LOAD_SECTORS : JUST_SEEK);
if (!edited_track.trkd) { // no track
for (ii = 0; ii < mem_size; ii++) {
sprintf(line, (ii == mem_size/2)?
" track not found ":
" ");
tprint(mem_x, mem_y+ii, line, (activedbg == WNDMEM) ? W_SEL : W_NORM);
}
mem_max = 0;
goto title;
}
mem_max = edited_track.trklen;
if (editor == ED_LOG)
for (mem_max = ii = 0; ii < edited_track.s; ii++)
mem_max += edited_track.hdr[ii].datlen;
}
else if (editor == ED_MEM)
mem_max = 0x10000;
else if (editor == ED_CMOS)
mem_max = sizeof(cmos);
else if (editor == ED_NVRAM)
mem_max = sizeof(nvram);
redraw:
if (mem_dump == 2)
{ // tsconf
#define tprx(a,b,c,d,e) sprintf(line, "%s: %02X:%04X", (c), (d), (e)); tprint(mem_x + (a), mem_y + (b), line, W_NORM);
#define tpre(a,b,c,d,e) sprintf(line, "%s: %d:%d", (c), (d), (e)); tprint(mem_x + (a), mem_y + (b), line, W_NORM);
#define tprh(a,b,c,d) sprintf(line, "%s: %02X", (c), (d)); tprint(mem_x + (a), mem_y + (b), line, W_NORM);
#define tprd(a,b,c,d) sprintf(line, "%s: %d", (c), (d)); tprint(mem_x + (a), mem_y + (b), line, W_NORM);
#define tprs(a,b,c,d) sprintf(line, "%s: %s", (c), (d)); tprint(mem_x + (a), mem_y + (b), line, W_NORM);
#define tprc(a,b,c,d) sprintf(line, "%s:%s", (c), (d)); tprint(mem_x + (a), mem_y + (b), line, W_NORM);
#define tprl(a,b,c) sprintf(line, "%s", (c)); tprint(mem_x + (a), mem_y + (b), line, W_NORM);
#define tprn(a,b,c) sprintf(line, "%d", (c)); tprint(mem_x + (a), mem_y + (b), line, W_NORM);
const char d_onoff[2][4] = {"dis", "ena"};
const char d_vmode[4][5] = {"ZX ", "16c ", "256c", "text"};
const char d_rres[4][8] = {"256x192", "320x200", "320x240", "360x288"};
const char d_clk[4][7] = {"3.5MHz", "7MHz ", "14MHz ", "unk "};
const char d_lock[4][5] = {"512", "128", "aut", "1MB"};
const char d_dma[16][8] =
{
"unk ",
"unk ",
"RAM-RAM",
"BLT-RAM",
"SPI-RAM",
"RAM-SPI",
"IDE-RAM",
"RAM-IDE",
"FIL-RAM",
"RAM-CRM",
"unk ",
"RAM-SFL",
"unk ",
"unk ",
"unk ",
"unk "
};
const char d_dmast[11][6] =
{
"RAM ",
"BLT ",
"SPI_R",
"SPI_W",
"IDE_R",
"IDE_W",
"FILL ",
"CRAM ",
"SFILE",
"INIT ",
"NOP "
};
tprs(0, 0, "VMod", d_vmode[comp.ts.vmode]);
tprd(0, 1, "RRes", comp.ts.rres);
tprs(0, 2, "Gfx ", d_onoff[!comp.ts.nogfx]);
tprs(0, 3, "TSU ", d_onoff[!comp.ts.notsu]);
tprh(0, 4, "Vpg ", comp.ts.vpage);
tprh(0, 5, "Spg ", comp.ts.sgpage);
tprh(0, 6, "T0pg", comp.ts.t0gpage[2]);
tprh(0, 7, "T1pg", comp.ts.t1gpage[2]);
tprh(0, 8, "TMpg", comp.ts.tmpage);
tprh(0, 9, "Gpal", comp.ts.gpal);
tprh(0, 10, "T0pl", comp.ts.t0pal << 2);
tprh(0, 11, "T1pl", comp.ts.t1pal << 2);
tprd(12, 0, "HSI", comp.ts.hsint);
tprd(12, 1, "VSI", comp.ts.vsint);
tprl(10, 2, "DMA LIN FRM");
tprn(11, 3, comp.ts.intdma);
tprn(15, 3, comp.ts.intline);
tprn(19, 3, comp.ts.intframe);
tprd(9, 5, "GX ", comp.ts.g_xoffs);
tprd(9, 6, "GY ", comp.ts.g_yoffs);
tprd(9, 7, "T0X", comp.ts.t0_xoffs);
tprd(9, 8, "T0Y", comp.ts.t0_yoffs);
tprd(9, 9, "T1X", comp.ts.t1_xoffs);
tprd(9, 10, "T1Y", comp.ts.t1_yoffs);
tprh(9, 11, "Brd", comp.ts.border);
tprl(21, 0, "SP T1 T0 T1Z T0Z");
tprn(22, 1, comp.ts.s_en);
tprn(25, 1, comp.ts.t1_en);
tprn(28, 1, comp.ts.t0_en);
tprn(31, 1, comp.ts.t1z_en);
tprn(34, 1, comp.ts.t0z_en);
tprs(23, 3, "CLK", d_clk[comp.ts.zclk]);
tprl(23, 4, "Cache: ");
tprn(30, 4, comp.ts.cache_win0);
tprn(32, 4, comp.ts.cache_win1);
tprn(34, 4, comp.ts.cache_win2);
tprn(36, 4, comp.ts.cache_win3);
tprh(23, 5, "FMAddr", comp.ts.fmaddr);
tprc(18, 7, "RAM0", d_onoff[comp.ts.w0_ram]);
tprc(18, 8, "MAP0", d_onoff[!comp.ts.w0_map_n]);
tprc(19, 9, "WE0", d_onoff[comp.ts.w0_we]);
tprc(19, 10, "Lck", d_lock[comp.ts.lck128]);
tprs(27, 7, "DMA", "");
tprx(27, 8, "S", comp.ts.dma.saddr >> 14, comp.ts.dma.saddr & 0x3FFF);
tprx(27, 9, "D", comp.ts.dma.daddr >> 14, comp.ts.dma.daddr & 0x3FFF);
tpre(27, 10, "L", comp.ts.dma.num, comp.ts.dma.len * 2);
tprs(27, 11, "St", d_dmast[comp.ts.dma.state]);
}
else
{
