求解结果如下:

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

在这里插入图片描述

mat 初始化,eigenvalue初始化

Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
Eigen::Matrix<float,3,1> vd_3d;
  v_3d << 3, 2, 1;

求解目标函数结构体构造与实例

 struct CurveFittingCost {
   CurveFittingCost(Point2d point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}

   template<typename T>
   bool operator() (
  	 const T *const camera_pose,
  	 T *residual)const
   {
  	 T point[3];
  	 point[0] = T(_point3d.depth_x);
  	 point[1] = T(_point3d.depth_x);
  	 point[2] = T(_point3d.depth_z);
  	 T p[3];
  	 ceres::AngleAxisRotatePoint(camera_pose, point, p);
  	 p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] += camera_pose[5];
  	 //p[0] / p[2],p[1] / p[2] is norm coordinate,the real depth id p[2]
  	 T x_p = p[0] / p[2];
  	 T y_p = p[1] / p[2];
  	 T predicted_x = x_p * static_cast<T>(camera_matrix_temp(0, 0)) + static_cast<T>(camera_matrix_temp(0, 2));
  	 T predicted_y = y_p * static_cast<T>(camera_matrix_temp(1, 1)) + static_cast<T>(camera_matrix_temp(1, 2));
  	 residual[0] = T(_point2d.x) - predicted_x;
  	 residual[1] = T(_point2d.y) - predicted_y;
  	 return true;
   }
   
   static ceres::CostFunction* Create( Point2d point2d, Point3f point3d) {
  	 return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(
  		 new CurveFittingCost(point2d, point3d)));
   }
   const Point2d _point2d;
   const Point3f _point3d;
  };

   struct CurveFittingCost {
  	CurveFittingCost(Point2f point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}
  	
  	template<typename T>
  	bool operator() (
  		const T *const camera_pose,
  		T *residual)const
  	{
  		T point[3];
  		point[0] = T(_point3d.depth_x);
  		point[1] = T(_point3d.depth_y);
  		point[2] = T(_point3d.depth_z);
  		T p[3];
  		ceres::AngleAxisRotatePoint(camera_pose, point, p);
  		p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
  		p[0] / p[2],p[1] / p[2] is norm coordinate,the real depth id p[2]
  		T predicted_x = p[0] / p[2] * camera_matrix(0,0) + camera_matrix(0,2);
  		T predicted_y = p[1] / p[2] * camera_matrix(1,1) + camera_matrix(1,2);
  		residual[0] = T(_point2d.x) - predicted_x;
  		residual[1] = T(_point2d.y) - predicted_y;
  		return true;
  	}
  
  	static ceres::CostFunction* Create(const Point2f point2d, const Point3f point3d) {
  		return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(
  			new CurveFittingCost(point2d, point3d)));
  	}
  	const Point2f _point2d;
  	const Point3f _point3d;
  };

   struct PnPCeres
  {
   PnPCeres(Point2f uv, Point3f xyz) : _uv(uv), _xyz(xyz) {}
   // 残差的计算
   template <typename T>
   bool operator() (
    const T *const camera, // 位姿参数,有6维
    T* residual) const     // 残差
   {
    T p[3];
    T point[3];
    point[0] = T(_xyz.x);
    point[1] = T(_xyz.y);
    point[2] = T(_xyz.z);
    ceres::AngleAxisRotatePoint(camera, point, p);//计算RP
    p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5];
    T xp = p[0] / p[2];
    T yp = p[1] / p[2];//xp,yp是归一化坐标,深度为p[2]
    T u_ = xp*camera_matrix(0, 0) + camera_matrix(0, 2);
    T v_ = yp*camera_matrix(1, 1) + camera_matrix(1, 2);
    residual[0] = T(_uv.x) - u_;
    residual[1] = T(_uv.y) - v_;
    return true;
   }
   static ceres::CostFunction* Create(const Point2f uv, const Point3f xyz) {
    return (new ceres::AutoDiffCostFunction<PnPCeres, 2, 6>(
  	  new PnPCeres(uv, xyz)));
   }
   const Point2f _uv;
   const Point3f _xyz;

  };

目标函数调用方式

struct CurveFittingCost {
 CurveFittingCost(Point2d point2d, Point3f point3d) : _point2d(point2d), _point3d(point3d) {}

 template<typename T>
 bool operator() (
	 const T *const camera_pose,
	 T *residual)const
 {
	 T point[3];
	 point[0] = T(_point3d.depth_x);
	 point[1] = T(_point3d.depth_x);
	 point[2] = T(_point3d.depth_z);
	 T p[3];
	 ceres::AngleAxisRotatePoint(camera_pose, point, p);
	 p[0] += camera_pose[3]; p[1] += camera_pose[4]; p[2] += camera_pose[5];
	 //p[0] / p[2],p[1] / p[2] is norm coordinate,the real depth id p[2]
	 T x_p = p[0] / p[2];
	 T y_p = p[1] / p[2];
	 T predicted_x = x_p * static_cast<T>(camera_matrix_temp(0, 0)) + static_cast<T>(camera_matrix_temp(0, 2));
	 T predicted_y = y_p * static_cast<T>(camera_matrix_temp(1, 1)) + static_cast<T>(camera_matrix_temp(1, 2));
	 residual[0] = T(_point2d.x) - predicted_x;
	 residual[1] = T(_point2d.y) - predicted_y;
	 return true;
 }
 
 static ceres::CostFunction* Create( Point2d point2d, Point3f point3d) {
	 return (new ceres::AutoDiffCostFunction<CurveFittingCost, 2, 6>(
		 new CurveFittingCost(point2d, point3d)));
 }
 const Point2d _point2d;
 const Point3f _point3d;
};

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐