/**
* \brief 基本的相机坐标系，认为参考点的方向向量为z轴
*
* \param center 参考点的位置
* \param up 视点向上方向的向量.
*
* \return 世界坐标系转相机坐标系的矩阵
*/
Mat3 LookAt(const Vec3 ¢er, const Vec3 &up) {
Vec3 zc = center.normalized();//向量n
Vec3 xc = up.cross(zc).normalized();//向量u
Vec3 yc = zc.cross(xc);//向量v
Mat3 R;
R.row(0) = xc;
R.row(1) = yc;
R.row(2) = zc;
return R;
}
void EncodeTriangulation
(
const std::vector<Mat34> & Pi, // Projection matrices
const Mat2X & x_ij, // corresponding observations
double gamma, // Start upper bound
Mat & A,
Vec & C
)
{
// Build A, C matrix.
const size_t nbCamera = Pi.size();
A.resize(5*nbCamera,3);
C.resize(5*nbCamera,1);
int cpt = 0;
for (size_t i = 0; i < nbCamera; ++i)
{
const Mat3 R = Pi[i].block<3,3>(0,0);
const Vec3 t = Pi[i].block<3,1>(0,3);
const Mat2X pt = x_ij.col(i);
// A (Rotational part):
A.block<1,3>(cpt,0) = R.row(0) - pt(0) * R.row(2) - gamma * R.row(2);
A.block<1,3>(cpt+1,0) = R.row(1) - pt(1) * R.row(2) - gamma * R.row(2);
A.block<1,3>(cpt+2,0) = - R.row(2);
A.block<1,3>(cpt+3,0) = - R.row(0) + pt(0) * R.row(2) - gamma * R.row(2);
A.block<1,3>(cpt+4,0) = - R.row(1) + pt(1) * R.row(2) - gamma * R.row(2);
// C (translation part):
C(cpt) = - t(0) + pt(0) * t(2) + gamma * t(2);
C(cpt+1) = - t(1) + pt(1) * t(2) + gamma * t(2);
C(cpt+2) = t(2);
C(cpt+3) = t(0) - pt(0) * t(2) + gamma * t(2);
C(cpt+4) = t(1) - pt(1) * t(2) + gamma * t(2);
//- Next entry
cpt += 5;
}
}
int main(int argc,char* argv[]) {
if (argc < 4) {
printf("./match_g2o pose_stamped.txt key.match map_point.txt\n");
return 1;
}
srand(time(NULL));
Rcl << 1,0,0,0,0,1,0,-1,0;
Tcl << 0,0.06,0;
//read pose file
FILE* pose_stamped = fopen(argv[1],"r");
if (!pose_stamped)
return 1;
char buffer[2048];
std::vector<Mat3> rotations;
std::vector<Eigen::Vector3d> translations;
while (fgets(buffer,2048,pose_stamped)) {
double t,x,y,z,qx,qy,qz,qw;
if (sscanf(buffer,"%lf %lf %lf %lf %lf %lf %lf %lf",&t,&x,&y,&z,&qw,&qx,&qy,&qz)==8) {
double r[9];
quaternionToRotation(qx,qy,qz,qw,r);
Mat3 Rwl;
memcpy(Rwl.data(),r,9*sizeof(double));
Eigen::Vector3d Twl(x,y,z);
rotations.push_back(Rcl * Rwl.transpose());
translations.push_back(- Rcl * Rwl.transpose() * Twl + Tcl);
} else {
printf("Error parsing: %s\n",buffer);
}
}
fclose(pose_stamped);
struct timespec start,end;
clock_gettime(CLOCK_MONOTONIC,&start);
int count_points = 0;
double RMSE = 0;
FILE* key_match = fopen(argv[2],"r");
FILE* map_point = fopen(argv[3],"w");
if (!(key_match && map_point))
return 1;
while (fgets(buffer,2048,key_match)) {
#if DEBUG_SINGLE
printf("key.match: %s",buffer);
#endif
int id;
char* tok = strtok(buffer," ");
std::vector<double> uc,vc;
std::vector<int> index;
while (tok) {
id = atoi(tok);
index.push_back(id);
tok = strtok(NULL," \n");
double u = atof(tok);
tok = strtok(NULL," \n");
double v = atof(tok);
tok = strtok(NULL," \n");
uc.push_back(u - cx);
vc.push_back(cy - v);
}
//optimize
Eigen::Vector3d bestEstimate, x1, x2;
double leastError = -1;
for (unsigned int i=0;i<index.size()-1;i++) {
for (unsigned int j=i+1;j<index.size();j++) {
double u1 = uc[i], v1 = vc[i];
double u2 = uc[j], v2 = vc[j];
Mat3 R1 = rotations[index[i]], R2 = rotations[index[j]];
Eigen::Vector3d T1 = translations[index[i]], T2 = translations[index[j]];
Mat3 Rcc = R2 * R1.transpose();
Eigen::Vector3d Tcc = T1 - R1 * R2.transpose() * T2;
Eigen::Vector3d r1 = Rcc.row(0), r2 = Rcc.row(1), r3 = Rcc.row(2);
Eigen::Vector3d uv(-u1/fx,-v1/fy,1);
Eigen::Vector3d mult_u = r1 + u2/fx * r3;
Eigen::Vector3d mult_v = r2 + v2/fy * r3;
double z_est[2] = {mult_u.dot(Tcc) / mult_u.dot(uv),
mult_v.dot(Tcc) / mult_v.dot(uv) } ;
for (int k=0;k<1;k++) {
x1 << -u1*z_est[k]/fx, -v1*z_est[k]/fy, z_est[k];
x2 = Rcc * (x1 - Tcc);
if (x1(2) >= 0 || x2(2) >= 0)
break;
double u_est = -fx * x2(0) / x2(2);
double v_est = -fy * x2(1) / x2(2);
double error = (u_est-u2) * (u_est-u2) + (v_est-v2) * (v_est-v2);
if (leastError < 0 || error < leastError) {
leastError = error;
bestEstimate = R1.transpose() * (x1 - T1);
}
}
}
}
//record result
RMSE += leastError;
#if DEBUG_SINGLE
printf("reprojection: ");
char* c = buffer;
c += sprintf(c,"transformation:\n");
for (unsigned int i=0;i<index.size();i++) {
id = index[i];
Eigen::Vector3d xc = rotations[id] * bestEstimate + translations[id];
double u = - fx * xc(0) / xc(2);
double v = - fy * xc(1) / xc(2);
printf("%d %f %f ",id,u+cx,cy-v);
c += sprintf(c,"%4.2f %4.2f %4.2f\n%4.2f %4.2f %4.2f\n%4.2f %4.2f %4.2f\n",
rotations[id](0,0),rotations[id](0,1),rotations[id](0,2),
rotations[id](1,0),rotations[id](1,1),rotations[id](1,2),
rotations[id](2,0),rotations[id](2,1),rotations[id](2,2));
c += sprintf(c,"[%4.2f %4.2f %4.2f]\n",translations[id](0),translations[id](1),translations[id](2));
}
printf("\n%s",buffer);
printf("estimate: %f %f %f %lu %f\n",bestEstimate(0),bestEstimate(1),bestEstimate(2),index.size(),leastError);
#endif
fprintf(map_point,"%f %f %f %lu %f\n",bestEstimate(0),bestEstimate(1),bestEstimate(2),index.size(),leastError);
count_points++;
}
clock_gettime(CLOCK_MONOTONIC,&end);
double dt = end.tv_sec - start.tv_sec + 0.000000001 * (end.tv_nsec - start.tv_nsec);
RMSE = sqrt(RMSE / count_points);
printf("Optimized %d map points (%fs, RMSE = %f)\n",count_points, dt, RMSE);
fclose(key_match);
fclose(map_point);
return 0;
}
