ошибка: «данные не выровнены» от Eigen при выполнении настройки пучка с помощью g2o

Я определил производный класс с помощью 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_ а также inliners результат от выходов 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.

Я подтверждаю, что проблема исходит от оптимизации частей. Кто может помочь?

1

Решение

Задача ещё не решена.

Другие решения

Других решений пока нет …

По вопросам рекламы [email protected]