I defined a derived class with 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_;
};
I rewrite computerError() and 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;
}
Optimizing poses via following codes(variables Tcr_estimated_ and inliners result from outputs of 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);
While running the whole system, the terminal gave following error information:
/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.
I confirms problem comes from the optimization parts. Who can help?