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

bool Bundle_Adjustment_Ceres::Adjust
(
  SfM_Data & sfm_data,     // the SfM scene to refine
  const Optimize_Options options
)
{
  //----------
  // Add camera parameters
  // - intrinsics
  // - poses [R|t]

  // Create residuals for each observation in the bundle adjustment problem. The
  // parameters for cameras and points are added automatically.
  //----------

  ceres::Problem problem;

  // Data wrapper for refinement:
  Hash_Map<IndexT, std::vector<double> > map_intrinsics;
  Hash_Map<IndexT, std::vector<double> > map_poses;

  // Setup Poses data & subparametrization
 for (Poses::const_iterator itPose = sfm_data.poses.begin(); itPose != sfm_data.poses.end(); ++itPose)
  {
    const IndexT indexPose = itPose->first;

    const Pose3 & pose = itPose->second;
    const Mat3 R = pose.rotation();
    const Vec3 t = pose.translation();

    double angleAxis[3];
    ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis);
    // angleAxis + translation
    map_poses[indexPose] = {angleAxis[0], angleAxis[1], angleAxis[2], t(0), t(1), t(2)};

    double * parameter_block = &map_poses[indexPose][0];
    problem.AddParameterBlock(parameter_block, 6);
    if (options.extrinsics_opt == Extrinsic_Parameter_Type::NONE)
    {
      // set the whole parameter block as constant for best performance
      problem.SetParameterBlockConstant(parameter_block);
    }
    else  // Subset parametrization
    {
      std::vector<int> vec_constant_extrinsic;
      // If we adjust only the translation, we must set ROTATION as constant
      if (options.extrinsics_opt == Extrinsic_Parameter_Type::ADJUST_TRANSLATION)
      {
        // Subset rotation parametrization
        vec_constant_extrinsic.push_back(0);
        vec_constant_extrinsic.push_back(1);
        vec_constant_extrinsic.push_back(2);
      }
      // If we adjust only the rotation, we must set TRANSLATION as constant
      if (options.extrinsics_opt == Extrinsic_Parameter_Type::ADJUST_ROTATION)
      {
        // Subset translation parametrization
        vec_constant_extrinsic.push_back(3);
        vec_constant_extrinsic.push_back(4);
        vec_constant_extrinsic.push_back(5);
      }
      if (!vec_constant_extrinsic.empty())
      {
        ceres::SubsetParameterization *subset_parameterization =
          new ceres::SubsetParameterization(6, vec_constant_extrinsic);
        problem.SetParameterization(parameter_block, subset_parameterization);
      }
    }
  }

  // Setup Intrinsics data & subparametrization
  for (Intrinsics::const_iterator itIntrinsic = sfm_data.intrinsics.begin();
    itIntrinsic != sfm_data.intrinsics.end(); ++itIntrinsic)
  {
    const IndexT indexCam = itIntrinsic->first;

    if (isValid(itIntrinsic->second->getType()))
    {
      map_intrinsics[indexCam] = itIntrinsic->second->getParams();

      double * parameter_block = &map_intrinsics[indexCam][0];
      problem.AddParameterBlock(parameter_block, map_intrinsics[indexCam].size());
      if (options.intrinsics_opt == Intrinsic_Parameter_Type::NONE)
      {
        // set the whole parameter block as constant for best performance
        problem.SetParameterBlockConstant(parameter_block);
      }
      else
      {
        const std::vector<int> vec_constant_intrinsic =
          itIntrinsic->second->subsetParameterization(options.intrinsics_opt);
        if (!vec_constant_intrinsic.empty())
        {
          ceres::SubsetParameterization *subset_parameterization =
            new ceres::SubsetParameterization(
              map_intrinsics[indexCam].size(), vec_constant_intrinsic);
          problem.SetParameterization(parameter_block, subset_parameterization);
        }
      }
    }
//.........这里部分代码省略.........

bool Bundle_Adjustment_Ceres::Adjust(
  SfM_Data & sfm_data,     // the SfM scene to refine
  bool bRefineRotations,   // tell if pose rotations will be refined
  bool bRefineTranslations,// tell if the pose translation will be refined
  bool bRefineIntrinsics,  // tell if the camera intrinsic will be refined
  bool bRefineStructure)   // tell if the structure will be refined
{
  //----------
  // Add camera parameters
  // - intrinsics
  // - poses [R|t]

  // Create residuals for each observation in the bundle adjustment problem. The
  // parameters for cameras and points are added automatically.
  //----------

  ceres::Problem problem;

  // Data wrapper for refinement:
  Hash_Map<IndexT, std::vector<double> > map_intrinsics;
  Hash_Map<IndexT, std::vector<double> > map_poses;

  // Setup Poses data & subparametrization
  for (Poses::const_iterator itPose = sfm_data.poses.begin(); itPose != sfm_data.poses.end(); ++itPose)
  {
    const IndexT indexPose = itPose->first;

    const Pose3 & pose = itPose->second;
    const Mat3 R = pose.rotation();
    const Vec3 t = pose.translation();

    double angleAxis[3];
    ceres::RotationMatrixToAngleAxis((const double*)R.data(), angleAxis);
    map_poses[indexPose].reserve(6); //angleAxis + translation
    map_poses[indexPose].push_back(angleAxis[0]);
    map_poses[indexPose].push_back(angleAxis[1]);
    map_poses[indexPose].push_back(angleAxis[2]);
    map_poses[indexPose].push_back(t(0));
    map_poses[indexPose].push_back(t(1));
    map_poses[indexPose].push_back(t(2));

    double * parameter_block = &map_poses[indexPose][0];
    problem.AddParameterBlock(parameter_block, 6);
    if (!bRefineTranslations && !bRefineIntrinsics)
    {
      //set the whole parameter block as constant for best performance.
      problem.SetParameterBlockConstant(parameter_block);
    }
    else  {
      // Subset parametrization
      std::vector<int> vec_constant_extrinsic;
      if(!bRefineRotations)
      {
        vec_constant_extrinsic.push_back(0);
        vec_constant_extrinsic.push_back(1);
        vec_constant_extrinsic.push_back(2);
      }
      if(!bRefineTranslations)
      {
        vec_constant_extrinsic.push_back(3);
        vec_constant_extrinsic.push_back(4);
        vec_constant_extrinsic.push_back(5);
      }
      if (!vec_constant_extrinsic.empty())
      {
        ceres::SubsetParameterization *subset_parameterization =
          new ceres::SubsetParameterization(6, vec_constant_extrinsic);
        problem.SetParameterization(parameter_block, subset_parameterization);
      }
    }
  }

  // Setup Intrinsics data & subparametrization
  for (Intrinsics::const_iterator itIntrinsic = sfm_data.intrinsics.begin();
    itIntrinsic != sfm_data.intrinsics.end(); ++itIntrinsic)
  {
    const IndexT indexCam = itIntrinsic->first;

    if (isValid(itIntrinsic->second->getType()))
    {
      map_intrinsics[indexCam] = itIntrinsic->second->getParams();

      double * parameter_block = &map_intrinsics[indexCam][0];
      problem.AddParameterBlock(parameter_block, map_intrinsics[indexCam].size());
      if (!bRefineIntrinsics)
      {
        //set the whole parameter block as constant for best performance.
        problem.SetParameterBlockConstant(parameter_block);
      }
    }
    else
    {
      std::cerr << "Unsupported camera type." << std::endl;
    }
  }

  // Set a LossFunction to be less penalized by false measurements
  //  - set it to NULL if you don't want use a lossFunction.
  ceres::LossFunction * p_LossFunction = new ceres::HuberLoss(Square(4.0));
  // TODO: make the LOSS function and the parameter an option
//.........这里部分代码省略.........

/// Save SfM_Data in an ASCII BAF (Bundle Adjustment File).
// --Header
// #Intrinsics
// #Poses
// #Landmarks
// --Data
// Intrinsic parameters [foc ppx ppy, ...]
// Poses [angle axis, camera center]
// Landmarks [X Y Z #observations id_intrinsic id_pose x y ...]
//--
//- Export also a _imgList.txt file with View filename and id_intrinsic & id_pose.
// filename id_intrinsic id_pose
// The ids allow to establish a link between 3D point observations & the corresponding views
//--
// Export missing poses as Identity pose to keep tracking of the original id_pose indexes
static bool Save_BAF(
  const SfM_Data & sfm_data,
  const std::string & filename,
  ESfM_Data flags_part)
{
  std::ofstream stream(filename.c_str());
  if (!stream.is_open())
    return false;

  bool bOk = false;
  {
    stream
      << sfm_data.GetIntrinsics().size() << '\n'
      << sfm_data.GetViews().size() << '\n'
      << sfm_data.GetLandmarks().size() << '\n';

    const Intrinsics & intrinsics = sfm_data.GetIntrinsics();
    for (Intrinsics::const_iterator iterIntrinsic = intrinsics.begin();
      iterIntrinsic != intrinsics.end(); ++iterIntrinsic)
    {
      //get params
      const std::vector<double> intrinsicsParams = iterIntrinsic->second.get()->getParams();
      std::copy(intrinsicsParams.begin(), intrinsicsParams.end(),
        std::ostream_iterator<double>(stream, " "));
      stream << '\n';
    }

    const Poses & poses = sfm_data.GetPoses();
    for (Views::const_iterator iterV = sfm_data.GetViews().begin();
      iterV != sfm_data.GetViews().end();
      ++ iterV)
    {
      const View * view = iterV->second.get();
      if (!sfm_data.IsPoseAndIntrinsicDefined(view))
      {
        const Mat3 R = Mat3::Identity();
        const double * rotation = R.data();
        std::copy(rotation, rotation+9, std::ostream_iterator<double>(stream, " "));
        const Vec3 C = Vec3::Zero();
        const double * center = C.data();
        std::copy(center, center+3, std::ostream_iterator<double>(stream, " "));
        stream << '\n';
      }
      else
      {
        // [Rotation col major 3x3; camera center 3x1]
        const double * rotation = poses.at(view->id_pose).rotation().data();
        std::copy(rotation, rotation+9, std::ostream_iterator<double>(stream, " "));
        const double * center = poses.at(view->id_pose).center().data();
        std::copy(center, center+3, std::ostream_iterator<double>(stream, " "));
        stream << '\n';
      }
    }

    const Landmarks & landmarks = sfm_data.GetLandmarks();
    for (Landmarks::const_iterator iterLandmarks = landmarks.begin();
      iterLandmarks != landmarks.end();
      ++iterLandmarks)
    {
      // Export visibility information
      // X Y Z #observations id_cam id_pose x y ...
      const double * X = iterLandmarks->second.X.data();
      std::copy(X, X+3, std::ostream_iterator<double>(stream, " "));
      const Observations & obs = iterLandmarks->second.obs;
      stream << obs.size() << " ";
      for (Observations::const_iterator iterOb = obs.begin();
        iterOb != obs.end(); ++iterOb)
      {
        const IndexT id_view = iterOb->first;
        const View * v = sfm_data.GetViews().at(id_view).get();
        stream
          << v->id_intrinsic << ' '
          << v->id_pose << ' '
          << iterOb->second.x(0) << ' ' << iterOb->second.x(1) << ' ';
      }
      stream << '\n';
    }

    stream.flush();
    bOk = stream.good();
    stream.close();
  }

  // Export View filenames & ids as an imgList.txt file
  {
//.........这里部分代码省略.........

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++) {
//.........这里部分代码省略.........