C++ Mat3::row方法代码示例

		/**
		 * \brief	基本的相机坐标系，认为参考点的方向向量为z轴
		 *
		 * \param	center	参考点的位置
		 * \param	up	  	视点向上方向的向量.
		 *
		 * \return	世界坐标系转相机坐标系的矩阵
		 */
		Mat3 LookAt(const Vec3 &center, 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++) {
//.........这里部分代码省略.........