void EdgeSE3Prior::initialEstimate(const OptimizableGraph::VertexSet& /*from_*/, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *v = static_cast<VertexSE3*>(_vertices[0]);
SE3Quat newEstimate = _offsetParam->offset().inverse() * measurement();
if (_information.block<3,3>(0,0).squaredNorm()==0){ // do not set translation
newEstimate.setTranslation(v->estimate().translation());
}
if (_information.block<3,3>(3,3).squaredNorm()==0){ // do not set rotation
newEstimate.setRotation(v->estimate().rotation());
}
v->setEstimate(newEstimate);
}
void jac_quat3_euler3(Eigen::Matrix<double, 6, 6>& J, const SE3Quat& t)
{
const Vector3d& tr0 = t.translation();
const Quaterniond& q0 = t.rotation();
double delta=1e-6;
double idelta= 1. / (2. * delta);
for (int i=0; i<6; i++){
SE3Quat ta, tb;
if (i<3){
Vector3d tra=tr0;
Vector3d trb=tr0;
tra[i] -= delta;
trb[i] += delta;
ta = SE3Quat(q0, tra);
tb = SE3Quat(q0, trb);
} else {
Quaterniond qa=q0;
Quaterniond qb=q0;
if (i == 3) {
qa.x() -= delta;
qb.x() += delta;
}
else if (i == 4) {
qa.y() -= delta;
qb.y() += delta;
}
else if (i == 5) {
qa.z() -= delta;
qb.z() += delta;
}
qa.normalize();
qb.normalize();
ta = SE3Quat(qa, tr0);
tb = SE3Quat(qb, tr0);
}
Vector3d dtr = (tb.translation() - ta.translation())*idelta;
Vector3d taAngles, tbAngles;
quat_to_euler(ta.rotation(), taAngles(2), taAngles(1), taAngles(0));
quat_to_euler(tb.rotation(), tbAngles(2), tbAngles(1), tbAngles(0));
Vector3d da = (tbAngles - taAngles) * idelta; //TODO wraparounds not handled
for (int j=0; j<6; j++){
if (j<3){
J(j, i) = dtr(j);
} else {
J(j, i) = da(j-3);
}
}
}
}
inline Matrix3d d_Tinvpsi_d_psi(const SE3Quat & T, const Vector3d & psi) {
Matrix3d R = T.rotation().toRotationMatrix();
Vector3d x = invert_depth(psi);
Vector3d r1 = R.col(0);
Vector3d r2 = R.col(1);
Matrix3d J;
J.col(0) = r1;
J.col(1) = r2;
J.col(2) = -R*x;
J*=1./psi.z();
return J;
}
Isometry3d fromSE3Quat(const SE3Quat& t)
{
Isometry3d result = (Eigen::Isometry3d) t.rotation();
result.translation() = t.translation();
return result;
}
int main(int argc, char **argv)
{
int num_points = 0;
// check for arg, # of points to use in projection SBA
if (argc > 1)
num_points = atoi(argv[1]);
double euc_noise = 0.1; // noise in position, m
double pix_noise = 1.0; // pixel noise
// double outlier_ratio = 0.1;
SparseOptimizer optimizer;
optimizer.setVerbose(false);
// variable-size block solver
BlockSolverX::LinearSolverType * linearSolver
= new LinearSolverCholmod<g2o
::BlockSolverX::PoseMatrixType>();
BlockSolverX * solver_ptr
= new BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
vector<Vector3d> true_points;
for (size_t i=0;i<1000; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
// set up camera params
Vector2d focal_length(500,500); // pixels
Vector2d principal_point(320,240); // 640x480 image
double baseline = 0.075; // 7.5 cm baseline
// set up camera params and projection matrices on vertices
g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
principal_point[0],principal_point[1],
baseline);
// set up two poses
int vertex_id = 0;
for (size_t i=0; i<2; ++i)
{
// set up rotation and translation for this node
Vector3d t(0,0,i);
Quaterniond q;
q.setIdentity();
SE3Quat cam(q,t); // camera pose
// set up node
VertexSCam *vc = new VertexSCam();
vc->setEstimate(cam);
vc->setId(vertex_id); // vertex id
cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;
// set first cam pose fixed
if (i==0)
vc->setFixed(true);
// make sure projection matrices are set
vc->setAll();
// add to optimizer
optimizer.addVertex(vc);
vertex_id++;
}
// set up point matches for GICP
for (size_t i=0; i<true_points.size(); ++i)
{
// get two poses
VertexSE3* vp0 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
VertexSE3* vp1 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
// calculate the relative 3D position of the point
Vector3d pt0,pt1;
pt0 = vp0->estimate().inverse().map(true_points[i]);
pt1 = vp1->estimate().inverse().map(true_points[i]);
// add in noise
pt0 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
pt1 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
// form edge, with normals in varioius positions
Vector3d nm0, nm1;
nm0 << 0, i, 1;
nm1 << 0, i, 1;
nm0.normalize();
nm1.normalize();
Edge_V_V_GICP * e // new edge with correct cohort for caching
= new Edge_V_V_GICP();
e->vertices()[0] // first viewpoint
= dynamic_cast<OptimizableGraph::Vertex*>(vp0);
e->vertices()[1] // second viewpoint
= dynamic_cast<OptimizableGraph::Vertex*>(vp1);
EdgeGICP meas;
meas.pos0 = pt0;
meas.pos1 = pt1;
meas.normal0 = nm0;
meas.normal1 = nm1;
e->setMeasurement(meas);
meas = e->measurement();
// e->inverseMeasurement().pos() = -kp;
// use this for point-plane
e->information() = meas.prec0(0.01);
// use this for point-point
// e->information().setIdentity();
// e->setRobustKernel(true);
e->setHuberWidth(0.01);
optimizer.addEdge(e);
}
// set up SBA projections with some number of points
true_points.clear();
for (int i=0;i<num_points; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
// add point projections to this vertex
for (size_t i=0; i<true_points.size(); ++i)
{
g2o::VertexSBAPointXYZ * v_p
= new g2o::VertexSBAPointXYZ();
v_p->setId(vertex_id++);
v_p->setMarginalized(true);
v_p->setEstimate(true_points.at(i)
+ Vector3d(Sample::gaussian(1),
Sample::gaussian(1),
Sample::gaussian(1)));
optimizer.addVertex(v_p);
for (size_t j=0; j<2; ++j)
{
Vector3d z;
dynamic_cast<g2o::VertexSCam*>
(optimizer.vertices().find(j)->second)
->mapPoint(z,true_points.at(i));
if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
{
z += Vector3d(Sample::gaussian(pix_noise),
Sample::gaussian(pix_noise),
Sample::gaussian(pix_noise/16.0));
g2o::Edge_XYZ_VSC * e
= new g2o::Edge_XYZ_VSC();
e->vertices()[0]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
e->vertices()[1]
= dynamic_cast<g2o::OptimizableGraph::Vertex*>
(optimizer.vertices().find(j)->second);
e->setMeasurement(z);
//e->inverseMeasurement() = -z;
e->information() = Matrix3d::Identity();
e->setRobustKernel(false);
e->setHuberWidth(1);
optimizer.addEdge(e);
}
}
} // done with adding projection points
// move second cam off of its true position
VertexSE3* vc =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
SE3Quat cam = vc->estimate();
cam.setTranslation(Vector3d(-0.1,0.1,0.2));
vc->setEstimate(cam);
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
optimizer.setVerbose(true);
optimizer.optimize(20);
cout << endl << "Second vertex should be near 0,0,1" << endl;
cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
->estimate().translation().transpose() << endl;
cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
->estimate().translation().transpose() << endl;
}
