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; }