void write(std::ostream& out, const Mat& m, const int*, int) const
{
static const char* numpyTypes[] =
{
"uint8", "int8", "uint16", "int16", "int32", "float32", "float64", "uint64"
};
out << "array([";
writeMat(out, m, m.cols > 1 ? '[' : ' ', '[', m.cols*m.channels() == 1);
out << "], type='" << numpyTypes[m.depth()] << "')";
}
//#define DEBUG_MODE
int SingleSLAM::poseUpdate3D(bool largeErr) {
propagateFeatureStates();
//get the feature points corresponding to the map points
std::vector<Track2DNode*> nodes;
int num = getStaticMappedTrackNodes(nodes);
if (num < 1) {
double* cR = m_camPos.current()->R;
double* cT = m_camPos.current()->t;
CamPoseItem* camPos = m_camPos.add(currentFrame(), camId,cR, cT);
updateCamParamForFeatPts(K, camPos);
warn(
"[camera id:%d]intra-camera pose update failed! less than five static map points (%d)",
camId, num);
//leaveBACriticalSection();
//CoSLAM::ptr->pause();
//enterBACriticalSection();
return -1;
}
//choose the feature points for pose estimation
std::vector<FeaturePoint*> featPts;
chooseStaticFeatPts(featPts);
std::vector<FeaturePoint*> mappedFeatPts;
mappedFeatPts.reserve(nRowBlk * nColBlk * 2);
for (size_t i = 0; i < featPts.size(); i++) {
if (featPts[i]->mpt)
mappedFeatPts.push_back(featPts[i]);
}
//get the 2D-3D corresponding points
int n3D2Ds = mappedFeatPts.size();
Mat_d ms(n3D2Ds, 2), Ms(n3D2Ds, 3), covs(n3D2Ds, 9);
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;
memcpy(covs.data, fp->mpt->cov, sizeof(double) * 9);
}
Mat_d R(3, 3), t(3, 1);
double* cR = m_camPos.current()->R;
double* cT = m_camPos.current()->t;
IntraCamPoseOption opt;
//test
Mat_d old_errs(n3D2Ds, 1);
//get reprojection error beform pose update
for (int i = 0; i < n3D2Ds; i++) {
FeaturePoint* fp = mappedFeatPts[i];
double m[2];
project(K.data, cR, cT, fp->mpt->M, m);
old_errs[i] = dist2(m, fp->m);
}
//end of test
intraCamEstimate(K.data, cR, cT, Ms.rows, 0, Ms.data, ms.data,
Param::maxErr, R.data, t.data, &opt);
Mat_d new_errs(n3D2Ds, 1);
for (int i = 0; i < n3D2Ds; i++) {
FeaturePoint* fp = mappedFeatPts[i];
double m[2];
project(K.data, R, t, fp->mpt->M, m);
new_errs[i] = dist2(m, fp->m);
}
//find outliers
int numOut = 0;
double errThres = largeErr ? 6.0 : 2.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 < errThres) { //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->setUncertain();
// //test
// printf("poseUpdate:1\n");
// }
} 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.data, t.data);
updateCamParamForFeatPts(K, camPos);
// if (currentFrame() > 9)
// MyApp::bStop = true;
#ifdef DEBUG_MODE
//if the number of outliers are too much (may due to some distant points)
if (currentFrame() >= 76) {
//test
printf("f:%d,cam:%d : n3d2d:%d, num:%d, numOut:%d\n", currentFrame(),
camId, n3D2Ds, num, numOut);
char dirPath[1024];
sprintf(dirPath, "/home/tsou/slam_posefailed/%s", MyApp::timeStr);
mkdir(dirPath, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
savePGM(m_img, "/home/tsou/slam_posefailed/%s/%d_img_%d.pgm",
MyApp::timeStr, currentFrame(), camId);
writeMat(Ms, "/home/tsou/slam_posefailed/%s/%d_pts3d_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(ms, "/home/tsou/slam_posefailed/%s/%d_pts2d_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(covs, "/home/tsou/slam_posefailed/%s/%d_cov3d_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(3, 3, K, "/home/tsou/slam_posefailed/%s/%d_K_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(old_errs, "/home/tsou/slam_posefailed/%s/%d_old_errs_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(new_errs, "/home/tsou/slam_posefailed/%s/%d_new_errs_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(3, 3, cR, "/home/tsou/slam_posefailed/%s/%d_oldR_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(3, 1, cT, "/home/tsou/slam_posefailed/%s/%d_oldT_%d.txt",
MyApp::timeStr, currentFrame(), camId);
writeMat(R, "/home/tsou/slam_posefailed/%s/%d_R_%d.txt", MyApp::timeStr,
currentFrame(), camId);
writeMat(t, "/home/tsou/slam_posefailed/%s/%dT_%d.txt", MyApp::timeStr,
currentFrame(), camId);
//test
logInfo("program paused for debug at camera %d!\n", currentFrame(), camId);
leaveBACriticalSection();
CoSLAM::ptr->pause();
enterBACriticalSection();
}
#endif
return num;
}
void write(std::ostream& out, const Mat& m, const int*, int) const
{
out << "{";
writeMat(out, m, ',', ' ', m.cols==1);
out << "}";
}
void write(std::ostream& out, const Mat& m, const int*, int) const
{
writeMat(out, m, ' ', ' ', m.cols*m.channels() == 1);
if(m.rows > 1)
out << "\n";
}
void write(std::ostream& out, const Mat& m, const int*, int) const
{
out << "[";
writeMat(out, m, m.cols > 1 ? '[' : ' ', '[', m.cols*m.channels() == 1);
out << "]";
}
int main(void) {
benchmark(400,1,multiply2);
int i,j;
FILE* in = fopen("input.txt","r");
// reading matrix a
fscanf(in,"%d %d",&m,&p);
a = read_matrix(in,m,p);
// reading matrix b
fscanf(in,"%d %d",&q,&n);
b = read_matrix(in,q,n);
if (p!=q){
printf("incompatible matrices --not supported\n");
return 0;
}
//initializing matrix c
c = (int**) malloc(sizeof(int *)*m);
for (i = 0; i < m; ++i){
c[i] = (int *) malloc(sizeof(int)*n);
for (j = 0; j < n; ++j)
c[i][j] = 0;
}
long long t1,t2;
FILE* out = fopen("output.txt","w");
// multiply(a,b,c);
// multiply(a,b,c);
// multiply(a,b,c);
for (i = 0; i < m; ++i)
for (j = 0; j < n; ++j)
c[i][j] = 0;
fprintf(out,"result of non-threaded multiplication\n");
t1 = curTime();
multiply(a,b,c);
t2 = curTime();
writeMat(c,m,n,out);
fprintf(out,"elapsed time = %lld usec\n\n",t2-t1);
for (i = 0; i < m; ++i)
for (j = 0; j < n; ++j)
c[i][j] = 0;
fprintf(out,"result of multiplication1 (thread for each cell)\n");
t1 = curTime();
multiply1();
t2 = curTime();
writeMat(c,m,n,out);
fprintf(out,"elapsed time = %lld usec\n\n",t2-t1);
for (i = 0; i < m; ++i)
for (j = 0; j < n; ++j)
c[i][j] = 0;
fprintf(out,"result of multiplication2 (thread for each row)\n");
t1 = curTime();
multiply2();
t2 = curTime();
writeMat(c,m,n,out);
fprintf(out,"elapsed time = %lld usec\n\n",t2-t1);
fclose(out);
print(c,m,n);
return 0;
}
void write(std::ostream& out, const Mat& m, const int*, int) const
{
out << "[";
writeMat(out, m, ';', ' ', m.rows == 1);
out << "]";
}
