Add fixed y camera position to g2o optimization

[ghost]

Hi, I am using images collected from a car to run ORB-SLAM2. I found there was sever scale shift in the SLAM results from my monocular data. So I would like to add constraints to make it more stable. One thing I can do is to assume the car was driving on a flat surface so it had a fixed y position. Basically, from frame to frame, the y position of the car did not change. However, I do not know how to add this to the g2o code in Optimizer.cc. Can anyone help?

[sjulier]

You could add a unitary edge to each camera pose which would provide a prior constraint on the y coordinate of the camera. You'd need to check if the vertex stores world-to-camera or camera-to-world transformation. For the constraint you want, I think you need the world-to-camera transformation.

[ghost]

Hee#define CONSTANT_Y_EDGE_WEIGHT 1

class ConstantYEdge : public g2o::BaseUnaryEdge<1, double, g2o::VertexSE3Expmap> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ConstantYEdge(double y) : BaseUnaryEdge(), _y(y) {}

void computeError()
{
    const g2o::VertexSE3Expmap* v = static_cast<const g2o::VertexSE3Expmap*> (_vertices[0]);
    g2o::SE3Quat SE3quat_recov = v->estimate();
    cv::Mat pose = Converter::toCvMat(SE3quat_recov); 
    //cout << pose << endl;
    _error(0, 0) = CONSTANT_Y_EDGE_WEIGHT * (pose.at<float>(1, 3) - _y);
}
virtual bool read(istream& in) { return true; }
virtual bool write(ostream& out) const { return true; }

public: double _y;

};

int Optimizer::PoseOptimization(Frame pFrame) { g2o::SparseOptimizer optimizer; g2o::BlockSolver_6_3::LinearSolverType linearSolver;

linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();

g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);

g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);

int nInitialCorrespondences=0;

// Set Frame vertex
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();

cv::Mat Tcw = pFrame->mTcw;

vSE3->setEstimate(Converter::toSE3Quat(Tcw));
//vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
vSE3->setId(0);
vSE3->setFixed(false);
optimizer.addVertex(vSE3);

cv::Mat cameraCenter = pFrame->GetCameraCenter();
ConstantYEdge* yedge = new ConstantYEdge(cameraCenter.at<float>(1));
yedge->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
optimizer.addEdge(yedge);