1

我用 g2o 定义了一个派生类：

 class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2,Eigen::Vector2d,g2o::VertexSE3Expmap>
  {
    public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    virtual void computeError();
    virtual void linearizeOplus();
    virtual bool read(std::istream& in) {}
    virtual bool write(std::ostream& os) const {}

    Vector3d point_;
    Camera* camera_;    

  };

我重写了computerError() 和linearizeOplus()：

void EdgeProjectXYZ2UVPoseOnly::computeError()
  {
    const g2o::VertexSE3Expmap* pose=static_cast<const g2o::VertexSE3Expmap*>(_vertices[0]);
    _error=_measurement-camera_->camera2pixel(pose->estimate().map(point_));
  }

  void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()
  {
    g2o::VertexSE3Expmap* pose=static_cast<g2o::VertexSE3Expmap*>(_vertices[0]);
    g2o::SE3Quat T(pose->estimate());
    Vector3d xyz_trans=T.map(point_);
    double x=xyz_trans[0];
    double y=xyz_trans[1];
    double z=xyz_trans[2];
    double zz=z*z;

    _jacobianOplusXi(0,0)=x*y*camera_->fx_/zz;
    _jacobianOplusXi(0,1)=-(1+x*x/zz)*camera_->fx_;
    _jacobianOplusXi(0,2)=y*camera_->fx_/z;
    _jacobianOplusXi(0,3)=-camera_->fx_/z;
    _jacobianOplusXi(0,4)=0;
    _jacobianOplusXi(0,5)=x*camera_->fx_/zz;

    _jacobianOplusXi(1,0)=camera_->fy_*(1+y*y/zz);
    _jacobianOplusXi(1,1)=-x*y*camera_->fy_/zz;
    _jacobianOplusXi(1,2)=-x*camera_->fy_/z;
    _jacobianOplusXi(1,3)=0;
    _jacobianOplusXi(1,4)=-camera_->fy_/z;
    _jacobianOplusXi(1,5)=y*camera_->fy_/zz;

  }

通过以下代码优化姿势（变量Tcr_estimated_和内联器的输出结果cv::solvePnPRansac()）：

 typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,2>> Block; 
     Block::LinearSolverType* linearSolver=new g2o::LinearSolverDense<Block::PoseMatrixType>();
     Block* solver_ptr=new Block(linearSolver);
     g2o::OptimizationAlgorithmLevenberg* solver=new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
     g2o::SparseOptimizer optimizer;
     optimizer.setAlgorithm(solver);

     g2o::VertexSE3Expmap* pose=new g2o::VertexSE3Expmap();
     pose->setId(0);
     pose->setEstimate(g2o::SE3Quat(Tcr_estimated_.rotation_matrix(),Tcr_estimated_.translation()));     
     optimizer.addVertex(pose);

     for(int i=0;i<inliers.rows;i++)
     {
       int index=inliers.at<int>(i,0);
       myVO::EdgeProjectXYZ2UVPoseOnly* edge=new myVO::EdgeProjectXYZ2UVPoseOnly();
       edge->setId(i);
       edge->setVertex(0,pose);
       edge->point_=Vector3d(pts3d[index].x,pts3d[index].y,pts3d[index].z);
       edge->camera_=curr_->camera_.get();
       edge->setMeasurement(Vector2d(pts2d[index].x,pts2d[index].y));
       edge->setInformation(Eigen::Matrix2d::Identity());     
       optimizer.addEdge(edge);
     }     

     optimizer.initializeOptimization();    
     optimizer.optimize(10);

在运行整个系统时，终端给出以下错误信息：

 /usr/include/eigen3/Eigen/src/Core/MapBase.h:168:void Eigen::MapBase<Derived, 0>::checkSanity() const [with Derived = Eigen::Map<Eigen::Matrix<double, 2, 6, 0, 2, 6>, 32, Eigen::Stride<0, 0> >]: suppose '((size_t(m_data) % (((int)1 >= (int)internal::traits<Derived>::Alignment) ? (int)1:  (int)internal::traits<Derived>::Alignment)) == 0) && "data is not aligned"'fails.

我确认问题来自优化部分。谁能帮忙？

4

0 回答 0