void BundleRTS::output() {
bool updateNonKeyCameras = firstKeyFrame && pCoSLAM;
//for updating the camera poses of non-key frames
if (updateNonKeyCameras)
constructCameraGraphs();
const int cnp = 11; //5:intrinsic parameters 6:extrinsic parameters
const int pnp = 3;
double* pParamVec = m_paramVec;
//update the camera positions
double dq[4], q[4];
for (int c = 0; c < numCams; c++) {
//rotation
_MK_QUAT_FRM_VEC(dq, pParamVec+5);
quatMultFast(dq, m_globs.rot0params + c * FULLQUATSZ, q);
//update the camera poses
quat2mat(q, keyCamPoses[c]->R);
memcpy(keyCamPoses[c]->t, pParamVec + 8, sizeof(double) * 3);
//update the camera poses of fixed nodes in camera graphs
if (updateNonKeyCameras) {
int camId = keyCamPoses[c]->camId;
int nodeId = nonKeyCamNodeId[camId][keyCamPoses[c]];
memcpy(camGraphs[camId].poseNodes[nodeId].R, keyCamPoses[c]->R,
sizeof(double) * 9);
memcpy(camGraphs[camId].poseNodes[nodeId].t, keyCamPoses[c]->t,
sizeof(double) * 3);
}
pParamVec += cnp;
}
//update the map points
double* pParamPoints = m_paramVec + numCams * cnp;
for (int i = 0; i < numPts; i++) {
memcpy(mapPoints[i]->M, pParamPoints, sizeof(double) * pnp);
pParamPoints += pnp;
}
//update the camera poses of non-key frames
if (updateNonKeyCameras) {
updateNonKeyCameraPoses();
updateNewPosesPoints();
}
}
bool absOrient(const int npts, const double pts1[], const double pts2[], double R[], double t[]) {
assert(npts >= 3);
double A[16], v[4];
double* r1 = new double[npts * 3];
double* r2 = new double[npts * 3];
double pc1[3], pc2[3];
aoNormCoord(npts, pts1, r1, pc1);
aoNormCoord(npts, pts2, r2, pc2);
aoGetMatrix(npts, r1, r2, A);
bool res = aoMaxEig(A, v);
if (!res)
return false;
quat2mat(v, R);
mat33ProdVec(R, pc1, pc2, t, -1, 1);
delete[] r1;
delete[] r2;
return true;
}
