// qEst = qEst + (qDot - beta*gradF.normalised)*deltaT
// inputs: qEst, w, a, deltaT, beta
// output: qEst
void UpdateAttitude( Quaterniond& qEst, // Reference to the current esitmate- will be update to reflect the new estimate
const Quaterniond& w, // [0, wx, wy, wz] rad/s
const Quaterniond& a, // [0, ax, ay, az] m/s/s
const double deltaT_sec,// sample period (seconds)
const double beta ) // Gain factor to account for all zero mean noise (sqrt(3/4)*[o,wx_noise, wy_noise, wz_noise])
{
// rate of change of orientation
Quaterniond qDot;
qDot = (qEst*w).coeffs() * 0.5;
// Jacobian of the objective function F
MatrixXd J(3,4);
J << -2*qEst.y(), 2*qEst.z(), -2*qEst.w(), 2*qEst.x(),
2*qEst.x(), 2*qEst.w(), 2*qEst.z(), 2*qEst.y(),
0, -4*qEst.x(), -4*qEst.y(), 0;
cout << J << endl;
// The objective function F minimising a measured gravitational field with an assumed gravity vector of 0,0,1
MatrixXd F(3,1);
F << 2*(qEst.x()*qEst.z() - qEst.w()*qEst.y()) - a.x(),
2*(qEst.w()*qEst.x() + qEst.y()*qEst.z()) - a.y(),
2*(0.5 - qEst.x()*qEst.x() - qEst.y()*qEst.y()) - a.z();
cout << F << endl;
// The gradient of the solution solution surface
MatrixXd gradF(4,1);
gradF = J.transpose() * F;
//qEst = qEst + (qDot - beta*gradF.normalized)*deltaT
qEst.coeffs() += (qDot.coeffs() - beta*gradF.normalized())*deltaT_sec;
}
bool VertexCam::read(std::istream& is)
{
// first the position and orientation (vector3 and quaternion)
Vector3d t;
for (int i=0; i<3; i++){
is >> t[i];
}
Vector4d rc;
for (int i=0; i<4; i++) {
is >> rc[i];
}
Quaterniond r;
r.coeffs() = rc;
r.normalize();
// form the camera object
SBACam cam(r,t);
// now fx, fy, cx, cy, baseline
double fx, fy, cx, cy, tx;
// try to read one value
is >> fx;
if (is.good()) {
is >> fy >> cx >> cy >> tx;
cam.setKcam(fx,fy,cx,cy,tx);
} else{
void mouse_drag(int mouse_x, int mouse_y)
{
using namespace igl;
using namespace std;
using namespace Eigen;
bool tw_using = TwMouseMotion(mouse_x,mouse_y);
if(is_rotating)
{
glutSetCursor(GLUT_CURSOR_CYCLE);
Quaterniond q;
auto & camera = s.camera;
switch(rotation_type)
{
case ROTATION_TYPE_IGL_TRACKBALL:
{
// Rotate according to trackball
igl::trackball<double>(
width,
height,
2.0,
down_camera.m_rotation_conj.coeffs().data(),
down_x,
down_y,
mouse_x,
mouse_y,
q.coeffs().data());
break;
}
case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
{
// Rotate according to two axis valuator with fixed up vector
two_axis_valuator_fixed_up(
width, height,
2.0,
down_camera.m_rotation_conj,
down_x, down_y, mouse_x, mouse_y,
q);
break;
}
default:
break;
}
switch(center_type)
{
default:
case CENTER_TYPE_ORBIT:
camera.orbit(q.conjugate());
break;
case CENTER_TYPE_FPS:
camera.turn_eye(q.conjugate());
break;
}
}
}
Quaterniond propagateQUATERNIONS(Quaterniond& quaternions,
double time,
double timeStep,
Vector3d bodyrates)
{
VectorXd parameters(3);
parameters << bodyrates[0], bodyrates[1], bodyrates[2];
VectorXd quats(4);
quats << quaternions.coeffs().coeffRef(0), quaternions.coeffs().coeffRef(1),
quaternions.coeffs().coeffRef(2), quaternions.coeffs().coeffRef(3);
rk4 (quats, 4, time, timeStep, derivQUATERNIONS, parameters);
//Transform VectorXd to quaternions
Quaterniond propagatedQuaternion(quats(0),quats(1), quats(2),quats(3));
return propagatedQuaternion;
}
int main()
{ // Quaterniond specified in (w,x,y,z)
Quaterniond qEst = Quaterniond(0,0,0,1); // unit z (normalized) // initial guess
Quaterniond w = Quaterniond(0, M_PI*0.5, 0, 0); // pi/2 rad/s around x
Quaterniond a = Quaterniond(0, 0, 0, 1); //(0, ax, ay, az) in m/s/s (normalized)
AngleAxisd offset = AngleAxisd(M_PI/10,Vector3d::UnitX());
a = a*offset;
cout << "a: " <<endl << a.coeffs() <<endl;
double deltaT = 0.01;
double beta = 0.033;
UpdateAttitude(qEst, w, a, deltaT, beta);
cout << "qEst:" << endl << qEst.coeffs() << endl;
// manual quaternion multiplication
Vector4d result;
result <<
qEst.w()*w.w() - qEst.x()*w.x() - qEst.y()*w.y() - qEst.z()*w.z() ,
qEst.w()*w.x() + qEst.x()*w.w() + qEst.y()*w.z() - qEst.z()*w.y() ,
qEst.w()*w.y() - qEst.x()*w.z() + qEst.y()*w.w() + qEst.z()*w.x() ,
qEst.w()*w.z() + qEst.x()*w.y() - qEst.y()*w.x() + qEst.z()*w.w() ;
cout << "manual qDot: " << endl << 0.5*result << endl;
}
void mouse_drag(int mouse_x, int mouse_y)
{
using namespace igl;
using namespace Eigen;
if(is_rotating)
{
glutSetCursor(GLUT_CURSOR_CYCLE);
Quaterniond q;
auto & camera = s.camera;
switch(rotation_type)
{
case ROTATION_TYPE_IGL_TRACKBALL:
{
// Rotate according to trackball
igl::trackball<double>(
width,
height,
2.0,
down_camera.m_rotation_conj.coeffs().data(),
down_x,
down_y,
mouse_x,
mouse_y,
q.coeffs().data());
break;
}
case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
{
// Rotate according to two axis valuator with fixed up vector
two_axis_valuator_fixed_up(
width, height,
2.0,
down_camera.m_rotation_conj,
down_x, down_y, mouse_x, mouse_y,
q);
break;
}
default:
break;
}
camera.orbit(q.conjugate());
}else
{
TwEventMouseMotionGLUT(mouse_x, mouse_y);
}
glutPostRedisplay();
}
int main()
{
double euc_noise = 0.01; // noise in position, m
// double outlier_ratio = 0.1;
SparseOptimizer optimizer;
optimizer.setVerbose(false);
// variable-size block solver
BlockSolverX::LinearSolverType * linearSolver = new LinearSolverDense<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 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();
Eigen::Isometry3d cam; // camera pose
cam = q;
cam.translation() = t;
// set up node
VertexSE3 *vc = new VertexSE3();
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);
// add to optimizer
optimizer.addVertex(vc);
vertex_id++;
}
// set up point matches
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() * true_points[i];
pt1 = vp1->estimate().inverse() * 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->setVertex(0, vp0); // first viewpoint
e->setVertex(1, vp1); // second viewpoint
EdgeGICP meas;
meas.pos0 = pt0;
meas.pos1 = pt1;
meas.normal0 = nm0;
meas.normal1 = nm1;
e->setMeasurement(meas);
// e->inverseMeasurement().pos() = -kp;
meas = e->measurement();
// 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);
}
// move second cam off of its true position
VertexSE3* vc =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
Eigen::Isometry3d cam = vc->estimate();
cam.translation() = Vector3d(0,0,0.2);
vc->setEstimate(cam);
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
optimizer.setVerbose(true);
optimizer.optimize(5);
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;
}
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 LinearSolverCSparse<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();
Eigen::Isometry3d cam; // camera pose
cam = q;
cam.translation() = t;
// 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() * true_points[i];
pt1 = vp1->estimate().inverse() * 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);
Eigen::Isometry3d cam = vc->estimate();
cam.translation() = 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;
}
void mouse_drag(int mouse_x, int mouse_y)
{
using namespace igl;
using namespace std;
using namespace Eigen;
if(is_rotating)
{
glutSetCursor(GLUT_CURSOR_CYCLE);
Quaterniond q;
switch(rotation_type)
{
case ROTATION_TYPE_IGL_TRACKBALL:
{
// Rotate according to trackball
igl::trackball<double>(
width,
height,
2.0,
down_camera.m_rotation_conj.coeffs().data(),
down_x,
down_y,
mouse_x,
mouse_y,
q.coeffs().data());
break;
}
case ROTATION_TYPE_TWO_AXIS_VALUATOR_FIXED_UP:
{
// Rotate according to two axis valuator with fixed up vector
two_axis_valuator_fixed_up(
width, height,
2.0,
down_camera.m_rotation_conj,
down_x, down_y, mouse_x, mouse_y,
q);
break;
}
default:
break;
}
camera.orbit(q.conjugate());
}
if(is_dragging)
{
push_scene();
push_object();
if(new_leaf_on_drag)
{
assert(s.C.size() >= 1);
// one new node
s.C.conservativeResize(s.C.rows()+1,3);
const int nc = s.C.rows();
assert(s.sel.size() >= 1);
s.C.row(nc-1) = s.C.row(s.sel(0));
// one new bone
s.BE.conservativeResize(s.BE.rows()+1,2);
s.BE.row(s.BE.rows()-1) = RowVector2i(s.sel(0),nc-1);
// select just last node
s.sel.resize(1,1);
s.sel(0) = nc-1;
// reset down_C
down_C = s.C;
new_leaf_on_drag = false;
}
if(new_root_on_drag)
{
// two new nodes
s.C.conservativeResize(s.C.rows()+2,3);
const int nc = s.C.rows();
Vector3d obj;
int nhits = unproject_in_mesh(mouse_x,height-mouse_y,ei,obj);
if(nhits == 0)
{
Vector3d pV_mid = project(Vcen);
obj = unproject(Vector3d(mouse_x,height-mouse_y,pV_mid(2)));
}
s.C.row(nc-2) = obj;
s.C.row(nc-1) = obj;
// select last node
s.sel.resize(1,1);
s.sel(0) = nc-1;
// one new bone
s.BE.conservativeResize(s.BE.rows()+1,2);
s.BE.row(s.BE.rows()-1) = RowVector2i(nc-2,nc-1);
// reset down_C
down_C = s.C;
new_root_on_drag = false;
}
double z = 0;
Vector3d obj,win;
int nhits = unproject_in_mesh(mouse_x,height-mouse_y,ei,obj);
project(obj,win);
z = win(2);
for(int si = 0;si<s.sel.size();si++)
{
const int c = s.sel(si);
Vector3d pc = project((RowVector3d) down_C.row(c));
pc(0) += mouse_x-down_x;
pc(1) += (height-mouse_y)-(height-down_y);
if(nhits > 0)
{
pc(2) = z;
}
s.C.row(c) = unproject(pc);
}
pop_object();
pop_scene();
}
glutPostRedisplay();
}