void InitIncrementalPose::initPose(const Matrix33& R, const Vector31& T, Pose& pose) const {
pose.sett(RealPoint3D<double>(T.GetX(), T.GetY(), T.GetZ()));
Matrix<double>& pR = pose.R();
pR[0][0] = R.Get00(); pR[1][0] = R.Get01(); pR[2][0] = R.Get02();
pR[0][1] = R.Get10(); pR[1][1] = R.Get11(); pR[2][1] = R.Get12();
pR[0][2] = R.Get20(); pR[1][2] = R.Get21(); pR[2][2] = R.Get22();
pose.calcEulerAngles();
pose.setorientationSynchronWithAngles(true);
pose.setderivationsSynchronWithAngles(false);
}
void MultiCamInitIncrementalPose::initPoseMono(unsigned int camera, size_t frame, unsigned int maxIter,
unsigned int& inliers, RealPoint3D<double>& t, Matrix<double>& R) const {
// Selecting points appearing in the camera
vector<size_t> inCameraPoints;
inCameraPoints.reserve(bundle_.frame(frame).size());
for(size_t i = 0; i < bundle_.frame(frame).size(); ++i) {
if(bundle_.frame(frame)[i].inTrackNumber() > 2 && bundle_.frame(frame)[i].inCamera(camera) && bundle_.frame(frame)[i].track()->point()->numInliers() >= 3)
inCameraPoints.push_back(i);
}
// Checking the number of points
if(inCameraPoints.size() < 3) {
// Not enought points, stop here!
inliers = 0;
return;
}
// Main RANSAC loop
unsigned int maxInliers = 0;
RealPoint3D<double> maxT;
Matrix<double> maxR(3, 3);
const Transformation P_W_Cn0(calibration_.translation(camera), calibration_.rotation(camera));
#pragma omp parallel for
for(unsigned int iter = 0; iter < maxIter; ++iter) {
// Selecting 3 random points
size_t index[3];
select3Index(inCameraPoints, index);
// Extracting points
double Xw[3], Yw[3], Zw[3], TabXi[3], TabYi[3], TabZi[3];
for(size_t i = 0; i < 3; ++i) {
Track& track = *(bundle_.frame(frame)[inCameraPoints[index[i]]].track());
RealPoint3D<double> tmp = P_W_Cn0.applyInv(track.point()->coords());
Xw[i] = tmp.x();
Yw[i] = tmp.y();
Zw[i] = tmp.z();
mulTransMat33Vect3(P_W_Cn0.R(), bundle_.frame(frame)[inCameraPoints[index[i]]].ray(camera).direction(), tmp);
TabXi[i] = tmp.x();
TabYi[i] = tmp.y();
TabZi[i] = tmp.z();
}
// Calculating pose
Matrix33 R3Pose[4];
Vector31 T3Pose[4];
MPoseFinsterwalder poseEstimator;
int nbSol = poseEstimator.CalculePose(Xw, Yw, Zw, TabXi, TabYi, TabZi, R3Pose, T3Pose);
// Checking each solution
for(unsigned int sol = 0; sol < nbSol; ++sol) {
// Calculating temporary pose
Vector31 tmpT;
Matrix33 tmpR = R3Pose[sol].transpose();
tmpT = -1.0*(tmpR*T3Pose[sol]);
Matrix<double> tmp_mat(3, 3);
tmp_mat[0][0] = tmpR.Get00(); tmp_mat[1][0] = tmpR.Get01(); tmp_mat[2][0] = tmpR.Get02();
tmp_mat[0][1] = tmpR.Get10(); tmp_mat[1][1] = tmpR.Get11(); tmp_mat[2][1] = tmpR.Get12();
tmp_mat[0][2] = tmpR.Get20(); tmp_mat[1][2] = tmpR.Get21(); tmp_mat[2][2] = tmpR.Get22();
Transformation P_Cn0_Cni(RealPoint3D<double>(tmpT.GetX(), tmpT.GetY(), tmpT.GetZ()), tmp_mat);
Transformation P_W_MCSi = P_W_Cn0*P_Cn0_Cni*P_W_Cn0.inv();
// Calculating angular errors
unsigned int numInliers = 0;
for(size_t i = 0; i < bundle_.frame(frame).size(); ++i) {
if(bundle_.frame(frame)[i].inTrackNumber() > 2) {
for(unsigned int camera = 0; camera < bundle_.numCameras(); ++camera) {
if(bundle_.frame(frame)[i].inCamera(camera) && bundle_.frame(frame)[i].track()->point()->numInliers() >= 3) {
Transformation P_MCS_Cn(calibration_.translation(camera), calibration_.rotation(camera));
Transformation P_W_Cni = P_W_MCSi*P_MCS_Cn;
RealPoint3D<double> point3D = P_W_Cni.applyInv(bundle_.frame(frame)[i].track()->point()->coords());
RealPoint3D<double> rayDir;
mulTransMat33Vect3(P_MCS_Cn.R(), bundle_.frame(frame)[i].ray(camera).direction(), rayDir);
const double error = 1.0 - syuSqr(point3D.dot(rayDir))/(point3D.norm2()*rayDir.norm2());
if(error < maxSqrAngularError_) ++numInliers;
}
}
}
}
// Checking if our solution is better
#pragma omp critical
{
if(maxInliers < numInliers) {
maxInliers = numInliers;
maxT = P_Cn0_Cni.t();
maxR = P_Cn0_Cni.R();
}
}
}
}
// Saving RANSAC result
inliers = maxInliers;
Transformation P_Cn0_Cni(maxT, maxR);
Transformation P_W_MCSi = P_W_Cn0*P_Cn0_Cni*P_W_Cn0.inv();
R = P_W_MCSi.R();
t = P_W_MCSi.t();
}
