C++ Quaterniond::coeffs方法代码示例

// 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;
}

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);
//.........这里部分代码省略.........

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 ));
//.........这里部分代码省略.........

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));
//.........这里部分代码省略.........